Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
Wpf3DSample / Rotation3DHelper
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; }
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment