四元数到欧拉角算法 – 如何转换为’Y =向上’和用手之间？

我有一个在四元数和欧拉角之间转换的算法。

public static Vector3 ToEulerAngles(this Quaternion q) { // Store the Euler angles in radians Vector3 pitchYawRoll = new Vector3(); double sqw = qW * qW; double sqx = qX * qX; double sqy = qY * qY; double sqz = qZ * qZ; // If quaternion is normalised the unit is one, otherwise it is the correction factor double unit = sqx + sqy + sqz + sqw; double test = qX * qY + qZ * qW; if (test > 0.4999f * unit) // 0.4999f OR 0.5f - EPSILON { // Singularity at north pole pitchYawRoll.Y = 2f * (float)Math.Atan2(qX, qW); // Yaw pitchYawRoll.X = PI * 0.5f; // Pitch pitchYawRoll.Z = 0f; // Roll return pitchYawRoll; } else if (test < -0.4999f * unit) // -0.4999f OR -0.5f + EPSILON { // Singularity at south pole pitchYawRoll.Y = -2f * (float)Math.Atan2(qX, qW); // Yaw pitchYawRoll.X = -PI * 0.5f; // Pitch pitchYawRoll.Z = 0f; // Roll return pitchYawRoll; } else { pitchYawRoll.Y = (float)Math.Atan2(2f * qY * qW - 2f * qX * qZ, sqx - sqy - sqz + sqw); // Yaw pitchYawRoll.X = (float)Math.Asin(2f * test / unit); // Pitch pitchYawRoll.Z = (float)Math.Atan2(2f * qX * qW - 2f * qY * qZ, -sqx + sqy - sqz + sqw); // Roll } return pitchYawRoll; } 

此方法仅适用于Z轴向上的右手笛卡尔坐标系。

为了使Y轴指向而不是Z，我会改变什么？ （交换X和Z会有效吗？）

我如何容纳左手坐标系？

编辑：

 public static Quaternion CreateFromYawPitchRoll(float yaw, float pitch, float roll) { float num = roll * 0.5f; float num2 = (float)Math.Sin((double)num); float num3 = (float)Math.Cos((double)num); float num4 = pitch * 0.5f; float num5 = (float)Math.Sin((double)num4); float num6 = (float)Math.Cos((double)num4); float num7 = yaw * 0.5f; float num8 = (float)Math.Sin((double)num7); float num9 = (float)Math.Cos((double)num7); Quaternion result; result.X = num9 * num5 * num3 + num8 * num6 * num2; result.Y = num8 * num6 * num3 - num9 * num5 * num2; result.Z = num9 * num6 * num2 - num8 * num5 * num3; result.W = num9 * num6 * num3 + num8 * num5 * num2; return result; } 

Answer 1

以下是使用相同的偏航，俯仰，滚动定义的更改方法：

 public static Quaternion CreateFromYawPitchRoll(float yaw, float pitch, float roll) { float rollOver2 = roll * 0.5f; float sinRollOver2 = (float)Math.Sin((double)rollOver2); float cosRollOver2 = (float)Math.Cos((double)rollOver2); float pitchOver2 = pitch * 0.5f; float sinPitchOver2 = (float)Math.Sin((double)pitchOver2); float cosPitchOver2 = (float)Math.Cos((double)pitchOver2); float yawOver2 = yaw * 0.5f; float sinYawOver2 = (float)Math.Sin((double)yawOver2); float cosYawOver2 = (float)Math.Cos((double)yawOver2); Quaternion result; result.X = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2; result.Y = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2; result.Z = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2; result.W = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2; return result; } 

对于ToEulerAngles （省略的奇点）：

 pitchYawRoll.Y = (float)Math.Atan2(2f * qX * qW + 2f * qY * qZ, 1 - 2f * (sqz + sqw)); // Yaw pitchYawRoll.X = (float)Math.Asin(2f * ( qX * qZ - qW * qY ) ); // Pitch pitchYawRoll.Z = (float)Math.Atan2(2f * qX * qY + 2f * qZ * qW, 1 - 2f * (sqy + sqz)); // Roll 

我进行了以下测试：

 var q = CreateFromYawPitchRoll(0.2f, 0.3f, 0.7f); var e = ToEulerAngles(q); var q2 = CreateFromYawPitchRoll(eY, eX, eZ); 

结果如下;

 e = (0.3, 0.2, 0.7) //pitch, yaw, roll q2 = q 

资料来源： 维基百科