33 lines
965 B
C#
33 lines
965 B
C#
using Microsoft.Xna.Framework;
|
|
|
|
namespace KavTest.Extensions
|
|
{
|
|
public static class QuaternionExtensions
|
|
{
|
|
public static Vector3 EulerAngles(this Quaternion q)
|
|
{
|
|
Vector3 angles;
|
|
|
|
double sinr_cosp = 2 * (q.W * q.X + q.Y * q.Z);
|
|
double cosr_cosp = 1 - 2 * (q.X * q.X + q.Y * q.Y);
|
|
angles.X = (float) System.Math.Atan2(sinr_cosp, cosr_cosp);
|
|
|
|
double sinp = 2 * (q.W * q.Y - q.Z * q.X);
|
|
if (System.Math.Abs(sinp) >= 1)
|
|
{
|
|
angles.Y = (float) System.Math.PI / 2 * System.Math.Sign(sinp);
|
|
}
|
|
else
|
|
{
|
|
angles.Y = (float) System.Math.Asin(sinp);
|
|
}
|
|
|
|
double siny_cosp = 2 * (q.W * q.Z + q.X * q.Y);
|
|
double cosy_cosp = 1 - 2 * (q.Y * q.Y + q.Z * q.Z);
|
|
angles.Z = (float) System.Math.Atan2(siny_cosp, cosy_cosp);
|
|
|
|
return angles;
|
|
}
|
|
}
|
|
}
|