C# Euler<=&燃气轮机；Unity3d引擎中的类四元数

我使用了两个例子（也来自这个网站），但结果与前面提到的Unity不一样

四元数.Euler和.Euler是单位函数。FromQ不执行奇点检查，FromQ2执行

结果:

eulers = (100,55,-11):
Quaternion.Euler(eulers) == (0.6, 0.4, -0.4, 0.5)
ToQ(eulers)); == (0.5, -0.4, 0.2, 0.7) // 0.5, -0.4 right but in wrong order

FromQ(ToQ(eulers)) == (55.0, 100.0, -11.0)
FromQ2(ToQ(eulers)) == (-55.5, -6.3, 71.0) // something right

Quaternion.Euler(eulers).eulerAngles == (80.0, 235.0, 169.0)
FromQ2(Quaternion.Euler(eulers)) == (65.8, 1.9, 99.8)
ToQ(eulers).eulerAngles == (70.0, 286.9, 341.4)
FromQ(Quaternion.Euler(eulers)) == (-65.8, 76.0, 4.6)

It must be:
FromQ() = FromQ2() = .eulerAngles,
ToQ() = Quaternion.Euler()

代码如下：

有人能纠正这个代码吗？我需要返回值的代码​​与Unity函数返回的值相同

更新 下面是固定代码：。 这两个函数（FromQ和ToQ）都工作得很好。但我有一个关于奇点的问题。它不能正确地检测奇异点

For example (90, 0, 50) in quaternion is (0.6, -0.3, 0.3, 0.6).
test = x * y + z * w = 0 (must be close to 0.5 or -0.5)

FromQ无法计算正确的结果，所以这里有奇点。对于（90,50,0）-（0.6,0.3，-0.3,0.6）也一样

我只看到一个解决方案——将“test”计算为xw yz。但我不确定这是否正确

---

如何修复它？

这可能只值得一个部分答案，但这里是“ToQ（）=Quaternion.Euler（）”：

你问题的“FromQ”部分是另一回事。欧拉角

我找到了解决办法

public static Quaternion ToQ (Vector3 v)
{
    return ToQ (v.y, v.x, v.z);
}

public static Quaternion ToQ (float yaw, float pitch, float roll)
{
    yaw *= Mathf.Deg2Rad;
    pitch *= Mathf.Deg2Rad;
    roll *= Mathf.Deg2Rad;
    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.w = cosYawOver2 * cosPitchOver2 * cosRollOver2 + sinYawOver2 * sinPitchOver2 * sinRollOver2;
    result.x = cosYawOver2 * sinPitchOver2 * cosRollOver2 + sinYawOver2 * cosPitchOver2 * sinRollOver2;
    result.y = sinYawOver2 * cosPitchOver2 * cosRollOver2 - cosYawOver2 * sinPitchOver2 * sinRollOver2;
    result.z = cosYawOver2 * cosPitchOver2 * sinRollOver2 - sinYawOver2 * sinPitchOver2 * cosRollOver2;

    return result;
}

public static Vector3 FromQ2 (Quaternion q1)
{
    float sqw = q1.w * q1.w;
    float sqx = q1.x * q1.x;
    float sqy = q1.y * q1.y;
    float sqz = q1.z * q1.z;
    float unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
    float test = q1.x * q1.w - q1.y * q1.z;
    Vector3 v;

    if (test>0.4995f*unit) { // singularity at north pole
        v.y = 2f * Mathf.Atan2 (q1.y, q1.x);
        v.x = Mathf.PI / 2;
        v.z = 0;
        return NormalizeAngles (v * Mathf.Rad2Deg);
    }
    if (test<-0.4995f*unit) { // singularity at south pole
        v.y = -2f * Mathf.Atan2 (q1.y, q1.x);
        v.x = -Mathf.PI / 2;
        v.z = 0;
        return NormalizeAngles (v * Mathf.Rad2Deg);
    }
    Quaternion q = new Quaternion (q1.w, q1.z, q1.x, q1.y);
    v.y = (float)Math.Atan2 (2f * q.x * q.w + 2f * q.y * q.z, 1 - 2f * (q.z * q.z + q.w * q.w));     // Yaw
    v.x = (float)Math.Asin (2f * (q.x * q.z - q.w * q.y));                             // Pitch
    v.z = (float)Math.Atan2 (2f * q.x * q.y + 2f * q.z * q.w, 1 - 2f * (q.y * q.y + q.z * q.z));      // Roll
    return NormalizeAngles (v * Mathf.Rad2Deg);
}

