我正在将角度轴表示法转换为欧拉角。我决定检查并确保从转换得到的欧拉角会回到原始的角度轴。我输出了值，但它们不匹配！我已经阅读了http://forum.onlineconversion.com/showthread.php?t=5408和http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles以及此网站上类似的转换问题。
在下面的代码中，我从角度“angle”和轴（rx，ry，rz）开始，然后将其转换为四元数（q0，q1，q2，q3）。我将四元数转换为欧拉角（roll，pitch，yaw）。然后为了检查它，我将（roll，pitch，yaw）转换回角度轴作为cAngle和（cRx，cRy，cRz）。然后我对（roll，pitch，yaw）进行一些边界检查，以使数字保持在- pi和pi之间，然后将它们打印出来。应该是cAngle = angle和（cRx，cRy，cRz）=（rx，ry，rz），但这两者都是错误的。
旋转顺序通常是Z*Y*X。我的数学有什么问题吗？我计划最终添加特殊情况，当俯仰角为0或PI时，例如http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/，但现在我认为问题是独立的。
在下面的代码中，我从角度“angle”和轴（rx，ry，rz）开始，然后将其转换为四元数（q0，q1，q2，q3）。我将四元数转换为欧拉角（roll，pitch，yaw）。然后为了检查它，我将（roll，pitch，yaw）转换回角度轴作为cAngle和（cRx，cRy，cRz）。然后我对（roll，pitch，yaw）进行一些边界检查，以使数字保持在- pi和pi之间，然后将它们打印出来。应该是cAngle = angle和（cRx，cRy，cRz）=（rx，ry，rz），但这两者都是错误的。
旋转顺序通常是Z*Y*X。我的数学有什么问题吗？我计划最终添加特殊情况，当俯仰角为0或PI时，例如http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/，但现在我认为问题是独立的。
//input is angle 'angle' and axis '(rx,ry,rz)'
//convert rx,ry,rz, angle, into roll, pitch, yaw
double q0 = Math.Cos(angle / 2);
double q1 = Math.Sin(angle / 2) *Math.Cos(rx);
double q2 = Math.Sin(angle / 2) * Math.Cos(ry);
double q3 = Math.Sin(angle / 2) * Math.Cos(rz);
double roll = Math.Atan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1 * q1 + q2 * q2));
double pitch = Math.Asin(2 * (q0 * q2 - q3 * q1));
double yaw = Math.Atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3));
//convert back to angle axis
double cAngle = 2 * Math.Cos(Math.Cos(roll / 2) * Math.Cos(pitch / 2) * Math.Cos(yaw / 2) + Math.Sin(roll / 2) * Math.Sin(pitch / 2) * Math.Sin(yaw / 2));
double cRx = Math.Acos((Math.Sin(roll / 2) * Math.Cos(pitch / 2) * Math.Cos(yaw / 2) - Math.Cos(roll / 2) * Math.Sin(pitch / 2) * Math.Sin(yaw / 2)) / Math.Sin(cAngle / 2));
double cRy = Math.Acos((Math.Cos(roll / 2) * Math.Sin(pitch / 2) * Math.Cos(yaw / 2) + Math.Sin(roll / 2) * Math.Cos(pitch / 2) * Math.Sin(yaw / 2)) / Math.Sin(cAngle / 2));
double cRz = Math.Acos((Math.Cos(roll / 2) * Math.Cos(pitch / 2) * Math.Sin(yaw / 2) - Math.Sin(roll / 2) * Math.Sin(pitch / 2) * Math.Cos(yaw / 2)) / Math.Sin(cAngle / 2));
//stay within +/- PI of 0 to keep the number small
if (roll > 3.1416) roll = -Math.PI + (roll - Math.PI);
if (roll < -3.1416) roll = Math.PI + (roll - (-1) * Math.PI);
if (pitch > 3.1416) pitch = -Math.PI + (pitch - Math.PI);
if (pitch < -3.1416) pitch = Math.PI + (pitch - (-1) * 3.1416F);
if (yaw > 3.1416) yaw = -Math.PI + (yaw - Math.PI);
if (yaw < -3.1416) yaw = Math.PI + (yaw - (-1) * Math.PI);
Console.WriteLine("original angle, axis " + angle + ": " + rx + ", " + ry + ", " + rz);
Console.WriteLine("converted angle, axis " + cAngle + ": " + cRx + ", " + cRy + ", " + cRz);
Console.WriteLine("quats " + q0 + ", " + q1 + ", " + q2 + ", " + q3);
Console.WriteLine("roll,pitch,yaw: " + roll + ", " + pitch + ", " + yaw);