今回は3次元空間でのインバース・キネマティクス(Inverse Kinematics、IK)を試してみる。
(ただし、3次元空間でのインバース・キネマティクスの考え方が正しいかどうかは分かりません。もし間違いがあればご指摘ください。)

インバース・キネマティクスとは、末端の位置を決めたあと、関節の位置を決める手順のこと。たとえば肩と手の位置を決めたあと、肘の位置を決めるなど。末端が目標に近づくように、各関節を微調整することを繰り返すというもの。

インバース・キネマッティクスのクラスを作成した。以下のとおり。

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Media.Media3D;
 
namespace sample1
{
    public class Node
    {
        public int NodeType;
        public Point3D Point;
        public double Length;
        public double Angle;
        public Vector3D OriginalRotateAxis;
        public Vector3D RotateAxis;
        public Quaternion Quaternion;
 
        public Node(int nodeType, Point3D position, double length, double initialAngle)
        {
            this.NodeType = nodeType;
            this.Point = position;
            this.Length = length;
            this.Angle = initialAngle;
            if (this.NodeType == 0)
            {
                this.OriginalRotateAxis = new Vector3D(0, 0, 1);
            }
            else if (this.NodeType == 1)
            {
                this.OriginalRotateAxis = new Vector3D(0, 1, 0);
            }
 
            this.RotateAxis = this.OriginalRotateAxis;
        }
    }
 
    public static class InverseKinematics
    {
        public static bool MoveNodes(Node[] nodes, double moveAngle, Point3D target)
        {
            // 先端ノード
            Node tipNode = nodes[nodes.Length - 1];
 
            for(int i = nodes.Length - 2; i >= 0; i--)
            {
                Node currentNode = nodes[i];
 
                // 目標ベクトル(自ノードから目標へ向かうベクトル)
                Vector3D targetVector = target - currentNode.Point;
 
                // 先端ベクトル(自ノードから先端ノードへ向かうベクトル)
                Vector3D tipVector = tipNode.Point - currentNode.Point;
 
                // 目標ベクトルと先端ベクトルのなす角を求める
                double deltaAngle = 0;
                {
                    var q2 = new Quaternion();
                    var axis = Vector3D.CrossProduct(currentNode.RotateAxis, currentNode.OriginalRotateAxis);
                    if (axis.Length > 0)
                    {
                        var angle = Vector3D.AngleBetween(currentNode.RotateAxis, currentNode.OriginalRotateAxis);
                        q2 = new Quaternion(axis, angle);
                    }
                    var m2 = new Matrix3D();
                    m2.Rotate(q2);
                    var targetVector2 = m2.Transform(targetVector);
                    var tipVector2 = m2.Transform(tipVector);
                    if (currentNode.NodeType == 0)
                    {
                        Vector3D a = new Vector3D(targetVector2.X, targetVector2.Y, 0);
                        Vector3D b = new Vector3D(tipVector2.X, tipVector2.Y, 0);
                        deltaAngle = Vector3D.AngleBetween(a, b);
                        var c = Vector3D.CrossProduct(a, b);
                        if (c.Z > 0)
                        {
                            deltaAngle = -deltaAngle;
                        }
                    }
                    else
                    {
                        Vector3D a = new Vector3D(targetVector2.X, 0, targetVector2.Z);
                        Vector3D b = new Vector3D(tipVector2.X, 0, tipVector2.Z);
                        deltaAngle = Vector3D.AngleBetween(a, b);
                        var c = Vector3D.CrossProduct(a, b);
                        if (c.Y > 0)
                        {
                            deltaAngle = -deltaAngle;
                        }
                    }
                }
 
                if (deltaAngle == 0)
                {
                    // 動かす必要なし
                    continue;
                }
 
                // 1ステップで動かす角を制限する
                if (deltaAngle > moveAngle)
                {
                    deltaAngle = moveAngle;
                }
                else if (deltaAngle < -moveAngle)
                {
                    deltaAngle = -moveAngle;
                }
 
                // 自ノードに回転角を足しこみ、先端ノードをすこし動かす
                currentNode.Angle += deltaAngle;
                var axisAngle = new AxisAngleRotation3D(currentNode.RotateAxis, deltaAngle);
                var rt = new RotateTransform3D(axisAngle, new Point3D(0, 0, 0));
                Vector3D deltaVector = rt.Transform(tipVector);
                tipNode.Point = currentNode.Point + deltaVector;
            }
 
            // 各ノードの回転角を適用し、新しい位置を求める
            for (int i = 0; i < nodes.Length; i++)
            {
                nodes[i].Quaternion = new Quaternion(nodes[i].OriginalRotateAxis, nodes[i].Angle);
                if (i > 0)
                {
                    nodes[i].Quaternion = Quaternion.Multiply(nodes[i - 1].Quaternion, nodes[i].Quaternion);
                }
 
                var m = new Matrix3D();
                m.Rotate(nodes[i].Quaternion);
                if (i < nodes.Length - 1)
                {
                    var deltaVector = m.Transform(new Vector3D(nodes[i].Length, 0, 0));
                    nodes[i + 1].Point = nodes[i].Point + deltaVector;
                }
 
                nodes[i].RotateAxis = m.Transform(nodes[i].OriginalRotateAxis);
            }
 
            // ゴールしたら true を返す
            bool isGoal = true;
            var v = target - nodes[nodes.Length - 1].Point;
            if (v.Length > 0.1)
            {
                isGoal = false;
            }
            return isGoal;
        }
    }
}

