Quaternion.cpp

/***************************************************************************
 *   This file is part of OpenCAL: Open Computer Animation Library         *
 *   I created OpenCAL as my master's thesis Computer Science (multimedia) *
 *   at the tUL university in Diepenbeek, Belgium                          *
 *                                                                         *
 *   Copyright (C) 2003-2004 by Jeroen Dierckx                             *
 *   jeroen.dierckx@student.luc.ac.be                                      *
 *                                                                         *
 ***************************************************************************/

// Includes
#include "Quaternion.h"
using namespace OpenCAL::Utils;

#include <math.h>

using namespace std;


// Static members
const Quaternion Quaternion::identity(1.0f, Vector3::zero);


/******************************
* Constructors and destructor *
******************************/

Quaternion::Quaternion()
: m_scalar(1.0f), m_vector(0.0f, 0.0f, 0.0f)
{
}

Quaternion::Quaternion(float scalar, const Vector3 &vector)
: m_scalar(scalar), m_vector(vector)
{
}

Quaternion::~Quaternion()
{
}


/************************
* Get and set functions *
************************/


/*********************
* Operator functions *
*********************/

Quaternion Quaternion::operator+(const Quaternion &q) const
{
    return Quaternion(
        m_scalar + q.m_scalar,
        m_vector + q.m_vector
    );
}

Quaternion Quaternion::operator-(const Quaternion &q) const
{
    return Quaternion(
        m_scalar - q.m_scalar,
        m_vector - q.m_vector
    );
}

Quaternion Quaternion::operator*(const Quaternion &q) const
{
    return Quaternion(
        m_scalar * q.m_scalar - m_vector.dotProduct(q.m_vector),
        m_scalar * q.m_vector + q.m_scalar * m_vector + m_vector.crossProduct(q.m_vector)
    );
}

Quaternion Quaternion::operator*(const Vector3 &v) const
{
    return operator*(Quaternion(0, v));
}

Quaternion Quaternion::operator*(float factor) const
{
    return Quaternion(
        m_scalar * factor,
        m_vector * factor
    );
}

Quaternion Quaternion::operator/(float factor) const
{
    return Quaternion(
        m_scalar / factor,
        m_vector / factor
    );
}

void Quaternion::operator+=(const Quaternion &q)
{
    m_scalar += q.m_scalar;
    m_vector += q.m_vector;
}

void Quaternion::operator-=(const Quaternion &q)
{
    m_scalar -= q.m_scalar;
    m_vector -= q.m_vector;
}

void Quaternion::operator*=(const Quaternion &q)
{
    float scalar = m_scalar * q.m_scalar - m_vector.dotProduct(q.m_vector);
    m_vector = m_scalar * q.m_vector + q.m_scalar * m_vector + m_vector.crossProduct(q.m_vector);
    m_scalar = scalar;
}

void Quaternion::operator*=(const Vector3 &v)
{
    operator*=(Quaternion(0, v));
}

void Quaternion::operator*=(float factor)
{
    m_scalar *= factor;
    m_vector *= factor;
}

void Quaternion::operator/=(float factor)
{
    m_scalar /= factor;
    m_vector /= factor;
}

Quaternion Quaternion::operator-() const
{
    return Quaternion(
        -m_scalar,
        -m_vector
    );
}

bool Quaternion::operator==(const Quaternion &q) const
{
    return m_scalar == q.m_scalar && m_vector == q.m_vector;
}

bool Quaternion::operator!=(const Quaternion &q) const
{
    return m_scalar != q.m_scalar || m_vector != q.m_vector;
}

// Friend functions
Quaternion OpenCAL::Utils::operator*(const Vector3 &v, const Quaternion &q)
{
    return Quaternion(0, v) * q;
}

Quaternion OpenCAL::Utils::operator*(float factor, const Quaternion &q)
{
    return Quaternion(
        factor * q.m_scalar,
        factor * q.m_vector
    );
}

Quaternion OpenCAL::Utils::operator/(float factor, const Quaternion &q)
{
    return Quaternion(
        factor / q.m_scalar,
        factor / q.m_vector
    );
}


