OpenRaider/src/Quaternion.cpp

/* -*- Mode: C++; tab-width: 3; indent-tabs-mode: t; c-basic-offset: 3 -*- */
/*================================================================
 *
 * Project : Hel
 * Author  : Terry 'Mongoose' Hendrix II
 * Website : http://www.westga.edu/~stu7440/
 * Email   : stu7440@westga.edu
 * Object  : Quaternion
 * License : No use w/o permission (C) 2002 Mongoose
 * Comments: Quaternion now in C++ class form fresh from the grove
 *
 *
 *           This file was generated using Mongoose's C++
 *           template generator script.  <stu7440@westga.edu>
 *
 *-- History -------------------------------------------------
 *
 * 2002.12.16:
 * Mongoose - Created, based on mtk3d ( freyja )
 =================================================================*/

#include <math.h>

#include <Quaternion.h>


////////////////////////////////////////////////////////////
// Constructors
////////////////////////////////////////////////////////////

Quaternion::Quaternion()
{
    mW = 0;
    mX = 0;
    mY = 0;
    mZ = 0;
}


Quaternion::Quaternion(vec_t w, vec_t x, vec_t y, vec_t z)
{
    mW = w;
    mX = x;
    mY = y;
    mZ = z;
}


Quaternion::Quaternion(vec4_t v)
{
    mW = v[0];
    mX = v[1];
    mY = v[2];
    mZ = v[3];
}


Quaternion::~Quaternion()
{
}


////////////////////////////////////////////////////////////
// Public Accessors
////////////////////////////////////////////////////////////

void Quaternion::getMatrix(matrix_t m)
{
    m[ 0] = 1.0f - 2.0f * (mY*mY + mZ*mZ);
    m[ 1] = 2.0f * (mX*mY - mW*mZ);
    m[ 2] = 2.0f * (mX*mZ + mW*mY);
    m[ 3] = 0.0f;

    m[ 4] = 2.0f * (mX*mY + mW*mZ);
    m[ 5] = 1.0f - 2.0f * (mX*mX + mZ*mZ);
    m[ 6] = 2.0f * (mY*mZ - mW*mX);
    m[ 7] = 0.0f;

    m[ 8] = 2.0f * (mX*mZ - mW*mY);
    m[ 9] = 2.0f * (mY*mZ + mW*mX);
    m[10] = 1.0 - 2.0f * (mX*mX + mY*mY);
    m[11] = 0.0f;

    m[12] = 0.0f;
    m[13] = 0.0f;
    m[14] = 0.0f;
    m[15] = 1.0f;
}


Quaternion Quaternion::operator =(const Quaternion &q)
{
    mW  = q.mW;
    mX = q.mX;
    mY = q.mY;
    mZ = q.mZ;

    return (*this);
}


Quaternion Quaternion::operator *(const Quaternion &q)
{
    return multiply(*this, q);
}


Quaternion Quaternion::operator /(const Quaternion &q)
{
    return divide(*this, q);
}


Quaternion Quaternion::operator +(const Quaternion &q)
{
    return add(*this, q);
}


Quaternion Quaternion::operator -(const Quaternion &q)
{
    return subtract(*this, q);
}


bool Quaternion::operator ==(const Quaternion &q)
{
    //return (mX == q.mX && mY == q.mY && mZ == q.mZ && mW == q.mW);
    return (equalEpsilon(mX, q.mX) && equalEpsilon(mY, q.mY) &&
            equalEpsilon(mZ, q.mZ) && equalEpsilon(mW, q.mW));
}


Quaternion Quaternion::conjugate()
{
    return Quaternion(mW, -mX, -mY, -mZ);
}


Quaternion Quaternion::scale(vec_t s)
{
    return Quaternion(mW * s, mX * s, mY * s, mZ * s);
}


Quaternion Quaternion::inverse()
{
    return conjugate().scale(1/magnitude());
}


vec_t Quaternion::dot(Quaternion a, Quaternion b)
{
    return ((a.mW * b.mW) + (a.mX * b.mX) + (a.mY * b.mY) + (a.mZ * b.mZ));
}


vec_t Quaternion::magnitude()
{
    return sqrt(dot(*this, *this));
}


////////////////////////////////////////////////////////////
// Public Mutators
////////////////////////////////////////////////////////////

void Quaternion::setIdentity()
{
    mW = 1.0;
    mX = 0.0;
    mY = 0.0;
    mZ = 0.0;
}


void Quaternion::set(vec_t angle, vec_t x, vec_t y, vec_t z)
{
    vec_t temp, dist;


    // Normalize
    temp = x*x + y*y + z*z;

    dist = (float)(1.0 / sqrt(temp));

    x *= dist;
    y *= dist;
    z *= dist;

    mX = x;
    mY = y;
    mZ = z;

    mW = (float)cos(angle / 2.0f);
}


void Quaternion::normalize()
{
    vec_t dist, square;


    square = mX * mX + mY * mY + mZ * mZ + mW * mW;

    if (square > 0.0)
    {
        dist = (float)(1.0 / sqrt(square));
    }
    else
    {
        dist = 1;
    }

    mX *= dist;
    mY *= dist;
    mZ *= dist;
    mW *= dist;
}


void Quaternion::copy(Quaternion q)
{
    mW = q.mW;
    mX = q.mX;
    mY = q.mY;
    mZ = q.mZ;
}


