C:/development/RenderTools/src/common/Matrix.cpp

Go to the documentation of this file.

#include "Matrix.h"
#include "Error.h"
#include "RenderTools.h"
#ifndef RT_AS3
#       ifndef RT_GLES1
#               include "Program.h"
#       endif
#endif

namespace RenderTools {


float   smallest( float a, float b ){
        return( a < b ? a : b );
}

float   largest( float a, float b ){
        return( a < b ? a : b );
}

double  smallest( double a, double b ){
        return( a < b ? a : b );
}

double  largest( double a, double b ){
        return( a > b ? a : b );
}

float   smallest( float a, double b ){
        return( a < ( float )b ? a : ( float )b );
}

float   largest( float a, double b ){
        return( a > ( float )b ? a : ( float )b );
}

float   smallest( double a, float b ){
        return( ( float )a < b ? ( float )a : b );
}

float   largest( double a, float b ){
        return( ( float )a > b ? ( float )a : b );
}

float clamp( float v, float min, float max ){
        if( v < min ){
                v = min;
        }
        else if( v > max ){
                v = max;
        }
        return( v );
}

float clampMin( float x, float min ){
        return( ( x < min ) ? min : x );
}

float clampMax( float x, float max ){
        return( ( x > max ) ? max : x );
}

float sgn( float v ){
        return( v < 0.0f ? -1.0f : 1.0f );
}
        
float toRadians( float degrees ){
        return( ( degrees / 180.0f ) * M_PI );
}
        
float toDegrees( float radians ){
        return( 180.0f * ( radians / M_PI ) );
}

float distance( const Vec2 & v1, const Vec2 & v2 ){
        return( length( v2 - v1 ) );
}

float distance( const Vec3 & v1, const Vec3 & v2 ){
        return( length( v2 - v1 ) );
}

float distance( const Vec4 & v1, const Vec4 & v2 ){
        return( length( v2 - v1 ) );
}
        
float distance2( const Vec2 & v1, const Vec2 & v2 ){
        return( length2( v2 - v1 ) );
}

float distance2( const Vec3 & v1, const Vec3 & v2 ){
        return( length2( v2 - v1 ) );
}

float distance2( const Vec4 & v1, const Vec4 & v2 ){
        return( length2( v2 - v1 ) );
}
        
float length( const Vec2 & v ){
        return( v.len() );
}
        
float length( const Vec3 & v ){
        return( v.len() );
}
        
float length( const Vec4 & v ){
        return( v.len() );
}
        
float length2( const Vec2 & v ){
        return( v.hyp() );
}
        
float length2( const Vec3 & v ){
        return( v.hyp() );
}
        
float length2( const Vec4 & v ){
        return( v.hyp() );
}
        
Vec2 normalize( const Vec2 & v ){
        return( v.normalized() );
}
        
Vec3 normalize( const Vec3 & v ){
        return( v.normalized() );
}
        
Vec4 normalize( const Vec4 & v ){
        return( v.normalized() );
}
        
float dot( const Vec2 & v1, const Vec2 & v2 ){
        return( v1.dot( v2 ) );
}
        
float dot( const Vec3 & v1, const Vec3 & v2 ){
        return( v1.dot( v2 ) );
}
        
float dot( const Vec4 & v1, const Vec4 & v2 ){
        return( v1.dot( v2 ) );
}
        
float cross( const Vec2 & v1, const Vec2 & v2 ){
        return( dot( v1.cross(), v2 ) );
}

Vec3 cross( const Vec3 & v1, const Vec3 & v2 ){
        return( v1.cross( v2 ) );
}
        
float mix( float f1, float f2, float f ){
        return( f2 * f + f1 * ( 1.0f - f ) );
}
        
Vec2 mix( const Vec2 & v1, const Vec2 & v2, float f ){
        return( v2 * f + v1 * ( 1.0f - f ) );
}
        
Vec3 mix( const Vec3 & v1, const Vec3 & v2, float f ){
        return( v2 * f + v1 * ( 1.0f - f ) );
}
        
Vec4 mix( const Vec4 & v1, const Vec4 & v2, float f ){
        return( v2 * f + v1 * ( 1.0f - f ) );
}
        
Mat2 mix( const Mat2 & m1, const Mat2 & m2, float f ){
        Mat2 m;
        int i = 0;
        for( i = 0; i < 4; i++ ){
                m[i] = f * m2[i] + ( 1.0f - f ) * m1[i];
        }
        return( m );
}
        
Mat3 mix( const Mat3 & m1, const Mat3 & m2, float f ){
        Mat3 m;
        int i = 0;
        for( i = 0; i < 9; i++ ){
                m[i] = f * m2[i] + ( 1.0f - f ) * m1[i];
        }
        return( m );
}
        
Mat4 mix( const Mat4 & m1, const Mat4 & m2, float f ){
        Mat4 m;
        int i = 0;
        for( i = 0; i < 16; i++ ){
                m[i] = f * m2[i] + ( 1.0f - f ) * m1[i];
        }
        return( m );
}

Vec2 operator * ( float s, const Vec2 & v ){
        return( v * s );
}
        
Vec3 operator * ( float s, const Vec3 & v ){
        return( v * s );
}
        
Vec4 operator * ( float s, const Vec4 & v ){
        return( v * s );
}

istream & operator >> ( istream & s, Vec2 & v ){
        s >> v[0] >> v[1];
        return s;
}

istream & operator >> ( istream & s, Vec3 & v ){
        s >> v[0] >> v[1] >> v[2];
        return s;
}

istream & operator >> ( istream & s, Vec4 & v ){
        s >> v[0] >> v[1] >> v[2] >> v[3];
        return s;
}

istream & operator >> ( istream & s, Quat & v ){
        s >> v[0] >> v[1] >> v[2] >> v[3];
        return s;
}

istream & operator >> ( istream & s, Mat2 & v ){
        s >> v[0] >> v[1] >> v[2] >> v[3] >> v[4];
        return s;
}

istream & operator >> ( istream & s, Mat3 & v ){
        s >> v[0] >> v[1] >> v[2];
        s >> v[3] >> v[4] >> v[5];
        s >> v[6] >> v[7] >> v[8];
        return s;
}

istream & operator >> ( istream & s, Mat4 & v ){
        s >> v[0] >> v[1] >> v[2] >> v[3];
        s >> v[4] >> v[5] >> v[6] >> v[7];
        s >> v[8] >> v[9] >> v[10] >> v[11];
        s >> v[12] >> v[13] >> v[14] >> v[15];
        return s;
}

ostream & operator << ( ostream & s, const Vec2 & v ) { 
        static char txt[256];
        sprintf( txt, "%g %g", v[0], v[1] );
        s << txt;
        return s;
}

ostream & operator << ( ostream & s, const Vec3 & v ) { 
        static char txt[256];
        sprintf( txt, "%g %g %g", v[0], v[1], v[2] );
        s << txt;
        return s;
}

ostream & operator << ( ostream & s, const Vec4 & v ) { 
        static char txt[256];
        sprintf( txt, "%g %g %g %g", v[0], v[1], v[2], v[3] );
        s << txt;
        return s;
}

ostream & operator << ( ostream & s, const Quat & v ) { 
        static char txt[256];
        sprintf( txt, "%g %g %g %g", v[0], v[1], v[2], v[3] );
        s << txt;
        return s;
}

ostream & operator << ( ostream & s, const Mat2 & v ) { 
        static char txt[256];
        sprintf( txt, "%g %g %g %g", \
                v[0], v[1], \
                v[2], v[3] );
        s << txt;
        return s;
}

ostream & operator << ( ostream & s, const Mat3 & v ) { 
        static char txt[256];
        sprintf( txt, "%g %g %g %g %g %g %g %g %g", \
                v[0], v[1], v[2], \
                v[3], v[4], v[5], \
                v[6], v[7], v[8] );
        s << txt;
        return s;
}

ostream & operator << ( ostream & s, const Mat4 & v ) { 
        static char txt[256];
        sprintf( txt, "%g %g %g %g %g %g %g %g %g %g %g %g %g %g %g %g", \
                v[0], v[1], v[2], v[3], \
                v[4], v[5], v[6], v[7], \
                v[8], v[9], v[10], v[11], \
                v[12], v[13], v[14], v[15] );
        s << txt;
        return s;
}


Vec2::Vec2( float v0, float v1 ){
        m_v[0] = v0;
        m_v[1] = v1;
}

Vec2::Vec2( float xy ){
        m_v[0] = xy;
        m_v[1] = xy;
}

Vec2::Vec2( const float * v ){
        memcpy( m_v, v, 2 * sizeof( float ) );
}

bool Vec2::operator < ( const float v ) const {
        return( hyp() < v * v);
}

bool Vec2::operator < ( const Vec2 & v ) const {
        return( hyp() < v.hyp() );
}

bool Vec2::operator < ( const Vec3 & v ) const {
        return( hyp() < v.hyp() );
}

bool Vec2::operator < ( const Vec4 & v ) const {
        return( hyp() < v.hyp() );
}

Vec2 Vec2::operator - () const {
        return( Vec2( -m_v[0], -m_v[1] ) );
}

Vec2 Vec2::operator + ( const Vec2 & b ) const {
        Vec2 a;
        a[0] = m_v[0] + b[0];
        a[1] = m_v[1] + b[1];
        return( a );
}

Vec2 Vec2::operator - ( const Vec2 & b ) const {
        Vec2 a;
        a[0] = m_v[0] - b[0];
        a[1] = m_v[1] - b[1];
        return( a );
}

Vec2 Vec2::operator * ( float b ) const {
        Vec2 a;
        a[0] = m_v[0] * b;
        a[1] = m_v[1] * b;
        return( a );
}

Vec2 Vec2::operator * ( const Vec2 & b ) const {
        Vec2 a;
        a[0] = m_v[0] * b[0];
        a[1] = m_v[1] * b[1];
        return( a );
}

Vec2 Vec2::operator / ( const Vec2 & b ) const {
        Vec2 a;
        a[0] = m_v[0] / b[0];
        a[1] = m_v[1] / b[1];
        return( a );
}

Vec2 Vec2::operator / ( float b ) const {
        Vec2 a;
        a[0] = m_v[0] / b;
        a[1] = m_v[1] / b;
        return( a );
}

Vec2 & Vec2::operator += ( const Vec2 & b ){
        return( * this = * this + b );
}

Vec2 & Vec2::operator -= ( const Vec2 & b ){
        return( * this = * this - b );
}

Vec2 & Vec2::operator *= ( float b ){
        return( * this = * this * b );
}

Vec2 & Vec2::operator /= ( float b ){
        return( * this = * this / b );
}

Vec2 & Vec2::operator /= ( const Vec2 & b ){
        return( * this = * this / b );
}

float & Vec2::operator [] ( int index ) {
        return( m_v[ index ] );
}

const float & Vec2::operator [] ( const int index ) const {
        return( m_v[ index ] );
}

const float * Vec2::operator * ( void ) const {
        return( m_v );
}

bool Vec2::operator == ( const Vec2 & b ) const {
        return( equals( b ) );
}

float Vec2::hyp( void ) const {
        return( m_v[0] * m_v[0] + m_v[1] * m_v[1] );
}

float Vec2::len( void ) const {
        float h = hyp() + 0.000001f;
        return( sqrt( h ) );
}

void Vec2::normalize( void ){
        float l = len();
        m_v[0] /= l;
        m_v[1] /= l;
}

Vec2 Vec2::normalized( void ) const {
        float l = len();
        return( Vec2( m_v[0] / l, m_v[1] / l ) );
}

float Vec2::dot( const Vec2 & b ) const {
        return( m_v[0] * b[0] + m_v[1] * b[1] );
}

Vec2 Vec2::abs( void ) const {
        return( Vec2( fabs( m_v[0] ), fabs( m_v[1] ) ) );
}

Vec2 Vec2::sgn( void ) const {
        return( Vec2( m_v[0] < 0.0 ? -1.0 : 1.0, m_v[1] < 0.0 ? -1.0 : 1.0 ) );
}

Vec2 Vec2::clamp( float min, float max ) const {
        Vec2 v( * this );
        for( int i = 0; i < 2; i++ ){
                if( v[i] < min ){
                        v[i] = min;
                }
                else if( v[i] > max ){
                        v[i] = max;
                }
        }
        return( v );
}

Vec2 Vec2::cross( void ) const {
        Vec2 a;
        a[0] = -m_v[1];
        a[1] =  m_v[0];
        return( a );
}

float Vec2::cross( const Vec2 & v ) const {
        return( v.dot( cross() ) );
}

Vec2 Vec2::fromAngle( float rad ) {
        return( Vec2( cos( rad ), sin( rad ) ) );
}

Vec3 Vec3::fromAxisAngle( const Vec3 & cp, const Vec3 & axis, float angle ) {
        Vec3 p( cp );
        Vec3 v( 0.0, 0.0, 0.0 );
        float ca, sa;

        ca = cos( angle );
        sa = sin( angle );

        v[0] += ( ca + ( 1.0f - ca ) * axis[0] * axis[0] ) * p[0];
        v[0] += ( ( 1.0f - ca ) * axis[0] * axis[1] - axis[2] * sa ) * p[1];
        v[0] += ( ( 1.0f - ca ) * axis[0] * axis[2] + axis[1] * sa ) * p[2];

        v[1] += ( ( 1.0f - ca ) * axis[0] * axis[1] + axis[2] * sa ) * p[0];
        v[1] += ( ca + ( 1.0f - ca ) * axis[1] * axis[1] ) * p[1];
        v[1] += ( ( 1.0f - ca ) * axis[1] * axis[2] - axis[0] * sa ) * p[2];

        v[2] += ( ( 1.0f - ca ) * axis[0] * axis[2] - axis[1] * sa ) * p[0];
        v[2] += ( ( 1.0f - ca ) * axis[1] * axis[2] + axis[0] * sa ) * p[1];
        v[2] += ( ca + ( 1.0f - ca ) * axis[2] * axis[2] ) * p[2];

        return( v );
}

float Vec2::toAngle( void ) const {
        float a;
        float vx = RenderTools::sgn( m_v[0] ) * fmod( fabs( m_v[0] ), 1.0f );
        a = acos( vx );
        if( m_v[1] < 0.0f ){
                a = 2.0f * M_PI - a;
        }
        return( a );
}

bool Vec2::equals( const Vec2 & compare, float epsilon ) const {
        return( equals( *compare, epsilon ) );
}

bool Vec2::equals( const float * compare, float epsilon ) const {
        for( int i = 0; i < 2; i++ ){
                if( fabs( compare[i] - m_v[i] ) > epsilon ){
                        return( false );
                }
        }
        return( true );
}


Mat2::Mat2( float v00, float v01, float v10, float v11 ){
        m_v[0] = v00;
        m_v[1] = v01;
        m_v[2] = v10;
        m_v[3] = v11;
}

Mat2::Mat2( const float * v ){
        memcpy( m_v, v, 4 * sizeof( float ) );
}

Vec2 Mat2::operator * ( const Vec2 & b ) const {
        Vec2 a;
        a[0] = m_v[0] * b[0] + m_v[2] * b[1];
        a[1] = m_v[1] * b[0] + m_v[3] * b[1];
        return( a );
}

Mat2 Mat2::operator * ( const Mat2 & b ) const {
        Mat2 a;
        a[0] = m_v[0] * b[0] + m_v[2] * b[1];
        a[2] = m_v[0] * b[2] + m_v[2] * b[3];
        a[1] = m_v[1] * b[0] + m_v[3] * b[1];
        a[3] = m_v[1] * b[2] + m_v[3] * b[3];
        return( a );
}

Mat2 & Mat2::operator *= ( const Mat2 & b ){
        return( * this = * this * b );
}

Mat2 & Mat2::operator /= ( float b ){
        return( * this = * this / b );
}

Mat2 Mat2::operator * ( float b ) const {
        Mat2 a;
        a[0] = m_v[0] * b;
        a[1] = m_v[1] * b;
        a[2] = m_v[2] * b;
        a[3] = m_v[3] * b;
        return( a );
}

Mat2 Mat2::operator / ( float b ) const {
        Mat2 a;
        a[0] = m_v[0] / b;
        a[1] = m_v[1] / b;
        a[2] = m_v[2] / b;
        a[3] = m_v[3] / b;
        return( a );
}

float & Mat2::operator [] ( int index ) {
        return( m_v[ index ] );
}

const float & Mat2::operator [] ( const int index ) const {
        return( m_v[ index ] );
}

const float * Mat2::operator * ( void ) const {
        return( m_v );
}

const float & Mat2::getElement( int column, int row ) const {
        return( m_v[ column * 2 + row ] );
}
        
float & Mat2::getElement( int column, int row ){
        return( m_v[ column * 2 + row ] );
}
        
void Mat2::setElement( int col, int row, float v ){
        m_v[ col * 2 + row ] = v;
}

bool Mat2::operator == ( const Mat2 & b ) const{
        return( equals( b ) );
}

Vec2 Mat2::getRow( int index ) const{
        return( Vec2( m_v[ index ], m_v[ 2 + index ] ) );
}

Vec2 Mat2::getCol( int index ) const{
        return( Vec2( m_v[ index * 2 + 0 ], m_v[ index * 2 + 1 ] ) );
}

void Mat2::setRow( int index, const Vec2 & row ){
        m_v[ index ] = row[0];
        m_v[ 2 + index ] = row[1];
}

void Mat2::setCol( int index, const Vec2 & col ){
        m_v[ index * 2 + 0 ] = col[0];
        m_v[ index * 2 + 1 ] = col[1];
}

bool Mat2::equals( const Mat2 & compare, float epsilon ) const {
        return( equals( *compare, epsilon ) );
}

bool Mat2::equals( const float * compare, float epsilon ) const {
        for( int i = 0; i < 4; i++ ){
                if( fabs( compare[i] - m_v[i] ) > epsilon ){
                        return( false );
                }
        }
        return( true );
}

Mat2 Mat2::transpose() const {
        return( Mat2( m_v[0], m_v[2], m_v[1], m_v[3] ) );
}

float Mat2::determinant() const {
        return( m_v[0] * m_v[3] - m_v[1] * m_v[2] );
}

Mat2 Mat2::inverse() const {
        float d = determinant();

        if( d == 0.0f ){
                Error::warning( Error::INVALID_VALUE, __FILE__, __LINE__, "matrix is singular" );
                return( Mat2::fromIdentity() );
        }

        Mat2    r;
        r[0] =  m_v[3]; r[2] = -m_v[2];
        r[1] = -m_v[1]; r[3] =  m_v[0];
        r /= d;

        return( r );
}

void Mat2::setIdentity( void ){
        m_v[0] = 1.0f;
        m_v[1] = 0.0f;
        m_v[2] = 0.0f;
        m_v[3] = 1.0f;
}

Mat2 Mat2::fromIdentity( void ){
        Mat2 m;
        m.setIdentity();
        return( m );
}


Vec3::Vec3( float x, float y, float z ){
        m_v[0] = x;
        m_v[1] = y;
        m_v[2] = z;
}

Vec3::Vec3( float xyz ){
        m_v[0] = xyz;
        m_v[1] = xyz;
        m_v[2] = xyz;
}

Vec3::Vec3( const float * v ){
        memcpy( m_v, v, 3 * sizeof( float ) );
}

Vec3::Vec3( const Vec2 & xy, float z ){
        m_v[0] = xy[0];
        m_v[1] = xy[1];
        m_v[2] = z;
}


bool Vec3::operator < ( const float v ) const {
        return( hyp() < v * v );
}

bool Vec3::operator < ( const Vec2 & v ) const {
        return( hyp() < v.hyp() );
}

bool Vec3::operator < ( const Vec3 & v ) const {
        return( hyp() < v.hyp() );
}

bool Vec3::operator < ( const Vec4 & v ) const {
        return( hyp() < v.hyp() );
}

Vec3 Vec3::operator - () const {
        return( Vec3( -m_v[0], -m_v[1], -m_v[2] ) );
}

Vec3 Vec3::operator + ( const Vec3 & b ) const {
        Vec3 a;
        a[0] = m_v[0] + b[0];
        a[1] = m_v[1] + b[1];
        a[2] = m_v[2] + b[2];
        return( a );
}

Vec3 Vec3::operator - ( const Vec3 & b ) const {
        Vec3 a;
        a[0] = m_v[0] - b[0];
        a[1] = m_v[1] - b[1];
        a[2] = m_v[2] - b[2];
        return( a );
}

Vec3 Vec3::operator * ( float b ) const {
        Vec3 a;
        a[0] = m_v[0] * b;
        a[1] = m_v[1] * b;
        a[2] = m_v[2] * b;
        return( a );
}

Vec3 Vec3::operator * ( const Vec3 & b ) const {
        Vec3 a;
        a[0] = m_v[0] * b[0];
        a[1] = m_v[1] * b[1];
        a[2] = m_v[2] * b[2];
        return( a );
}

Vec3 Vec3::operator / ( const Vec3 & b ) const {
        Vec3 a;
        a[0] = m_v[0] / b[0];
        a[1] = m_v[1] / b[1];
        a[2] = m_v[2] / b[2];
        return( a );
}

Vec3 Vec3::operator / ( float b ) const {
        if( b == 0.0f ){
                Error::warning( Error::DIVISION_BY_ZERO, __FILE__, __LINE__ );
                return( Vec3() );
        }
        Vec3 a;
        a[0] = m_v[0] / b;
        a[1] = m_v[1] / b;
        a[2] = m_v[2] / b;
        return( a );
}

Vec3 & Vec3::operator += ( const Vec3 & b ){
        return( * this = * this + b );
}

Vec3 & Vec3::operator -= ( const Vec3 & b ){
        return( *( ( Vec3 * )this ) = * this - b );
}

Vec3 & Vec3::operator *= ( float b ){
        return( *( ( Vec3 * )this ) = * this * b );
}

Vec3 & Vec3::operator /= ( float b ){
        return( *( ( Vec3 * )this ) = * this / b );
}

Vec3 & Vec3::operator /= ( const Vec3 & b ){
        return( *( ( Vec3 * )this ) = * this / b );
}

float & Vec3::operator [] ( int index ) {
        return( m_v[ index ] );
}

const float & Vec3::operator [] ( const int index ) const {
        return( m_v[ index ] );
}

const float * Vec3::operator * ( void ) const {
        return( m_v );
}

bool Vec3::operator == ( const Vec3 & b ) const {
        return( equals( b ) );
}

float Vec3::hyp( void ) const {
        return( m_v[0] * m_v[0] + m_v[1] * m_v[1] + m_v[2] * m_v[2] );
}

float Vec3::len( void ) const {
        return( sqrt( hyp() ) );
}

void Vec3::normalize( void ){
        float l = len();
        if( l == 0.0f ){
                Error::warning( Error::DIVISION_BY_ZERO, __FILE__, __LINE__ );
                return;
        }
        m_v[0] /= l;
        m_v[1] /= l;
        m_v[2] /= l;
}

Vec3 Vec3::normalized( void ) const {
        float l = len();
        if( l == 0.0f ){
                Error::warning( Error::DIVISION_BY_ZERO, __FILE__, __LINE__ );
                return( Vec3() );
        }
        return( Vec3( m_v[0] / l, m_v[1] / l, m_v[2] / l ) );
}

Vec2 Vec3::toPolar( void ) const {
        Vec2 p;
        if( m_v[0] == 0.0f ){
                Error::warning( Error::DIVISION_BY_ZERO, __FILE__, __LINE__ );
                return( Vec2() );
        }
        p[0] = atan2( m_v[1], m_v[0] );
        p[1] = acos( RenderTools::clamp( m_v[2], -1.0f, 1.0f ) );
        return( p );
}

float Vec3::getAngle( const Vec3 & x, const Vec3 & y ) const {
        float a = acos( RenderTools::clamp( RenderTools::dot( x, *this ), -1.0f, 1.0f ) );
        if( RenderTools::dot( y, * this ) < 0.0f ) a = 2.0f * M_PI - a;
        return( a );
}

float Vec3::dot( const Vec3 & b ) const {
        return( m_v[0] * b[0] + m_v[1] * b[1] + m_v[2] * b[2] );
}

Vec3 Vec3::abs( void ) const {
        return( Vec3( fabs( m_v[0] ), fabs( m_v[1] ), fabs( m_v[2] ) ) );
}

Vec3 Vec3::sgn( void ) const {
        return( Vec3( m_v[0] < 0.0f ? -1.0f : 1.0f, m_v[1] < 0.0f ? -1.0f : 1.0f, m_v[2] < 0.0f ? -1.0f : 1.0f ) );
}

Vec3 Vec3::clamp( float min, float max ) const {
        Vec3 v( *this );
        for( int i = 0; i < 3; i++ ){
                if( v[i] < min ){
                        v[i] = min;
                }
                else if( v[i] > max ){
                        v[i] = max;
                }
        }
        return( v );
}

Vec3 Vec3::cross( const Vec3 & b ) const {
        Vec3 a;
        a[0] = m_v[1] * b[2] - m_v[2] * b[1];
        a[1] = m_v[2] * b[0] - m_v[0] * b[2];
        a[2] = m_v[0] * b[1] - m_v[1] * b[0];
        return( a );
}

bool Vec3::equals( const Vec3 & compare, float epsilon ) const {
        return( equals( *compare, epsilon ) );
}

bool Vec3::equals( const float * compare, float epsilon ) const {
        for( int i = 0; i < 3; i++ ){
                if( fabs( compare[i] - m_v[i] ) > epsilon ){
                        return( false );
                }
        }
        return( true );
}

Vec2 Vec3::xy() const {
        return( Vec2( m_v[0], m_v[1] ) );
}

Vec3 Vec3::fromPolar( const Vec2 & polar ){
        float sx = sin( polar[0] );
        float cx = cos( polar[0] );
        float sy = sin( polar[1] );
        float cy = cos( polar[1] );
        Vec3 p;
        p[0] = sy * cx;
        p[1] = sy * sx;
        p[2] = cy;
        return( p );
}


Mat3::Mat3( float v00, float v01, float v02,
                        float v10, float v11, float v12,
                        float v20, float v21, float v22 ){
        m_v[0] = v00;
        m_v[1] = v01;
        m_v[2] = v02;
        m_v[3] = v10;
        m_v[4] = v11;
        m_v[5] = v12;
        m_v[6] = v20;
        m_v[7] = v21;
        m_v[8] = v22;
}


Mat3::Mat3( const float * v ){
        memcpy( m_v, v, 9 * sizeof( float ) );
}

Mat3::Mat3( const Vec3 & c0, const Vec3 & c1, const Vec3 & c2 ){
        m_v[0] = c0[0];
        m_v[1] = c0[1];
        m_v[2] = c0[2];
        m_v[3] = c1[0];
        m_v[4] = c1[1];
        m_v[5] = c1[2];
        m_v[6] = c2[0];
        m_v[7] = c2[1];
        m_v[8] = c2[2];
}

Vec3 Mat3::operator * ( const Vec3 & b ) const {
        Vec3 a;
        a[0] = m_v[0] * b[0] + m_v[3] * b[1] + m_v[6] * b[2];
        a[1] = m_v[1] * b[0] + m_v[4] * b[1] + m_v[7] * b[2];
        a[2] = m_v[2] * b[0] + m_v[5] * b[1] + m_v[8] * b[2];
        return( a );
}

Mat3 Mat3::operator * ( const Mat3 & b ) const {
        Mat3 a;

        a[0] = m_v[0] * b[0] + m_v[3] * b[1] + m_v[6] * b[2];
        a[3] = m_v[0] * b[3] + m_v[3] * b[4] + m_v[6] * b[5];
        a[6] = m_v[0] * b[6] + m_v[3] * b[7] + m_v[6] * b[8];

        a[1] = m_v[1] * b[0] + m_v[4] * b[1] + m_v[7] * b[2];
        a[4] = m_v[1] * b[3] + m_v[4] * b[4] + m_v[7] * b[5];
        a[7] = m_v[1] * b[6] + m_v[4] * b[7] + m_v[7] * b[8];

        a[2] = m_v[2] * b[0] + m_v[5] * b[1] + m_v[8] * b[2];
        a[5] = m_v[2] * b[3] + m_v[5] * b[4] + m_v[8] * b[5];
        a[8] = m_v[2] * b[6] + m_v[5] * b[7] + m_v[8] * b[8];

        return( a );
}

Mat3 Mat3::operator * ( float b ) const {
        Mat3 a;
        a[0] = m_v[0] * b;
        a[1] = m_v[1] * b;
        a[2] = m_v[2] * b;
        a[3] = m_v[3] * b;
        a[4] = m_v[4] * b;
        a[5] = m_v[5] * b;
        a[6] = m_v[6] * b;
        a[7] = m_v[7] * b;
        a[8] = m_v[8] * b;
        return( a );
}

Mat3 Mat3::operator / ( float b ) const {
        Mat3 a;
        a[0] = m_v[0] / b;
        a[1] = m_v[1] / b;
        a[2] = m_v[2] / b;
        a[3] = m_v[3] / b;
        a[4] = m_v[4] / b;
        a[5] = m_v[5] / b;
        a[6] = m_v[6] / b;
        a[7] = m_v[7] / b;
        a[8] = m_v[8] / b;
        return( a );
}

Mat3 & Mat3::operator *= ( const Mat3 & b ){
        return( * this = * this * b );
}

Mat3 & Mat3::operator *= ( float b ){
        return( * this = * this * b );
}

Mat3 & Mat3::operator /= ( float b ){
        return( * this = * this / b );
}

float & Mat3::operator [] ( int index ) {
        return( m_v[ index ] );
}

const float & Mat3::operator [] ( const int index ) const {
        return( m_v[ index ] );
}

const float * Mat3::operator * ( void ) const {
        return( m_v );
}

const float & Mat3::getElement( int column, int row ) const {
        return( m_v[ column * 3 + row ] );
}

float & Mat3::getElement( int column, int row ){
        return( m_v[ column * 3 + row ] );
}

void Mat3::setElement( int col, int row, float v ){
        m_v[ col * 3 + row ] = v;
}

bool Mat3::operator == ( const Mat3 & b ) const {
        return( equals( b ) );
}

Vec3 Mat3::getRow( int index ) const {
        return( Vec3( m_v[ index ], m_v[ 3 + index ], m_v[ 6 + index ] ) );
}

Vec3 Mat3::getCol( int index ) const {
        return( Vec3( m_v[ index * 3 + 0 ], m_v[ index * 3 + 1 ], m_v[ index * 3 + 2 ] ) );
}

void Mat3::setRow( int index, const Vec3 & row ){
        m_v[ index ] = row[0];
        m_v[ 3 + index ] = row[1];
        m_v[ 6 + index ] = row[2];
}

void Mat3::setCol( int index,  const Vec3 & col ){
        m_v[ index * 3 + 0 ] = col[0];
        m_v[ index * 3 + 1 ] = col[1];
        m_v[ index * 3 + 2 ] = col[2];
}

bool Mat3::equals(  const Mat3 & compare, float epsilon ) const {
        return( equals( * compare, epsilon ) );
}

bool Mat3::equals(  const float * compare, float epsilon ) const {
        for( int i = 0; i < 9; i++ ){
                if( fabs( compare[i] - m_v[i] ) > epsilon ){
                        return( false );
                }
        }
        return( true );
}

Mat3 Mat3::transpose( void ) const {

        Mat3 a;

        a[0] = m_v[0];
        a[3] = m_v[1];
        a[6] = m_v[2];

        a[1] = m_v[3];
        a[4] = m_v[4];
        a[7] = m_v[5];

        a[2] = m_v[6];
        a[5] = m_v[7];
        a[8] = m_v[8];

        return( a );

}

Mat3 Mat3::adjugate( void ) const {

        Vec3 x = getRow( 1 ).cross( getRow( 2 ) );
        Vec3 y = getRow( 2 ).cross( getRow( 0 ) );
        Vec3 z = getRow( 0 ).cross( getRow( 1 ) );

        return( Mat3( x, y, z ) );
}

float Mat3::determinant( void ) const {
        return( m_v[0] * ( ( m_v[4] * m_v[8] ) - ( m_v[5] * m_v[7] ) ) -
                        m_v[1] * ( ( m_v[3] * m_v[8] ) - ( m_v[5] * m_v[6] ) ) +
                        m_v[2] * ( ( m_v[3] * m_v[7] ) - ( m_v[4] * m_v[6] ) ) );
}

float Mat3::trace( void ) const {
        return( m_v[0] + m_v[4] + m_v[8] );
}

Mat3 Mat3::inverse( void ) const {
        float d = determinant();

        if( d == 0.0f ){
                Error::warning( Error::INVALID_VALUE, __FILE__, __LINE__, "matrix is singular" );
                return( Mat3::fromIdentity() );
        }

        return( adjugate().transpose() / d );
}

void Mat3::setIdentity( void ){
        m_v[0] = 1.0f;
        m_v[1] = 0.0f;
        m_v[2] = 0.0f;
        m_v[3] = 0.0f;
        m_v[4] = 1.0f;
        m_v[5] = 0.0f;
        m_v[6] = 0.0f;
        m_v[7] = 0.0f;
        m_v[8] = 1.0f;
}

Mat3 Mat3::fromIdentity(){
        Mat3 m;
        m.setIdentity();
        return( m );
}



Vec4::Vec4( float x, float y, float z, float w ){
        m_v[0] = x;
        m_v[1] = y;
        m_v[2] = z;
        m_v[3] = w;
}

Vec4::Vec4( float xyzw ){
        m_v[0] = xyzw;
        m_v[1] = xyzw;
        m_v[2] = xyzw;
        m_v[3] = xyzw;
}

Vec4::Vec4( const float * v ){
        memcpy( m_v, v, 4 * sizeof( float ) );
}

Vec4::Vec4( const Vec2 & xy, float z, float w ){
        m_v[0] = xy[0];
        m_v[1] = xy[1];
        m_v[2] = z;
        m_v[3] = w;
}

Vec4::Vec4( const Vec3 & xyz, float w ){
        m_v[0] = xyz[0];
        m_v[1] = xyz[1];
        m_v[2] = xyz[2];
        m_v[3] = w;
}

Vec4::Vec4( const Vec2 & xy, const Vec2 & zw ){
        m_v[0] = xy[0];
        m_v[1] = xy[1];
        m_v[2] = zw[0];
        m_v[3] = zw[1];
}

Vec4::Vec4( float x, const Vec3 & yzw ){
        m_v[0] = x;
        m_v[1] = yzw[0];
        m_v[2] = yzw[1];
        m_v[3] = yzw[2];
}

Vec4::Vec4( float x, float y, const Vec2 & zw ){
        m_v[0] = x;
        m_v[1] = y;
        m_v[2] = zw[0];
        m_v[3] = zw[1];
}


bool Vec4::operator < ( const float v ) const {
        return( hyp() < v * v );
}

bool Vec4::operator < ( const Vec2 & v ) const {
        return( hyp() < v.hyp() );
}

bool Vec4::operator < ( const Vec3 & v ) const {
        return( hyp() < v.hyp() );
}

bool Vec4::operator < ( const Vec4 & v ) const {
        return( hyp() < v.hyp() );
}

Vec4 Vec4::operator - () const {
        return( Vec4( -m_v[0], -m_v[1], -m_v[2], -m_v[3] ) );
}

Vec4 Vec4::operator + ( const Vec4 & b ) const {
        Vec4 a;
        a[0] = m_v[0] + b[0];
        a[1] = m_v[1] + b[1];
        a[2] = m_v[2] + b[2];
        a[3] = m_v[3] + b[3];
        return( a );
}

Vec4 Vec4::operator - ( const Vec4 & b ) const {
        Vec4 a;
        a[0] = m_v[0] - b[0];
        a[1] = m_v[1] - b[1];
        a[2] = m_v[2] - b[2];
        a[3] = m_v[3] - b[3];
        return( a );
}

Vec4 Vec4::operator * ( const float b ) const {
        Vec4 a;
        a[0] = m_v[0] * b;
        a[1] = m_v[1] * b;
        a[2] = m_v[2] * b;
        a[3] = m_v[3] * b;
        return( a );
}

Vec4 Vec4::operator * ( const Vec4 & b ) const {
        Vec4 a;
        a[0] = m_v[0] * b[0];
        a[1] = m_v[1] * b[1];
        a[2] = m_v[2] * b[2];
        a[3] = m_v[3] * b[3];
        return( a );
}

Vec4 Vec4::operator / ( const Vec4 & b ) const {
        Vec4 a;
        a[0] = m_v[0] / b[0];
        a[1] = m_v[1] / b[1];
        a[2] = m_v[2] / b[2];
        a[3] = m_v[3] / b[3];
        return( a );
}

Vec4 Vec4::operator / ( float b ) const {
        Vec4 a;
        a[0] = m_v[0] / b;
        a[1] = m_v[1] / b;
        a[2] = m_v[2] / b;
        a[3] = m_v[3] / b;
        return( a );
}

Vec4 & Vec4::operator += ( const Vec4 & b ){
        return( * this = * this + b );
}

Vec4 & Vec4::operator -= ( const Vec4 & b ){
        return( * this = * this - b );
}

Vec4 & Vec4::operator *= ( float b ){
        return( * this = * this * b );
}

Vec4 & Vec4::operator /= ( float b ){
        return( * this = * this / b );
}

Vec4 & Vec4::operator /= ( const Vec4 & b ){
        return( * this = * this / b );
}

float & Vec4::operator [] ( int index ) {
        return( m_v[ index ] );
}

const float & Vec4::operator [] ( const int index ) const {
        return( m_v[ index ] );
}

const float * Vec4::operator * ( void ) const {
        return( m_v );
}

bool Vec4::operator == ( const Vec4 & b ) const {
        return( equals( b ) );
}

Vec2 Vec4::xy() const {
        return( Vec2( m_v[0], m_v[1] ) );
}

Vec2 Vec4::zw() const {
        return( Vec2( m_v[2], m_v[3] ) );
}

Vec3 Vec4::xyz() const {
        return( Vec3( m_v[0], m_v[1], m_v[2] ) );
}

float Vec4::hyp( void ) const {
        return( m_v[0] * m_v[0] + m_v[1] * m_v[1] + m_v[2] * m_v[2] + m_v[3] * m_v[3] );
}

float Vec4::len( void ) const {
        float h = hyp() + 0.000001f;
        return( sqrt( h ) );
}

void Vec4::normalize( void ){
        float l = len();
        if( l == 0.0f ){
                Error::warning( Error::DIVISION_BY_ZERO, __FILE__, __LINE__ );
                return;
        }
        m_v[0] /= l;
        m_v[1] /= l;
        m_v[2] /= l;
        m_v[3] /= l;
}

Vec4 Vec4::normalized( void ) const {
        float l = len();
        return( Vec4( m_v[0] / l, m_v[1] / l, m_v[2] / l, m_v[3] / l ) );
}

float Vec4::dot( const Vec4 & b ) const {
        return( m_v[0] * b[0] + m_v[1] * b[1] + m_v[2] * b[2] + m_v[3] * b[3] );
}

Vec4 Vec4::abs( void ) const {
        return( Vec4( fabs( m_v[0] ), fabs( m_v[1] ), fabs( m_v[2] ), fabs( m_v[3] ) ) );
}

Vec4 Vec4::sgn( void ) const {
        return( Vec4( m_v[0] < 0.0 ? -1.0 : 1.0, m_v[1] < 0.0 ? -1.0 : 1.0, m_v[2] < 0.0 ? -1.0 : 1.0, m_v[3] < 0.0 ? -1.0 : 1.0 ) );
}

Vec4 Vec4::clamp( float min, float max ) const {
        Vec4 v( *this );
        for( int i = 0; i < 4; i++ ){
                if( v[i] < min ){
                        v[i] = min;
                }
                else if( v[i] > max ){
                        v[i] = max;
                }
        }
        return( v );
}

bool Vec4::equals( const Vec4 & compare, float epsilon ) const {
        return( equals( * compare, epsilon ) );
}

bool Vec4::equals(  const float * compare, float epsilon ) const {
        for( int i = 0; i < 4; i++ ){
                if( fabs( compare[i] - m_v[i] ) > epsilon ){
                        return( false );
                }
        }
        return( true );
}


Quat::Quat( float x, float y, float z, float w ){
        m_v[0] = x;
        m_v[1] = y;
        m_v[2] = z;
        m_v[3] = w;
}

Quat::Quat( const float * v ){
        memcpy( m_v, v, 4 * sizeof( float ) );
}

Vec2 Quat::xy() const {
        return( Vec2( m_v[0], m_v[1] ) );
}
                                         
Vec2 Quat::zw() const {
        return( Vec2( m_v[2], m_v[3] ) );
}
                                         
Vec3 Quat::xyz() const {
        return( Vec3( m_v[0], m_v[1], m_v[2] ) );
}

bool Quat::equals( const Quat & compare, float epsilon ) const {
        return( equals( *compare, epsilon ) );
}

bool Quat::equals( const float * compare, float epsilon ) const {
        for( int i = 0; i < 4; i++ ){
                if( fabs( compare[i] - m_v[i] ) > epsilon ){
                        return( false );
                }
        }
        return( true );
}

float & Quat::operator [] ( int index ) {
        return( m_v[ index ] );
}

const float & Quat::operator [] ( const int index ) const {
        return( m_v[ index ] );
}

bool Quat::operator == ( const Quat & q ) const {
        return( equals( q ) );
}

bool Quat::operator != ( const Quat & q ) const {
        return( ! equals( q ) );
}

Quat Quat::operator + ( const Quat & q ) const {
        Quat result;
        for( int i = 0; i < 4; ++i ){
                result.m_v[i] = m_v[i] + q.m_v[i];
        }
        return( result );
}

Quat Quat::operator - ( const Quat & q ) const {
        Quat result;
        for( int i = 0; i < 4; ++i ){
                result.m_v[i] = m_v[i] - q.m_v[i];
        }
        return( result );
}

Quat Quat::operator * ( const Quat & q ) const {

        Quat result;

        result.m_v[0] =
                m_v[0] * q.m_v[0] -
                m_v[1] * q.m_v[1] -
                m_v[2] * q.m_v[2] -
                m_v[3] * q.m_v[3];

        result.m_v[1] =
                m_v[0] * q.m_v[1] +
                m_v[1] * q.m_v[0] +
                m_v[2] * q.m_v[3] -
                m_v[3] * q.m_v[2];

        result.m_v[2] =
                m_v[0] * q.m_v[2] +
                m_v[2] * q.m_v[0] +
                m_v[3] * q.m_v[1] -
                m_v[1] * q.m_v[3];

        result.m_v[3] =
                m_v[0] * q.m_v[3] +
                m_v[3] * q.m_v[0] +
                m_v[1] * q.m_v[2] -
                m_v[2] * q.m_v[1];

        return( result );
}

Quat Quat::operator * ( float scalar ) const {
        Quat result;
        for( int i = 0; i < 4; ++i ){
                result.m_v[i] = scalar * m_v[i];
        }
        return( result );
}

Quat Quat::operator / ( float scalar ) const {
        Quat result;
        int i;

        if( scalar != 0 ){
                for( i = 0; i < 4; ++i ){
                        result.m_v[i] = m_v[i] / scalar;
                }
        }
        else{
                for( i = 0; i < 4; ++i ){
                        result.m_v[i] = 0.0f;
                }
        }

        return( result );
}

Quat Quat::operator - () const {
        Quat result;
        for( int i = 0; i < 4; ++i ){
                result.m_v[i] = -m_v[i];
        }
        return( result );
}

Quat & Quat::operator += ( const Quat & q ){
        for( int i = 0; i < 4; ++i ){
                m_v[i] += q.m_v[i];
        }
        return( * this );
}

Quat & Quat::operator -= ( const Quat & q ){
        for( int i = 0; i < 4; ++i ){
                m_v[i] -= q.m_v[i];
        }
        return( * this );
}

Quat & Quat::operator *= ( float scalar ){
        for( int i = 0; i < 4; ++i ){
                m_v[i] *= scalar;
        }
        return( * this );
}

Quat & Quat::operator /= ( float scalar ){
        int i;

        if( scalar != 0.0f ){
                for( i = 0; i < 4; ++i ){
                        m_v[i] /= scalar;
                }
        }
        else{
                for( i = 0; i < 4; ++i ){
                        m_v[i] = 0.0f;
                }
        }

        return( * this );
}

const float * Quat::operator * ( void ) const {
        return( m_v );
}

Quat Quat::fromMat3( const Mat3 & rot ){
        // Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
        // article "Quat Calculus and Fast Animation".

        Quat q;
        const int next[3] = { 1, 2, 0 };

        float trace = rot.trace();
        float root;

        if( trace > 0.0f ){
                root = sqrt( trace + 1.0f );
                q.m_v[0] = root / 2.0f;
                root = 0.5f / root;
                q.m_v[1] = ( rot[7] - rot[5] ) * root;
                q.m_v[2] = ( rot[2] - rot[6] ) * root;
                q.m_v[3] = ( rot[3] - rot[1] ) * root;
        }
        else{
                int i = 0;
                if( rot[4] > rot[0] ){
                        i = 1;
                }
                if( rot[8] > rot[ i * 3 + i ] ){
                        i = 2;
                }
                int j = next[i];
                int k = next[j];

                root = sqrt( rot[ i * 3 + i ] - rot[ j * 3 + j ] - rot[ k * 3 + k ] + 1.0f );
                float * quat[3] = { & q.m_v[1], & q.m_v[2], & q.m_v[3] };
                * quat[i] = root / 2.0f;
                root = 0.5f / root;
                q.m_v[0] = ( rot[ k * 3 + j ] - rot[ j * 3 + k ] ) * root;
                *quat[j] = ( rot[ j * 3 + i ] + rot[ i * 3 + j ] ) * root;
                *quat[k] = ( rot[ k * 3 + i ] + rot[ i * 3 + k ] ) * root;
        }
        return( q );
}

Mat3 Quat::toMat3( void ) const {
        Mat3 rot;
        float x  = 2.0f * m_v[1];
        float y  = 2.0f * m_v[2];
        float z  = 2.0f * m_v[3];
        float wx = x * m_v[0];
        float wy = y * m_v[0];
        float wz = z * m_v[0];
        float xx = x * m_v[1];
        float xy = y * m_v[1];
        float xz = z * m_v[1];
        float yy = y * m_v[2];
        float yz = z * m_v[2];
        float zz = z * m_v[3];

        rot[0] = 1.0f - ( yy + zz );
        rot[1] = xy - wz;
        rot[2] = xz + wy;
        rot[3] = xy + wz;
        rot[4] = 1.0f - ( xx + zz );
        rot[5] = yz - wx;
        rot[6] = xz - wy;
        rot[7] = yz + wx;
        rot[8] = 1.0f - ( xx + yy );
        return( rot );
}

Quat Quat::fromAxisAngle( const Vec3 & axis, float angle ){
        Quat q;
        float halfAngle = 0.5f * angle;
        float sn = sin( halfAngle );
        q.m_v[0] = cos( halfAngle );
        q.m_v[1] = sn * axis[0];
        q.m_v[2] = sn * axis[1];
        q.m_v[3] = sn * axis[2];
        return( q );
}

void Quat::toAxisAngle( Vec3 & axis, float & angle ) const {

        float sqrLength = m_v[1] * m_v[1] + m_v[2] * m_v[2] + m_v[3] * m_v[3];

        if( sqrLength > 0.00000f ){
                angle = 2.0f * acos( m_v[0] );

                float length = sqrt( sqrLength );
                axis[0] = m_v[1] / length;
                axis[1] = m_v[2] / length;
                axis[2] = m_v[3] / length;
        }
        else{
                angle = 0.0f;

                axis[0] = 1.0f;
                axis[1] = 0.0f;
                axis[2] = 0.0f;
        }
}

float Quat::len() const {
        return( sqrt(   m_v[0]*m_v[0] + m_v[1]*m_v[1] + \
                                        m_v[2]*m_v[2] + m_v[3]*m_v[3] ) );
}

float Quat::hyp() const {
        return( m_v[0] * m_v[0] + m_v[1] * m_v[1] + \
                        m_v[2] * m_v[2] + m_v[3] * m_v[3] );
}

float Quat::dot( const Quat & q ) const {
        return( m_v[0] * q.m_v[0] + m_v[1] * q.m_v[1] + \
                        m_v[2] * q.m_v[2] + m_v[3] * q.m_v[3] );
}

void Quat::normalize(){

        float length = len();

        if( length == 0.0f ){
                Error::warning( Error::DIVISION_BY_ZERO, __FILE__, __LINE__ );
                return;
        }
        m_v[0] /= length;
        m_v[1] /= length;
        m_v[2] /= length;
        m_v[3] /= length;
}


Quat Quat::normalized() const {
        Quat norm;

        float length = len();

        if( length == 0.0f ){
                Error::warning( Error::DIVISION_BY_ZERO, __FILE__, __LINE__ );
                return( Quat() );
        }
        norm[0] = m_v[0] / length;
        norm[1] = m_v[1] / length;
        norm[2] = m_v[2] / length;
        norm[3] = m_v[3] / length;

        return( norm );
}

Quat Quat::inverse() const {
        Quat inverse;

        float norm = hyp();

        if( norm > 0.0f ){
                inverse.m_v[0] =  m_v[0] / norm;
                inverse.m_v[1] = -m_v[1] / norm;
                inverse.m_v[2] = -m_v[2] / norm;
                inverse.m_v[3] = -m_v[3] / norm;
        }
        else{
                Error::warning( Error::DIVISION_BY_ZERO, __FILE__, __LINE__ );
                return( Quat() );
        }

        return( inverse );
}

Quat Quat::conjugate() const {
        return( Quat( m_v[0], -m_v[1], -m_v[2], -m_v[3] ) );
}

Quat Quat::exp() const {

        Quat result;

        float angle = sqrt( m_v[1] * m_v[1] + m_v[2] * m_v[2] + m_v[3] * m_v[3] );

        float sn = sin( angle );
        result.m_v[0] = cos( angle );

        int i;

        if( fabs( sn ) >= 0.00000f ){
                float coeff = sn / angle;
                for( i = 1; i < 4; ++i ){
                        result.m_v[i] = coeff * m_v[i];
                }
        }
        else{
                for( i = 1; i < 4; ++i ){
                        result.m_v[i] = m_v[i];
                }
        }

        return( result );
}

Quat Quat::log() const {
        Quat result;
        result.m_v[0] = 0.0f;

        int i;

        if( fabs( m_v[0] ) < 1.0f ){
                float angle = acos( m_v[0] );
                float sn = sin( angle );
                if( fabs( sn ) >= 0.00000f ){
                        float coeff = angle / sn;
                        for( i = 1; i < 4; ++i ){
                                result.m_v[i] = coeff * m_v[i];
                        }
                        return result;
                }
        }

        for( i = 1; i < 4; ++i ){
                result.m_v[i] = m_v[i];
        }

        return( result );
}


Mat4::Mat4(     float v00, float v01, float v02, float v03,
                        float v10, float v11, float v12, float v13,
                        float v20, float v21, float v22, float v23,
                        float v30, float v31, float v32, float v33 ){

        m_v[0] = v00;
        m_v[1] = v01;
        m_v[2] = v02;
        m_v[3] = v03;
        m_v[4] = v10;
        m_v[5] = v11;
        m_v[6] = v12;
        m_v[7] = v13;
        m_v[8] = v20;
        m_v[9] = v21;
        m_v[10] = v22;
        m_v[11] = v23;
        m_v[12] = v30;
        m_v[13] = v31;
        m_v[14] = v32;
        m_v[15] = v33;
}

Mat4::Mat4( const float * v ){
        memcpy( m_v, v, 16 * sizeof( float ) );
}

Vec3 Mat4::operator * ( const Vec3 & b ) const {
        Vec3 a;
        a[0] = b[0] * m_v[0] + b[1] * m_v[4] + b[2] * m_v[8] + m_v[12];
        a[1] = b[0] * m_v[1] + b[1] * m_v[5] + b[2] * m_v[9] + m_v[13];
        a[2] = b[0] * m_v[2] + b[1] * m_v[6] + b[2] * m_v[10] + m_v[14];
        return( a );
}

Vec4 Mat4::operator * ( const Vec4 & b ) const {
        Vec4 a;
        a[0] = b[0] * m_v[0] + b[1] * m_v[4] + b[2] * m_v[8] + b[3] * m_v[12];
        a[1] = b[0] * m_v[1] + b[1] * m_v[5] + b[2] * m_v[9] + b[3] * m_v[13];
        a[2] = b[0] * m_v[2] + b[1] * m_v[6] + b[2] * m_v[10] + b[3] * m_v[14];
        a[3] = b[0] * m_v[3] + b[1] * m_v[7] + b[2] * m_v[11] + b[3] * m_v[15];
        return( a );
}

Mat3 Mat4::operator * ( const Mat3 & b ) const {
        Mat3 a;

        a[0] = m_v[0] * b[0] + m_v[4] * b[1] + m_v[8] * b[2];
        a[3] = m_v[0] * b[3] + m_v[4] * b[4] + m_v[8] * b[5];
        a[6] = m_v[0] * b[6] + m_v[4] * b[7] + m_v[8] * b[8];

        a[1] = m_v[1] * b[0] + m_v[5] * b[1] + m_v[9] * b[2];
        a[4] = m_v[1] * b[3] + m_v[5] * b[4] + m_v[9] * b[5];
        a[7] = m_v[1] * b[6] + m_v[5] * b[7] + m_v[9] * b[8];

        a[2] = m_v[2] * b[0] + m_v[6] * b[1] + m_v[10] * b[2];
        a[5] = m_v[2] * b[3] + m_v[6] * b[4] + m_v[10] * b[5];
        a[8] = m_v[2] * b[6] + m_v[6] * b[7] + m_v[10] * b[8];

        return( a );
}

Mat4 Mat4::operator * ( const Mat4 & b ) const {
        Mat4 a;

   unsigned int i;
   for( i = 0; i < 4; i++ ){
                const float ai0 = m_v[ i ],  ai1 = m_v[ 4 + i ],  ai2 = m_v[ 8 + i ], ai3 = m_v[ 12 + i ];
                a[ i      ] = ai0 * b[ 0  ] + ai1 * b[ 1  ] + ai2 * b[ 2  ] + ai3 * b[ 3  ];
                a[ 4 + i  ] = ai0 * b[ 4  ] + ai1 * b[ 5  ] + ai2 * b[ 6  ] + ai3 * b[ 7  ];
                a[ 8 + i  ] = ai0 * b[ 8  ] + ai1 * b[ 9  ] + ai2 * b[ 10 ] + ai3 * b[ 11 ];
                a[ 12 + i ] = ai0 * b[ 12 ] + ai1 * b[ 13 ] + ai2 * b[ 14 ] + ai3 * b[ 15 ];
   }

        return( a );
}

Mat4 Mat4::operator * ( float f ) const {
        Mat4 m( m_v );

        m.m_v[0] *= f;
        m.m_v[1] *= f;
        m.m_v[2] *= f;

        m.m_v[4] *= f;
        m.m_v[5] *= f;
        m.m_v[6] *= f;

        m.m_v[8] *= f;
        m.m_v[9] *= f;
        m.m_v[10] *= f;

        return( m );
}

Mat4 & Mat4::operator *= ( const Mat4 & b ){
        return( * this = * this * b );
}

Mat4 & Mat4::operator *= ( float f ){
        m_v[0] *= f;
        m_v[1] *= f;
        m_v[2] *= f;

        m_v[4] *= f;
        m_v[5] *= f;
        m_v[6] *= f;

        m_v[8] *= f;
        m_v[9] *= f;
        m_v[10] *= f;

        return( * this );
}

Mat4 & Mat4::operator *= ( const Vec3 & v ){
        m_v[0] *= v[0];
        m_v[1] *= v[0];
        m_v[2] *= v[0];

        m_v[4] *= v[1];
        m_v[5] *= v[1];
        m_v[6] *= v[1];

        m_v[8] *= v[2];
        m_v[9] *= v[2];
        m_v[10] *= v[2];

        return( * this );
}

Mat4 Mat4::operator + ( const Vec2 & v ) const {
        Mat4 m( m_v );

        m.m_v[12] += v[0];
        m.m_v[13] += v[1];

        return( m );
}

Mat4 Mat4::operator + ( const Vec3 & v ) const {
        Mat4 m( m_v );

        m.m_v[12] += v[0];
        m.m_v[13] += v[1];
        m.m_v[14] += v[2];

        return( m );
}

Mat4 & Mat4::operator += ( const Vec2 & v ){
        m_v[12] += v[0];
        m_v[13] += v[1];
        return( * this );
}

Mat4 & Mat4::operator += ( const Vec3 & v ){
        m_v[12] += v[0];
        m_v[13] += v[1];
        m_v[14] += v[2];
        return( * this );
}

float & Mat4::operator [] ( int index ) {
        return( m_v[ index ] );
}

const float & Mat4::operator [] ( const int index ) const {
        return( m_v[ index ] );
}

const float * Mat4::operator * ( void ) const {
        return( m_v );
}

const float & Mat4::getElement( int column, int row ) const {
        return( m_v[ column * 4 + row ] );
}

float & Mat4::getElement( int column, int row ){
        return( m_v[ column * 4 + row ] );
}

void Mat4::setElement( int col, int row, float v ){
        m_v[ col * 4 + row ] = v;
}

bool Mat4::operator == ( const Mat4 & b ) const {
        return( equals( b ) );
}

Mat4 & Mat4::operator = ( const Mat3 & b ){
        m_v[0] = b[0];
        m_v[1] = b[1];
        m_v[2] = b[2];
        m_v[4] = b[3];
        m_v[5] = b[4];
        m_v[6] = b[5];
        m_v[8] = b[6];
        m_v[9] = b[7];
        m_v[10] = b[8];
        return( * this );
}

Mat4 & Mat4::operator = ( const Vec3 & b ){
        m_v[12] = b[0];
        m_v[13] = b[1];
        m_v[14] = b[2];
        return( * this );
}

Vec4 Mat4::getRow( int index ) const {
        return( Vec4( m_v[ index ], m_v[ 4 + index ], m_v[ 8 + index ], m_v[ 12 + index ] ) );
}

Vec4 Mat4::getCol( int index ) const {
        return( Vec4( m_v[ index * 4 + 0 ], m_v[ index * 4 + 1 ], m_v[ index * 4 + 2 ], m_v[ index * 4 + 3 ] ) );
}

void Mat4::setRow( int index, const Vec4 & row ){
        m_v[ index ] = row[0];
        m_v[ 4 + index ] = row[1];
        m_v[ 8 + index ] = row[2];
        m_v[ 12 + index ] = row[3];
}

void Mat4::setCol( int index, const Vec4 & col ){
        m_v[ index * 4 + 0 ] = col[0];
        m_v[ index * 4 + 1 ] = col[1];
        m_v[ index * 4 + 2 ] = col[2];
        m_v[ index * 4 + 3 ] = col[3];
}

bool Mat4::equals( const Mat4 & compare, float epsilon ) const {
        return( equals( * compare, epsilon ) );
}

bool Mat4::equals( const float * compare, float epsilon ) const {
        for( int i = 0; i < 16; i++ ){
                if( fabs( compare[i] - m_v[i] ) > epsilon ){
                        return( false );
                }
        }
        return( true );
}


void Mat4::translate( const Vec3 & b ){
        m_v[12] = m_v[0] * b[0] + m_v[4] * b[1] + m_v[8]  * b[2] + m_v[12];
        m_v[13] = m_v[1] * b[0] + m_v[5] * b[1] + m_v[9]  * b[2] + m_v[13];
        m_v[14] = m_v[2] * b[0] + m_v[6] * b[1] + m_v[10] * b[2] + m_v[14];
        m_v[15] = m_v[3] * b[0] + m_v[7] * b[1] + m_v[11] * b[2] + m_v[15];
}

void Mat4::setTranslation( const Vec3 & b ){
        m_v[12] = b[0];
        m_v[13] = b[1];
        m_v[14] = b[2];
}

Vec3 Mat4::translation( void ) const {
        return( Vec3( m_v[12], m_v[13], m_v[14] ) );
}

Mat3 Mat4::rotation( void ) const {
        return( Mat3( m_v[0], m_v[1], m_v[2], m_v[4], m_v[5], m_v[6], m_v[8], m_v[9], m_v[10] ) );
}

Mat4 Mat4::transpose( void ) const {

        Mat4 a;

        a[0] = m_v[0];
        a[4] = m_v[1];
        a[8] = m_v[2];
        a[12] = m_v[3];

        a[1] = m_v[4];
        a[5] = m_v[5];
        a[9] = m_v[6];
        a[13] = m_v[7];

        a[2] = m_v[8];
        a[6] = m_v[9];
        a[10] = m_v[10];
        a[14] = m_v[11];

        a[3] = m_v[12];
        a[7] = m_v[13];
        a[11] = m_v[14];
        a[15] = m_v[15];

        return( a );
}

Mat4 Mat4::inverse( void ) const {
        Mat4 a;

        float wtmp[4][8];
        float m0, m1, m2, m3, s;
        float *r0, *r1, *r2, *r3;

        r0 = wtmp[0], r1 = wtmp[1], r2 = wtmp[2], r3 = wtmp[3];

        r0[0] = m_v[0], r0[1] = m_v[4],
        r0[2] = m_v[8], r0[3] = m_v[12],
        r0[4] = 1.0f, r0[5] = r0[6] = r0[7] = 0.0f,

        r1[0] = m_v[1], r1[1] = m_v[5],
        r1[2] = m_v[9], r1[3] = m_v[13],
        r1[5] = 1.0f, r1[4] = r1[6] = r1[7] = 0.0f,

        r2[0] = m_v[2], r2[1] = m_v[6],
        r2[2] = m_v[10], r2[3] = m_v[14],
        r2[6] = 1.0f, r2[4] = r2[5] = r2[7] = 0.0f,

        r3[0] = m_v[3], r3[1] = m_v[7],
        r3[2] = m_v[11], r3[3] = m_v[15],
        r3[7] = 1.0f, r3[4] = r3[5] = r3[6] = 0.0f;

        if( fabs( r3[0] ) > fabs( r2[0] ) ){
                swap( r3, r2 );
        }
        if( fabs( r2[0] ) > fabs( r1[0] ) ){
                swap( r2, r1 );
        }
        if( fabs( r1[0] ) > fabs( r0[0] ) ){
                swap( r1, r0 );
        }
        if( r0[0] == 0.0f ){
                Error::warning( Error::INVALID_VALUE, __FILE__, __LINE__, "matrix is singular" );
                return( Mat4::fromIdentity() );
        }

        // eliminate first variable
        m1 = r1[0] / r0[0]; m2 = r2[0] / r0[0]; m3 = r3[0] / r0[0];
        s = r0[1]; r1[1] -= m1 * s; r2[1] -= m2 * s; r3[1] -= m3 * s;
        s = r0[2]; r1[2] -= m1 * s; r2[2] -= m2 * s; r3[2] -= m3 * s;
        s = r0[3]; r1[3] -= m1 * s; r2[3] -= m2 * s; r3[3] -= m3 * s;
        s = r0[4];
        if( s != 0.0 ){ r1[4] -= m1 * s; r2[4] -= m2 * s; r3[4] -= m3 * s; }
        s = r0[5];
        if (s != 0.0 ){ r1[5] -= m1 * s; r2[5] -= m2 * s; r3[5] -= m3 * s; }
        s = r0[6];
        if (s != 0.0 ){ r1[6] -= m1 * s; r2[6] -= m2 * s; r3[6] -= m3 * s; }
        s = r0[7];
        if (s != 0.0 ){ r1[7] -= m1 * s; r2[7] -= m2 * s; r3[7] -= m3 * s; }

        // choose pivot - or die 
        if ( fabs( r3[1] ) > fabs( r2[1] ) ){
                swap( r3, r2 );
        }
        if ( fabs( r2[1] ) > fabs( r1[1] ) ){
                swap( r2, r1 );
        }
        if( r1[1] == 0.0f ){
                Error::warning( Error::INVALID_VALUE, __FILE__, __LINE__, "matrix is singular" );
                return( Mat4::fromIdentity() );
        }

        // eliminate second variable 
        m2 = r2[1] / r1[1]; m3 = r3[1] / r1[1];
        r2[2] -= m2 * r1[2]; r3[2] -= m3 * r1[2];
        r2[3] -= m2 * r1[3]; r3[3] -= m3 * r1[3];
        s = r1[4]; if( 0.0f != s ){ r2[4] -= m2 * s; r3[4] -= m3 * s; }
        s = r1[5]; if( 0.0f != s ){ r2[5] -= m2 * s; r3[5] -= m3 * s; }
        s = r1[6]; if( 0.0f != s ){ r2[6] -= m2 * s; r3[6] -= m3 * s; }
        s = r1[7]; if( 0.0f != s ){ r2[7] -= m2 * s; r3[7] -= m3 * s; }

        // choose pivot - or die 
        if ( fabs( r3[2] ) > fabs( r2[2] ) ){
                swap( r3, r2 );
        }
        if( r2[2] == 0.0f ){
                Error::warning( Error::INVALID_VALUE, __FILE__, __LINE__, "matrix is singular" );
                return( Mat4::fromIdentity() );
        }

        // eliminate third variable 
        m3 = r3[2] / r2[2];
        r3[3] -= m3 * r2[3], r3[4] -= m3 * r2[4],
        r3[5] -= m3 * r2[5], r3[6] -= m3 * r2[6],
        r3[7] -= m3 * r2[7];

        // last check 
        if( r3[3] == 0.0f ){
                Error::warning( Error::INVALID_VALUE, __FILE__, __LINE__, "matrix is singular" );
                return( Mat4::fromIdentity() );
        }

        s = 1.0f / r3[3]; // now back substitute row 3 
        r3[4] *= s; r3[5] *= s; r3[6] *= s; r3[7] *= s;

        m2 = r2[3]; // now back substitute row 2
        s  = 1.0f / r2[2];
        r2[4] = s * ( r2[4] - r3[4] * m2 ), r2[5] = s * ( r2[5] - r3[5] * m2 ),
        r2[6] = s * ( r2[6] - r3[6] * m2 ), r2[7] = s * ( r2[7] - r3[7] * m2 );
        m1 = r1[3];
        r1[4] -= r3[4] * m1, r1[5] -= r3[5] * m1,
        r1[6] -= r3[6] * m1, r1[7] -= r3[7] * m1;
        m0 = r0[3];
        r0[4] -= r3[4] * m0, r0[5] -= r3[5] * m0,
        r0[6] -= r3[6] * m0, r0[7] -= r3[7] * m0;

        m1 = r1[2]; // now back substitute row 1 
        s  = 1.0f / r1[1];
        r1[4] = s * ( r1[4] - r2[4] * m1 ), r1[5] = s * ( r1[5] - r2[5] * m1 ),
        r1[6] = s * ( r1[6] - r2[6] * m1 ), r1[7] = s * ( r1[7] - r2[7] * m1 );
        m0 = r0[2];
        r0[4] -= r2[4] * m0, r0[5] -= r2[5] * m0,
        r0[6] -= r2[6] * m0, r0[7] -= r2[7] * m0;

        m0 = r0[1]; // now back substitute row 0 
        s  = 1.0f / r0[0];
        r0[4] = s * ( r0[4] - r1[4] * m0 ), r0[5] = s * ( r0[5] - r1[5] * m0 ),
        r0[6] = s * ( r0[6] - r1[6] * m0 ), r0[7] = s * ( r0[7] - r1[7] * m0 );

        a[0] = r0[4]; 
        a[1] = r1[4]; 
        a[2] = r2[4]; 
        a[3] = r3[4]; 

        a[4] = r0[5],
        a[5] = r1[5],
        a[6] = r2[5],
        a[7] = r3[5],

        a[8] = r0[6]; 
        a[9] = r1[6]; 
        a[10] = r2[6]; 
        a[11] = r3[6]; 

        a[12] = r0[7],
        a[13] = r1[7],
        a[14] = r2[7],
        a[15] = r3[7];

        return( a );
}

void Mat4::setIdentity( void ){
        m_v[0] = 1.0f;
        m_v[1] = 0.0f;
        m_v[2] = 0.0f;
        m_v[3] = 0.0f;
        m_v[4] = 0.0f;
        m_v[5] = 1.0f;
        m_v[6] = 0.0f;
        m_v[7] = 0.0f;
        m_v[8] = 0.0f;
        m_v[9] = 0.0f;
        m_v[10] = 1.0f;
        m_v[11] = 0.0f;
        m_v[12] = 0.0f;
        m_v[13] = 0.0f;
        m_v[14] = 0.0f;
        m_v[15] = 1.0f;
}

Mat4 Mat4::fromTransformation(  const Vec3 & translation,  const Mat3 & rotation ){
        Mat4 r;
        r.m_v[0] = rotation[0];
        r.m_v[1] = rotation[1];
        r.m_v[2] = rotation[2];
        r.m_v[3] = 0.0f;
        r.m_v[4] = rotation[3];
        r.m_v[5] = rotation[4];
        r.m_v[6] = rotation[5];
        r.m_v[7] = 0.0f;
        r.m_v[8] = rotation[6];
        r.m_v[9] = rotation[7];
        r.m_v[10] = rotation[8];
        r.m_v[11] = 0.0f;
        r.m_v[12] = translation[0];
        r.m_v[13] = translation[1];
        r.m_v[14] = translation[2];
        r.m_v[15] = 1.0f;
        return( r );
}

Mat4 Mat4::fromAxisAngle( const Vec3 & v, float radians, const Vec3 & t ){
        Vec3 u = v.normalized();

        float u0u0 = u[0] * u[0];
        float u0u1 = u[0] * u[1];
        float u0u2 = u[0] * u[2];
        float u1u1 = u[1] * u[1];
        float u1u2 = u[1] * u[2];
        float u2u2 = u[2] * u[2];

        float cosa = cos( radians );
        float cosa1 = 1.0f - cosa;
        float sina = sin( radians );

        float u0sina = u[0] * sina;
        float u1sina = u[1] * sina;
        float u2sina = u[2] * sina;
        

        return( Mat4(   u0u0 + cosa * ( 1.0 - u0u0 ),   u0u1 * cosa1 - u2sina,                  u0u2 * cosa1 + u1sina,                  t[0],
                                        u0u1 * cosa1 + u2sina,                  u1u1 + cosa * ( 1.0 - u1u1 ),   u1u2 * cosa1 - u0sina,                  t[1],
                                        u0u2 * cosa1 - u1sina,                  u1u2 * cosa1 + u0sina,                  u2u2 + cosa * ( 1.0 - u2u2 ),   t[2],
                                        0.0f,                                                   0.0f,                                                   0.0f,                                                   1.0f ) );
}


Mat4 Mat4::fromQuaternion( const Quat & q ){
        Mat4 r;
        Mat3 m3( q.toMat3() );

        r.setIdentity();

        r.m_v[0] = m3[0];
        r.m_v[1] = m3[1];
        r.m_v[2] = m3[2];

        r.m_v[4] = m3[3];
        r.m_v[5] = m3[4];
        r.m_v[6] = m3[5];

        r.m_v[8] = m3[6];
        r.m_v[9] = m3[7];
        r.m_v[10] = m3[8];

        return( r );
}

Vec4 Mat4::toPolar( void ) const { 
        Vec2 px = getCol( 0 ).xyz().toPolar();
        Vec2 py = getCol( 1 ).xyz().toPolar();
        return( Vec4( px[0], px[1], py[0], py[1] ) );
}

Mat4 Mat4::fromPolar( const Vec4 & polar ){
        Mat4 m;
        m.setCol( 0, Vec4( Vec3( polar.xy() ), 0.0f ) );
        m.setCol( 1, Vec4( Vec3( polar.zw() ), 0.0f ) );
        m.setCol( 2, Vec4( cross( m.getCol(0).xyz(), m.getCol(1).xyz() ), 0.0f ) );
        m.setCol( 3, Vec4( 0, 0, 0, 1 ) );
        return( m );
}

Mat4 Mat4::fromPlaneEquation( const Vec4 & equation ){
        Mat4 mat;
        Vec3 normal = normalize( Vec3( equation[0], equation[1], equation[2] ) );
        Mat4 dirMat = fromDirectionalAxis( normal, 'z' );
        mat.setCol( 0, dirMat.getCol(0) );
        mat.setCol( 1, dirMat.getCol(1) );
        mat.setCol( 2, dirMat.getCol(2) );
        mat.setCol( 3, Vec4( normal * equation[3], 1.0f ) );
        return( mat );
}

Mat4 Mat4::fromDirectionalAxis( const Vec3 & axis, char principalAxis ){
        Mat4 mat;
        if( length( axis ) < 0.00001f ){
                // the matrix is degenerate
                mat.setIdentity();
                return( mat );
        }
        // the axis is vaid, so we use it as principal x-axis
        Vec3 x = normalize( axis );
        // take the (right-handed) perpendicular
        Vec3 y( -x[1], x[0], 0.0f );
        // and create the z-axis from that
        Vec3 z = cross( x, y );
        // test if it's degenerate 
        if( length( z ) < 0.000001f ){
                // x happens to be the z-axis, so we set y to
                y = Vec3( 0.0f, 1.0f, 0.0f );
                // and create the z-axis from that
                z = cross( x, y );
        }

        // now re-order according to what is to be the principal axis
        switch( principalAxis ){
                case 'x':
                        // nothing needs to be done
                        mat.setCol( 0, Vec4( normalize( x ), 0.0f ) );
                        mat.setCol( 1, Vec4( normalize( y ), 0.0f ) );
                        mat.setCol( 2, Vec4( normalize( z ), 0.0f ) );
                        break;
                case 'y':
                        // cw rotation around z
                        mat.setCol( 0, Vec4( -normalize( y ), 0.0f ) );
                        mat.setCol( 1, Vec4( normalize( x ), 0.0f ) );
                        mat.setCol( 2, Vec4( normalize( z ), 0.0f ) );
                        break;
                case 'z':
                        // ccw around y
                        mat.setCol( 0, Vec4( -normalize( z ), 0.0f ) );
                        mat.setCol( 1, Vec4( normalize( y ), 0.0f ) );
                        mat.setCol( 2, Vec4( normalize( x ), 0.0f ) );
                        break;
        }
        return( mat );
}

Mat4 Mat4::fromIdentity(){
        Mat4 m;
        m.setIdentity();
        return( m );
}

Mat4 Mat4::fromOrtho( float left, float right, float bottom, float top, float zNear, float zFar ){
        Mat4 m;
        m.ortho( left, right, bottom, top, zNear, zFar );
        return( m );
}

Mat4 Mat4::fromFrustum( float left, float right, float bottom, float top, float zNear, float zFar ){
        Mat4 m;
        m.frustum( left, right, bottom, top, zNear, zFar );
        return( m );
}

Mat4 Mat4::fromPerspective( float fovy, float aspect, float zNear, float zFar ){
        Mat4 m;
        m.perspective( fovy, aspect, zNear, zFar );
        return( m );
}

Mat4 Mat4::fromLookAt( const Vec3 & eye, const Vec3 & center, const Vec3 & up ){
        Mat4 m;
        m.lookAt( eye, center, up );
        return( m );
}

void Mat4::ortho( float left, float right, float bottom, float top, float zNear, float zFar ){
        m_v[0] = 2.0f / ( right - left );
        m_v[1] = 0.0f;
        m_v[2] = 0.0f;
        m_v[3] = 0.0f;
        m_v[4] = 0.0f;
        m_v[5] = 2.0f / ( top - bottom );
        m_v[6] = 0.0f;
        m_v[7] = 0.0f;
        m_v[8] = 0.0f;
        m_v[9] = 0.0f;
        m_v[10] = -2.0f / ( zFar - zNear );
        m_v[11] = 0.0f;
        m_v[12] = ( -right - left ) / ( right - left );
        m_v[13] = ( -top - bottom ) / ( top - bottom );
        m_v[14] = ( -zFar - zNear ) / ( zFar - zNear );
        m_v[15] = 1.0f;
}

void Mat4::frustum( float left, float right, float bottom, float top, float zNear, float zFar ){
        m_v[0] = ( 2.0f * zNear ) / ( right - left );
        m_v[1] = 0.0f;
        m_v[2] = 0.0f;
        m_v[3] = 0.0f;
        m_v[4] = 0.0f;
        m_v[5] = ( 2.0f * zNear ) / ( top - bottom );
        m_v[6] = 0.0f;
        m_v[7] = 0.0f;
        m_v[8] = ( right + left) / ( right - left );
        m_v[9] = ( top + bottom) / ( top - bottom );
        m_v[10] = ( -zFar - zNear ) / ( zFar  - zNear );
        m_v[11] = -1.0f;
        m_v[12] = 0.0f;
        m_v[13] = 0.0f;
        m_v[14] = ( -2.0f * zFar * zNear ) / ( zFar - zNear );
        m_v[15] = 0.0f;
}

void Mat4::perspective( float fovy, float aspect, float zNear, float zFar ){
        float range = tan( toRadians( fovy / 2.0f ) ) * zNear;
        float left = -range * aspect;
        float right = range * aspect;
        float bottom = -range;
        float top = range;

        frustum( left, right, bottom, top, zNear, zFar );
}

void Mat4::lookAt( const Vec3 & eye, const Vec3 & center, const Vec3 & up ){
        Mat4 m;
        Vec3 forward, side;

        forward = center - eye;
        forward.normalize();

        side = cross( forward, up );
        side.normalize();

    m[0] = side[0];
    m[4] = side[1];
    m[8] = side[2];

    m[1] = up[0];
    m[5] = up[1];
    m[9] = up[2];

    m[2] = -forward[0];
    m[6] = -forward[1];
    m[10] = -forward[2];

        ( * this ) *= m;

        translate( -eye );
}

namespace Matrix{