/************
* Magnitude *
************/

float Quaternion::magnitude() const
{
    return sqrtf(
        m_scalar * m_scalar +
        m_vector.getX() * m_vector.getX() +
        m_vector.getY() * m_vector.getY() +
        m_vector.getZ() * m_vector.getZ()
    );
}

float Quaternion::magnitudeSquared() const
{
    return (
        m_scalar * m_scalar +
        m_vector.getX() * m_vector.getX() +
        m_vector.getY() * m_vector.getY() +
        m_vector.getZ() * m_vector.getZ()
    );
}

void Quaternion::inverse()
{
    float msi = 1.0f / magnitudeSquared();

    m_scalar *= msi;
    m_vector *= -msi;
}

Quaternion Quaternion::inversed() const
{
    float msi = 1.0f / magnitudeSquared();

    return Quaternion(m_scalar * msi, -m_vector * msi);
}

void Quaternion::inversed(Quaternion *q) const
{
    float msi = 1.0f / magnitudeSquared();

    q->setScalar(m_scalar * msi);
    q->setVector(-m_vector * msi);
}

void Quaternion::normalize()
{
    float mag = magnitude();

    m_scalar /= mag;
    m_vector /= mag;
}

Quaternion Quaternion::normalized() const
{
    float mag = magnitude();

    return Quaternion(m_scalar / mag, m_vector / mag);
}

void Quaternion::normalized(Quaternion *q) const
{
    float mag = magnitude();

    q->setScalar(m_scalar / mag);
    q->setVector(m_vector / mag);
}


/**************
* Dot product *
**************/

float Quaternion::dotProduct(const Quaternion &q) const
{
    return m_scalar * q.m_scalar + m_vector.dotProduct(q.m_vector);
}


Quaternion Quaternion::slerped(float u, const Quaternion &source, const Quaternion &dest) const
{
    float cosAngle = source.dotProduct(dest);
    float angle, sinAngle, a, b;

    if(cosAngle > 0) // Interpolate to dest
    {
        angle = acosf(cosAngle);
        sinAngle = sinf(angle);
        a = sinf((1.0f - u) * angle) / sinAngle;
        b = sinf(u * angle) / sinAngle;
    }
    else // Interpolate to -dest
    {
        angle = 2 * Math::pi - acosf(cosAngle);
        sinAngle = sinf(angle);
        a = sinf((1.0f - u) * angle) / sinAngle;
        b = -sinf(u * angle) / sinAngle;
    }

    return (a * source + b * dest).normalized();
}


/************************
* Axis - Angle rotation *
************************/

Quaternion Quaternion::createFromAxisAngle(const Vector3 &axis, float degrees)
{
    while(degrees < 0.0f)
        degrees += 360.0f;
    while(degrees >= 360.0f)
        degrees -= 360.0f;

    float halfAngle = Math::degToRad(degrees * 0.5f);

    return Quaternion(
        cosf(halfAngle),
        sinf(halfAngle) * axis
    );
}

void Quaternion::fromAxisAngle(const Vector3 &axis, float degrees)
{
    while(degrees < 0.0f)
        degrees += 360.0f;
    while(degrees >= 360.0f)
        degrees -= 360.0f;

    float halfAngle = Math::degToRad(degrees * 0.5f);

    m_scalar = cosf(halfAngle);
    m_vector = sinf(halfAngle) * axis;
}

