C# 四元数到欧拉角算法-如何转换为';Y=向上';在惯用手之间呢?
我有一个在四元数和欧拉角之间转换的算法C# 四元数到欧拉角算法-如何转换为';Y=向上';在惯用手之间呢?,c#,math,quaternions,C#,Math,Quaternions,我有一个在四元数和欧拉角之间转换的算法 public static Vector3 ToEulerAngles(this Quaternion q) { // Store the Euler angles in radians Vector3 pitchYawRoll = new Vector3(); double sqw = q.W * q.W; double sqx = q.X * q.X; d
public static Vector3 ToEulerAngles(this Quaternion q)
{
// Store the Euler angles in radians
Vector3 pitchYawRoll = new Vector3();
double sqw = q.W * q.W;
double sqx = q.X * q.X;
double sqy = q.Y * q.Y;
double sqz = q.Z * q.Z;
// 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;
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 = 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(q.X, q.W); // Yaw
pitchYawRoll.X = -PI * 0.5f; // 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
}
return pitchYawRoll;
}
以下是使用相同偏航、俯仰、横摇定义的更改方法:
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
(未注明奇点):
我进行了以下测试:
var q = CreateFromYawPitchRoll(0.2f, 0.3f, 0.7f);
var e = ToEulerAngles(q);
var q2 = CreateFromYawPitchRoll(e.Y, e.X, e.Z);
结果如下:
e = (0.3, 0.2, 0.7) //pitch, yaw, roll
q2 = q
来源:四元数和欧拉角与坐标系对齐和头部无关。偏航、俯仰和滚转分别定义围绕z、y和x轴的旋转。轴的方向无关紧要。如果我将其与XNA的Quaternion.CreateFromYawPitchRoll结合使用,则不会得到原始的四元数。然后,这些功能可能使用不同的轴偏航/俯仰/滚动关联。交换euler角度,以便定义匹配。您可以根据我所做的编辑发布一个示例,其中现在包括四元数。CreateFromYawPitchRoll?回答很好,谢谢。奇点是否与最初编写的一样正确?
if(test>0.4999f*单位)//0.4999f或0.5f-ε{//北极俯仰角奇点yawroll.Y=2f*(float)Math.Atan2(q.Y,q.W);//偏航pitchYawRoll.X=PI*0.5f;//俯仰pitchYawRoll.Z=0f;//滚转返回pitchYawRoll;}
刚刚选中,PI不会产生正确的结果。例如,来自YawPitchRoll((float)Math.PI,0f,0f)测试的必须是=q.X*q.Z-q.W*q.Y
。然而,关于奇点值,我不是绝对肯定。你最好试一试。那个“测试”的方程式不可能是正确的。它为(0,0,0)和(PI,0,0)生成一个值0。
var q = CreateFromYawPitchRoll(0.2f, 0.3f, 0.7f);
var e = ToEulerAngles(q);
var q2 = CreateFromYawPitchRoll(e.Y, e.X, e.Z);
e = (0.3, 0.2, 0.7) //pitch, yaw, roll
q2 = q