---

quaternion.cpp

//FILE:         quaternion.cpp
//AUTHOR:       Nathan Cournia <nathan@cournia.com>

#include <cassert>
#include "quaternion.h"

const math::quaternion math::quaternion::IDENTITY( 
        0.0, 0.0, 0.0, //xyz
        1.0 //w
);

void
math::quaternion::get_rotation_matrix( math::matrix4& m ) const
{
        real_t x2 = m_v.x( ) + m_v.x( );
        real_t y2 = m_v.y( ) + m_v.y( );
        real_t z2 = m_v.z( ) + m_v.z( );

        real_t xx = m_v.x( ) * x2;
        real_t xy = m_v.x( ) * y2;
        real_t xz = m_v.x( ) * z2;
        real_t xw = x2 * m_w;

        real_t yy = m_v.y( ) * y2;
        real_t yz = m_v.y( ) * z2;
        real_t yw = y2 * m_w;

        real_t zz = m_v.z( ) * z2;
        real_t zw = z2 * m_w;

        m.set( 0, 0, 1.0 - ( yy + zz ) );
        m.set( 0, 1, ( xy - zw ) );
        m.set( 0, 2, ( xz + yw ) );
        m.set( 0, 3, 0.0 );

        m.set( 1, 0, ( xy + zw ) );
        m.set( 1, 1, 1.0 - ( xx + zz ) );
        m.set( 1, 2, ( yz - xw ) );
        m.set( 1, 3, 0.0 );

        m.set( 2, 0, ( xz - yw ) );
        m.set( 2, 1, ( yz + xw ) );
        m.set( 2, 2, 1.0 - ( xx + yy ) );
        m.set( 2, 3, 0.0 );

        m.set( 3, 0, 0.0 );
        m.set( 3, 1, 0.0 );
        m.set( 3, 2, 0.0 );
        m.set( 3, 3, 1.0 );
}

math::matrix4
math::quaternion::get_rotation_matrix( void ) const
{
        math::matrix4 m;
        get_rotation_matrix( m );
        return m;
}

void
math::quaternion::get_transposed_rotation_matrix( math::matrix4& m ) const
{
        real_t x2 = m_v.x( ) + m_v.x( );
        real_t y2 = m_v.y( ) + m_v.y( );
        real_t z2 = m_v.z( ) + m_v.z( );

        real_t xx = m_v.x( ) * x2;
        real_t xy = m_v.x( ) * y2;
        real_t xz = m_v.x( ) * z2;
        real_t xw = x2 * m_w;

        real_t yy = m_v.y( ) * y2;
        real_t yz = m_v.y( ) * z2;
        real_t yw = y2 * m_w;

        real_t zz = m_v.z( ) * z2;
        real_t zw = z2 * m_w;

        m.set( 0, 0, 1.0 - ( yy + zz ) );
        m.set( 0, 1, ( xy + zw ) );
        m.set( 0, 2, ( xz - yw ) );
        m.set( 0, 3, 0.0 );

        m.set( 1, 0, ( xy - zw ) );
        m.set( 1, 1, 1.0 - ( xx + zz ) );
        m.set( 1, 2, ( yz + xw ) );
        m.set( 1, 3, 0.0 );

        m.set( 2, 0, ( xz + yw ) );
        m.set( 2, 1, ( yz - xw ) );
        m.set( 2, 2, 1.0 - ( xx + yy ) );
        m.set( 2, 3, 0.0 );

        m.set( 3, 0, 0.0 );
        m.set( 3, 1, 0.0 );
        m.set( 3, 2, 0.0 );
        m.set( 3, 3, 1.0 );
}

math::matrix4
math::quaternion::get_transposed_rotation_matrix( void ) const
{
        math::matrix4 m;
        get_transposed_rotation_matrix( m );
        return m;
}

//not communative
math::quaternion
math::quaternion::operator* ( const math::quaternion& rhs ) const
{
        return math::quaternion( 
                rhs.m_w * m_v.x() + rhs.m_v.x() * m_w + 
                        rhs.m_v.z() * m_v.y() - rhs.m_v.y() * m_v.z(), //x
                rhs.m_w * m_v.y() + rhs.m_v.y() * m_w + 
                        rhs.m_v.x() * m_v.z() - rhs.m_v.z() * m_v.x(), //y
                rhs.m_w * m_v.z() + rhs.m_v.z() * m_w + 
                        rhs.m_v.y() * m_v.x() - rhs.m_v.x() * m_v.y(),  //z
                rhs.m_w * m_w - rhs.m_v.x() * m_v.x() - 
                        rhs.m_v.y() * m_v.y() - rhs.m_v.z() * m_v.z() //w
                );
}

math::vector3 
math::quaternion::operator* ( const math::vector3& rhs ) const
{
        math::matrix4 rotation;
        get_rotation_matrix( rotation );
        return rotation * rhs;
}
        

void
math::quaternion::from_angle_axis( real_t radians, const math::vector3& axis )
{
        //axis should be normalized
        assert( math::equals( axis.length( ), 1.0 ) );

        radians *= 0.5;
        math::multiply( m_v, axis, sin( radians ) );
        m_w = cos( radians );
}
        
void
math::quaternion::get_angle_axis( real_t& radians, math::vector3& axis ) const
{
        //math::quaternion should be normalized
        assert( math::equals( magnitude( ), 1.0 ) );

        //angle
        radians = acos( m_w ) * 2.0;

        //axis
        real_t sin_a = sqrt( 1.0 - m_w * m_w );

        //make sure we don't divide by zero
        if( math::equals( sin_a, 0.0 ) ) {
                sin_a = 1.0;
        }

        //set the axis
        axis.set( 
                m_v.x( ) / sin_a, 
                m_v.y( ) / sin_a, 
                m_v.z( ) / sin_a 
        );
}

math::quaternion
math::quaternion::slerp( real_t percent, const math::quaternion& qa, 
        const math::quaternion& qb )
{
        real_t cos_a = qa.w( ) * qb.w( ) + 
        //real_t cos_a = qa.m_w * qb.m_w + 
                math::dot( qa.v( ), qb.v( ) );
        real_t angle = acos( cos_a );

        if( math::equals( angle, 0.0 ) ) {
                //same
                return qa;
        }

        real_t sin_a = sin( angle );
        real_t inverse_sin = 1.0 / sin_a;
        real_t a = sin( (1.0 - percent) * angle ) * inverse_sin;
        real_t b = sin( percent * angle) * inverse_sin;
        return math::quaternion( 
                        a * qa.x( ) + b * qb.x( ),
                        a * qa.y( ) + b * qb.y( ),
                        a * qa.z( ) + b * qb.z( ),
                        a * qa.w( ) + b * qb.w( )
                );
}

std::ostream& 
operator<< ( std::ostream& o, const math::quaternion& q )
{
        return o << q.m_v << " " << q.m_w;
}

---