/// <summary>
/// クォータニオンからオイラー角に変換
/// </summary>
/// <param name="value">クォータニオン</param>
/// <returns>オイラー角</returns>
public static AxisOfRotation <float> QuaternionToEulerAngles(Quaternion <float> value)
{
AxisOfRotation <float> axis = new AxisOfRotation <float>();
double x2z2 = Math.Pow(value.X, 2) - Math.Pow(value.Z, 2);
double y2w2 = Math.Pow(value.Y, 2) - Math.Pow(value.W, 2);
axis.Set(
(float)Math.Atan2(2.0 * ((value.X * value.Y) + (value.Z * value.W)), x2z2 - y2w2),
(float)Math.Asin(2.0 * ((value.X * value.Z) - (value.Y * value.W))),
(float)Math.Atan2(2.0 * ((value.Y * value.Z) + (value.X * value.W)), x2z2 + y2w2)
);
return(axis);
}
/// <summary>
/// オイラー角度からクォータニオンに変換
/// </summary>
/// <param name="value">オイラー角</param>
/// <returns>クォータニオン</returns>
public static Quaternion <float> EulerAnglesToQuaternion(AxisOfRotation <float> value)
{
Quaternion <float> axis = new Quaternion <float>();
double cosRoll = Math.Cos(value.Roll / 2.0);
double sinRoll = Math.Sin(value.Roll / 2.0);
double cosPitch = Math.Cos(value.Pitch / 2.0);
double sinPitch = Math.Sin(value.Pitch / 2.0);
double cosYaw = Math.Cos(value.Yaw / 2.0);
double sinYaw = Math.Sin(value.Yaw / 2.0);
axis.Set(
(float)((cosRoll * cosPitch * cosYaw) + (sinRoll * sinPitch * sinYaw)),
(float)((sinRoll * cosPitch * cosYaw) - (cosRoll * sinPitch * sinYaw)),
(float)((cosRoll * sinPitch * cosYaw) + (sinRoll * cosPitch * sinYaw)),
(float)((cosRoll * cosPitch * sinYaw) - (sinRoll * sinPitch * cosYaw))
);
Console.WriteLine("X : " + axis.X);
Console.WriteLine("Y : " + axis.Y);
Console.WriteLine("Z : " + axis.Z);
Console.WriteLine("W : " + axis.W);
return(axis);
}
