Conversion between Euler <=> Quaternion like in Unity3d engine

Question

I've used two examples (from this site too), but results are not the same as those that said Unity.

Quaternion.Euler and .eulerAngles are Unity functions. FromQ doesn't perform singularity check, FromQ2 does.

Results:

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()

The code is here: http://pastebin.ru/eAlTHdYf

Can anyone correct this code? I need code that will return the values ​​that are identical to the values that Unity functions returns.

UPDATE

Here is fixed code: http://pastebin.com/riRLRvch. Both functions (FromQ and ToQ) work well. But I have a problem with a singularity. It can't detect the singularity properly.

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 can't calculate correct result so we have the singularity here. The same for (90, 50, 0) - (0.6, 0.3, -0.3, 0.6).

I see only one solution - calculate "test" as xw-yz. But I'm not sure this is right.

How to fix it?

Answer 1

I've found solution

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