Euler <=> Quaternion之间的转换就像在Unity3d引擎中一样

Question

我已经使用了两个例子(也来自这个网站),但结果与Unity说的结果不一样.

Quaternion.Euler和.eulerAngles是Unity函数.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()

代码在这里:http://pastebin.ru/eAlTHdYf

任何人都可以更正此代码？我需要的代码将返回与Unity函数返回的值相同的值.

UPDATE

这是固定代码:http://pastebin.com/riRLRvch.这两个功能(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.但我不确定这是对的.

怎么解决？

Answer 1

我找到了解决方案

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