math/Matrix4x4.cpp

Go to the documentation of this file.

//## begin module%36FE7C2C01F4.cm preserve=no
//        %X% %Q% %Z% %W%
//## end module%36FE7C2C01F4.cm

//## begin module%36FE7C2C01F4.cp preserve=no
//## end module%36FE7C2C01F4.cp

//## Module: Matrix4x4%36FE7C2C01F4; Pseudo Package body
//## Source file: C:\user\gipson\projects\mpk\code\geometry\Matrix4x4.cpp

//## begin module%36FE7C2C01F4.additionalIncludes preserve=no
//## end module%36FE7C2C01F4.additionalIncludes

//## begin module%36FE7C2C01F4.includes preserve=yes
#include "../additional/streams/mystreams.h"
#include <assert.h>
#include "math2.h"
//## end module%36FE7C2C01F4.includes

// Vector4
#include "Vector4.h"
// Matrix4x4
#include "Matrix4x4.h"
#include "Matrixmxn.h"
//## begin module%36FE7C2C01F4.additionalDeclarations preserve=yes
//## end module%36FE7C2C01F4.additionalDeclarations


// Class Matrix4x4 


//---------- Constants ----------------
static const double DEFAULT_TOL = 0.1;




Matrix4x4::Matrix4x4 ()
  //## begin Matrix4x4::Matrix4x4%925324837.hasinit preserve=no
  //## end Matrix4x4::Matrix4x4%925324837.hasinit
  //## begin Matrix4x4::Matrix4x4%925324837.initialization preserve=yes
  //## end Matrix4x4::Matrix4x4%925324837.initialization
{
  //## begin Matrix4x4::Matrix4x4%925324837.body preserve=yes
        for( int row = 0 ; row < 4 ; row++ )
        {
                for( int col = 0; col < 4; col++ ) 
                {
                        if( row == col )
                        {
                                values[ row ][ col ] = 1 ;
                        }
                        else
                        {
                                values[ row ][ col ] = 0 ;
                        }
                }
        }
  //## end Matrix4x4::Matrix4x4%925324837.body
}


Matrix4x4::~Matrix4x4()
{
  //## begin Matrix4x4::~Matrix4x4%.body preserve=yes
  //## end Matrix4x4::~Matrix4x4%.body
}



//## Other Operations (implementation)
Matrix4x4 Matrix4x4::operator * (const Matrix4x4& right) const
{
  //## begin Matrix4x4::operator*%923255744.body preserve=yes
        //improve: unwind this loop
        Matrix4x4 returnMe ;
        for( int row = 0; row < 4; row++ )
        {
                for( int col = 0; col < 4; col++ )
                {
                        returnMe.values[ row ][ col ] = 0 ;
                        for( int index = 0; index < 4 ; index++ )
                        {
                                returnMe.values[ row ][ col ] += values[ row ][ index ] * right.values[ index ][ col ] ;
                        }
                }
        }
        return returnMe ;
        //IMPROVE: outguess the compiler by unwinding these loops?
  //## end Matrix4x4::operator*%923255744.body
}

Vector4 Matrix4x4::operator * (const Vector4& right) const
{
        Vector4 returnMe( 0.0, 0.0, 0.0 ) ;
    int row;
    int col;
        for(  row = 2; row >= 0; --row )
        {
                for( col = 2; col >= 0; --col )
                {
                        returnMe[ row ] += values[ row ][ col ] * right[ col ] ;
                }
        }

    for( row = 0; row < 3; row++ )
    {
        returnMe[ row ] += values[ row ][ 3 ];
    }
        return returnMe ;
}

Matrix4x4 Matrix4x4::Inverse () const
{
  //## begin Matrix4x4::Inverse%925324833.body preserve=yes
        //IMPROVE: cache the inverse calculation for quicker computation

        Matrix4x4 J ;
        double a = values[ 0 ][ 0 ] ;
        double b = values[ 0 ][ 1 ] ;
        double c = values[ 0 ][ 2 ] ;
        double d = values[ 1 ][ 0 ] ;
        double e = values[ 1 ][ 1 ] ;
        double f = values[ 1 ][ 2 ] ;
        double g = values[ 2 ][ 0 ] ;
        double h = values[ 2 ][ 1 ] ;
        double i = values[ 2 ][ 2 ] ;
        double x = values[ 0 ][ 3 ] ;
        double y = values[ 1 ][ 3 ] ;
        double z = values[ 2 ][ 3 ] ;
        assert( values[ 3 ][ 0 ] == 0 );
        assert( values[ 3 ][ 1 ] == 0 );
        assert( values[ 3 ][ 2 ] == 0 );
        assert( values[ 3 ][ 3 ] == 1 );

        //this is SHANE output for the inverse of a 4x4 matrix of the form
        //
        //      abcx
        //  defy
        //      ghiz
        //      0001

      J( 0, 0 ) = a;
      J( 0, 1 ) = d;
      J( 0, 2 ) = g;
      J( 0, 3 ) = -a*x -d*y -g*z;       //x
      J( 1, 0 ) = b;
      J( 1, 1 ) = e;
      J( 1, 2 ) = h;
      J( 1, 3 ) = -b*x -e*y -h*z; //y
      J( 2, 0 ) = c;
      J( 2, 1 ) = f;
      J( 2, 2 ) = i;
      J( 2, 3 ) = -c*x -f*y -i*z;       //z
      J( 3, 0 ) = 0.0;
      J( 3, 1 ) = 0.0;
      J( 3, 2 ) = 0.0;
      J( 3, 3 ) = 1.0;

        return J ;
  //## end Matrix4x4::Inverse%925324833.body
}

