Wpf3DSample / Rotation3DHelper

Rotation3DHelper.cs

using System;
using System.Diagnostics;
using System.Windows.Media.Media3D;

namespace RotationTest
{
    public static class Rotation3DHelper
    {
        public static readonly Vector3D UnitX = new Vector3D(1, 0, 0);
        public static readonly Vector3D UnitY = new Vector3D(0, 1, 0);
        public static readonly Vector3D UnitZ = new Vector3D(0, 0, 1);

        public static double ToDegrees(double radians) => radians * 180 / Math.PI;
        public static double ToRadians(double degrees) => degrees * Math.PI / 180;

        public static Quaternion CreateQuaternionInRadians(Vector3D axis, double angleInRadians) => new Quaternion(axis, ToDegrees(angleInRadians));

        // 四元数 → 行列
        public static Matrix3D ToMatrix3D(this Quaternion q)
        {
            var m = new Matrix3D();
            m.Rotate(q);
            return m;
        }

        // オイラー角 → 四元数
        public static Quaternion ToQuaternion(this EulerAngles e) =>
            CreateQuaternionInRadians(UnitY, e.Yaw) *
            CreateQuaternionInRadians(UnitX, e.Pitch) *
            CreateQuaternionInRadians(UnitZ, e.Roll);

        // 2 点の座標 → オイラー角
        public static EulerAngles ToEulerAngles(Vector3D rotatedUnitZ, Vector3D rotatedUnitY)
        {
            var y_yaw = Math.Atan2(rotatedUnitZ.X, rotatedUnitZ.Z);

            var m_yaw_inv = CreateQuaternionInRadians(UnitY, -y_yaw).ToMatrix3D();
            rotatedUnitZ = rotatedUnitZ * m_yaw_inv;
            rotatedUnitY = rotatedUnitY * m_yaw_inv;

            var x_pitch = Math.Atan2(-rotatedUnitZ.Y, rotatedUnitZ.Z);

            var m_pitch_inv = CreateQuaternionInRadians(UnitX, -x_pitch).ToMatrix3D();
            rotatedUnitY = rotatedUnitY * m_pitch_inv;

            // 本来は -rotatedUnitY.X だけでよいはずです。しかし、X=0 のときに π でなく -π となってしまうため、場合分けします。
            var z_roll = Math.Atan2(rotatedUnitY.X == 0 ? 0 : -rotatedUnitY.X, rotatedUnitY.Y);

            return new EulerAngles { Yaw = y_yaw, Pitch = x_pitch, Roll = z_roll };
        }
    }

    [DebuggerDisplay(@"\{Yaw={Yaw}, Pitch={Pitch}, Roll={Roll}\}")]
    public struct EulerAngles
    {
        public double Yaw { get; set; }
        public double Pitch { get; set; }
        public double Roll { get; set; }
    }
}