四元数到欧拉角算法 - 如何转换为'Y = UP'和霸道的？ [英] Quaternion to Euler angles algorithm - How to convert to 'Y = Up' and between handedness?

问题描述

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

 公共静态的Vector3 ToEulerAngles（此四元Q）
 {
 //存储欧拉弧度
的Vector3 pitchYawRoll =新的Vector3角（）; 
 
双SQW = q.W * q.W; 
双SQX = q.X * q.X; 
双SQY = q.Y * q.Y; 
双SQZ = q.Z * q.Z; 
 
 //如果四元数归一化的单元是一个，否则它是校正因子
双单元= SQX + SQY + SQZ + SQW; 
双重考验= q.X * q.Y + q.Z * q.W; 
 
如果（试验> 0.4999f *单位​​）// 0.4999f或0.5F  -  EPSILON 
 {
 //奇异的北极
 pitchYawRoll.Y = 2F *（浮点）Math.Atan2（QX，QW）; //偏航
 pitchYawRoll.X = PI * 0.5F; //间距
 pitchYawRoll.Z = 0F; //滚动
返回pitchYawRoll; 
} 
，否则如果（测试< -0.4999f *单位​​）// -0.4999f OR -0.5f + EPSILON 
 {
 //奇异在南极
 pitchYawRoll.Y = -2F *（浮点）Math.Atan2（QX，QW）; //偏航
 pitchYawRoll.X = -PI * 0.5F; //间距
 pitchYawRoll.Z = 0F; //滚动
返回pitchYawRoll; 
} 
，否则
 {
 pitchYawRoll.Y =（浮点）Math.Atan2（2F * QY * QW  -  2F * QX * QZ，SQX  -  SQY  -  SQZ + SQW） ; //偏航
 pitchYawRoll.X =（浮点）Math.Asin（2F *测试/单位）; //间距
 pitchYawRoll.Z =（浮点）Math.Atan2（2F * q.X * q.W  -  2F * q.Y * q.Z，-sqx + SQY  -  SQZ + SQW）; //滚动
} 
 
返回pitchYawRoll; 
} 
  

这方法只适用于右手直角坐标系的Z轴朝上。

我会为了使Y轴点起来，而不是的Z改变什么？ （可否交换X和Z的工作？）

我怎么能适应左手坐标系

编辑？

 公共静态四元CreateFromYawPitchRoll（浮点偏航，浮动间距，浮动辊）
 {
浮动NUM =卷* 0.5F; 
浮动NUM2 =（浮点）Math.Sin（（双）NUM）; 
浮动NUM3 =（浮点）Math.Cos（（双）NUM）; 
浮动num4 =距* 0.5F; 
浮动num5 =（浮点）Math.Sin（（双）num4）; 
浮动NUM6 =（浮点）Math.Cos（（双）num4）; 
浮动num7 =偏航* 0.5F; 
浮动num8 =（浮点）Math.Sin（（双）num7）; 
浮动num9 =（浮点）Math.Cos（（双）num7）; 
四元数的结果; 
 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; 
返回结果; 
} 
  

解决方案

下面被改变的方法使用偏航，俯仰，滚转相同的定义：

 公共静态四元CreateFromYawPitchRoll（浮点偏航，浮动间距，浮动辊）
 {
浮动rollOver2 =卷* 0.5F; 
浮动sinRollOver2 =（浮点）Math.Sin（（双）rollOver2）; 
浮动cosRollOver2 =（浮点）Math.Cos（（双）rollOver2）; 
浮动pitchOver2 =距* 0.5F; 
浮动sinPitchOver2 =（浮点）Math.Sin（（双）pitchOver2）; 
浮动cosPitchOver2 =（浮点）Math.Cos（（双）pitchOver2）; 
浮动yawOver2 =偏航* 0.5F; 
浮动sinYawOver2 =（浮点）Math.Sin（（双）yawOver2）; 
浮动cosYawOver2 =（浮点）Math.Cos（（双）yawOver2）; 
四元数的结果; 
 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; 
返回结果; 
} 
  

有关 ToEulerAngles （奇ommitted）：

  pitchYawRoll.Y =（浮点）Math.Atan2（2F * QX * QW + 2F * QY * QZ ，1  -  2F *（SQZ + SQW））; //偏航
 pitchYawRoll.X =（浮点）Math.Asin（2F *（* q.X q.Z  -  q.W * q.Y））; //间距
 pitchYawRoll.Z =（浮点）Math.Atan2（2F * q.X * q.Y + 2F * q.Z * q.W，1  -  2F *（SQY + SQZ））; //滚动
  

我进行了如下测试：

 变种q = CreateFromYawPitchRoll（0.2F，0.3f，0.7f）; 
变种E = ToEulerAngles（Q）; 
无功Q2 = CreateFromYawPitchRoll（e.Y，e.X，e.Z）; 
  

结果如下;

  E =（0.3，0.2，0.7）//俯仰，偏航，滚动
 Q2 = q 
  

来源：维基百科

I have an algorithm for converting between a Quaternion and Euler angles.

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

This method only works for a right-handed Cartesian coordinate system with the Z axis pointing up.

What would I change in order to make the Y axis point up instead of Z? (Would swapping X and Z work?)

How can I accommodate left handed coordinate systems?

EDIT:

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

解决方案

Here are changed methods that use the same definition of yaw, pitch, roll:

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

For ToEulerAngles (singularities ommitted):

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 

I performed the following test:

var q = CreateFromYawPitchRoll(0.2f, 0.3f, 0.7f);
var e = ToEulerAngles(q);
var q2 = CreateFromYawPitchRoll(e.Y, e.X, e.Z);

with the following results;

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

Source: Wikipedia

这篇关于四元数到欧拉角算法 - 如何转换为'Y = UP'和霸道的？的文章就介绍到这了，希望我们推荐的答案对大家有所帮助，也希望大家多多支持IT屋！