public static void ToEulerAngles(QuaternionR q, out double roll, out double pitch, out double yaw)
{
double ysqr = q.Y * q.Y;
double t0 = 2.0 * (q.W * q.X + q.Y * q.Z);
double t1 = 1.0 - 2.0 * (q.X * q.X + ysqr);
roll = Math.Atan2(t0, t1);
double t2 = 2.0 * (q.W * q.Y - q.Z * q.X);
if (t2 > 1.0)
{
t2 = 1.0;
}
if (t2 < -1.0)
{
t2 = -1.0;
}
pitch = Math.Asin(t2);
double t3 = 2.0 * (q.W * q.Z + q.X * q.Y);
double t4 = 1.0 - 2.0 * (ysqr + q.Z * q.Z);
yaw = Math.Atan2(t3, t4);
}
public static Vector3R RotateReverse(Vector3R vector, QuaternionR quat, bool assumeUnitQuat)
{
Vector3 v = new Vector3(vector.X, vector.Y, vector.Z);
Quaternion q = new Quaternion(quat.X, quat.Y, quat.Z, assumeUnitQuat ? 1 : quat.W);
Vector3 result = Vector3.Transform(v, Quaternion.Conjugate(q));
return(new Vector3R {
X = result.X, Y = result.Y, Z = result.Z
});
}
public static QuaternionR Negate(QuaternionR quat)
{
return(new QuaternionR {
X = -quat.X, Y = -quat.Y, Z = -quat.Z, W = -quat.W
});
}
public static QuaternionR FlipZAxis(QuaternionR quat)
{
return(new QuaternionR {
X = -quat.X, Y = -quat.Y, Z = quat.Z, W = quat.W
});
}
public static Vector3R TransformToWorldFrame(Vector3R vBody, QuaternionR quat, bool assumeUnitQuat = true)
{
return(Rotate(vBody, quat, assumeUnitQuat));
}
public static Vector3R TransformToBodyFrame(Vector3R vWorld, QuaternionR quat, bool assumeUnitQuat = true)
{
return(RotateReverse(vWorld, quat, assumeUnitQuat));
}
