おもしろデバイス.jp

目次へ戻る

今回も WPF の 3D 機能を使った Leap Motion のサンプルアプリケーションの続きです。
前回は手のひらだけでしたが、今回は指も描画します。

Leap Motion では指以外にペンなどのツールも認識でき、指とツールはまとめて Frame オブジェクトの Pointables プロパティに格納されます。Frame オブジェクトはまた、指のみの Fingers プロパティ、ツールのみの Tools プロパティも持っているのですが、今回は Fingers プロパティではなく、Pointables プロパティを使いたいと思います。


Pointable オブジェクトも、手のひらと同じく、突然検出されなくなったり、新しく検出できたりすることに備えておく必要があります。
やはり Pointable オブジェクトも Id を持っており、前回処理時と同じかどうかは Id で調べることができます。


指先を描画するときの考え方は手のひらの場合とほぼ同じですが、Pointable オブジェクトには手のひらの法線ベクトルに相当する情報がありません。
TipPosition プロパティは指先の位置を表しています。
Direction プロパティは指先の方向ベクトルです。

指先のモデルも直方体にしますので、法線ベクトルを考えないと指先方向を軸に回転してしまうことになります。
したがって今回は手のひらの法線ベクトルを利用することにします。
非常におおざっぱですが、指先の法線ベクトルは指先の方向ベクトルと手のひらの法線ベクトルの平面上にあるものと想定して、指先の法線ベクトルを導きだすわけです。


指先のモデルは、Z軸のマイナス方向に伸びた細長い直方体です。
描画時は

1. Direction プロパティに合わせるようにモデルを回転
2. Hand オブジェクトの PalmNormal プロパティから考えた法線ベクトルに合わせるようにモデルを回転
3. TipPosition プロパティに合わせるようにモデルを平行移動

としています。


MainWindows.xaml.cs を以下のように書き換えます。

using System;
using System.Collections;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Media.Media3D;
using System.Windows.Navigation;
using System.Windows.Shapes;
 
namespace LeapSample
{
    /// <summary>
    /// MainWindow.xaml の相互作用ロジック
    /// </summary>
    public partial class MainWindow : Window
    {
        private Leap.Controller controller = new Leap.Controller();
        private ModelVisual3D trackingAreaModel = null;
        private Hashtable hands = new Hashtable();
        private Hashtable pointables = new Hashtable();
 
        public MainWindow()
        {
            InitializeComponent();
 
            var root = this.Content as Grid;
            root.Children.Add(new TextBlock());
 
            Init();
 
            CompositionTarget.Rendering += CompositionTarget_Rendering;
        }
 
        private void CompositionTarget_Rendering(object sender, EventArgs e)
        {
            var root = this.Content as Grid;
            var textBlock = root.Children[0] as TextBlock;
 
            Leap.Frame frame = controller.Frame();
            string msg = string.Format("IsValid={0}\n", frame.IsValid);
            msg += string.Format("Id={0}\n", frame.Id);
            msg += string.Format("Timestamp={0}\n", frame.Timestamp);
            msg += string.Format("InteractionBox={0}\n", frame.InteractionBox);
            msg += string.Format("CurrentFramesPerSecond={0}\n", frame.CurrentFramesPerSecond);
            msg += string.Format("Hands.Count={0}\n", frame.Hands.Count);
            msg += string.Format("Pointables.Count={0}\n", frame.Pointables.Count);
            msg += string.Format("Fingers.Count={0}\n", frame.Fingers.Count);
            msg += string.Format("Tools.Count={0}\n", frame.Tools.Count);
            textBlock.Text = msg;
 
            var td = root.Children[1] as _3DTools.TrackballDecorator;
            var viewport = td.Content as Viewport3D;
 
            if (frame.IsValid)
            {
                if (trackingAreaModel == null)
                {
                    trackingAreaModel = CreateTrackingAreaModel(frame.InteractionBox);
                    viewport.Children.Add(trackingAreaModel);
                }
            }
            else
            {
                viewport.Children.Remove(trackingAreaModel);
                trackingAreaModel = null;
            }
 
            Hashtable unusedHands = (Hashtable)hands.Clone();
 
            foreach (var hand in frame.Hands)
            {
                if (!hand.IsValid) continue;
                drawHand(hand);
                unusedHands.Remove(hand.Id);
            }
 
            foreach (var id in unusedHands.Keys)
            {
                viewport.Children.Remove((ModelVisual3D)unusedHands[id]);
                hands.Remove(id);
            }
 
            Hashtable unusedPointables = (Hashtable)pointables.Clone();
 
            foreach (var pointable in frame.Pointables)
            {
                if (!pointable.IsValid) continue;
                drawPointable(pointable);
                unusedPointables.Remove(pointable.Id);
            }
 
            foreach (var id in unusedPointables.Keys)
            {
                viewport.Children.Remove((ModelVisual3D)unusedPointables[id]);
                pointables.Remove(id);
            }
        }
 