Matrix4x4& Matrix4x4::operator *= (const Matrix4x4& right)
{
  //## begin Matrix4x4::operator*=%925324834.body preserve=yes
        Matrix4x4 result ;
        result = ( *this ) * right ;    //IMPROVE: get rid of the temp storage
        ( *this ) = result ; //IMPROVE: put all this inline
        return *this ;
  //## end Matrix4x4::operator*=%925324834.body
}

//=============================================================================
// GetValuesOpenGl 
//
// Description: Gets the matrix values in an openGlCompatible way
//=============================================================================
void Matrix4x4::GetValuesOpenGl ( double values[ 16 ] )
{
        int i;
        int j;
        for( i = 0; i < 4; i++ )
        {
                for( j = 0; j < 4; j++ )
                {
                        values[ i + j * 4 ] = this->values[ i ][ j ];
                }
        }
}

Matrix4x4 Matrix4x4::Identity ()
{
  //## begin Matrix4x4::Identity%925324838.body preserve=yes
        Matrix4x4 returnMe ;
        for( int row = 0 ; row < 4 ; row++ )
        {
                for( int col = 0; col < 4; col++ ) 
                {
                        if( row == col )
                        {
                                returnMe( row, col ) = 1 ;
                        }
                        else
                        {
                                returnMe( row, col ) = 0 ;
                        }
                }
        }
        return returnMe ;
  //## end Matrix4x4::Identity%925324838.body
}

void Matrix4x4::Scale (const double x, const double y, const double z)
{
  //## begin Matrix4x4::Scale%927223872.body preserve=yes
        Matrix4x4 multiplyMe ;
        multiplyMe( 0, 0 ) = x ;
        multiplyMe( 1, 1 ) = y ;
        multiplyMe( 2, 2 ) = z ;
        *this = multiplyMe * ( *this ) ;                        //IMPROVE: verify the order of the multiplication
  //## end Matrix4x4::Scale%927223872.body
}

void Matrix4x4::Translate (const double x, const double y, const double z)
{
  //## begin Matrix4x4::Translate%927223873.body preserve=yes
        Matrix4x4 multiplyMe ;
        multiplyMe( 0, 3 ) = x ;
        multiplyMe( 1, 3 ) = y ;
        multiplyMe( 2, 3 ) = z ;
        *this = multiplyMe * ( *this ) ;                        //IMPROVE: verify the order of the multiplication
  //## end Matrix4x4::Translate%927223873.body
}

void Matrix4x4::Rotate (const Vector4& axis, double angle)
{
  //## begin Matrix4x4::Rotate%928263345.body preserve=yes
        Matrix4x4 multiplyMe ;
        const double& x = axis[ 0 ] ;
        const double& y = axis[ 1 ] ;
        const double& z = axis[ 2 ] ;
        double c = CosDeg( angle ) ;
        double s = SinDeg( angle ) ;

        multiplyMe( 0, 0 ) = x * x + ( 1 - x * x ) * c ;
        multiplyMe( 0, 1 ) = x * y * ( 1 - c ) + z * s ;
        multiplyMe( 0, 2 ) = z * x * ( 1 - c ) - y * s ;

        multiplyMe( 1, 0 ) = x * y * ( 1 - c ) - z * s ;
        multiplyMe( 1, 1 ) = y * y + ( 1 - y * y ) * c ;
        multiplyMe( 1, 2 ) = y * z * ( 1 - c ) + x * s ; 

        multiplyMe( 2, 0 ) = z * x * ( 1 - c ) + y * s ;
        multiplyMe( 2, 1 ) = y * z * ( 1 - c ) - x * s ;
        multiplyMe( 2, 2 ) = z * z + ( 1 - z * z ) * c ;

        *this = multiplyMe * ( *this ) ;                        //IMPROVE: verify the order of the multiplication
  //## end Matrix4x4::Rotate%928263345.body
}

