QuaternionCheck.cs

using System;
using System.Numerics;

namespace QuaternionCheck
{
    class Program
    {
        static void Main(string[] args)
        {
            // 回転させたい点
            var pointVector = new Vector3(1, 0, 0);

            // w = 0とすると、Quaternionを点として扱える
            // QuaternionはQuaternion同士で掛けるので、Vector3をQuaternionにする
            var point = new Quaternion(pointVector, 0);
            
            // 回転軸
            var axis = new Vector3(0, 0, 1);
            
            // 回転軸で90度回転するQuaternionを作る
            var quaternion = new Quaternion(
                x: axis.X,// x * sin(t/2)、x = 0なので0
                y: axis.Y,// y * sin(t/2)、y = 0なので0
                z: axis.Z * 0.7071f,// z * sin(t/2) 、z = 1で、角度は半分なのでSin(45°)
                w: 0.7071f);// cos(t/2)、半分なのでCos(45°)

            // 共役クォーニタオンを作る
            // x, y, zを-にする
            var inverseQuaternion = new Quaternion(
                x: -axis.X,// x * sin(t/2)、x = 0なので0
                y: -axis.Y,// y * sin(t/2)、y = 0なので0
                z: -axis.Z * 0.7071f,// z * sin(t/2) 、z = 1で、角度は半分なのでSin(45°)
                w: 0.7071f);// cos(t/2)、半分なのでCos(45°)

            // t/2にしたり、共役クォーニタオンを作る理由は、
            // 回転はx, y, zの空間上で起こるものだけど、クォーニタオン同士の掛け算ではwも掛けられる
            // そこで共役クォーニタオンを作り、半分は+w、もう半分は-wのような概念でw部分を打ち消し、
            // x, y, zの空間をt/2を2回転させることでx, y, zの空間のt回転となる

            // Quaternionの掛け算は内積と外積を使う
            Quaternion Multiply(Quaternion q1, Quaternion q2)
            {
                // まずスカラWとベクタXYZを分解
                var q1Scala = q1.W;
                var q1Vector = new Vector3(q1.X, q1.Y, q1.Z);
                var q2Scala = q2.W;
                var q2Vector = new Vector3(q2.X, q2.Y, q2.Z);

                // スカラ部分は、スカラ部分同士の掛け算から内積を引く
                // 内積は各要素同士を掛けて足すだけ
                // 内積は数学ではV1・V2と書くのでdot
                var dot = q1Vector.X * q2Vector.X + q1Vector.Y * q2Vector.Y + q1Vector.Z * q2Vector.Z;
                var resultScala = q1.W * q2.W - dot;

                // ベクター部分は外積を使う
                // 外積は数学ではV1×V2と書くのでcross
                // これはV1とV2の両方に垂直なベクトルを表す
                // フレミングの左手だと、中指がV1、人差し指がV2、親指が結果になる
                var cross = new Vector3(
                    // 外積のコツは、Q1Q2 - Q1Q2をまず書く
                    // Xを求めるならXの次から、つまりY, Zを前に書いて、その逆のZ, Yを後ろに置く
                    x: q1Vector.Y * q2Vector.Z - q1Vector.Z * q2Vector.Y,
                    // Yを求めるならYの次から、つまりZ, Xを前に書いて、その逆のX, Zを後ろに置く
                    y: q1Vector.Z * q2Vector.X - q1Vector.X * q2Vector.Z,
                    // Zを求めるならZの次から、つまりX, Yを前に書いて、その逆のY, Xを後ろに置く
                    z: q1Vector.X * q2Vector.Y - q1Vector.Y * q2Vector.X
                    );

                // 結果のベクタ部分は以下を足したもの
                var resultVector =
                    q2Scala * q1Vector // 一方のスカラともう一方のベクタ
                    + q1Scala * q2Vector // その逆
                    + cross; // 外積

                return new Quaternion(resultVector, resultScala);
            };

            // 回転は、前から、半回転クオータニオン、点クオータニオン、半回転クオータニオンの共役、を掛ける
            var result = Multiply(Multiply(quaternion , point), inverseQuaternion); // 入れ子だけど前から掛けている
            // 結果は点クオータニオンなのでXYZだけを取り出す
            var resultPoint = new Vector3(result.X, result.Y, result.Z);
            Console.WriteLine(resultPoint); // <0, 0.99998075, 0>

            // C#標準と合わせてチェック
            var resultPointByStandard = Vector3.Transform(pointVector, Quaternion.CreateFromAxisAngle(axis, MathF.PI / 2f));
            Console.WriteLine(resultPointByStandard); // <5.9604645E-08, 0.99999994, 0>、浮動小数点の誤差程度
        }
    }
}