        private void Init()
        {
            var root = this.Content as Grid;
 
            // trackball
            var td = new _3DTools.TrackballDecorator();
            root.Children.Add(td);
 
            // viewport
            var viewport = new Viewport3D();
            td.Content = viewport;
 
            // camera
            var camera = new PerspectiveCamera();
            camera.Position = new Point3D(0, 1000, 1200);
            camera.UpDirection = new Vector3D(0, 1, 0);
            camera.LookDirection = (new Point3D(0, 100, 0) - camera.Position);
            viewport.Camera = camera;
 
            // light
            var model = new ModelVisual3D();
            var light = new DirectionalLight { Color = Colors.White, Direction = new Vector3D(-1, -1, -1) };
            model.Content = light;
            viewport.Children.Add(model);
 
            // axis
            viewport.Children.Add(CreateAxisModel());
        }
 
        private ModelVisual3D CreateAxisModel()
        {
            var xAxis = new _3DTools.ScreenSpaceLines3D();
            xAxis.Points.Add(new Point3D(-2000, 0, 0));
            xAxis.Points.Add(new Point3D(2000, 0, 0));
            xAxis.Color = Colors.Red;
            xAxis.Thickness = 1;
            var yAxis = new _3DTools.ScreenSpaceLines3D();
            yAxis.Points.Add(new Point3D(0, -2000, 0));
            yAxis.Points.Add(new Point3D(0, 2000, 0));
            yAxis.Color = Colors.Green;
            yAxis.Thickness = 1;
            var zAxis = new _3DTools.ScreenSpaceLines3D();
            zAxis.Points.Add(new Point3D(0, 0, -2000));
            zAxis.Points.Add(new Point3D(0, 0, 2000));
            zAxis.Color = Colors.Blue;
            zAxis.Thickness = 1;
            var axis = new ModelVisual3D();
            axis.Children.Add(xAxis);
            axis.Children.Add(yAxis);
            axis.Children.Add(zAxis);
            return axis;
        }
 
        private ModelVisual3D CreateTrackingAreaModel(Leap.InteractionBox ib)
        {
            double k = (ib.Center.y + ib.Height / 2) / (ib.Center.y - ib.Height / 2);
            var lb = new Point3D((ib.Center.x - ib.Width / 2) * k, ib.Center.y + ib.Height / 2, (ib.Center.z - ib.Depth / 2) * k);    // left back
            var rb = new Point3D((ib.Center.x + ib.Width / 2) * k, ib.Center.y + ib.Height / 2, (ib.Center.z - ib.Depth / 2) * k);    // right back
            var rf = new Point3D((ib.Center.x + ib.Width / 2) * k, ib.Center.y + ib.Height / 2, (ib.Center.z + ib.Depth / 2) * k);    // right front
            var lf = new Point3D((ib.Center.x - ib.Width / 2) * k, ib.Center.y + ib.Height / 2, (ib.Center.z + ib.Depth / 2) * k);    // left front
            var origin = new Point3D(0, 0, 0);
 
            var m = new _3DTools.ScreenSpaceLines3D();
            m.Points.Add(lb);
            m.Points.Add(rb);
            m.Points.Add(rb);
            m.Points.Add(rf);
            m.Points.Add(rf);
            m.Points.Add(lf);
            m.Points.Add(lf);
            m.Points.Add(lb);
            m.Points.Add(origin);
            m.Points.Add(lb);
            m.Points.Add(origin);
            m.Points.Add(rb);
            m.Points.Add(origin);
            m.Points.Add(rf);
            m.Points.Add(origin);
            m.Points.Add(lf);
            m.Color = Colors.SlateGray;
            return m;
        }
 