また、MainWindow.xaml.cs は以下のとおり。

描画するごとにこの繰り返しを行うことでアニメーションしている。末端が目標(赤いボール)に到達したら(目標のある範囲に末端が入ったら)、新たな目標をランダムな位置に設定するようにした。また、たまに目標に届かない場合があり、この場合もしばらくしたら、新たな目標をランダムな位置に設定するようにした。

using System;
using System.Collections.Generic;
using System.Diagnostics;
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 sample1
{
    /// <summary>
    /// MainWindow.xaml の相互作用ロジック
    /// </summary>
    public partial class MainWindow : Window
    {
        public MainWindow()
        {
            InitializeComponent();
            Init();
 
            CompositionTarget.Rendering += CompositionTarget_Rendering;
        }
 
        private Node[] _nodes = null;
        private Sphere3D _target = null;
        private List<ModelVisual3D> _cubeModelList = new List<ModelVisual3D>();
        private List<ModelVisual3D> _cylinderModelList = new List<ModelVisual3D>();
        private List<ModelVisual3D> _rotateAxisModelList = new List<ModelVisual3D>();
        private Random _random = new Random();
        private int _goalCount = 0;
        private int _processCount = 0;
 
        private void CompositionTarget_Rendering(object sender, EventArgs e)
        {
            var target = _target as Sphere3D;
            var targettg = target.Transform as Transform3DGroup;
            var targettt = targettg.Children[1] as TranslateTransform3D;
            var targetPoint = new Point3D(targettt.OffsetX, targettt.OffsetY, targettt.OffsetZ);
 
            var isGoal = InverseKinematics.MoveNodes(_nodes, 1, 0, targetPoint);
 
            for (int i = 0; i < _nodes.Count(); ++i)
            {
                if (i < _cubeModelList.Count())
                {
                    var line = _cubeModelList[i] as ModelVisual3D;
                    var tg2 = line.Transform as Transform3DGroup;
                    var rt2 = tg2.Children[0] as RotateTransform3D;
                    Quaternion q2 = _nodes[i].Quaternion;
                    var r2 = new AxisAngleRotation3D(q2.Axis, q2.Angle);
                    rt2.Rotation = r2;
                    var tt2 = tg2.Children[1] as TranslateTransform3D;
                    tt2.OffsetX = _nodes[i].Point.X;
                    tt2.OffsetY = _nodes[i].Point.Y;
                    tt2.OffsetZ = _nodes[i].Point.Z;
                }
 
                if (i < _cylinderModelList.Count())
                {
                    var cylinder = _cylinderModelList[i] as Cylinder3D;
                    var tg = cylinder.Transform as Transform3DGroup;
                    var rt = tg.Children[2] as RotateTransform3D;
                    if (i > 0)
                    {
                        Quaternion q = _nodes[i].Quaternion;
                        var r = new AxisAngleRotation3D(q.Axis, q.Angle);
                        rt.Rotation = r;
                    }
                    var tt = tg.Children[3] as TranslateTransform3D;
                    tt.OffsetX = _nodes[i].Point.X;
                    tt.OffsetY = _nodes[i].Point.Y;
                    tt.OffsetZ = _nodes[i].Point.Z;
                }
 
                if (i < _rotateAxisModelList.Count())
                {
                    var rotateAxis = _rotateAxisModelList[i] as _3DTools.ScreenSpaceLines3D;
                    rotateAxis.Points[0] = _nodes[i].Point - _nodes[i].RotateAxis;
                    rotateAxis.Points[1] = _nodes[i].Point + _nodes[i].RotateAxis;
                }
            }
 
            if (_processCount++ > 300)
            {
                _processCount = 0;
            }
            if (isGoal)
            {
                if (_goalCount++ > 50)
                {
                    _goalCount = 0;
                    _processCount = 0;
                }
            }
            if (_processCount == 0)
            {
                targettt.OffsetX = (_random.NextDouble() - 0.5) * 4;
                targettt.OffsetY = (_random.NextDouble() - 0.5) * 4;
                targettt.OffsetZ = (_random.NextDouble() - 0.5) * 4;
                Trace.WriteLine(string.Format("({0},{1},{2})", targettt.OffsetX, targettt.OffsetY, targettt.OffsetZ));
            }
        }
 
        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(14, 13, 12);
            camera.LookDirection = new Vector3D(-14, -13, -12);
            camera.UpDirection = new Vector3D(0, 1, 0);
            viewport.Camera = camera;
 
            // light
            var light = new DirectionalLight();
            light.Color = Colors.White;
            light.Direction = new Vector3D(-2, -3, -1);
            var lightModel = new ModelVisual3D();
            lightModel.Content = light;
            viewport.Children.Add(lightModel);
 
            // axis
            var xAxis = new _3DTools.ScreenSpaceLines3D();
            xAxis.Points.Add(new Point3D(-100, 0, 0));
            xAxis.Points.Add(new Point3D(100, 0, 0));
            xAxis.Color = Colors.Red;
            xAxis.Thickness = 1;
            var yAxis = new _3DTools.ScreenSpaceLines3D();
            yAxis.Points.Add(new Point3D(0, -100, 0));
            yAxis.Points.Add(new Point3D(0, 100, 0));
            yAxis.Color = Colors.Green;
            yAxis.Thickness = 1;
            var zAxis = new _3DTools.ScreenSpaceLines3D();
            zAxis.Points.Add(new Point3D(0, 0, -100));
            zAxis.Points.Add(new Point3D(0, 0, 100));
            zAxis.Color = Colors.Blue;
            zAxis.Thickness = 1;
            var axis = new ModelVisual3D();
            axis.Children.Add(xAxis);
            axis.Children.Add(yAxis);
            axis.Children.Add(zAxis);
            viewport.Children.Add(axis);
 
            // nodes
            var nodes = new Node[] {
                new Node(1, new Point3D(3, 0, 0), 2, 0),
                new Node(0, new Point3D(5, 0, 0), 3, 90),
                new Node(0, new Point3D(8, 0, 0), 2, -90),
                new Node(0, new Point3D(9, 0, 0), 1, 0),
            };
 
            for (var i = 0; i < nodes.Count(); ++i)
            {
                // cylinder
                var cylinderModel = new Cylinder3D();
                var cylinderMaterial = new MaterialGroup();
                cylinderMaterial.Children.Add(new DiffuseMaterial(new SolidColorBrush(Colors.Gold)));
                cylinderMaterial.Children.Add(new SpecularMaterial(new SolidColorBrush(Colors.Gold), 60));
                cylinderModel.Material = cylinderMaterial;
                var cylinderTrans = new Transform3DGroup();
                cylinderTrans.Children.Add(new ScaleTransform3D(0.3, 0.5, 0.3));
                if (nodes[i].NodeType == 0)
                {
                    var r = new AxisAngleRotation3D(new Vector3D(1, 0, 0), 90);
                    cylinderTrans.Children.Add(new RotateTransform3D(r));
                }
                else
                {
                    var r = new AxisAngleRotation3D(new Vector3D(0, 0, 1), 0);
                    cylinderTrans.Children.Add(new RotateTransform3D(r));
                }
                cylinderTrans.Children.Add(new RotateTransform3D());
                cylinderTrans.Children.Add(new TranslateTransform3D(nodes[i].Point.X, nodes[i].Point.Y, nodes[i].Point.Z));
                cylinderModel.Transform = cylinderTrans;
                viewport.Children.Add(cylinderModel);
                _cylinderModelList.Add(cylinderModel);
 
                // rotateAxis
                var rotateAxisModel = new _3DTools.ScreenSpaceLines3D();
                if (nodes[i].NodeType == 0)
                {
                    Vector3D axisVector = new Vector3D(0, 2, 0);
                    rotateAxisModel.Points.Add(nodes[i].Point - axisVector);
                    rotateAxisModel.Points.Add(nodes[i].Point + axisVector);
                }
                else
                {
                    Vector3D axisVector = new Vector3D(0, 0, 2);
                    rotateAxisModel.Points.Add(nodes[i].Point - axisVector);
                    rotateAxisModel.Points.Add(nodes[i].Point + axisVector);
                }
                rotateAxisModel.Color = Colors.Gray;
                viewport.Children.Add(rotateAxisModel);
                _rotateAxisModelList.Add(rotateAxisModel);
 
                // cube
                if (i < nodes.Count() - 1)
                {
                    double len = nodes[i].Length;
                    var point0 = new Point3D(0, -0.2, -0.2);    // bottom-back-left
                    var point1 = new Point3D(len, -0.2, -0.2);    // bottom-back-right
                    var point2 = new Point3D(len, -0.2, 0.2);    // bottom-front-right
                    var point3 = new Point3D(0, -0.2, 0.2);        // bottom-front-left
                    var point4 = new Point3D(0, 0.2, -0.2);        // top-back-left
                    var point5 = new Point3D(len, 0.2, -0.2);    // top-back-right
                    var point6 = new Point3D(len, 0.2, 0.2);    // top-front-right
                    var point7 = new Point3D(0, 0.2, 0.2);        // top-front-left
                    var cubeGroup = new Model3DGroup();
                    var cubeMaterial = new DiffuseMaterial(new SolidColorBrush(Colors.Blue));
                    cubeGroup.Children.Add(new GeometryModel3D(CreateMesh(point7, point6, point2, point3), cubeMaterial));    // front
                    cubeGroup.Children.Add(new GeometryModel3D(CreateMesh(point6, point5, point1, point2), cubeMaterial));    // right
                    cubeGroup.Children.Add(new GeometryModel3D(CreateMesh(point5, point4, point0, point1), cubeMaterial));    // back
                    cubeGroup.Children.Add(new GeometryModel3D(CreateMesh(point4, point7, point3, point0), cubeMaterial));    // left
                    cubeGroup.Children.Add(new GeometryModel3D(CreateMesh(point1, point0, point3, point2), cubeMaterial));    // bottom
                    cubeGroup.Children.Add(new GeometryModel3D(CreateMesh(point4, point5, point6, point7), cubeMaterial));    // top
                    var cubeModel = new ModelVisual3D();
                    cubeModel.Content = cubeGroup;
                    var cubeTrans = new Transform3DGroup();
                    cubeTrans.Children.Add(new RotateTransform3D());
                    cubeTrans.Children.Add(new TranslateTransform3D(nodes[i].Point.X, nodes[i].Point.Y, nodes[i].Point.Z));
                    cubeModel.Transform = cubeTrans;
                    viewport.Children.Add(cubeModel);
                    _cubeModelList.Add(cubeModel);
                }
            }
 
            // target
            var target = new Sphere3D();
            var mg2 = new MaterialGroup();
            mg2.Children.Add(new DiffuseMaterial(new SolidColorBrush(Colors.Red)));
            mg2.Children.Add(new SpecularMaterial(new SolidColorBrush(Colors.Red), 60));
            target.Material = mg2;
            var trans2 = new Transform3DGroup();
            trans2.Children.Add(new ScaleTransform3D(0.3, 0.3, 0.3));
            trans2.Children.Add(new TranslateTransform3D(-2, 3, 1));
            target.Transform = trans2;
            viewport.Children.Add(target);
 
            this._nodes = nodes;
            this._target = target;
        }
 
        private static MeshGeometry3D CreateMesh(Point3D p0, Point3D p1, Point3D p2, Point3D p3)
        {
            var mesh = new MeshGeometry3D();
            mesh.Positions = new Point3DCollection(new Point3D[] { p0, p1, p2, p3 });
            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) });
            return mesh;
        }
    }
}

実行結果は以下のとおり。ノードは4つ、末端は関節とならないので、関節は根元側の4つ。一番根元はY軸方向に回転する関節で、その他はZ軸方向に回転する関節とした。

wpf3d-10

以上。