/// <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); }