        public ModelVisual3D GetCube(Color colour, Point3D nearPoint, Point3D farPoint)
        {
            var p0 = new Point3D(farPoint.X, farPoint.Y, farPoint.Z);
            var p1 = new Point3D(nearPoint.X, farPoint.Y, farPoint.Z);
            var p2 = new Point3D(nearPoint.X, farPoint.Y, nearPoint.Z);
            var p3 = new Point3D(farPoint.X, farPoint.Y, nearPoint.Z);
            var p4 = new Point3D(farPoint.X, nearPoint.Y, farPoint.Z);
            var p5 = new Point3D(nearPoint.X, nearPoint.Y, farPoint.Z);
            var p6 = new Point3D(nearPoint.X, nearPoint.Y, nearPoint.Z);
            var p7 = new Point3D(farPoint.X, nearPoint.Y, nearPoint.Z);
 
            var materialGroup = new MaterialGroup();
            materialGroup.Children.Add(new DiffuseMaterial(new SolidColorBrush(colour)));
            var specMat = new SpecularMaterial(new SolidColorBrush(Colors.White), 30);
            materialGroup.Children.Add(specMat);
 
            var cube = new Model3DGroup();
            cube.Children.Add(CreateRectangleModel(materialGroup, p7, p6, p2, p3));        // front side rectangle
            cube.Children.Add(CreateRectangleModel(materialGroup, p6, p5, p1, p2));        // right side rectangle
            cube.Children.Add(CreateRectangleModel(materialGroup, p5, p4, p0, p1));        // back side rectangle
            cube.Children.Add(CreateRectangleModel(materialGroup, p4, p7, p3, p0));        // left side rectangle
            cube.Children.Add(CreateRectangleModel(materialGroup, p4, p5, p6, p7));        // top side rectangle
            cube.Children.Add(CreateRectangleModel(materialGroup, p1, p0, p3, p2));        // bottom side rectangle
 
            var model = new ModelVisual3D();
            model.Content = cube;
            return model;
        }
 
        private GeometryModel3D CreateRectangleModel(MaterialGroup material, Point3D p1, Point3D p2, Point3D p3, Point3D p4)
        {
            var upperGeometry = new GeometryModel3D();
            var mesh = new MeshGeometry3D();
            mesh.Positions = new Point3DCollection(new Point3D[] { p1, p2, p3, p4, });
            mesh.TriangleIndices = new Int32Collection(new int[] { 0, 2, 1, 0, 3, 2 });
            mesh.TextureCoordinates = new PointCollection(new Point[] { new Point(0, 0), new Point(1, 0), new Point(1, 1), new Point(0, 1) });
            upperGeometry.Geometry = mesh;
            upperGeometry.Material = material;
            return upperGeometry;
        }
 
