matrix.cpp Source File

00001 //FILE:         matrix.cpp
00002 //AUTHOR:       Nathan Cournia <nathan@cournia.com>
00003 
00004 #include <iomanip>
00005 #include "matrix.h"
00006 
00008 const math::matrix4 math::matrix4::IDENTITY(
00009         1.0, 0.0, 0.0, 0.0,
00010         0.0, 1.0, 0.0, 0.0,
00011         0.0, 0.0, 1.0, 0.0,
00012         0.0, 0.0, 0.0, 1.0
00013 );
00014 
00016 const math::matrix4 math::matrix4::ZERO(
00017         0.0, 0.0, 0.0, 0.0,
00018         0.0, 0.0, 0.0, 0.0,
00019         0.0, 0.0, 0.0, 0.0,
00020         0.0, 0.0, 0.0, 0.0
00021 );
00022 
00024 math::matrix4::matrix4( void )
00025 { }
00026 
00028 math::matrix4::matrix4( 
00029         real_t d00, real_t d01, real_t d02, 
00030         real_t d10, real_t d11, real_t d12, 
00031         real_t d20, real_t d21, real_t d22 )
00032 {
00033         m_data[ 0 ][ 0 ] = d00;
00034         m_data[ 0 ][ 1 ] = d01;
00035         m_data[ 0 ][ 2 ] = d02;
00036         m_data[ 0 ][ 3 ] = 0.0;
00037 
00038         m_data[ 1 ][ 0 ] = d10;
00039         m_data[ 1 ][ 1 ] = d11;
00040         m_data[ 1 ][ 2 ] = d12;
00041         m_data[ 1 ][ 3 ] = 0.0;
00042 
00043         m_data[ 2 ][ 0 ] = d20;
00044         m_data[ 2 ][ 1 ] = d21;
00045         m_data[ 2 ][ 2 ] = d22;
00046         m_data[ 2 ][ 3 ] = 0.0;
00047 
00048         m_data[ 3 ][ 0 ] = 0.0;
00049         m_data[ 3 ][ 1 ] = 0.0;
00050         m_data[ 3 ][ 2 ] = 0.0;
00051         m_data[ 3 ][ 3 ] = 1.0;
00052 }
00053 
00055 math::matrix4::matrix4(
00056         real_t d00, real_t d01, real_t d02, real_t d03, 
00057         real_t d10, real_t d11, real_t d12, real_t d13,
00058         real_t d20, real_t d21, real_t d22, real_t d23,
00059         real_t d30, real_t d31, real_t d32, real_t d33 )
00060 {
00061         m_data[ 0 ][ 0 ] = d00;
00062         m_data[ 0 ][ 1 ] = d01;
00063         m_data[ 0 ][ 2 ] = d02;
00064         m_data[ 0 ][ 3 ] = d03;
00065 
00066         m_data[ 1 ][ 0 ] = d10;
00067         m_data[ 1 ][ 1 ] = d11;
00068         m_data[ 1 ][ 2 ] = d12;
00069         m_data[ 1 ][ 3 ] = d13;
00070 
00071         m_data[ 2 ][ 0 ] = d20;
00072         m_data[ 2 ][ 1 ] = d21;
00073         m_data[ 2 ][ 2 ] = d22;
00074         m_data[ 2 ][ 3 ] = d23;
00075 
00076         m_data[ 3 ][ 0 ] = d30;
00077         m_data[ 3 ][ 1 ] = d31;
00078         m_data[ 3 ][ 2 ] = d32;
00079         m_data[ 3 ][ 3 ] = d33;
00080 }
00081 
00083 math::matrix4::matrix4( const math::matrix4& m )
00084 {
00085         for( unsigned int r = 0; r < 4; ++r ) {
00086                 for( unsigned int c = 0; c < 4; ++c ) {
00087                         m_data[ r ][ c ] = m.m_data[ r ][ c ];
00088                 }
00089         }
00090 }
00091 
00093 math::matrix4&
00094 math::matrix4::operator= ( const math::matrix4& m )
00095 {
00096         for( unsigned int r = 0; r < 4; ++r ) {
00097                 for( unsigned int c = 0; c < 4; ++c ) {
00098                         m_data[ r ][ c ] = m.m_data[ r ][ c ];
00099                 }
00100         }
00101         return *this;
00102 }
00103 
00105 void
00106 math::matrix4::set( 
00107         real_t d00, real_t d01, real_t d02, 
00108         real_t d10, real_t d11, real_t d12, 
00109         real_t d20, real_t d21, real_t d22 )
00110 {
00111         m_data[ 0 ][ 0 ] = d00;
00112         m_data[ 0 ][ 1 ] = d01;
00113         m_data[ 0 ][ 2 ] = d02;
00114         m_data[ 0 ][ 3 ] = 0.0;
00115 
00116         m_data[ 1 ][ 0 ] = d10;
00117         m_data[ 1 ][ 1 ] = d11;
00118         m_data[ 1 ][ 2 ] = d12;
00119         m_data[ 1 ][ 3 ] = 0.0;
00120 
00121         m_data[ 2 ][ 0 ] = d20;
00122         m_data[ 2 ][ 1 ] = d21;
00123         m_data[ 2 ][ 2 ] = d22;
00124         m_data[ 2 ][ 3 ] = 0.0;
00125 
00126         m_data[ 3 ][ 0 ] = 0.0;
00127         m_data[ 3 ][ 1 ] = 0.0;
00128         m_data[ 3 ][ 2 ] = 0.0;
00129         m_data[ 3 ][ 3 ] = 1.0;
00130 }
00131 
00133 void
00134 math::matrix4::set(
00135         real_t d00, real_t d01, real_t d02, real_t d03, 
00136         real_t d10, real_t d11, real_t d12, real_t d13,
00137         real_t d20, real_t d21, real_t d22, real_t d23,
00138         real_t d30, real_t d31, real_t d32, real_t d33 )
00139 {
00140         m_data[ 0 ][ 0 ] = d00;
00141         m_data[ 0 ][ 1 ] = d01;
00142         m_data[ 0 ][ 2 ] = d02;
00143         m_data[ 0 ][ 3 ] = d03;
00144 
00145         m_data[ 1 ][ 0 ] = d10;
00146         m_data[ 1 ][ 1 ] = d11;
00147         m_data[ 1 ][ 2 ] = d12;
00148         m_data[ 1 ][ 3 ] = d13;
00149 
00150         m_data[ 2 ][ 0 ] = d20;
00151         m_data[ 2 ][ 1 ] = d21;
00152         m_data[ 2 ][ 2 ] = d22;
00153         m_data[ 2 ][ 3 ] = d23;
00154 
00155         m_data[ 3 ][ 0 ] = d30;
00156         m_data[ 3 ][ 1 ] = d31;
00157         m_data[ 3 ][ 2 ] = d32;
00158         m_data[ 3 ][ 3 ] = d33;
00159 }
00160 
00162 math::vector3
00163 math::matrix4::operator* ( const math::vector3& v ) const
00164 {
00165         math::vector3 res;
00166         for( unsigned int i = 0; i < 3; ++i ) {
00167                 res( i ) = 
00168                         m_data[ i ][ 0 ] * v.x( ) +
00169                         m_data[ i ][ 1 ] * v.y( ) +
00170                         m_data[ i ][ 2 ] * v.z( ) +
00171                         m_data[ i ][ 3 ];
00172         }
00173         return res;
00174 }
00175 
00177 math::matrix4
00178 math::matrix4::operator*( const math::matrix4& rhs ) const
00179 {
00180         math::matrix4 dest;
00181         for( unsigned int r = 0; r < 4; ++r ) {
00182                 for( unsigned int c = 0; c < 4; ++c ) {
00183                         dest.m_data[ r ][c ] = 
00184                                 m_data[ r ][ 0 ] * rhs.m_data[ 0 ][ c ] +
00185                                 m_data[ r ][ 1 ] * rhs.m_data[ 1 ][ c ] +
00186                                 m_data[ r ][ 2 ] * rhs.m_data[ 2 ][ c ] +
00187                                 m_data[ r ][ 3 ] * rhs.m_data[ 3 ][ c ];
00188                 }
00189         }
00190         return dest;
00191 }
00192 
00194 math::matrix4
00195 math::matrix4::operator+( const math::matrix4& rhs ) const
00196 {
00197         math::matrix4 dest;
00198         for( unsigned int r = 0; r < 4; ++r ) {
00199                 for( unsigned int c = 0; c < 4; ++c ) {
00200                         dest.m_data[ r ][ c ] = m_data[ r ][ c ] + rhs.m_data[ r ][ c ];
00201                 }
00202         }
00203         return dest;
00204 }
00205 
00207 math::matrix4
00208 math::matrix4::operator-( const math::matrix4& rhs ) const
00209 {
00210         math::matrix4 dest;
00211         for( unsigned int r = 0; r < 4; ++r ) {
00212                 for( unsigned int c = 0; c < 4; ++c ) {
00213                         dest.m_data[ r ][c ] = m_data[ r ][ c ] - rhs.m_data[ r ][ c ];
00214                 }
00215         }
00216         return dest;
00217 }
00218 
00220 void
00221 math::matrix4::transpose( void )
00222 {
00223         for( unsigned int r = 0; r < 4; ++r ) {
00224                 for( unsigned int c = r; c < 4; ++c ) {
00225                         math::swap( m_data[ r ][ c ], m_data[ c ][ r ] );
00226                 }
00227         }
00228 }
00229 
00231 math::vector3
00232 operator* ( const math::vector3& v, const math::matrix4& m )
00233 {
00234         math::vector3 res;
00235         for( unsigned int i = 0; i < 3; ++i ) {
00236                 res( i ) = 
00237                         m.m_data[ 0 ][ i ] * v.x( ) +
00238                         m.m_data[ 1 ][ i ] * v.y( ) +
00239                         m.m_data[ 2 ][ i ] * v.z( ) +
00240                         m.m_data[ 3 ][ i ];
00241         }
00242         return res;
00243 }
00244 
00246 void
00247 math::matrix4::from_angle_axis( real_t radians, const math::vector3& axis )
00248 {
00249         real_t rcos = cos( radians );
00250         real_t rsin = sin( radians );
00251         real_t xy = axis.x( ) * axis.y( );
00252         real_t xz = axis.x( ) * axis.z( );
00253         real_t yz = axis.y( ) * axis.z( );
00254         real_t t = 1.0 - rcos;
00255         
00256         m_data[ 0 ][ 0 ] = rcos + axis.x( ) * axis.x( ) * t;
00257         m_data[ 0 ][ 1 ] = -axis.z( ) * rsin + xy * t;
00258         m_data[ 0 ][ 2 ] = axis.y( ) * rsin + xz * t;
00259         m_data[ 0 ][ 3 ] = 0.0;
00260 
00261         m_data[ 1 ][ 0 ] = axis.z( ) * rsin + xy * t;
00262         m_data[ 1 ][ 1 ] = rcos + axis.y( ) * axis.y( ) * t;
00263         m_data[ 1 ][ 2 ] = -axis.x( ) * rsin + yz * t;
00264         m_data[ 1 ][ 3 ] = 0.0;
00265 
00266         m_data[ 2 ][ 0 ] = -axis.y( ) * rsin + xz * t;
00267         m_data[ 2 ][ 1 ] = axis.x( ) * rsin + yz * t;
00268         m_data[ 2 ][ 2 ] = rcos + axis.z( ) * axis.z( ) * t;
00269         m_data[ 2 ][ 3 ] = 0.0;
00270 
00271         m_data[ 3 ][ 0 ] = 0.0;
00272         m_data[ 3 ][ 1 ] = 0.0;
00273         m_data[ 3 ][ 2 ] = 0.0;
00274         m_data[ 3 ][ 3 ] = 1.0;
00275 }
00276 
00278 //assumes an inverse exist
00279 math::matrix4
00280 math::matrix4::get_inverted_rotation( void ) const
00281 {
00282         math::matrix4 inv( IDENTITY );
00283         inv.m_data[ 0 ][ 0 ] = m_data[ 1 ][ 1 ] * m_data[ 2 ][ 2 ] -
00284                 m_data[ 1 ][ 2 ] * m_data[ 2 ][ 1 ];
00285         inv.m_data[ 0 ][ 1 ] = m_data[ 0 ][ 2 ] * m_data[ 2 ][ 1 ] -
00286                 m_data[ 0 ][ 1 ] * m_data[ 2 ][ 2 ];
00287         inv.m_data[ 0 ][ 2 ] = m_data[ 0 ][ 1 ] * m_data[ 1 ][ 2 ] -
00288                 m_data[ 0 ][ 2 ] * m_data[ 1 ][ 1 ];
00289         inv.m_data[ 1 ][ 0 ] = m_data[ 1 ][ 2 ] * m_data[ 2 ][ 0 ] -
00290                 m_data[ 1 ][ 0 ] * m_data[ 2 ][ 2 ];
00291         inv.m_data[ 1 ][ 1 ] = m_data[ 0 ][ 0 ] * m_data[ 2 ][ 2 ] -
00292                 m_data[ 0 ][ 2 ] * m_data[ 2 ][ 0 ];
00293         inv.m_data[ 1 ][ 2 ] = m_data[ 0 ][ 2 ] * m_data[ 1 ][ 0 ] -
00294                 m_data[ 0 ][ 0 ] * m_data[ 1 ][ 2 ];
00295         inv.m_data[ 2 ][ 0 ] = m_data[ 1 ][ 0 ] * m_data[ 2 ][ 1 ] -
00296                 m_data[ 1 ][ 1 ] * m_data[ 2 ][ 0 ];
00297         inv.m_data[ 2 ][ 1 ] = m_data[ 0 ][ 1 ] * m_data[ 2 ][ 0 ] -
00298                 m_data[ 0 ][ 0 ] * m_data[ 2 ][ 1 ];
00299         inv.m_data[ 2 ][ 2 ] = m_data[ 0 ][ 0 ] * m_data[ 1 ][ 1 ] -
00300                 m_data[ 0 ][ 1 ] * m_data[ 1 ][ 0 ];
00301 
00302         real_t det = m_data[ 0 ][ 0 ] * inv.m_data[ 0 ][ 0 ] +
00303                 m_data[ 0 ][ 1 ] * inv.m_data[ 1 ][ 0 ] +
00304                 m_data[ 0 ][ 2 ] * inv.m_data[ 2 ][ 0 ];
00305 
00306         if( math::equals( det, 0.0 ) ) {
00307                 //fuck, no inverse
00308                 return IDENTITY;
00309         }
00310 
00311         det = 1.0 / det;
00312         for( unsigned int r = 0; r < 3; ++r ) {
00313                 for( unsigned int c = 0; c < 3; ++c ) {
00314                         inv.m_data[ r ][ c ] *= det;
00315                 }
00316         }
00317 
00318         return inv;
00319 }
00320 
00322 std::ostream&
00323 operator<< ( std::ostream& o, const math::matrix4& m )
00324 {
00325         for( unsigned int r = 0; r < 3; ++r ) {
00326                 o << std::setw( 5 ) << m.m_data[ r ][ 0 ] << " "
00327                         << std::setw( 5 ) << m.m_data[ r ][ 1 ] << " "
00328                         << std::setw( 5 ) << m.m_data[ r ][ 2 ] << " " 
00329                         << std::setw( 5 ) << m.m_data[ r ][ 3 ] << " "
00330                         << std::endl;
00331         }
00332         
00333         //for the last line do not put a line return
00334         o << std::setw( 5 ) << m.m_data[ 3 ][ 0 ] << " "
00335                         << std::setw( 5 ) << m.m_data[ 3 ][ 1 ] << " "
00336                         << std::setw( 5 ) << m.m_data[ 3 ][ 2 ] << " " 
00337                         << std::setw( 5 ) << m.m_data[ 3 ][ 3 ] << " ";
00338         return o;
00339 }