我花了一些时间实现了一些算法，用于在欧拉角和四元数之间进行转换。
我正在使用以下代码测试四元数的值是否相同：
Quaternion orientation0 = Prototype1.Mathematics.ToolBox.QuaternionFromYawPitchRoll(0, 0, 0);
Vector3 rotation = orientation0.ToEulerAngles();
Quaternion orientation1 = Prototype1.Mathematics.ToolBox.QuaternionFromYawPitchRoll(rotation.Y, rotation.X, rotation.Z);
Console.WriteLine(orientation0);
Console.WriteLine(orientation1);
我曾经使用过一个之前讨论过的方法，在这里，后来我又实现了另一种方法，在这里
public static Quaternion QuaternionFromYawPitchRoll(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);
// X = PI is giving incorrect result (pitch)
// Heading = Yaw
// Attitude = Pitch
// Bank = Roll
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;
result.W = cosYawOver2 * cosPitchOver2 * cosRollOver2 - sinYawOver2 * sinPitchOver2 * sinRollOver2;
result.X = sinYawOver2 * sinPitchOver2 * cosRollOver2 + cosYawOver2 * cosPitchOver2 * sinRollOver2;
result.Y = sinYawOver2 * cosPitchOver2 * cosRollOver2 + cosYawOver2 * sinPitchOver2 * sinRollOver2;
result.Z = cosYawOver2 * sinPitchOver2 * cosRollOver2 - sinYawOver2 * cosPitchOver2 * sinRollOver2;
return result;
}
public static Vector3 ToEulerAngles(this Quaternion q)
{
// Store the Euler angles in radians
Vector3 pitchYawRoll = new Vector3();
double sqx = q.X * q.X;
double sqy = q.Y * q.Y;
double sqz = q.Z * q.Z;
double sqw = q.W * q.W;
// If quaternion is normalised the unit is one, otherwise it is the correction factor
double unit = sqx + sqy + sqz + sqw;
double test = q.X * q.Y + q.Z * q.W;
//double test = q.X * q.Z - q.W * q.Y;
if (test > 0.4999f * unit) // 0.4999f OR 0.5f - EPSILON
{
// Singularity at north pole
pitchYawRoll.Y = 2f * (float)Math.Atan2(q.X, q.W); // Yaw
pitchYawRoll.X = PIOVER2; // 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(q.X, q.W); // Yaw
pitchYawRoll.X = -PIOVER2; // Pitch
pitchYawRoll.Z = 0f; // Roll
return pitchYawRoll;
}
else
{
pitchYawRoll.Y = (float)Math.Atan2(2f * q.Y * q.W - 2f * q.X * q.Z, sqx - sqy - sqz + sqw); // Yaw
pitchYawRoll.X = (float)Math.Asin(2f * test / unit); // Pitch
pitchYawRoll.Z = (float)Math.Atan2(2f * q.X * q.W - 2f * q.Y * q.Z, -sqx + sqy - sqz + sqw); // Roll
//pitchYawRoll.Y = (float)Math.Atan2(2f * q.X * q.W + 2f * q.Y * q.Z, 1 - 2f * (sqz + sqw)); // Yaw
//pitchYawRoll.X = (float)Math.Asin(2f * (q.X * q.Z - q.W * q.Y)); // Pitch
//pitchYawRoll.Z = (float)Math.Atan2(2f * q.X * q.Y + 2f * q.Z * q.W, 1 - 2f * (sqy + sqz)); // Roll
}
return pitchYawRoll;
}
除了pitch值为±PI时，所有我的实现都可以工作。
Quaternion orientation0 = Prototype1.Mathematics.ToolBox.QuaternionFromYawPitchRoll(0, PI, 0);
Vector3 rotation = orientation0.ToEulerAngles();
Quaternion orientation1 = Prototype1.Mathematics.ToolBox.QuaternionFromYawPitchRoll(rotation.Y, rotation.X, rotation.Z);
Console.WriteLine(orientation0);
Console.WriteLine(orientation1); // Not the same quaternion values
为什么这个方法对于特定的值不起作用？如果它是奇点，那么算法并没有识别出它，而'test'的值会非常接近0。