        private void drawHand(Leap.Hand hand)
        {
            ModelVisual3D model = null;
            if (!hands.Contains(hand.Id))
            {
                var root = this.Content as Grid;
                var td = root.Children[1] as _3DTools.TrackballDecorator;
                var viewport = td.Content as Viewport3D;
 
                model = GetCube(Colors.Red, new Point3D(30, 3, 30), new Point3D(-30, -3, -30));
                hands.Add(hand.Id, model);
                viewport.Children.Add(model);
            }
            else
            {
                model = (ModelVisual3D)hands[hand.Id];
            }
 
            var u1 = new Vector3D(0, -1, 0);
            var n1 = new Vector3D(hand.PalmNormal.x, hand.PalmNormal.y, hand.PalmNormal.z);
            var a1 = Vector3D.AngleBetween(u1, n1);
            Quaternion q1 = new Quaternion(Vector3D.CrossProduct(u1, n1), a1);
 
            var u2 = new Vector3D(0, 0, -1);
            Matrix3D m2 = new Matrix3D();
            m2.Rotate(q1);
            m2.Transform(u2);
            var d1 = new Vector3D(hand.Direction.x, hand.Direction.y, hand.Direction.z);
            var a2 = Vector3D.AngleBetween(u2, d1);
            Quaternion q2 = new Quaternion(Vector3D.CrossProduct(u2, d1), a2);
 
            var q = Quaternion.Multiply(q2, q1);
 
            Matrix3D m = new Matrix3D();
            m.Rotate(q);
            m.OffsetX = hand.PalmPosition.x;
            m.OffsetY = hand.PalmPosition.y;
            m.OffsetZ = hand.PalmPosition.z;
            model.Transform = new MatrixTransform3D(m);
        }
 
        private void drawPointable(Leap.Pointable pointable)
        {
            ModelVisual3D model = null;
            if (!pointables.Contains(pointable.Id))
            {
                var root = this.Content as Grid;
                var td = root.Children[1] as _3DTools.TrackballDecorator;
                var viewport = td.Content as Viewport3D;
 
                model = GetCube(Colors.Blue, new Point3D(5, 3, 20), new Point3D(-5, -3, 0));
                pointables.Add(pointable.Id, model);
                viewport.Children.Add(model);
            }
            else
            {
                model = (ModelVisual3D)pointables[pointable.Id];
            }
 
            Quaternion q = new Quaternion();
            if (pointable.Hand != null)
            {
                Leap.Hand hand = pointable.Hand;
                var d1 = new Vector3D(pointable.Direction.x, pointable.Direction.y, pointable.Direction.z);
                var n1 = new Vector3D(hand.PalmNormal.x, hand.PalmNormal.y, hand.PalmNormal.z);
                var n2 = Vector3D.CrossProduct(d1, n1);
                var v2 = Vector3D.CrossProduct(n2, d1);
 
                var v1 = new Vector3D(0, -1, 0);
                var a1 = Vector3D.AngleBetween(v1, v2);
                if (double.IsNaN(a1))
                {
                    return;
                }
                Quaternion q1 = new Quaternion(Vector3D.CrossProduct(v1, v2), a1);
 
                var v3 = new Vector3D(0, 0, -1);
                Matrix3D m2 = new Matrix3D();
                m2.Rotate(q1);
                m2.Transform(v3);
                var v4 = d1;
                var a2 = Vector3D.AngleBetween(v3, v4);
                if (double.IsNaN(a2))
                {
                    return;
                }
                Quaternion q2 = new Quaternion(Vector3D.CrossProduct(v3, v4), a2);
 
                q = Quaternion.Multiply(q2, q1);
            }
            else
            {
                var v3 = new Vector3D(0, 0, -1);
                var v4 = new Vector3D(pointable.Direction.x, pointable.Direction.y, pointable.Direction.z);
                var a2 = Vector3D.AngleBetween(v3, v4);
                Quaternion q2 = new Quaternion(Vector3D.CrossProduct(v3, v4), a2);
 
                q = q2;
            }
 
            Matrix3D m = new Matrix3D();
            m.Rotate(q);
            m.OffsetX = pointable.TipPosition.x;
            m.OffsetY = pointable.TipPosition.y;
            m.OffsetZ = pointable.TipPosition.z;
            var trans = new MatrixTransform3D(m);
            model.Transform = trans;
        }
    }
}

実行結果は以下となります。

Leap Motion デバイスの上で手を動かすと、それにあわせて、手のひらをあらわす赤い直方体と、指先をあらわす青い直方体が動きます。

以上です。

目次へ戻る