Quaternion Quaternion::slerp(Quaternion a, Quaternion b, vec_t time)
{
    /*******************************************************************
     * Spherical Linear Interpolation algorthim
     *-----------------------------------------------------------------
     *
     * Interpolate between A and B rotations ( Find qI )
     *
     * qI = (((qB . qA)^ -1)^ Time) qA
     *
     * http://www.magic-software.com/Documentation/quat.pdf
     *
     * Thanks to digiben for algorithms and basis of the notes in
     * this func
     *
     *******************************************************************/

    vec_t result, scaleA, scaleB, theta, sinTheta;
    Quaternion i;


    // Don't bother if it's the same rotation, it's the same as the result
    if (a == b)
    {
        return a;
    }

    // A . B
    result = dot(a, b);

    // If the dot product is less than 0, the angle is greater than 90 degrees
    if (result < 0.0f)
    {
        // Negate quaternion B and the result of the dot product
        b = Quaternion(-b.mW, -b.mX, -b.mY, -b.mZ);
        result = -result;
    }

    // Set the first and second scale for the interpolation
    scaleA = 1 - time;
    scaleB = time;

    // Next, we want to actually calculate the spherical interpolation.  Since this
    // calculation is quite computationally expensive, we want to only perform it
    // if the angle between the 2 quaternions is large enough to warrant it.  If the
    // angle is fairly small, we can actually just do a simpler linear interpolation
    // of the 2 quaternions, and skip all the complex math.  We create a "delta" value
    // of 0.1 to say that if the cosine of the angle (result of the dot product) between
    // the 2 quaternions is smaller than 0.1, then we do NOT want to perform the full on
    // interpolation using.  This is because you won't really notice the difference.

    // Check if the angle between the 2 quaternions was big enough
    // to warrant such calculations
    if (1 - result > 0.1f)
    {
        // Get the angle between the 2 quaternions, and then
        // store the sin() of that angle
        theta = (float)acos(result);
        sinTheta = (float)sin(theta);

        // Calculate the scale for qA and qB, according to
        // the angle and it's sine value
        scaleA = (float)sin((1 - time) * theta) / sinTheta;
        scaleB = (float)sin((time * theta)) / sinTheta;
    }

    // Calculate the x, y, z and w values for the quaternion by using a special
    // form of linear interpolation for quaternions.
    return (a.scale(scaleA) + b.scale(scaleB));
}


void Quaternion::setByMatrix(matrix_t matrix)
{
    float diagonal = matrix[0] + matrix[5] + matrix[10] + 1.0f;
    float scale = 0.0f;
    float w = 0.0f, x = 0.0f, y = 0.0f, z = 0.0f;


    if (diagonal > 0.00000001)
    {
        // Calculate the scale of the diagonal
        scale = (float)(sqrt(diagonal) * 2);

        w = 0.25f * scale;
        x = (matrix[9] - matrix[6]) / scale;
        y = (matrix[2] - matrix[8]) / scale;
        z = (matrix[4] - matrix[1]) / scale;
    }
    else
    {
        // If the first element of the diagonal is the greatest value
        if (matrix[0] > matrix[5] && matrix[0] > matrix[10])
        {
            // Find the scale according to the first element, and double it
            scale = (float)sqrt(1.0f + matrix[0] - matrix[5] - matrix[10])*2.0f;

            // Calculate the quaternion
            w = (matrix[9] - matrix[6]) / scale;
            x = 0.25f * scale;
            y = (matrix[4] + matrix[1]) / scale;
            z = (matrix[2] + matrix[8]) / scale;
        }
        // The second element of the diagonal is the greatest value
        else if (matrix[5] > matrix[10])
        {
            // Find the scale according to the second element, and double it
            scale = (float)sqrt(1.0f + matrix[5] - matrix[0] - matrix[10])*2.0f;

            // Calculate the quaternion
            w = (matrix[2] - matrix[8]) / scale;
            x = (matrix[4] + matrix[1]) / scale;
            y = 0.25f * scale;
            z = (matrix[9] + matrix[6]) / scale;
        }
        // The third element of the diagonal is the greatest value
        else
        {
            // Find the scale according to the third element, and double it
            scale = (float)sqrt(1.0f + matrix[10] - matrix[0] - matrix[5])*2.0f;

            // Calculate the quaternion
            w = (matrix[4] - matrix[1]) / scale;
            x = (matrix[2] + matrix[8]) / scale;
            y = (matrix[9] + matrix[6]) / scale;
            z = 0.25f * scale;
        }
    }

    mW = w;
    mX = x;
    mY = y;
    mZ = z;
}


////////////////////////////////////////////////////////////
// Private Accessors
////////////////////////////////////////////////////////////

Quaternion Quaternion::multiply(Quaternion a, Quaternion b)
{
    return Quaternion(a.mW * b.mW - a.mX * b.mX - a.mY * b.mY - a.mZ * b.mZ,
            a.mW * b.mX + a.mX * b.mW + a.mY * b.mZ - a.mZ * b.mY,
            a.mW * b.mY + a.mY * b.mW + a.mZ * b.mX - a.mX * b.mZ,
            a.mW * b.mZ + a.mZ * b.mW + a.mX * b.mY - a.mY * b.mX);
}


Quaternion Quaternion::divide(Quaternion a, Quaternion b)
{
    return (a * (b.inverse()));
}


Quaternion Quaternion::add(Quaternion a, Quaternion b)
{
    return Quaternion(a.mW + b.mW,
            a.mX + b.mX,
            a.mY + b.mY,
            a.mZ + b.mZ);
}


Quaternion Quaternion::subtract(Quaternion a, Quaternion b)
{
    return Quaternion(a.mW - b.mW,
            a.mX - b.mX,
            a.mY - b.mY,
            a.mZ - b.mZ);
}


////////////////////////////////////////////////////////////
// Private Mutators
////////////////////////////////////////////////////////////