        vector< Mat4 > stack[3];

#ifndef RT_AS3 
#       ifndef RT_GLES1
        void _setMatrixUniform( void ){
                ProgramPtr program = Program::getCurrent();
                PropertyPtr u = PropertyPtr();
                if( program ){
                        program->bind( "modelViewMatrix", toString( get( MODELVIEW ) ) );
                        program->bind( "projectionMatrix", toString( get( PROJECTION ) ) );
                        program->bind( "modelViewProjectionMatrix", toString( get( PROJECTION ) * get( MODELVIEW ) ) );
                }
        } 
#       else
        void _setMatrixUniform( void ){}
#       endif

#       ifndef RT_GLES2
        void _matrixMode( Mode mode ){
                switch( mode ){
                        case MODELVIEW:
                                glMatrixMode( GL_MODELVIEW );
                                break;
                        case PROJECTION:
                                glMatrixMode( GL_PROJECTION );
                                break;
                        case TEXTURE:
                                glMatrixMode( GL_TEXTURE );
                                break;
                }
        }
        void _pushMatrix( void ){ glPushMatrix( ); }
        void _popMatrix( void ){ glPopMatrix( ); }
        void _loadMatrix( const GLfloat * p ){ glLoadMatrixf( p ); }
#       else
        void _matrixMode( Mode mode ){}
        void _pushMatrix( void ){}
        void _popMatrix( void ){}
        void _loadMatrix( const GLfloat * p ){}
#       endif
#else // if RT_AS3 TODO: make connection with Actionscript runtime
        void _setMatrixUniform( void ){}
        void _matrixMode( Mode mode ){}
        void _pushMatrix( void ){}
        void _popMatrix( void ){}
        void _loadMatrix( const float * p ){}
#endif // RT_AS3

