C#でベクトル演算をする

KinectやRealSenseを使っているとベクトルや行列、クォータニオンを使ったコードを書くことが増えます。

Unityを使っていると、Unityが面倒みてくれるのでよいのですが、WPFなどの環境で使うとベクトル演算ができるクラスがありませんでした(Leap Motion SDKは独自でベクトル演算クラスを用意してくれています)。

探していたところ、NuGetに「System.Numerics.Vectors」というのがあるそうなので入れたみたら、ほしいものが一通り入っているようです。

d.hatena.ne.jp

サンプル

試しに、Kinectで両手の間の距離を計算するサンプルを作ってみます。 最新のｖ4.1.0では .NET Framework 4.6が必要なので、本サンプルはVisual Studio 2015で作成しています。

WPFアプリを作成し、NuGetから「System.Numerics.Vectors」をインストールします。またKinect SDKもNuGetから取得できるので合わせてインストールしておきます。

Bodyを処理するところで、右手と左手を取得します。つぎにPosition(CameraSpacePointクラス)をVector3クラスに変換してVector3.Distance()で距離を計算します。 変換については、拡張メソッドを作っておくとよさそうです。

//  右手と左手の距離を表示する
var right = body.Joints[JointType.HandRight];
var left = body.Joints[JointType.HandLeft];
if ( (right.TrackingState == TrackingState.Tracked) &&
     (left.TrackingState == TrackingState.Tracked) ) {
    var rightPos = new Vector3( right.Position.X,
                                right.Position.Y,
                                right.Position.Z );

    var leftPos = new Vector3( left.Position.X,
                                left.Position.Y,
                                left.Position.Z );

    // カメラ座標系の単位がmなので、cmに変換する
    var distance = Vector3.Distance( rightPos, leftPos ) * 100;
    TextMessage.Text = distance.ToString();
}

サンプルコード

CameraSpacePointからVector3に変換する拡張メソッド

using System.Numerics;
using Microsoft.Kinect;

namespace KinectVectorSample
{
    public static class CameraSpacePointExtensions
    {
        public static Vector3 ToVector3( this CameraSpacePoint point )
        {
            return new Vector3( point.X, point.Y, point.Z );
        }
    }
}

右手と左手の距離を表示する(拡張メソッド)

var right = body.Joints[JointType.HandRight];
var left = body.Joints[JointType.HandLeft];
if ( (right.TrackingState == TrackingState.Tracked) &&
        (left.TrackingState == TrackingState.Tracked) ) {

    // カメラ座標系の単位がmなので、cmに変換する
    var distance = Vector3.Distance( 
        right.Position.ToVector3(),
        left.Position.ToVector3() )
        * 100;
    TextMessage.Text = distance.ToString();
}

右腕の角度を求める

var hand = body.Joints[JointType.HandRight];
var elbow = body.Joints[JointType.ElbowRight];
var shoulder = body.Joints[JointType.ShoulderRight];

// 内積の定義
// cosθ = ( AとBの内積 ) / (Aの長さ * Bの長さ)
// http://www.sousakuba.com/Programming/gs_two_vector_angle.html
var v1 = hand.Position.ToVector3() - elbow.Position.ToVector3();
var v2 = shoulder.Position.ToVector3() - elbow.Position.ToVector3();

const float Rad2Deg = 180.0f / (float)Math.PI;
var dot = Math.Acos( Vector3.Dot( v1, v2 ) / (v1.Length() * v2.Length()) );
var angle = (float)(dot * Rad2Deg);
TextMessage.Text = angle.ToString();

APIリファレンス

System.Numerics 名前空間 ()