/// <summary>
/// Converts a quaternion to an Euler rotation vector in Radians
/// </summary>
/// <param name="quat">The quaternion to convert to euler angles</param>
/// <returns>Vector with x=roll, y=heading, z = pitch</returns>
public static Vector3 QuaternionToEulerRadians(pm.Quaternion quat)
{
double qx = quat.X;
double qy = quat.Y;
double qz = quat.Z;
double qw = quat.W;
double heading = Math.Atan2((2 * qy * qw) - (2 * qx * qz), 1 - (2 * qy * qy) - (2 * qz * qz));
double sin = (2 * qx * qy) + (2 * qz * qw);
// precision problems can rarely move this value slightly out of the valid range -1 <= sin <= 1
// clamp it
if (sin < -1)
{
sin = -1;
}
else if (sin > 1)
{
sin = 1;
}
double pitch = Math.Asin(sin);
double roll = Math.Atan2((2 * qx * qw) - (2 * qy * qz), 1 - (2 * qx * qx) - (2 * qz * qz));
return(new Vector3((float)roll, (float)heading, (float)pitch));
}
/// <summary>
/// Converts an Euler rotation vector in Radians to a quaternion
/// </summary>
/// <param name="value">Vector with x=roll, y=heading, z = pitch</param>
/// <returns>A quaternion representing the euler angles in value</returns>
public static pm.Quaternion EulerRadiansToQuaternion(Vector3 value)
{
// prevent conversion errors (gimbal lock) by disallowing 0 degree values
if (value.X == 0)
{
value.X = 0.00001f;
}
if (value.Y == 0)
{
value.Y = 0.00001f;
}
if (value.Z == 0)
{
value.Z = 0.00001f;
}
double heading = value.Y;
double pitch = value.Z;
double roll = value.X;
double c1 = Math.Cos(heading / 2);
double c2 = Math.Cos(pitch / 2);
double c3 = Math.Cos(roll / 2);
double s1 = Math.Sin(heading / 2);
double s2 = Math.Sin(pitch / 2);
double s3 = Math.Sin(roll / 2);
Microsoft.Robotics.PhysicalModel.Quaternion quat = new Microsoft.Robotics.PhysicalModel.Quaternion();
quat.W = (float)((c1 * c2 * c3) - (s1 * s2 * s3));
quat.X = (float)((s1 * s2 * c3) + (c1 * c2 * s3));
quat.Y = (float)((s1 * c2 * c3) + (c1 * s2 * s3));
quat.Z = (float)((c1 * s2 * c3) - (s1 * c2 * s3));
return(quat);
}