static Vector3 NormalizeAngles (Vector3 angles)
{
    angles.x = NormalizeAngle (angles.x);
    angles.y = NormalizeAngle (angles.y);
    angles.z = NormalizeAngle (angles.z);
    return angles;
}

static float NormalizeAngle (float angle)
{
    while (angle>360)
        angle -= 360;
    while (angle<0)
        angle += 360;
    return angle;
}

公共静态四元数ToQ（矢量3 v）
{
返回到q（v.y，v.x，v.z）；
}
公共静态四元数ToQ（浮动偏航、浮动俯仰、浮动滚转）
{
偏航*=Mathf.Deg2Rad；
螺距*=数学Deg2Rad；
滚动*=Mathf.Deg2Rad；
浮动滚动2=滚动*0.5f；
float sinRollOver2=（float）Math.Sin（（double）rollOver2）；
float cosRollOver2=（float）Math.Cos（（double）rollOver2）；
浮动俯仰2=俯仰*0.5f；
float sinPitchOver2=（float）Math.Sin（（双）pitchOver2）；
float cosPitchOver2=（float）Math.Cos（（double）pitchOver2）；
浮动yawOver2=偏航*0.5f；
float sinYawOver2=（float）Math.Sin（（double）yawOver2）；
float cosYawOver2=（float）Math.Cos（（double）yawOver2）；
四元数结果；
结果.w=cosYawOver2*cosPitchOver2*cosRollOver2+sinYawOver2*sinPitchOver2*sinRollOver2；
结果.x=cosYawOver2*sinPitchOver2*coslowover2+sinYawOver2*coslowover2*sinrolover2；
结果y=sinYawOver2*cosPitchOver2*cosRollOver2-cosYawOver2*sinPitchOver2*sinRollOver2；
结果.z=cosYawOver2*cosPitchOver2*sinRollOver2-sinYawOver2*sinPitchOver2*corollover2；
返回结果；
}
来自Q2的公共静态向量3（四元数q1）
{
浮点sqw=q1.w*q1.w；
浮点sqx=q1.x*q1.x；
浮点sqy=q1.y*q1.y；
浮点sqz=q1.z*q1.z；
float unit=sqx+sqy+sqz+sqw；//如果归一化为1，则为校正因子
浮动测试=q1.x*q1.w-q1.y*q1.z；
矢量3V；
如果（测试>0.4995f*单位）{//北极奇点
v、 y=2f*Mathf.Atan2（q1.y，q1.x）；
v、 x=Mathf.PI/2；
v、 z=0；
返回标准化值（v*Mathf.Rad2Deg）；
}
如果（test360）
角度-=360；
虽然（angle这个问题已经问了将近三年了，但我需要相同的代码，这里发布的代码似乎不正确，所以我调整了它们，发现如下：

公共静态四元数Euler（浮动偏航、浮动俯仰、浮动滚转）{
偏航*=Mathf.Deg2Rad；
螺距*=数学Deg2Rad；
滚动*=Mathf.Deg2Rad；
双偏航2=偏航*0.5f；
float cosYawOver2=（float）System.Math.Cos（yawOver2）；
float sinYawOver2=（float）System.Math.Sin（yawOver2）；
双俯仰2=俯仰*0.5f；
float cosPitchOver2=（float）System.Math.Cos（pitchOver2）；
float sinPitchOver2=（float）System.Math.Sin（pitchOver2）；
双滚动2=滚动*0.5f；
float cosRollOver2=（float）System.Math.Cos（rollOver2）；
float sinRollOver2=（float）System.Math.Sin（rollOver2）；
四元数结果；
结果.w=cosYawOver2*cosPitchOver2*cosRollOver2+sinYawOver2*sinPitchOver2*sinRollOver2；
结果.x=sinYawOver2*cosPitchOver2*cosRollOver2+cosYawOver2*sinPitchOver2*sinRollOver2；
结果.y=cosYawOver2*sinPitchOver2*cosRollOver2-sinYawOver2*cosproitchover2*sinRollOver2；
结果.z=cosYawOver2*cosPitchOver2*sinRollOver2-sinYawOver2*sinPitchOver2*corollover2；
返回结果；
}

根据一些快速测试，这与四元数匹配。Euler 100%
这是我的解决方案。这与Unity的四元数非常接近。Euler和Quaternion.eulerAngles。差异足够小，对于任何应用都不重要

public static Vector3 QuaternionToEuler(Quaternion q)
{
    Vector3 euler;

    // if the input quaternion is normalized, this is exactly one. Otherwise, this acts as a correction factor for the quaternion's not-normalizedness
    float unit = (q.x * q.x) + (q.y * q.y) + (q.z * q.z) + (q.w * q.w);

    // this will have a magnitude of 0.5 or greater if and only if this is a singularity case
    float test = q.x * q.w - q.y * q.z;

    if (test > 0.4995f * unit) // singularity at north pole
    {
        euler.x = Mathf.PI / 2;
        euler.y = 2f * Mathf.Atan2(q.y, q.x);
        euler.z = 0;
    }
    else if (test < -0.4995f * unit) // singularity at south pole
    {
        euler.x = -Mathf.PI / 2;
        euler.y = -2f * Mathf.Atan2(q.y, q.x);
        euler.z = 0;
    }
    else // no singularity - this is the majority of cases
    {
        euler.x = Mathf.Asin(2f * (q.w * q.x - q.y * q.z));
        euler.y = Mathf.Atan2(2f * q.w * q.y + 2f * q.z * q.x, 1 - 2f * (q.x * q.x + q.y * q.y));
        euler.z = Mathf.Atan2(2f * q.w * q.z + 2f * q.x * q.y, 1 - 2f * (q.z * q.z + q.x * q.x));
    }

    // all the math so far has been done in radians. Before returning, we convert to degrees...
    euler *= Mathf.Rad2Deg;

    //...and then ensure the degree values are between 0 and 360
    euler.x %= 360;
    euler.y %= 360;
    euler.z %= 360;

    return euler;
}

public static Quaternion EulerToQuaternion(Vector3 euler)
{
    float xOver2 = euler.x * Mathf.Deg2Rad * 0.5f;
    float yOver2 = euler.y * Mathf.Deg2Rad * 0.5f;
    float zOver2 = euler.z * Mathf.Deg2Rad * 0.5f;

    float sinXOver2 = Mathf.Sin(xOver2);
    float cosXOver2 = Mathf.Cos(xOver2);
    float sinYOver2 = Mathf.Sin(yOver2);
    float cosYOver2 = Mathf.Cos(yOver2);
    float sinZOver2 = Mathf.Sin(zOver2);
    float cosZOver2 = Mathf.Cos(zOver2);

    Quaternion result;
    result.x = cosYOver2 * sinXOver2 * cosZOver2 + sinYOver2 * cosXOver2 * sinZOver2;
    result.y = sinYOver2 * cosXOver2 * cosZOver2 - cosYOver2 * sinXOver2 * sinZOver2;
    result.z = cosYOver2 * cosXOver2 * sinZOver2 - sinYOver2 * sinXOver2 * cosZOver2;
    result.w = cosYOver2 * cosXOver2 * cosZOver2 + sinYOver2 * sinXOver2 * sinZOver2;

    return result;
}

公共静态向量3四元数Euler（四元数q）
{
矢量欧拉；
//如果输入的四元数是标准化的，则正好是一个。否则，这将作为四元数未标准化的校正因子
浮动单位=（q.x*q.x）+（q.y*q.y）+（q.z*q.z）+（q.w*q.w）；
//当且仅当这是奇点情况时，其大小为0.5或更大
浮动试验=q.x*q.w-q.y*q.z；
if（测试>0.4995f*单位）//北极奇点
{
euler.x=Mathf.PI/2；
euler.y=2f*Mathf.Atan2（q.y，q.x）；
euler.z=0；
}
else if（测试<-0.4995f*单位）//南极奇点
{
euler.x=-Mathf.PI/2；
euler.y=-2f*Mathf.Atan2（q.y，q.x）；
euler.z=0；
}
else//没有奇点-这是大多数情况
{
euler.x=Mathf.Asin（2f*（q.w*q.x-q.y*q.z））；
euler.y=Mathf.Atan2（2f*q.w*q.y+2f*q.z*q.x，1-2f*（q.x*q.x+q.y*q.y））；
euler.z=Mathf.Atan2（2f*q.w*q.z+2f*q.x*q.y，1-2f*（q.z*q.z+q.x*q.x））；
}
//到目前为止，所有的计算都是以弧度为单位的。在返回之前，我们将转换为度。。。
euler*=Mathf.Rad2Deg；
//…然后确保度值介于0和360之间
euler.x%=360；
euler.y%=360；
euler.z%=360；
返回欧拉；
}
公共静态四元数EulertoQuaderion（矢量3 euler）
{
浮点数xOver2=euler.x*Mathf.Deg2Rad*0.5f；
浮动yOver2=欧拉y*Mathf.Deg2Rad*0.5f；
float zOver2=e
public static Vector3 QuaternionToEuler(Quaternion q)
{
    Vector3 euler;

    // if the input quaternion is normalized, this is exactly one. Otherwise, this acts as a correction factor for the quaternion's not-normalizedness
    float unit = (q.x * q.x) + (q.y * q.y) + (q.z * q.z) + (q.w * q.w);

    // this will have a magnitude of 0.5 or greater if and only if this is a singularity case
    float test = q.x * q.w - q.y * q.z;

    if (test > 0.4995f * unit) // singularity at north pole
    {
        euler.x = Mathf.PI / 2;
        euler.y = 2f * Mathf.Atan2(q.y, q.x);
        euler.z = 0;
    }
    else if (test < -0.4995f * unit) // singularity at south pole
    {
        euler.x = -Mathf.PI / 2;
        euler.y = -2f * Mathf.Atan2(q.y, q.x);
        euler.z = 0;
    }
    else // no singularity - this is the majority of cases
    {
        euler.x = Mathf.Asin(2f * (q.w * q.x - q.y * q.z));
        euler.y = Mathf.Atan2(2f * q.w * q.y + 2f * q.z * q.x, 1 - 2f * (q.x * q.x + q.y * q.y));
        euler.z = Mathf.Atan2(2f * q.w * q.z + 2f * q.x * q.y, 1 - 2f * (q.z * q.z + q.x * q.x));
    }

    // all the math so far has been done in radians. Before returning, we convert to degrees...
    euler *= Mathf.Rad2Deg;

    //...and then ensure the degree values are between 0 and 360
    euler.x %= 360;
    euler.y %= 360;
    euler.z %= 360;

    return euler;
}

public static Quaternion EulerToQuaternion(Vector3 euler)
{
    float xOver2 = euler.x * Mathf.Deg2Rad * 0.5f;
    float yOver2 = euler.y * Mathf.Deg2Rad * 0.5f;
    float zOver2 = euler.z * Mathf.Deg2Rad * 0.5f;

    float sinXOver2 = Mathf.Sin(xOver2);
    float cosXOver2 = Mathf.Cos(xOver2);
    float sinYOver2 = Mathf.Sin(yOver2);
    float cosYOver2 = Mathf.Cos(yOver2);
    float sinZOver2 = Mathf.Sin(zOver2);
    float cosZOver2 = Mathf.Cos(zOver2);

    Quaternion result;
    result.x = cosYOver2 * sinXOver2 * cosZOver2 + sinYOver2 * cosXOver2 * sinZOver2;
    result.y = sinYOver2 * cosXOver2 * cosZOver2 - cosYOver2 * sinXOver2 * sinZOver2;
    result.z = cosYOver2 * cosXOver2 * sinZOver2 - sinYOver2 * sinXOver2 * cosZOver2;
    result.w = cosYOver2 * cosXOver2 * cosZOver2 + sinYOver2 * sinXOver2 * sinZOver2;

    return result;
}