XNAの四元数(クォータニオン)

2016/05/19：Transformについて追記

XNAは開発が終了しているが、まだ配布はされている。XNAはゲーム開発環境だけど、Vector3だとかQuaternionだとかがあるので、ちょっと3Dの座標がほしい時には結構便利。

XNAの座標系は画像検索とかするとわかりやすい図が出てくるが、この軸を書いた箱を大小1個ずつ用意し、大きな箱の上で小さな箱をぐるぐる動かしたりすると理解しやすい。ティッシュの空き箱にX,Y,Zの矢印やRight,Left,Up,Down,Backward,Forward等を書いておき、フリスクの空き箱にX,Y,Zと書いて動かすのがオススメ。脳内で考えるより楽。

さて、Quaternionだが、乗算のオーバーロードで回転を行うことができる。ただし元のqを先に置くか後に置くかで結果が変わる。先においた場合は移動物体に搭載したジャイロセンサの出力を突っ込む感じ。後においた場合は地上から見た回転を突っ込む感じ。
前置きだと乗算代入演算子で動かすことができる。後起きだとちゃんと乗算してから代入してやる必要があると思う。

public partial class Form1 : Form
{
    public Form1()
    {
        InitializeComponent();

        Quaternion q = new Quaternion(0, 0, 0, 1);

        q = q * Quaternion.CreateFromAxisAngle(Vector3.Right, MathHelper.ToRadians(90));
        //q = q * Quaternion.CreateFromAxisAngle(Vector3.Down, MathHelper.ToRadians(90));
        q = Quaternion.CreateFromAxisAngle(Vector3.Down, MathHelper.ToRadians(90)) * q;

        Vector3 v1 = q.Transform(new Vector3(1, 0, 0));
        Vector3 v2 = q.Transform(new Vector3(0, 1, 0));
        Vector3 v3 = q.Transform(new Vector3(0, 0, 1));

        MessageBox.Show(
            q +
            "\n" + ToString(v1) +
            "\n" + ToString(v2) +
            "\n" + ToString(v3));
    }

    string ToString(Vector3 v)
    {
        string format;
        format = "0.000";
        format = "+" + format + ";-" + format + "; " + format;

        return ("{" +
            v.X.ToString(format) + " " +
            v.Y.ToString(format) + " " +
            v.Z.ToString(format) + "}");
    }
}

static class MyExt
{
    static public Vector3 Transform(this Quaternion q, Vector3 v)
    {
        float W = -q.X * v.X - q.Y * v.Y - q.Z * v.Z;
        float X = q.Y * v.Z - q.Z * v.Y + q.W * v.X;
        float Y = q.Z * v.X - q.X * v.Z + q.W * v.Y;
        float Z = q.X * v.Y - q.Y * v.X + q.W * v.Z;

        return (new Vector3(
            x: Y * -q.Z + Z * q.Y - W * q.X + X * q.W,
            y: Z * -q.X + X * q.Z - W * q.Y + Y * q.W,
            z: X * -q.Y + Y * q.X - W * q.Z + Z * q.W
        ));
    }
}

2016/05/19：Transformについて

XNAのVector3にTransformがstaticで宣言されている。上記では拡張メソッドで実装しているが、下記のようにVector3.Transform(Vector3, Quaternion)でも同様の結果が得られる。こちらを使うほうが正しい。

Quaternion q = new Quaternion(0, 0, 0, 1);

q = q * Quaternion.CreateFromAxisAngle(Vector3.Right, MathHelper.ToRadians(90));
//q = q * Quaternion.CreateFromAxisAngle(Vector3.Down, MathHelper.ToRadians(90));
q = Quaternion.CreateFromAxisAngle(Vector3.Down, MathHelper.ToRadians(90)) * q;

Vector3 v1 = q.Transform(new Vector3(1, 0, 0));
Vector3 v2 = q.Transform(new Vector3(0, 1, 0));
Vector3 v3 = q.Transform(new Vector3(0, 0, 1));

Console.WriteLine(
    q +
    "\n" + ToString(v1) +
    "\n" + ToString(v2) +
    "\n" + ToString(v3));

v1 = Vector3.Transform(new Vector3(1, 0, 0), q);
v2 = Vector3.Transform(new Vector3(0, 1, 0), q);
v3 = Vector3.Transform(new Vector3(0, 0, 1), q);

Console.WriteLine(
    q +
    "\n" + ToString(v1) +
    "\n" + ToString(v2) +
    "\n" + ToString(v3));