        void initialize(){
                stack[PROJECTION].clear();
                stack[MODELVIEW].clear();
                stack[TEXTURE].clear();
                stack[PROJECTION].push_back( Mat4::fromIdentity() );
                stack[MODELVIEW].push_back( Mat4::fromIdentity() );
                stack[TEXTURE].push_back( Mat4::fromIdentity() );
#ifndef RT_AS3
                if( RenderTools::isGLInitialized() ){
                        _matrixMode( PROJECTION );
                        _pushMatrix();
                        _loadMatrix( * stack[PROJECTION][0] );
                        _matrixMode( MODELVIEW );
                        _pushMatrix();
                        _loadMatrix( * stack[MODELVIEW][0] );
                        _matrixMode( TEXTURE );
                        _pushMatrix();
                        _loadMatrix( * stack[TEXTURE][0] );
                        _setMatrixUniform();
                }
#endif
        }

        const Mat4 & get( Mode mode ){
                if( stack[ mode ].size() ){
                        return( stack[ mode ][ stack[ mode ].size() - 1 ] );
                }
                else{
                        Error::error( Error::STACK_UNDERFLOW, __FILE__, __LINE__ );
                        static const Mat4 r = Mat4::fromIdentity();
                        return( r );
                }
        }

        void set( Mode mode, const Mat4 & m ){
                if( stack[ mode ].size() ){
                        stack[ mode ][ stack[ mode ].size() - 1 ] = m;
#ifndef RT_AS3
                        if( RenderTools::isGLInitialized() ){
                                _matrixMode( mode );
                                _loadMatrix( * m );
                                _setMatrixUniform();
                        }
#endif
                }
                else{
                        Error::error( Error::STACK_UNDERFLOW, __FILE__, __LINE__ );
                }
        }