void Matrix4x4::Rotate2 (const Vector4& axis, double angle)
{
  //## begin Matrix4x4::Rotate%928263345.body preserve=yes
        Matrix4x4 multiplyMe ;
        const double& x = axis[ 0 ] ;
        const double& y = axis[ 1 ] ;
        const double& z = axis[ 2 ] ;
        double c = CosDeg( angle ) ;
        double s = SinDeg( angle ) ;

        multiplyMe( 0, 0 ) = x * x + ( 1 - x * x ) * c ;
        multiplyMe( 0, 1 ) = x * y * ( 1 - c ) - z * s ;
        multiplyMe( 0, 2 ) = z * x * ( 1 - c ) + y * s ;

        multiplyMe( 1, 0 ) = x * y * ( 1 - c ) + z * s ;
        multiplyMe( 1, 1 ) = y * y + ( 1 - y * y ) * c ;
        multiplyMe( 1, 2 ) = y * z * ( 1 - c ) - x * s ; 

        multiplyMe( 2, 0 ) = z * x * ( 1 - c ) - y * s ;
        multiplyMe( 2, 1 ) = y * z * ( 1 - c ) + x * s ;
        multiplyMe( 2, 2 ) = z * z + ( 1 - z * z ) * c ;

        *this = ( *this ) * multiplyMe;                 //IMPROVE: verify the order of the multiplication
  //## end Matrix4x4::Rotate%928263345.body
}

void Matrix4x4::Translate (const Vector4& offset)
{
  //## begin Matrix4x4::Translate%936229637.body preserve=yes
        Translate( offset[ 0 ], offset[ 1 ], offset[ 2 ] ) ;    //IMPROVE: inefficient
  //## end Matrix4x4::Translate%936229637.body
}

Matrix4x4 Matrix4x4::Transpose () const
{
  //## begin Matrix4x4::Transpose%945817286.body preserve=yes
        Matrix4x4 returnMe ;
        for( int i = 0; i < 4; i++ )
        {
                for( int j = 0; j < 4; j++ )
                {
                        returnMe( i, j ) = this->operator()( j, i ) ;
                }
        }
        return returnMe ;
  //## end Matrix4x4::Transpose%945817286.body
}

void Matrix4x4::SetValues (const Matrix4x4& values)
{
  //## begin Matrix4x4::SetValues%969569780.body preserve=yes
        for( int i = 0; i < 4; i++ )
        {
                for( int j = 0; j < 4; j++ )
                {
                        this->operator()( i, j ) = values( i, j );
                }
        }

  //## end Matrix4x4::SetValues%969569780.body
}

//=============================================================================
// SetValues
//
// Description: sets matrix values based on an array of doubles
//=============================================================================
void Matrix4x4::SetValues ( const double values[ 16 ] )
{
        for( int i = 0; i < 4; i++ )            //row
        {
                for( int j = 0; j < 4; j++ )    //column
                {
                        int index = i + ( j * 4 );
                        double value = values[ index ];
                        this->operator()( j, i ) = value;
                }
        }
}

// Additional Declarations
  //## begin Matrix4x4%36FE7C2C01F4.declarations preserve=yes
bool Matrix4x4::operator < (const Matrix4x4& right) const
{
        assert( false ) ;
        return false ;
}

bool Matrix4x4::operator ==(const Matrix4x4& right) const
{
        return Compare( right, DEFAULT_TOL );
}

bool Matrix4x4::operator !=(const Matrix4x4& right) const
{
        return !(Compare( right, DEFAULT_TOL ));
}


  //## end Matrix4x4%36FE7C2C01F4.declarations
//## begin module%36FE7C2C01F4.epilog preserve=yes
//## end module%36FE7C2C01F4.epilog

//=============================================================================
// Compare 
//
// Description: compares if the given matrix is equal to the current matrix 
//                              to within a given tolerance
//=============================================================================
bool Matrix4x4::Compare ( const Matrix4x4& right, const double& tol ) const
{
        for ( int i = 0; i < 4; i++ )
        {
                for ( int j = 0; j < 4; j++ )
                {
                        double diff = values[i][j] - right.values[i][j];
                        if ( ( diff > tol ) || ( diff < -tol ) )
                        {
                                return false;
                        }
                }
        }

        // if still in function, then all terms in matrix are equal to within tol
        return true;
}

Matrix4x4::operator Matrixmxn() const
{
        Matrixmxn matrix(4, 4);
        for (int i=0; i<4; i++)
                for (int j=0; j<4; j++)
                        matrix(i, j) = values[i][j];
        return matrix;
}

std::ostream & operator<<( std::ostream &os, const Matrix4x4& m ) 
{
        os << "[" 
                << m(0, 0) << " , " << m(0, 1) << " , " << m(0, 2) << " , " << m(0, 3) << " , " 
                << m(1, 0) << " , " << m(1, 1) << " , " << m(1, 2) << " , " << m(1, 3) << " , " 
                << m(2, 0) << " , " << m(2, 1) << " , " << m(2, 2) << " , " << m(2, 3) << " , " 
                << m(3, 0) << " , " << m(3, 1) << " , " << m(3, 2) << " , " << m(3, 3)
                << "]" << endl;
        return os ;
}

std::istream & operator>>( std::istream &is, Matrix4x4& m )
{
        eatwhite( is ) ;
        char leading = is.get() ;                       //get the leading '['
        assert( leading == '[' ) ;

        char delimiter;
        for( int i = 0; i < 4; i++ ) 
        {
                for (int j = 0; j < 4; j++)
                {
                        is >> m( i, j) ;
                        eatwhite( is ) ;
                        delimiter = is.get() ;
                        assert( ( delimiter == ',' ) || ( delimiter == ']' ) ) ;
                }
        }

        return is ;
}

---