void Quaternion::toAxisAngle(Vector3 *axis, float *degrees) const
{
/*
    float halfAngle = acosf(m_scalar);
    cout << "Half Angle: " << halfAngle << endl;

    *degrees = 2 * halfAngle * RADTODEG;
    *axis = m_vector / sinf(halfAngle);
*/

/*
    // From Euclideanspace.com
    float s = sqrtf(1 - m_scalar * m_scalar);
    if(s > -0.001f && s < 0.001f) s = 1.0f;

    *degrees = 2 * acosf(m_scalar) * RADTODEG;
    *axis = m_vector / s;
*/

    // From http://www.magic-software.com
    if(m_scalar > 1 || m_scalar < -1) // This can't happen if the quaternion is normalized
    {
        cerr << "The quaternion isn't normalized while converting to axis-angle!!" << endl;
        return;
    }

    if(m_scalar == 1 || m_scalar == -1) // No rotation
    {
        *degrees = 0.0f;
        *axis = Vector3(1,0,0);
    }
    else
    {
        *degrees = Math::radToDeg(2 * acosf(m_scalar));
        *axis = m_vector / sqrtf(1.0f - Math::squared(m_scalar));
    }
}


/***********************
* Euler angle rotation *
***********************/

void Quaternion::fromEulerAngle(float rotX, float rotY, float rotZ)
{
    Quaternion x, y, z;
    x.fromAxisAngle(Vector3(1.0f, 0.0f, 0.0f), rotX);
    y.fromAxisAngle(Vector3(0.0f, 1.0f, 0.0f), rotY);
    z.fromAxisAngle(Vector3(0.0f, 0.0f, 1.0f), rotZ);

    *this = z * y * x;
}

void Quaternion::fromEulerAngle(const Vector3 &rot)
{
    fromEulerAngle(rot.getX(), rot.getY(), rot.getZ());
}

Vector3 Quaternion::toEulerAngle() const
{
    return Vector3::zero;
}

void Quaternion::toEulerAngle(float *rotX, float *rotY, float *rotZ) const
{
    *rotX = *rotY = *rotZ = 0.0f;
}

void Quaternion::toEulerAngle(Vector3 *rot) const
{
    *rot = Vector3::zero;
}


/******************
* Matrix rotation *
******************/

void Quaternion::fromMatrix33(const Matrix33 &matrix)
{
    float m00 = matrix.get(0,0);
    float m11 = matrix.get(1,1);
    float m22 = matrix.get(2,2);

    m_scalar = sqrtf(m00 + m11 + m22 + 1.0f) * 0.5f;

    float temp = 1 - 2 * m_scalar * m_scalar;
    m_vector.setX(sqrtf((m00 + temp) * 0.5f));
    m_vector.setY(sqrtf((m11 + temp) * 0.5f));
    m_vector.setZ(sqrtf((m22 + temp) * 0.5f));
}

Matrix33 Quaternion::toMatrix33() const
{
    Matrix33 result;
    toMatrix33(&result);
    return result;
}

void Quaternion::toMatrix33(Matrix33 *matrix) const
{
    matrix->identity();

    float x2 = m_vector.getX() * m_vector.getX();
    float y2 = m_vector.getY() * m_vector.getY();
    float z2 = m_vector.getZ() * m_vector.getZ();

    float xy = m_vector.getX() * m_vector.getY();
    float xz = m_vector.getX() * m_vector.getZ();
    float yz = m_vector.getY() * m_vector.getZ();

    float sx = m_scalar * m_vector.getX();
    float sy = m_scalar * m_vector.getY();
    float sz = m_scalar * m_vector.getZ();


    matrix->set(0, 1 - 2 * (y2 + z2));
    matrix->set(1,     2 * (xy - sz));
    matrix->set(2,     2 * (xz + sy));

    matrix->set(3,     2 * (xy + sz));
    matrix->set(4, 1 - 2 * (x2 + z2));
    matrix->set(5,     2 * (yz - sx));

    matrix->set(6,     2 * (xz - sy));
    matrix->set(7,     2 * (yz + sx));
    matrix->set(8, 1 - 2 * (x2 + y2));
}


/******************
* Other functions *
******************/

void Quaternion::print() const
{
    cout << m_scalar << ", (";
    cout << m_vector.getX() << ", ";
    cout << m_vector.getY() << ", ";
    cout << m_vector.getZ() << ")" << endl;
}

ostream &OpenCAL::Utils::operator<<(ostream &stream, const Quaternion &q)
{
    stream << "[" << q.m_scalar << ", " << q.m_vector << "]";
    return stream;
}

---