Welcome to OGeek Q&A Community for programmer and developer-Open, Learning and Share
Welcome To Ask or Share your Answers For Others

Categories

0 votes
356 views
in Technique[技术] by (71.8m points)

c# - Conversion between Euler <=> Quaternion like in Unity3d engine

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?

See Question&Answers more detail:os

与恶龙缠斗过久,自身亦成为恶龙;凝视深渊过久,深渊将回以凝视…
Welcome To Ask or Share your Answers For Others

1 Reply

0 votes
by (71.8m points)

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

与恶龙缠斗过久,自身亦成为恶龙;凝视深渊过久,深渊将回以凝视…
OGeek|极客中国-欢迎来到极客的世界,一个免费开放的程序员编程交流平台!开放,进步,分享!让技术改变生活,让极客改变未来! Welcome to OGeek Q&A Community for programmer and developer-Open, Learning and Share
Click Here to Ask a Question

...