---

Quaternion.cc

Go to the documentation of this file.

/*
    File:       Quaternion.cc

    Function:   

    Author:     Andrew Willmott

    Notes:      
*/

#include "gcl/Quaternion.h"

Quaternion MakeQuat(const Vector &axis, GCLReal theta)
{
    GCLReal         s;
    Quaternion      q;
    
    theta /= 2.0;
    s = sin(theta);
    
    q[0] = s * axis[0];
    q[1] = s * axis[1];
    q[2] = s * axis[2];
    q[3] = cos(theta);
    
    return(q);
}

Quaternion MakeQuat(const Vector &point)
{
    return(Quaternion(point, 0.0));
}

Quaternion QuatMult(const Quaternion &a, const Quaternion &b)
{
    Quaternion  result;
    Vector      &va = (Vector&) a;
    Vector      &vb = (Vector&) b;

    result = Quaternion(a[3] * vb + b[3] * va + cross(va, vb), a[3] * b[3] - dot(va, vb));

    return(result);
}

Quaternion MakeQuat(const VecTrans &R)
{
    const Double epsilon = 1e-10;

    GCLReal     x2, y2, w2;
    GCLReal     s;
    Quaternion  q;
        
    /* Use the Matrix -> Quaternion algorithm from Shoemake's paper */

    /*
        This algorithm avoids near-zero divides by looking for a large
        component; first w, then x, y, or z. When the trace is greater
        than zero, |w| is greater than 1/2, which is as small as a
        largest component can be.  Otherwise the largest diagonal entry
        corresponds to the largest of |x|, |w| or |z|, one of which must
        be larger than |w|, and at least 1/2.
     */

    w2 = 0.25 * (1.0 + trace(R));

    if (w2 > epsilon)
    {
        s = sqrt(w2);
        q[3] = s;
        s = 0.25 / s;
        q[0] = (R[2][1] - R[1][2]) * s;
        q[1] = (R[0][2] - R[2][0]) * s;
        q[2] = (R[1][0] - R[0][1]) * s;
    }
    else
    {
        q[3] = 0.0;
        x2 = -0.5 * (R[1][1] + R[2][2]);

        if (x2 > epsilon)
        {
            s = sqrt(x2);
            q[0] = s;
            s = 0.5 / s;
            q[1] = R[1][0] * s;
            q[2] = R[2][0] * s;
        }
        else
        {
            q[0] = 0.0;
            y2 = 0.5 * (1.0 - R[2][2]);

            if (y2 > epsilon)
            {
                s = sqrt(y2);
                q[1] = s;
                s = 0.5 / s;
                q[2] = R[2][1] * s;
            }
            else
            {
                q[1] = 0.0;
                q[2] = 1.0;
            }
        }
    }

    q.Normalise();

    return(q);
}

---