        void push( Mode mode, const Mat4 & m ){
                stack[ mode ].push_back( m );
#ifndef RT_AS3
                if( RenderTools::isGLInitialized() ){
                        _matrixMode( mode );
                        _pushMatrix();
                        _loadMatrix( * m );
                        _setMatrixUniform();
                }
#endif
        }

        void push( Mode mode ){
                if( stack[ mode ].size() ){
                        stack[ mode ].push_back( stack[ mode ][ stack[ mode ].size() - 1 ] );
                }
                else{
                        stack[ mode ].push_back( Mat4::fromIdentity() );
                }
#ifndef RT_AS3
                if( RenderTools::isGLInitialized() ){
                        _matrixMode( mode );
                        _pushMatrix();
                        _setMatrixUniform();
                }
#endif
        }

        Mat4 pop( Mode mode ){
                if( stack[ mode ].size() == 1 ){
                        Error::warning( Error::INVALID_VALUE, __FILE__, __LINE__, "cannot pop the matrix stack beyond size 1" );
                }
                else if( stack[ mode ].size() ){
                        const Mat4 & m = stack[ mode ][ stack[ mode ].size() - 1 ];
                        stack[ mode ].pop_back();
#ifndef RT_AS3
                        if( RenderTools::isGLInitialized() ){
                                _matrixMode( mode );
                                _popMatrix();
                                _setMatrixUniform();
                        }
#endif
                        return( m );
                }
                else{
                        Error::error( Error::STACK_UNDERFLOW, __FILE__, __LINE__ );
                }
                return( Mat4() );
        }

        Vec3 project( const Vec3 & object, const Vec4 & viewport ){
                Vec4 tmp( object, 1.0f );
                tmp = get( MODELVIEW ) * tmp;
                tmp = get( PROJECTION ) * tmp;

                tmp /= tmp[3];
                tmp = tmp * 0.5f + Vec4( 0.5f );
                tmp[0] = tmp[0] * viewport[2] + viewport[0];
                tmp[1] = tmp[1] * viewport[3] + viewport[1];

                return( tmp.xyz() );
        }

        Vec3 unProject( const Vec3 & window, const Vec4 & viewport ){
                Mat4 inverse = ( get( PROJECTION ) * get( MODELVIEW ) ).inverse();

                Vec4 tmp( window, 1.0f );
                tmp[0] = ( tmp[0] - viewport[0] ) / viewport[2];
                tmp[1] = ( tmp[1] - viewport[1] ) / viewport[3];
                tmp = tmp * 2.0f - Vec4( 1.0f );

                Vec4 obj = inverse * tmp;
                obj /= obj[3];

                return( obj.xyz() );
        }

};// Matrix namespace

};// rendertools namespace

---