VCS/Quaternion6 - Sample is best!

開発環境	Microsoft Visual C# 2010 Express (SP1)
実行環境	Microsoft Windows XP Home Edition (SP3)
プロジェクトの種類	空のプロジェクト
プロジェクト名	Quaternion

参考

• 航法（航海）計算#大圏航法
• 四元数で３次元の回転を計算する方法 （ソースコード付き）

Program.cs

// Quaternion6 大圏コース逆算
using System;
using Microsoft.Xna.Framework; // .NET参照
 
class Program
{
    static void Main()
    {
        float startLatDeg = 35;
        float startLonDeg = 135;
        float angleDeg = 53.22f;
        float distNM = 4863.88f;
 
        float startLatRad = MathHelper.ToRadians(startLatDeg);
        float startLonRad = MathHelper.ToRadians(startLonDeg);
        float angleRad = MathHelper.ToRadians(angleDeg);
        float dist = distNM * 1.852f / 20000;
        Console.WriteLine(string.Format("dist={0:f2}", dist));
 
        // +Z=0 +X=90
        float y = (float)Math.Sin(startLatRad);
        float r = (float)Math.Cos(startLatRad);
        float z = (float)Math.Cos(startLonRad) * r;
        float x = (float)Math.Sin(startLonRad) * r;
        Vector3 axis = new Vector3(x, y, z); // 回転軸
        Print(axis);
 
        // +Z=0 +X=90
        float rad = startLatRad + dist * MathHelper.Pi;
        y = (float)Math.Sin(rad);
        r = (float)Math.Cos(rad);
        z = (float)Math.Cos(startLonRad) * r;
        x = (float)Math.Sin(startLonRad) * r;
        Vector3 v = new Vector3(x, y, z); // 回転する座標
        Print(v);
 
        Quaternion p = new Quaternion(v, 0);
        Quaternion rot = Quaternion.CreateFromAxisAngle(axis, angleRad);
        Quaternion q = Quaternion.Conjugate(rot) * p * rot;
        Console.WriteLine(string.Format("x={0:f2} y={1:f2} z={2:f2} w={3:f2} len={4:f2}",
            q.X, q.Y, q.Z, q.W, q.Length()));
        PrintLatLon(q.X, q.Y, q.Z);
 
        Console.ReadLine();
    }
 
    static void Print(Vector3 v)
    {
        Console.WriteLine(string.Format("x={0:f2} y={1:f2} z={2:f2} len={3:f2}",
            v.X, v.Y, v.Z, v.Length()));
        PrintLatLon(v.X, v.Y, v.Z);
    }
 
    static void PrintLatLon(float x, float y, float z)
    {
        Console.WriteLine(string.Format("lat={0:f1} lon={1:f1}",
            MathHelper.ToDegrees((float)Math.Asin(y)),
            MathHelper.ToDegrees((float)Math.Atan2(x, z))));
    }
}
 

出力

dist=0.45
x=0.58 y=0.57 z=-0.58 len=1.00
lat=35.0 lon=135.0
x=-0.31 y=0.90 z=0.31 len=1.00
lat=63.9 lon=-45.0
x=-0.71 y=0.57 z=-0.41 w=0.00 len=1.00
lat=35.0 lon=-120.0

Program.cs

// Quaternion6 2.Vector4 ver.
using System;
using Microsoft.Xna.Framework; // .NET参照
 
class Program
{
    static void Main()
    {
        float startLatDeg = 35;
        float startLonDeg = 135;
        float angleDeg = 53.22f;
        float distNM = 4863.88f;
 
        float startLatRad = MathHelper.ToRadians(startLatDeg);
        float startLonRad = MathHelper.ToRadians(startLonDeg);
        float angleRad = MathHelper.ToRadians(angleDeg);
        float dist = distNM * 1.852f / 20000;
        Console.WriteLine(string.Format("dist={0:f2}", dist));
 
        // +Z=0 +X=90
        float y = (float)Math.Sin(startLatRad);
        float r = (float)Math.Cos(startLatRad);
        float z = (float)Math.Cos(startLonRad) * r;
        float x = (float)Math.Sin(startLonRad) * r;
        Vector3 axis = new Vector3(x, y, z); // 回転軸
        Print(axis);
 
        // +Z=0 +X=90
        float rad = startLatRad + dist * MathHelper.Pi;
        y = (float)Math.Sin(rad);
        r = (float)Math.Cos(rad);
        z = (float)Math.Cos(startLonRad) * r;
        x = (float)Math.Sin(startLonRad) * r;
        Vector3 v = new Vector3(x, y, z); // 回転する座標
        Print(v);
 
        Vector4 p = new Vector4(v, 0);
        Vector4 rot = CreateFromAxisAngle(axis, angleRad);
        Vector4 conj = new Vector4(-rot.X, -rot.Y, -rot.Z, rot.W);
        Vector4 q = MulQ(MulQ(conj, p), rot);
        Console.WriteLine(string.Format("x={0:f2} y={1:f2} z={2:f2} w={3:f2} len={4:f2}",
            q.X, q.Y, q.Z, q.W, q.Length()));
        PrintLatLon(q.X, q.Y, q.Z);
 
        Console.ReadLine();
    }
 
    static Vector4 CreateFromAxisAngle(Vector3 axis, float angle)
    {
        // 回転軸の正規化
        float norm = axis.Length();
        if (norm <= 0) return Vector4.Zero;
        norm = 1 / (float)Math.Sqrt(norm);
        axis *= norm;
 
        angle *= 0.5f;
        float c = (float)Math.Cos(angle);
        float s = (float)Math.Sin(angle);
        Vector4 v = new Vector4(axis * s, c);
        return v;
    }
 
    static Vector4 MulQ(Vector4 v1, Vector4 v2)
    {
        Vector4 v;
        v.X = (v1.W * v2.X) + (v1.X * v2.W) + (v1.Y * v2.Z) - (v1.Z * v2.Y);
        v.Y = (v1.W * v2.Y) + (v1.Y * v2.W) + (v1.Z * v2.X) - (v1.X * v2.Z);
        v.Z = (v1.W * v2.Z) + (v1.Z * v2.W) + (v1.X * v2.Y) - (v1.Y * v2.X);
        v.W = (v1.W * v2.W) - (v1.X * v2.X) - (v1.Y * v2.Y) - (v1.Z * v2.Z);
        return v;
    }
 
    static void Print(Vector3 v)
    {
        Console.WriteLine(string.Format("x={0:f2} y={1:f2} z={2:f2} len={3:f2}",
            v.X, v.Y, v.Z, v.Length()));
        PrintLatLon(v.X, v.Y, v.Z);
    }
 
    static void PrintLatLon(float x, float y, float z)
    {
        Console.WriteLine(string.Format("lat={0:f1} lon={1:f1}",
            MathHelper.ToDegrees((float)Math.Asin(y)),
            MathHelper.ToDegrees((float)Math.Atan2(x, z))));
    }
}