Last active
January 14, 2017 07:52
-
-
Save sakapon/9ab43c8b90fd266ae61d764c307a3f86 to your computer and use it in GitHub Desktop.
Wpf3DSample / Rotation3DHelper
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
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