libcrbn: include/crbn/basic/mat4.hpp Source File

00001 
00002 #ifndef __matrix44_hpp__
00003 #define __matrix44_hpp__
00004 
00005 #include <stdio.h>
00006 #include <string.h>
00007 
00008 #include <crbn/basic/vec3.hpp>
00009 
00010 struct mat4 {
00011   float mat[ 16 ];
00012 
00013   mat4( const float f = 0.0f ) {
00014     memset( mat, 0, 16 * sizeof( float ) );
00015     mat[ 0 ] = mat[ 5 ] = mat[ 10 ] = mat[ 15 ] = f;
00016   }
00017   mat4( const float* tab ) {
00018     memcpy( mat, tab, 16 * sizeof( float ) );
00019   }
00020   mat4( const float  c0, const float  c1, const float  c2, const float  c3,
00021         const float  c4, const float  c5, const float  c6, const float  c7,
00022         const float  c8, const float  c9, const float c10, const float c11,
00023         const float c12, const float c13, const float c14, const float c15 ) {
00024     mat[  0 ] = c0;
00025     mat[  1 ] = c1;
00026     mat[  2 ] = c2;
00027     mat[  3 ] = c3;
00028     mat[  4 ] = c4;
00029     mat[  5 ] = c5;
00030     mat[  6 ] = c6;
00031     mat[  7 ] = c7;
00032     mat[  8 ] = c8;
00033     mat[  9 ] = c9;
00034     mat[ 10 ] = c10;
00035     mat[ 11 ] = c11;
00036     mat[ 12 ] = c12;
00037     mat[ 13 ] = c13;
00038     mat[ 14 ] = c14;
00039     mat[ 15 ] = c15;
00040   }
00041 };
00042 
00043 // a = identity matrix
00044 inline void mid( mat4& a ) {
00045   a = mat4( 1.0f );
00046 }
00047 
00048 // a = b * c
00049 inline void mmul( mat4& a, mat4& b, mat4& c ) {
00050   float* A = a.mat;
00051   float* B = b.mat;
00052   float* C = c.mat;
00053 
00054   A[ 0 ] = B[ 0 ] * C[ 0 ] + B[ 1 ] * C[ 4 ] + B[ 2 ] * C[ 8 ] + B[ 3 ] * C[ 12 ];
00055   A[ 1 ] = B[ 0 ] * C[ 1 ] + B[ 1 ] * C[ 5 ] + B[ 2 ] * C[ 9 ] + B[ 3 ] * C[ 13 ];
00056   A[ 2 ] = B[ 0 ] * C[ 2 ] + B[ 1 ] * C[ 6 ] + B[ 2 ] * C[ 10 ] + B[ 3 ] * C[ 14 ];
00057   A[ 3 ] = B[ 0 ] * C[ 3 ] + B[ 1 ] * C[ 7 ] + B[ 2 ] * C[ 11 ] + B[ 3 ] * C[ 15 ];
00058 
00059   A[ 4 ] = B[ 4 ] * C[ 0 ] + B[ 5 ] * C[ 4 ] + B[ 6 ] * C[ 8 ] + B[ 7 ] * C[ 12 ];
00060   A[ 5 ] = B[ 4 ] * C[ 1 ] + B[ 5 ] * C[ 5 ] + B[ 6 ] * C[ 9 ] + B[ 7 ] * C[ 13 ];
00061   A[ 6 ] = B[ 4 ] * C[ 2 ] + B[ 5 ] * C[ 6 ] + B[ 6 ] * C[ 10 ] + B[ 7 ] * C[ 14 ];
00062   A[ 7 ] = B[ 4 ] * C[ 3 ] + B[ 5 ] * C[ 7 ] + B[ 6 ] * C[ 11 ] + B[ 7 ] * C[ 15 ];
00063 
00064   A[ 8 ] = B[ 8 ] * C[ 0 ] + B[ 9 ] * C[ 4 ] + B[ 10 ] * C[ 8 ] + B[ 11 ] * C[ 12 ];
00065   A[ 9 ] = B[ 8 ] * C[ 1 ] + B[ 9 ] * C[ 5 ] + B[ 10 ] * C[ 9 ] + B[ 11 ] * C[ 13 ];
00066   A[ 10 ] = B[ 8 ] * C[ 2 ] + B[ 9 ] * C[ 6 ] + B[ 10 ] * C[ 10 ] + B[ 11 ] * C[ 14 ];
00067   A[ 11 ] = B[ 8 ] * C[ 3 ] + B[ 9 ] * C[ 7 ] + B[ 10 ] * C[ 11 ] + B[ 11 ] * C[ 15 ];
00068 
00069   A[ 12 ] = B[ 12 ] * C[ 0 ] + B[ 13 ] * C[ 4 ] + B[ 14 ] * C[ 8 ] + B[ 15 ] * C[ 12 ];
00070   A[ 13 ] = B[ 12 ] * C[ 1 ] + B[ 13 ] * C[ 5 ] + B[ 14 ] * C[ 9 ] + B[ 15 ] * C[ 13 ];
00071   A[ 14 ] = B[ 12 ] * C[ 2 ] + B[ 13 ] * C[ 6 ] + B[ 14 ] * C[ 10 ] + B[ 15 ] * C[ 14 ];
00072   A[ 15 ] = B[ 12 ] * C[ 3 ] + B[ 13 ] * C[ 7 ] + B[ 14 ] * C[ 11 ] + B[ 15 ] * C[ 15 ];
00073 }
00074 
00075 //     t
00076 // a =  b
00077 inline void mtranspose( mat4& a, mat4& b ) {
00078   float* A = a.mat;
00079   float* B = b.mat;
00080 
00081   A[  0 ] = B[  0 ];
00082   A[  1 ] = B[  4 ];
00083   A[  2 ] = B[  8 ];
00084   A[  3 ] = B[ 12 ];
00085   A[  4 ] = B[  1 ];
00086   A[  5 ] = B[  5 ];
00087   A[  6 ] = B[  9 ];
00088   A[  7 ] = B[ 13 ];
00089   A[  8 ] = B[  2 ];
00090   A[  9 ] = B[  6 ];
00091   A[ 10 ] = B[ 10 ];
00092   A[ 11 ] = B[ 14 ];
00093   A[ 12 ] = B[  3 ];
00094   A[ 13 ] = B[  7 ];
00095   A[ 14 ] = B[ 11 ];
00096   A[ 15 ] = B[ 15 ];
00097 }
00098 
00099 //      -1
00100 // a = b
00101 inline bool minvert( mat4& a, mat4& b ) {
00102   float* A = a.mat;
00103   float* B = b.mat;
00104 
00105   // Computation of matrix determinant
00106   float det = (B[  0 ] * A[  0 ] - B[  1 ] * A[  4 ] + B[  2 ] * A[  8 ] - B[ 3 ] * A[ 12 ]);
00107 
00108   if( fabsf( det ) < 1e-6f )
00109     return false;
00110 
00111   det = 1.0f / det;
00112   float mdet = -det;
00113 
00114   // 1st line
00115   A[ 0 ] = B[  5 ] * B[ 10 ] * B[ 15 ] + B[  6 ] * B[ 11 ] * B[ 13 ] + B[  7 ] * B[  9 ] * B[ 14 ]
00116          - B[ 13 ] * B[ 10 ] * B[  7 ] - B[ 14 ] * B[ 11 ] * B[  5 ] - B[ 15 ] * B[  9 ] * B[  6 ];
00117   A[ 1 ] = B[  1 ] * B[ 10 ] * B[ 15 ] + B[  2 ] * B[ 11 ] * B[ 13 ] + B[  3 ] * B[  9 ] * B[ 14 ]
00118          - B[ 13 ] * B[ 10 ] * B[  3 ] - B[ 14 ] * B[ 11 ] * B[  1 ] - B[ 15 ] * B[  9 ] * B[  2 ];
00119   A[ 2 ] = B[  1 ] * B[  6 ] * B[ 15 ] + B[  2 ] * B[  7 ] * B[ 13 ] + B[  3 ] * B[  5 ] * B[ 14 ]
00120          - B[ 13 ] * B[  6 ] * B[  3 ] - B[ 14 ] * B[  7 ] * B[  1 ] - B[ 15 ] * B[  5 ] * B[  2 ];
00121   A[ 3 ] = B[  1 ] * B[  6 ] * B[ 11 ] + B[  2 ] * B[  7 ] * B[  9 ] + B[  3 ] * B[  5 ] * B[ 10 ]
00122          - B[  9 ] * B[  6 ] * B[  3 ] - B[ 10 ] * B[  7 ] * B[  1 ] - B[ 11 ] * B[  5 ] * B[  2 ];
00123 
00124   // 2nd line
00125   A[  4 ] = B[  4 ] * B[ 10 ] * B[ 15 ] + B[  6 ] * B[ 11 ] * B[ 12 ] + B[  7 ] * B[  8 ] * B[ 14 ]
00126           - B[ 12 ] * B[ 10 ] * B[  7 ] - B[ 14 ] * B[ 11 ] * B[  4 ] - B[ 15 ] * B[  8 ] * B[  6 ];
00127   A[  5 ] = B[  0 ] * B[ 10 ] * B[ 15 ] + B[  2 ] * B[ 11 ] * B[ 12 ] + B[  3 ] * B[  8 ] * B[ 14 ]
00128           - B[ 12 ] * B[ 10 ] * B[  3 ] - B[ 14 ] * B[ 11 ] * B[  0 ] - B[ 15 ] * B[  8 ] * B[  2 ];
00129   A[  6 ] = B[  0 ] * B[  6 ] * B[ 15 ] + B[  2 ] * B[  7 ] * B[ 12 ] + B[  3 ] * B[  4 ] * B[ 14 ]
00130           - B[ 12 ] * B[  6 ] * B[  3 ] - B[ 14 ] * B[  7 ] * B[  0 ] - B[ 15 ] * B[  4 ] * B[  2 ];
00131   A[  7 ] = B[  0 ] * B[  6 ] * B[ 11 ] + B[  2 ] * B[  7 ] * B[  8 ] + B[  3 ] * B[  4 ] * B[ 10 ]
00132           - B[  8 ] * B[  6 ] * B[  3 ] - B[ 10 ] * B[  7 ] * B[  0 ] - B[ 11 ] * B[  4 ] * B[  2 ];
00133 
00134 
00135   // 3rd line
00136   A[  8 ] = B[  4 ] * B[  9 ] * B[ 15 ] + B[  5 ] * B[ 11 ] * B[ 12 ] + B[  7 ] * B[  8 ] * B[ 13 ]
00137           - B[ 12 ] * B[  9 ] * B[  7 ] - B[ 13 ] * B[ 11 ] * B[  4 ] - B[ 15 ] * B[  8 ] * B[  5 ];
00138   A[  9 ] = B[  0 ] * B[  9 ] * B[ 15 ] + B[  1 ] * B[ 11 ] * B[ 12 ] + B[  3 ] * B[  8 ] * B[ 13 ]
00139           - B[ 12 ] * B[  9 ] * B[  3 ] - B[ 13 ] * B[ 11 ] * B[  0 ] - B[ 15 ] * B[  8 ] * B[  1 ];
00140   A[ 10 ] = B[  0 ] * B[  5 ] * B[ 15 ] + B[  1 ] * B[  7 ] * B[ 12 ] + B[  3 ] * B[  4 ] * B[ 13 ]
00141           - B[ 12 ] * B[  5 ] * B[  3 ] - B[ 13 ] * B[  7 ] * B[  0 ] - B[ 15 ] * B[  4 ] * B[  1 ];
00142   A[ 11 ] = B[  0 ] * B[  5 ] * B[ 11 ] + B[  1 ] * B[  7 ] * B[  8 ] + B[  3 ] * B[  4 ] * B[  9 ]
00143           - B[  8 ] * B[  5 ] * B[  3 ] - B[  9 ] * B[  7 ] * B[  0 ] - B[ 11 ] * B[  4 ] * B[  1 ];
00144 
00145   // 4th line
00146   A[ 12 ] = B[  4 ] * B[  9 ] * B[ 14 ] + B[  5 ] * B[ 10 ] * B[ 12 ] + B[  6 ] * B[  8 ] * B[ 13 ]
00147           - B[ 12 ] * B[  9 ] * B[  6 ] - B[ 13 ] * B[ 10 ] * B[  4 ] - B[ 14 ] * B[  8 ] * B[  5 ];
00148   A[ 13 ] = B[  0 ] * B[  9 ] * B[ 14 ] + B[  1 ] * B[ 10 ] * B[ 12 ] + B[  2 ] * B[  8 ] * B[ 13 ]
00149           - B[ 12 ] * B[  9 ] * B[  2 ] - B[ 13 ] * B[ 10 ] * B[  0 ] - B[ 14 ] * B[  8 ] * B[  1 ];
00150   A[ 14 ] = B[  0 ] * B[  5 ] * B[ 14 ] + B[  1 ] * B[  6 ] * B[ 12 ] + B[  2 ] * B[  4 ] * B[ 13 ]
00151           - B[ 12 ] * B[  5 ] * B[  2 ] - B[ 13 ] * B[  6 ] * B[  0 ] - B[ 14 ] * B[  4 ] * B[  1 ];
00152   A[ 15 ] = B[  0 ] * B[  5 ] * B[ 10 ] + B[  1 ] * B[  6 ] * B[  8 ] + B[  2 ] * B[  4 ] * B[  9 ]
00153           - B[  8 ] * B[  5 ] * B[  2 ] - B[  9 ] * B[  6 ] * B[  0 ] - B[ 10 ] * B[  4 ] * B[  1 ];
00154 
00155   // we divide by déterminant (multiply by its inverse)
00156   A[  0 ] *=  det;  A[  1 ] *= mdet;  A[  2 ] *=  det;  A[  3 ] *= mdet;
00157   A[  4 ] *= mdet;  A[  5 ] *=  det;  A[  6 ] *= mdet;  A[  7 ] *=  det;
00158   A[  8 ] *=  det;  A[  9 ] *= mdet;  A[ 10 ] *=  det;  A[ 11 ] *= mdet;
00159   A[ 12 ] *= mdet;  A[ 13 ] *=  det;  A[ 14 ] *= mdet;  A[ 15 ] *=  det;
00160 
00161   return true;
00162 }
00163 
00164 // a = m * b
00165 inline void mmulp( vec3& a, mat4& m, vec3& b ) {
00166   float* f = m.mat, ff;
00167 
00168   a.x = f[  0 ] * b.x + f[  1 ] * b.y + f[  2 ] * b.z + f[  3 ];
00169   a.y = f[  4 ] * b.x + f[  5 ] * b.y + f[  6 ] * b.z + f[  7 ];
00170   a.z = f[  8 ] * b.x + f[  9 ] * b.y + f[ 10 ] * b.z + f[ 11 ];
00171   ff  = 1.0f / (f[ 12 ] * b.x + f[ 13 ] * b.y + f[ 14 ] * b.z + f[ 15 ]);
00172   a.x *= ff;
00173   a.y *= ff;
00174   a.z *= ff;
00175 }
00176 
00177 // a = m * b
00178 inline void mmulv( vec3& a, mat4& m, vec3& b ) {
00179   float* f = m.mat;
00180 
00181   a.x = f[  0 ] * b.x + f[  1 ] * b.y + f[  2 ] * b.z;
00182   a.y = f[  4 ] * b.x + f[  5 ] * b.y + f[  6 ] * b.z;
00183   a.z = f[  8 ] * b.x + f[  9 ] * b.y + f[ 10 ] * b.z;
00184 }
00185 
00186 //      t
00187 // a = m  * b
00188 inline void mtmulv( vec3& a, mat4& m, vec3& b ) {
00189   float* f = m.mat;
00190   a.x = f[  0 ] * b.x + f[  4 ] * b.y + f[  8 ] * b.z;
00191   a.y = f[  1 ] * b.x + f[  5 ] * b.y + f[  9 ] * b.z;
00192   a.z = f[  2 ] * b.x + f[  6 ] * b.y + f[ 10 ] * b.z;
00193 }
00194 
00195 // a print function for debugging
00196 inline void print( mat4& m, const char* str ) {
00197   float* mat = m.mat;
00198   printf( "%s:\n%12g %12g %12g %12g\n%12g %12g %12g %12g\n%12g %12g %12g %12g\n%12g %12g %12g %12g\n",
00199           str,
00200           mat[  0 ], mat[  1 ], mat[  2 ], mat[  3 ],
00201           mat[  4 ], mat[  5 ], mat[  6 ], mat[  7 ],
00202           mat[  8 ], mat[  9 ], mat[ 10 ], mat[ 11 ],
00203           mat[ 12 ], mat[ 13 ], mat[ 14 ], mat[ 15 ] );
00204 }
00205 
00206 #endif // __matrix44_hpp__