gml_ElMat4.h

Go to the documentation of this file.
00001 /** @file gml_ElMat4.h
00002  * 
00003  *     Elementar 4x4 matrix, 4x1 and 1x4 vector operations. 
00004  *     Matrices are column-major (columns stored in sequence).
00005  * 
00006  *   Matrix inversion from Cramer's rule technique as reported
00007  *   and implemented in
00008  *   http://www.intel.com/design/pentiumiii/sml/24504301.pdf
00009  * 
00010  *  Copyright (c) 2003 The CLIPS-IMAG Laboratory
00011  * 
00012  *  See the file "license.terms" for information on usage and redistribution
00013  *  of this file, and for a DISCLAIMER OF ALL WARRANTIES.
00014  * 
00015  *  Created on November 30, 2003 by FB
00016  */
00017 #ifndef __GML_ELMAT4__
00018 #define __GML_ELMAT4__
00019 
00020 // system includes
00021 #include <math.h>
00022 
00023 // project includes
00024 #include "gml/base/gml_Errors.h"
00025 
00026 
00027 
00028 
00029 /// gml_EM4_Invert --
00030 ///
00031 ///  inverts the 4x4 matrix <m> and stores the result in the 4x4 matrix <i>.
00032 ///  <m> and <i> MUST point to non overlapping memory.
00033 ///  returns an error if <m> is not invertible.
00034 
00035 template <class T>
00036 gml_TError gml_EM4_Invert (T* m, T* i);
00037 
00038 
00039 
00040 /// gml_EM4_MultVxM --
00041 ///
00042 ///  Multiplies the 1x4 vector <v> to the 4x4 matrix <m> and stores the
00043 ///  result in the 1x4 vector <r>.
00044 
00045 template <class T>
00046 inline
00047 void gml_EM4_MultVxM (T* v, T* m, T* r);
00048 
00049 
00050 
00051 /// gml_EM4_MultMxV --
00052 ///
00053 ///  Multiplies the 4x4 matrix <m> to the 4x1 vector <v> and stores the
00054 ///  result in the 4x1 vector <r>.
00055 
00056 template <class T>
00057 inline
00058 void gml_EM4_MultMxV (T* m, T* v, T* r);
00059 
00060 
00061 
00062 /// gml_EM4_MultMxM --
00063 ///
00064 ///  Multiplies the 4x4 matrix <m1> to the 4x4 matrix <m2> and stores the
00065 ///  result in the 4x4 matrix <r>.
00066 
00067 template <class T>
00068 void gml_EM4_MultMxM (T* m1, T* m2, T* r);
00069 
00070 
00071 
00072 /// gml_EM4_SetToID --
00073 ///
00074 ///  Set the matrix <i> to the identity matrix.
00075 
00076 template <class T>
00077 inline
00078 void gml_EM4_SetToID (T* i);
00079 
00080 
00081 
00082 
00083 
00084 
00085 
00086 
00087 
00088 
00089 
00090 ////////////////////////////////////////////////////////////////////////////////
00091 //                                                                            //
00092 //                         inline function definition                         //
00093 //                                                                            //
00094 ////////////////////////////////////////////////////////////////////////////////
00095 
00096 // gml_EM4_Invert --
00097 
00098 template <class T>
00099 gml_TError gml_EM4_Invert (T* m, T* i)
00100 {
00101   T     t[12];        /* temp array for pairs */
00102   T     s[16];        /* array of transpose source matrix */
00103   T     det;            /* determinant */
00104   int   k;
00105 
00106   /* transpose matrix */
00107   for (k = 0; k < 4; k++) {
00108     s[k]      = m[k*4];
00109     s[k + 4]  = m[k*4 + 1];
00110     s[k + 8]  = m[k*4 + 2];
00111     s[k + 12] = m[k*4 + 3];
00112   }
00113 
00114   /* calculate pairs for first 8 elements (cofactors) */
00115   t[0]  = s[10] * s[15];
00116   t[1]  = s[11] * s[14];
00117   t[2]  = s[9]  * s[15];
00118   t[3]  = s[11] * s[13];
00119   t[4]  = s[9]  * s[14];
00120   t[5]  = s[10] * s[13];
00121   t[6]  = s[8]  * s[15];
00122   t[7]  = s[11] * s[12];
00123   t[8]  = s[8]  * s[14];
00124   t[9]  = s[10] * s[12];
00125   t[10] = s[8]  * s[13];
00126   t[11] = s[9]  * s[12];
00127   
00128   /* calculate first 8 elements (cofactors) */
00129   i[0]  = t[0]*s[5] + t[3]*s[6] + t[4] *s[7];
00130   i[0] -= t[1]*s[5] + t[2]*s[6] + t[5] *s[7];
00131   i[1]  = t[1]*s[4] + t[6]*s[6] + t[9] *s[7];
00132   i[1] -= t[0]*s[4] + t[7]*s[6] + t[8] *s[7];
00133   i[2]  = t[2]*s[4] + t[7]*s[5] + t[10]*s[7];
00134   i[2] -= t[3]*s[4] + t[6]*s[5] + t[11]*s[7];
00135   i[3]  = t[5]*s[4] + t[8]*s[5] + t[11]*s[6];
00136   i[3] -= t[4]*s[4] + t[9]*s[5] + t[10]*s[6];
00137   i[4]  = t[1]*s[1] + t[2]*s[2] + t[5] *s[3];
00138   i[4] -= t[0]*s[1] + t[3]*s[2] + t[4] *s[3];
00139   i[5]  = t[0]*s[0] + t[7]*s[2] + t[8] *s[3];
00140   i[5] -= t[1]*s[0] + t[6]*s[2] + t[9] *s[3];
00141   i[6]  = t[3]*s[0] + t[6]*s[1] + t[11]*s[3];
00142   i[6] -= t[2]*s[0] + t[7]*s[1] + t[10]*s[3];
00143   i[7]  = t[4]*s[0] + t[9]*s[1] + t[10]*s[2];
00144   i[7] -= t[5]*s[0] + t[8]*s[1] + t[11]*s[2];
00145   
00146   /* calculate pairs for second 8 elements (cofactors) */
00147   t[0]  = s[2]*s[7];
00148   t[1]  = s[3]*s[6];
00149   t[2]  = s[1]*s[7];
00150   t[3]  = s[3]*s[5];
00151   t[4]  = s[1]*s[6];
00152   t[5]  = s[2]*s[5];
00153   t[6]  = s[0]*s[7];
00154   t[7]  = s[3]*s[4];
00155   t[8]  = s[0]*s[6];
00156   t[9]  = s[2]*s[4];
00157   t[10] = s[0]*s[5];
00158   t[11] = s[1]*s[4];
00159   
00160   /* calculate second 8 elements (cofactors) */
00161   i[8]  = t[0] *s[13] + t[3] *s[14] + t[4] *s[15];
00162   i[8] -= t[1] *s[13] + t[2] *s[14] + t[5] *s[15];
00163   i[9]  = t[1] *s[12] + t[6] *s[14] + t[9] *s[15];
00164   i[9] -= t[0] *s[12] + t[7] *s[14] + t[8] *s[15];
00165   i[10] = t[2] *s[12] + t[7] *s[13] + t[10]*s[15];
00166   i[10]-= t[3] *s[12] + t[6] *s[13] + t[11]*s[15];
00167   i[11] = t[5] *s[12] + t[8] *s[13] + t[11]*s[14];
00168   i[11]-= t[4] *s[12] + t[9] *s[13] + t[10]*s[14];
00169   i[12] = t[2] *s[10] + t[5] *s[11] + t[1] *s[9];
00170   i[12]-= t[4] *s[11] + t[0] *s[9]  + t[3] *s[10];
00171   i[13] = t[8] *s[11] + t[0] *s[8]  + t[7] *s[10];
00172   i[13]-= t[6] *s[10] + t[9] *s[11] + t[1] *s[8];
00173   i[14] = t[6] *s[9]  + t[11]*s[11] + t[3] *s[8];
00174   i[14]-= t[10]*s[11] + t[2] *s[8]  + t[7] *s[9];
00175   i[15] = t[10]*s[10] + t[4] *s[8]  + t[9] *s[9];
00176   i[15]-= t[8] *s[9]  + t[11]*s[10] + t[5] *s[8];
00177   
00178   /* calculate determinant */
00179   det = s[0]*i[0] + s[1]*i[1] + s[2]*i[2] + s[3]*i[3];
00180   
00181   if (det == (T)0)
00182     return gml_gDivideByZero;
00183 
00184   /* calculate matrix inverse */
00185   det = 1/det;
00186   for (k = 0; k < 16; k++)
00187     i[k] *= det;
00188 
00189   return gml_cNoError;
00190 }
00191 
00192 
00193 
00194 // gml_EM4_MultVxM --
00195 
00196 template <class T>
00197 inline
00198 void gml_EM4_MultVxM (T* v, T* m, T* r)
00199 {
00200   r[0] = v[0]*m[0]  + v[1]*m[1]  + v[2]*m[2]  + v[3]*m[3];
00201   r[1] = v[0]*m[4]  + v[1]*m[5]  + v[2]*m[6]  + v[3]*m[7];
00202   r[2] = v[0]*m[8]  + v[1]*m[9]  + v[2]*m[10] + v[3]*m[11];
00203   r[3] = v[0]*m[12] + v[1]*m[13] + v[2]*m[14] + v[3]*m[15];
00204 }
00205 
00206 
00207 
00208 // gml_EM4_MultMxV --
00209 
00210 template <class T>
00211 inline
00212 void gml_EM4_MultMxV (T* m, T* v, T* r)
00213 {
00214   r[0] = m[0] *v[0] + m[4] *v[1] + m[8] *v[2] + m[12]*v[3];
00215   r[1] = m[1] *v[0] + m[5] *v[1] + m[9] *v[2] + m[13]*v[3];
00216   r[2] = m[2] *v[0] + m[6] *v[1] + m[10]*v[2] + m[14]*v[3];
00217   r[3] = m[3] *v[0] + m[7] *v[1] + m[11]*v[2] + m[15]*v[3];
00218 }
00219 
00220 
00221 
00222 // gml_EM4_MultMxM --
00223 
00224 template <class T>
00225 void gml_EM4_MultMxM (T* m1, T* m2, T* r)
00226 {
00227   r[0]  = m1[0] *m2[0]  + m1[4] *m2[1]  + m1[8] *m2[2]  + m1[12]*m2[3];
00228   r[1]  = m1[1] *m2[0]  + m1[5] *m2[1]  + m1[9] *m2[2]  + m1[13]*m2[3];
00229   r[2]  = m1[2] *m2[0]  + m1[6] *m2[1]  + m1[10]*m2[2]  + m1[14]*m2[3];
00230   r[3]  = m1[3] *m2[0]  + m1[7] *m2[1]  + m1[11]*m2[2]  + m1[15]*m2[3];
00231 
00232   r[4]  = m1[0] *m2[4]  + m1[4] *m2[5]  + m1[8] *m2[6]  + m1[12]*m2[7];
00233   r[5]  = m1[1] *m2[4]  + m1[5] *m2[5]  + m1[9] *m2[6]  + m1[13]*m2[7];
00234   r[6]  = m1[2] *m2[4]  + m1[6] *m2[5]  + m1[10]*m2[6]  + m1[14]*m2[7];
00235   r[7]  = m1[3] *m2[4]  + m1[7] *m2[5]  + m1[11]*m2[6]  + m1[15]*m2[7];
00236 
00237   r[8]  = m1[0] *m2[8]  + m1[4] *m2[9]  + m1[8] *m2[10] + m1[12]*m2[11];
00238   r[9]  = m1[1] *m2[8]  + m1[5] *m2[9]  + m1[9] *m2[10] + m1[13]*m2[11];
00239   r[10] = m1[2] *m2[8]  + m1[6] *m2[9]  + m1[10]*m2[10] + m1[14]*m2[11];
00240   r[11] = m1[3] *m2[8]  + m1[7] *m2[9]  + m1[11]*m2[10] + m1[15]*m2[11];
00241 
00242   r[12] = m1[0] *m2[12] + m1[4] *m2[13] + m1[8] *m2[14] + m1[12]*m2[15];
00243   r[13] = m1[1] *m2[12] + m1[5] *m2[13] + m1[9] *m2[14] + m1[13]*m2[15];
00244   r[14] = m1[2] *m2[12] + m1[6] *m2[13] + m1[10]*m2[14] + m1[14]*m2[15];
00245   r[15] = m1[3] *m2[12] + m1[7] *m2[13] + m1[11]*m2[14] + m1[15]*m2[15];
00246 }
00247 
00248 
00249 
00250 // gml_EM4_SetToID --
00251 
00252 template <class T>
00253 inline
00254 void gml_EM4_SetToID (T* i)
00255 {
00256   *i++ = (T)1;
00257   *i++ = (T)0;
00258   *i++ = (T)0;
00259   *i++ = (T)0;
00260 
00261   *i++ = (T)0;
00262   *i++ = (T)1;
00263   *i++ = (T)0;
00264   *i++ = (T)0;
00265 
00266   *i++ = (T)0;
00267   *i++ = (T)0;
00268   *i++ = (T)1;
00269   *i++ = (T)0;
00270 
00271   *i++ = (T)0;
00272   *i++ = (T)0;
00273   *i++ = (T)0;
00274   *i   = (T)1;
00275 }
00276 
00277 
00278 
00279 
00280 #endif
00281 
00282 
00283