ML Reference
|
00001 // **InsertLicense** code 00002 //===================================================================================== 00004 00009 //===================================================================================== 00010 00011 #ifndef __mlMatrix6_H 00012 #define __mlMatrix6_H 00013 00014 // Include system independency file and project settings. 00015 #ifndef __mlLinearAlgebraSystem_H 00016 #include "mlLinearAlgebraSystem.h" 00017 #endif 00018 #ifndef __mlLinearAlgebraDefs_H 00019 #include "mlLinearAlgebraDefs.h" 00020 #endif 00021 00022 00023 #ifndef __mlFloatingPointMatrix_H 00024 #include "mlFloatingPointMatrix.h" 00025 #endif 00026 00027 #ifndef __mlVector6_H 00028 #include "mlVector6.h" 00029 #endif 00030 00031 // All declarations of this header will be in the ML_LINEAR_ALGEBRA namespace. 00032 ML_LA_START_NAMESPACE 00033 00034 //-------------------------------------------------------------------- 00036 //-------------------------------------------------------------------- 00037 template <class DT> 00038 class Tmat6 : public FloatingPointMatrix<Tvec6<DT>, 6> 00039 { 00040 public: 00041 00043 typedef DT ComponentType; 00044 00045 // Build 6x6 matrix from 36 zero elements. 00046 Tmat6(); 00047 00048 // Build matrix that has the argument at the diagonal values, zero otherwise 00049 Tmat6(const DT diagValue); 00050 00051 // Build the matrix of the six row vectors row0, ..., row5. 00052 Tmat6(const Tvec6<DT> &row0, const Tvec6<DT> &row1, const Tvec6<DT> &row2, 00053 const Tvec6<DT> &row3, const Tvec6<DT> &row4, const Tvec6<DT> &row5); 00054 00055 // Copy constructor from the Tmat6 mat. 00056 Tmat6(const Tmat6<DT> &m); 00057 00058 // Constructor from 36 floats given as array mat, read row by row. 00059 Tmat6(const float mat[36]); 00060 00061 // Constructor from 36 doubles given as array mat, read row by row. 00062 Tmat6(const double mat[36]); 00063 00064 // Initialize all matrix elements explicitly with scalars, 00065 // filling it row by row. 00066 Tmat6(const double in00, const double in01, const double in02, const double in03, const double in04, const double in05, 00067 const double in10, const double in11, const double in12, const double in13, const double in14, const double in15, 00068 const double in20, const double in21, const double in22, const double in23, const double in24, const double in25, 00069 const double in30, const double in31, const double in32, const double in33, const double in34, const double in35, 00070 const double in40, const double in41, const double in42, const double in43, const double in44, const double in45, 00071 const double in50, const double in51, const double in52, const double in53, const double in54, const double in55); 00072 00073 // Copy contents from float array m into *this. m must point to an 00074 // array with at least 36 valid entries, read row by row. 00075 void setValues(const float m[36]); 00076 00077 // Copy contents of *this into float array m. Note that range 00078 // and precision of the float values may not be sufficient for 00079 // higher precision matrix contents. m must point to an array with 00080 // at least 36 accessible entries, written row by row. 00081 void getValues(float m[36]) const; 00082 00083 // Copy contents from double array m into *this. m must point to an 00084 // array with at least 36 valid entries, read row by row. 00085 void setValues(const double m[36]); 00086 00087 // Copy contents of *this into double array m. m must point to an 00088 // array with at least 36 accessible entries, written row by row. 00089 void getValues(double m[36]) const; 00090 00091 // Set diagonal matrix with scale on diagonal. 00092 void setScaleMatrix(const DT scale); 00093 00094 // Return a matrix filled with values val. 00095 static Tmat6<DT> getMat(const double val); 00096 00097 // Set all values to val. 00098 void set(const DT val); 00099 00102 bool operator<(const Tmat6<DT> &) const { return false; } 00103 00104 // Assignment from a Tmat6. 00105 const Tmat6<DT>& operator=(const Tmat6<DT> &m); 00106 00107 // Component wise addition with a Tmat6. 00108 const Tmat6<DT>& operator+=(const Tmat6<DT> &m); 00109 00110 // Component wise subtraction by a Tmat6. 00111 const Tmat6<DT>& operator-=(const Tmat6<DT> &m); 00112 00113 // Multiplication by a scalar constant 00114 const Tmat6<DT>& operator*=(const DT d); 00115 00116 // Division by a scalar constant. 00117 // Division by zero is not handled and must be avoided by caller. 00118 const Tmat6<DT>& operator/=(const DT d); 00119 00120 //------------------------------------------------- 00121 // Special functions 00122 //------------------------------------------------- 00123 // Determine the (sub)determinant of columns given by col1, col2, col3, col4 and col5. 00124 DT determinantLower5(const int col1, const int col2, const int col3, const int col4, const int col5) const; 00125 00126 // Determinant. 00127 DT det() const; 00128 00129 // Returns the inverse. Gauss-Jordan elimination with partial pivoting. 00130 // If a non-NULL Boolean pointer is passed to isInvertible 00131 // then true is returned in *isInvertible in the case of a 00132 // successful inversion or false if the inversion is not possible 00133 // (function return is the identity then). 00134 // If a NULL pointer is passed as isInvertible the matrix must 00135 // be invertible, otherwise errors will occur. 00136 Tmat6<DT> inverse(bool* isInvertible=NULL) const; 00137 00138 // Transpose. 00139 Tmat6 transpose() const; 00140 00141 // Return identity matrix. 00142 static Tmat6 getIdentity(); 00143 00144 // Apply the function fct to each component. 00145 const Tmat6<DT> & apply(MLDblFuncPtr fct); 00146 00147 }; // end of class *Tmat6* 00148 00149 00150 00151 00152 //--------------------------------------------------------------------- 00154 00155 //--------------------------------------------------------------------- 00157 template <class DT> 00158 inline Tmat6<DT>::Tmat6() 00159 { 00160 ML_TRACE_IN_TIME_CRITICAL("Tmat6<DT>::Tmat6()"); 00161 00162 this->v[0] = this->v[1] = this->v[2] = this->v[3] = this->v[4] = this->v[5] = Tvec6<DT>(0); 00163 } 00164 00165 // Construct matrix that has the argument at the diagonal values, zero otherwise 00166 template <class DT> 00167 inline Tmat6<DT>::Tmat6(const DT diagValue) 00168 { 00169 ML_TRACE_IN_TIME_CRITICAL("Tmat6<DT>::Tmat6(diagValue)"); 00170 00171 this->v[0][0] = this->v[1][1] = this->v[2][2] = this->v[3][3] = this->v[4][4] = this->v[5][5] = diagValue; 00172 this->v[1][0] = this->v[2][0] = this->v[3][0] = this->v[4][0] = this->v[5][0] = this->v[2][1] = this->v[3][1] = this->v[4][1] = this->v[5][1] = this->v[3][2] = this->v[4][2] = this->v[5][2] = this->v[4][3] = this->v[5][3] = this->v[5][4] = 0; 00173 this->v[0][1] = this->v[0][2] = this->v[0][3] = this->v[0][4] = this->v[0][5] = this->v[1][2] = this->v[1][3] = this->v[1][4] = this->v[1][5] = this->v[2][3] = this->v[2][4] = this->v[2][5] = this->v[3][4] = this->v[3][5] = this->v[4][5] = 0; 00174 } 00175 00177 template <class DT> 00178 inline Tmat6<DT>::Tmat6(const Tvec6<DT>& row0, const Tvec6<DT>& row1, const Tvec6<DT>& row2, 00179 const Tvec6<DT>& row3, const Tvec6<DT>& row4, const Tvec6<DT>& row5) 00180 { 00181 ML_TRACE_IN_TIME_CRITICAL("Tmat6<DT>::Tmat6(const Tvec6<DT>& row0, ... , const Tvec6<DT>& row5)"); 00182 00183 this->v[0] = row0; 00184 this->v[1] = row1; 00185 this->v[2] = row2; 00186 this->v[3] = row3; 00187 this->v[4] = row4; 00188 this->v[5] = row5; 00189 } 00190 00192 template <class DT> 00193 inline Tmat6<DT>::Tmat6(const Tmat6<DT> &mat) 00194 { 00195 ML_TRACE_IN_TIME_CRITICAL("Tmat6<DT>::Tmat6(const Tmat6<DT> &mat)"); 00196 00197 this->v[0] = mat.v[0]; 00198 this->v[1] = mat.v[1]; 00199 this->v[2] = mat.v[2]; 00200 this->v[3] = mat.v[3]; 00201 this->v[4] = mat.v[4]; 00202 this->v[5] = mat.v[5]; 00203 } 00204 00206 template <class DT> 00207 inline Tmat6<DT>::Tmat6(const float mat[36]) 00208 { 00209 ML_TRACE_IN_TIME_CRITICAL("Tmat6<DT>::Tmat6(const float mat[36])"); 00210 00211 setValues(mat); 00212 } 00213 00215 template <class DT> 00216 inline Tmat6<DT>::Tmat6(const double mat[36]) 00217 { 00218 ML_TRACE_IN_TIME_CRITICAL("Tmat6<DT>::Tmat6(const double mat[36])"); 00219 00220 setValues(mat); 00221 } 00222 00224 template <class DT> 00225 inline Tmat6<DT> Tmat6<DT>::getMat(const double val) 00226 { 00227 ML_TRACE_IN_TIME_CRITICAL("Tmat6<DT>::getMat(const double val)"); 00228 00229 return Tmat6<DT>(val, val, val, val, val, val, 00230 val, val, val, val, val, val, 00231 val, val, val, val, val, val, 00232 val, val, val, val, val, val, 00233 val, val, val, val, val, val, 00234 val, val, val, val, val, val); 00235 } 00236 00238 template <class DT> 00239 inline void Tmat6<DT>::set(DT val) 00240 { 00241 ML_TRACE_IN_TIME_CRITICAL("Tmat6<DT>::set(DT val)"); 00242 00243 this->v[0] = this->v[1] = this->v[2] = this->v[3] = this->v[4] = this->v[5] = Tvec6<DT>(val); 00244 } 00245 00247 template <class DT> 00248 inline const Tmat6<DT> & Tmat6<DT>::operator=(const Tmat6<DT> &m) 00249 { 00250 ML_TRACE_IN_TIME_CRITICAL("Tmat6<DT>::operator=(const Tmat6<DT> &m)"); 00251 00252 if (&m != this){ 00253 this->v[0] = m.v[0]; 00254 this->v[1] = m.v[1]; 00255 this->v[2] = m.v[2]; 00256 this->v[3] = m.v[3]; 00257 this->v[4] = m.v[4]; 00258 this->v[5] = m.v[5]; 00259 } 00260 00261 return *this; 00262 } 00263 00265 template <class DT> 00266 inline const Tmat6<DT> & Tmat6<DT>::operator+=(const Tmat6<DT> &m) 00267 { 00268 ML_TRACE_IN_TIME_CRITICAL("Tmat6<DT>::operator+=(const Tmat6<DT> &m)"); 00269 00270 this->v[0] += m.v[0]; 00271 this->v[1] += m.v[1]; 00272 this->v[2] += m.v[2]; 00273 this->v[3] += m.v[3]; 00274 this->v[4] += m.v[4]; 00275 this->v[5] += m.v[5]; 00276 00277 return *this; 00278 } 00279 00281 template <class DT> 00282 inline const Tmat6<DT> & Tmat6<DT>::operator-=(const Tmat6<DT> &m) 00283 { 00284 ML_TRACE_IN_TIME_CRITICAL("Tmat6<DT>::operator-=(const Tmat6<DT> &m)"); 00285 00286 this->v[0] -= m.v[0]; 00287 this->v[1] -= m.v[1]; 00288 this->v[2] -= m.v[2]; 00289 this->v[3] -= m.v[3]; 00290 this->v[4] -= m.v[4]; 00291 this->v[5] -= m.v[5]; 00292 00293 return *this; 00294 } 00295 00297 template <class DT> 00298 inline const Tmat6<DT> & Tmat6<DT>::operator*=(const DT d) 00299 { 00300 ML_TRACE_IN_TIME_CRITICAL("Tmat6<DT>::operator*=(const DT d)"); 00301 00302 this->v[0] *= d; 00303 this->v[1] *= d; 00304 this->v[2] *= d; 00305 this->v[3] *= d; 00306 this->v[4] *= d; 00307 this->v[5] *= d; 00308 00309 return *this; 00310 } 00311 00313 template <class DT> 00314 inline const Tmat6<DT> & Tmat6<DT>::operator/=(const DT d) 00315 { 00316 ML_TRACE_IN("Tmat6<DT>::operator/=(const DT d)"); 00317 ML_TRY 00318 { 00319 if (MLValueIs0WOM(d)){ ML_CHECK_THROW(0); } 00320 this->v[0] /= d; 00321 this->v[1] /= d; 00322 this->v[2] /= d; 00323 this->v[3] /= d; 00324 this->v[4] /= d; 00325 this->v[5] /= d; 00326 } 00327 ML_CATCH_RETHROW; 00328 return *this; 00329 } 00330 00331 00332 //-------------------------------------------------------------------- 00334 00335 //-------------------------------------------------------------------- 00337 template <class DT> 00338 inline Tmat6<DT> operator-(const Tmat6<DT> &a) 00339 { 00340 ML_TRACE_IN_TIME_CRITICAL("mlMatrix6.h: operator-(const Tmat6<DT> &a)"); 00341 00342 return Tmat6<DT>(a) *= static_cast<DT>(-1.0); 00343 } 00344 00346 template <class DT> 00347 inline Tmat6<DT> operator+(const Tmat6<DT> &a, const Tmat6<DT> &b) 00348 { 00349 ML_TRACE_IN_TIME_CRITICAL("mlMatrix6.h: operator+(const Tmat6<DT> &a, const Tmat6<DT> &b)"); 00350 00351 return Tmat6<DT>(a) += b; 00352 } 00353 00355 template <class DT> 00356 inline Tmat6<DT> operator-(const Tmat6<DT> &a, const Tmat6<DT> &b) 00357 { 00358 ML_TRACE_IN_TIME_CRITICAL("mlMatrix6.h: operator-(const Tmat6<DT> &a, const Tmat6<DT> &b)"); 00359 00360 return Tmat6<DT>(a) -= b; 00361 } 00362 00364 template <class DT> 00365 inline Tmat6<DT> operator*(const Tmat6<DT> &a, const DT d) 00366 { 00367 ML_TRACE_IN_TIME_CRITICAL("mlMatrix6.h: operator*(const Tmat6<DT> &a, const DT d)"); 00368 00369 return Tmat6<DT>(a) *= d; 00370 } 00371 00373 template <class DT> 00374 inline Tmat6<DT> operator*(const DT d, const Tmat6<DT> &a) 00375 { 00376 ML_TRACE_IN_TIME_CRITICAL("mlMatrix6.h: operator*(const DT d, const Tmat6<DT> &a)"); 00377 00378 return Tmat6<DT>(a) *= d; 00379 } 00380 00383 template <class DT> 00384 inline Tmat6<DT> operator/(const Tmat6<DT> &a, const DT d) 00385 { 00386 ML_TRACE_IN("mlMatrix6.h: operator/(const Tmat6<DT> &a, const DT d)"); 00387 00388 Tmat6<DT> divided(a); 00389 ML_TRY 00390 { 00391 ML_CHECK_FLOAT_THROW(d); 00392 divided /= d; 00393 } 00394 ML_CATCH_RETHROW; 00395 return divided; 00396 } 00398 00399 //------------------------------------------------------------------- 00400 // 00402 00403 // 00404 //------------------------------------------------------------------- 00405 00406 //------------------------------------------------------------------- 00409 //------------------------------------------------------------------- 00410 template <class DT> 00411 Tmat6<DT>::Tmat6(const double in00, const double in01, const double in02, const double in03, const double in04, const double in05, 00412 const double in10, const double in11, const double in12, const double in13, const double in14, const double in15, 00413 const double in20, const double in21, const double in22, const double in23, const double in24, const double in25, 00414 const double in30, const double in31, const double in32, const double in33, const double in34, const double in35, 00415 const double in40, const double in41, const double in42, const double in43, const double in44, const double in45, 00416 const double in50, const double in51, const double in52, const double in53, const double in54, const double in55) 00417 { 00418 ML_TRACE_IN("Tmat6<DT>::Tmat6( )"); 00419 ML_TRY 00420 { 00421 this->v[0][0]=static_cast<DT>(in00); this->v[0][1]=static_cast<DT>(in01); this->v[0][2]=static_cast<DT>(in02); this->v[0][3]=static_cast<DT>(in03); this->v[0][4]=static_cast<DT>(in04); this->v[0][5]=static_cast<DT>(in05); 00422 this->v[1][0]=static_cast<DT>(in10); this->v[1][1]=static_cast<DT>(in11); this->v[1][2]=static_cast<DT>(in12); this->v[1][3]=static_cast<DT>(in13); this->v[1][4]=static_cast<DT>(in14); this->v[1][5]=static_cast<DT>(in15); 00423 this->v[2][0]=static_cast<DT>(in20); this->v[2][1]=static_cast<DT>(in21); this->v[2][2]=static_cast<DT>(in22); this->v[2][3]=static_cast<DT>(in23); this->v[2][4]=static_cast<DT>(in24); this->v[2][5]=static_cast<DT>(in25); 00424 this->v[3][0]=static_cast<DT>(in30); this->v[3][1]=static_cast<DT>(in31); this->v[3][2]=static_cast<DT>(in32); this->v[3][3]=static_cast<DT>(in33); this->v[3][4]=static_cast<DT>(in34); this->v[3][5]=static_cast<DT>(in35); 00425 this->v[4][0]=static_cast<DT>(in40); this->v[4][1]=static_cast<DT>(in41); this->v[4][2]=static_cast<DT>(in42); this->v[4][3]=static_cast<DT>(in43); this->v[4][4]=static_cast<DT>(in44); this->v[4][5]=static_cast<DT>(in45); 00426 this->v[5][0]=static_cast<DT>(in50); this->v[5][1]=static_cast<DT>(in51); this->v[5][2]=static_cast<DT>(in52); this->v[5][3]=static_cast<DT>(in53); this->v[5][4]=static_cast<DT>(in54); this->v[5][5]=static_cast<DT>(in55); 00427 } 00428 ML_CATCH_RETHROW; 00429 } 00430 00431 00432 //-------------------------------------------------------------------- 00435 //-------------------------------------------------------------------- 00436 template <class DT> 00437 void Tmat6<DT>::setValues(const float m[36]) 00438 { 00439 ML_TRACE_IN("Tmat6<DT>::setValues( )"); 00440 ML_TRY 00441 { 00442 this->v[0][0] = static_cast<DT>(m[ 0]); this->v[0][1] = static_cast<DT>(m[ 1]); this->v[0][2] = static_cast<DT>(m[ 2]); this->v[0][3] = static_cast<DT>(m[ 3]); this->v[0][4] = static_cast<DT>(m[ 4]); this->v[0][5] = static_cast<DT>(m[ 5]); 00443 this->v[1][0] = static_cast<DT>(m[ 6]); this->v[1][1] = static_cast<DT>(m[ 7]); this->v[1][2] = static_cast<DT>(m[ 8]); this->v[1][3] = static_cast<DT>(m[ 9]); this->v[1][4] = static_cast<DT>(m[10]); this->v[1][5] = static_cast<DT>(m[11]); 00444 this->v[2][0] = static_cast<DT>(m[12]); this->v[2][1] = static_cast<DT>(m[13]); this->v[2][2] = static_cast<DT>(m[14]); this->v[2][3] = static_cast<DT>(m[15]); this->v[2][4] = static_cast<DT>(m[16]); this->v[2][5] = static_cast<DT>(m[17]); 00445 this->v[3][0] = static_cast<DT>(m[18]); this->v[3][1] = static_cast<DT>(m[19]); this->v[3][2] = static_cast<DT>(m[20]); this->v[3][3] = static_cast<DT>(m[21]); this->v[3][4] = static_cast<DT>(m[22]); this->v[3][5] = static_cast<DT>(m[23]); 00446 this->v[4][0] = static_cast<DT>(m[24]); this->v[4][1] = static_cast<DT>(m[25]); this->v[4][2] = static_cast<DT>(m[26]); this->v[4][3] = static_cast<DT>(m[27]); this->v[4][4] = static_cast<DT>(m[28]); this->v[4][5] = static_cast<DT>(m[29]); 00447 this->v[5][0] = static_cast<DT>(m[30]); this->v[5][1] = static_cast<DT>(m[31]); this->v[5][2] = static_cast<DT>(m[32]); this->v[5][3] = static_cast<DT>(m[33]); this->v[5][4] = static_cast<DT>(m[34]); this->v[5][5] = static_cast<DT>(m[35]); 00448 } 00449 ML_CATCH_RETHROW; 00450 } 00451 00452 //-------------------------------------------------------------------- 00457 //-------------------------------------------------------------------- 00458 template <class DT> 00459 void Tmat6<DT>::getValues(float m[36]) const 00460 { 00461 ML_TRACE_IN("Tmat6<DT>::setValues( )"); 00462 ML_TRY 00463 { 00464 m[ 0] = static_cast<float>(this->v[0][0]); m[ 1] = static_cast<float>(this->v[0][1]); m[ 2] = static_cast<float>(this->v[0][2]); m[ 3] = static_cast<float>(this->v[0][3]); m[ 4] = static_cast<float>(this->v[0][4]); m[ 5] = static_cast<float>(this->v[0][5]); 00465 m[ 6] = static_cast<float>(this->v[1][0]); m[ 7] = static_cast<float>(this->v[1][1]); m[ 8] = static_cast<float>(this->v[1][2]); m[ 9] = static_cast<float>(this->v[1][3]); m[10] = static_cast<float>(this->v[1][4]); m[11] = static_cast<float>(this->v[1][5]); 00466 m[12] = static_cast<float>(this->v[2][0]); m[13] = static_cast<float>(this->v[2][1]); m[14] = static_cast<float>(this->v[2][2]); m[15] = static_cast<float>(this->v[2][3]); m[16] = static_cast<float>(this->v[2][4]); m[17] = static_cast<float>(this->v[2][5]); 00467 m[18] = static_cast<float>(this->v[3][0]); m[19] = static_cast<float>(this->v[3][1]); m[20] = static_cast<float>(this->v[3][2]); m[21] = static_cast<float>(this->v[3][3]); m[22] = static_cast<float>(this->v[3][4]); m[23] = static_cast<float>(this->v[3][5]); 00468 m[24] = static_cast<float>(this->v[4][0]); m[25] = static_cast<float>(this->v[4][1]); m[26] = static_cast<float>(this->v[4][2]); m[27] = static_cast<float>(this->v[4][3]); m[28] = static_cast<float>(this->v[4][4]); m[29] = static_cast<float>(this->v[4][5]); 00469 m[30] = static_cast<float>(this->v[5][0]); m[31] = static_cast<float>(this->v[5][1]); m[32] = static_cast<float>(this->v[5][2]); m[33] = static_cast<float>(this->v[5][3]); m[34] = static_cast<float>(this->v[5][4]); m[35] = static_cast<float>(this->v[5][5]); 00470 } 00471 ML_CATCH_RETHROW; 00472 } 00473 00474 //-------------------------------------------------------------------- 00477 //-------------------------------------------------------------------- 00478 template <class DT> 00479 void Tmat6<DT>::setValues(const double m[36]) 00480 { 00481 ML_TRACE_IN("Tmat6<DT>::setValues( )"); 00482 ML_TRY 00483 { 00484 this->v[0][0] = static_cast<DT>(m[ 0]); this->v[0][1] = static_cast<DT>(m[ 1]); this->v[0][2] = static_cast<DT>(m[ 2]); this->v[0][3] = static_cast<DT>(m[ 3]); this->v[0][4] = static_cast<DT>(m[ 4]); this->v[0][5] = static_cast<DT>(m[ 5]); 00485 this->v[1][0] = static_cast<DT>(m[ 6]); this->v[1][1] = static_cast<DT>(m[ 7]); this->v[1][2] = static_cast<DT>(m[ 8]); this->v[1][3] = static_cast<DT>(m[ 9]); this->v[1][4] = static_cast<DT>(m[10]); this->v[1][5] = static_cast<DT>(m[11]); 00486 this->v[2][0] = static_cast<DT>(m[12]); this->v[2][1] = static_cast<DT>(m[13]); this->v[2][2] = static_cast<DT>(m[14]); this->v[2][3] = static_cast<DT>(m[15]); this->v[2][4] = static_cast<DT>(m[16]); this->v[2][5] = static_cast<DT>(m[17]); 00487 this->v[3][0] = static_cast<DT>(m[18]); this->v[3][1] = static_cast<DT>(m[19]); this->v[3][2] = static_cast<DT>(m[20]); this->v[3][3] = static_cast<DT>(m[21]); this->v[3][4] = static_cast<DT>(m[22]); this->v[3][5] = static_cast<DT>(m[23]); 00488 this->v[4][0] = static_cast<DT>(m[24]); this->v[4][1] = static_cast<DT>(m[25]); this->v[4][2] = static_cast<DT>(m[26]); this->v[4][3] = static_cast<DT>(m[27]); this->v[4][4] = static_cast<DT>(m[28]); this->v[4][5] = static_cast<DT>(m[29]); 00489 this->v[5][0] = static_cast<DT>(m[30]); this->v[5][1] = static_cast<DT>(m[31]); this->v[5][2] = static_cast<DT>(m[32]); this->v[5][3] = static_cast<DT>(m[33]); this->v[5][4] = static_cast<DT>(m[34]); this->v[5][5] = static_cast<DT>(m[35]); 00490 } 00491 ML_CATCH_RETHROW; 00492 } 00493 00494 //-------------------------------------------------------------------- 00497 //-------------------------------------------------------------------- 00498 template <class DT> 00499 void Tmat6<DT>::getValues(double m[36]) const 00500 { 00501 ML_TRACE_IN("Tmat6<DT>::setValues( )"); 00502 ML_TRY 00503 { 00504 m[ 0] = static_cast<double>(this->v[0][0]); m[ 1] = static_cast<double>(this->v[0][1]); m[ 2] = static_cast<double>(this->v[0][2]); m[ 3] = static_cast<double>(this->v[0][3]); m[ 4] = static_cast<double>(this->v[0][4]); m[ 5] = static_cast<double>(this->v[0][5]); 00505 m[ 6] = static_cast<double>(this->v[1][0]); m[ 7] = static_cast<double>(this->v[1][1]); m[ 8] = static_cast<double>(this->v[1][2]); m[ 9] = static_cast<double>(this->v[1][3]); m[10] = static_cast<double>(this->v[1][4]); m[11] = static_cast<double>(this->v[1][5]); 00506 m[12] = static_cast<double>(this->v[2][0]); m[13] = static_cast<double>(this->v[2][1]); m[14] = static_cast<double>(this->v[2][2]); m[15] = static_cast<double>(this->v[2][3]); m[16] = static_cast<double>(this->v[2][4]); m[17] = static_cast<double>(this->v[2][5]); 00507 m[18] = static_cast<double>(this->v[3][0]); m[19] = static_cast<double>(this->v[3][1]); m[20] = static_cast<double>(this->v[3][2]); m[21] = static_cast<double>(this->v[3][3]); m[22] = static_cast<double>(this->v[3][4]); m[23] = static_cast<double>(this->v[3][5]); 00508 m[24] = static_cast<double>(this->v[4][0]); m[25] = static_cast<double>(this->v[4][1]); m[26] = static_cast<double>(this->v[4][2]); m[27] = static_cast<double>(this->v[4][3]); m[28] = static_cast<double>(this->v[4][4]); m[29] = static_cast<double>(this->v[4][5]); 00509 m[30] = static_cast<double>(this->v[5][0]); m[31] = static_cast<double>(this->v[5][1]); m[32] = static_cast<double>(this->v[5][2]); m[33] = static_cast<double>(this->v[5][3]); m[34] = static_cast<double>(this->v[5][4]); m[35] = static_cast<double>(this->v[5][5]); 00510 } 00511 ML_CATCH_RETHROW; 00512 } 00513 00514 00515 //------------------------------------------------------------------- 00517 //------------------------------------------------------------------- 00518 template <class DT> 00519 void Tmat6<DT>::setScaleMatrix(const DT scale) 00520 { 00521 ML_TRACE_IN("Tmat6<DT>::setScaleMatrix( )"); 00522 ML_TRY 00523 { 00524 this->v[0][0] = scale; this->v[0][1] = 0; this->v[0][2] = 0; this->v[0][3] = 0; this->v[0][4] = 0; this->v[0][5] = 0; 00525 this->v[1][0] = 0; this->v[1][1] = scale; this->v[1][2] = 0; this->v[1][3] = 0; this->v[1][4] = 0; this->v[1][5] = 0; 00526 this->v[2][0] = 0; this->v[2][1] = 0; this->v[2][2] = scale; this->v[2][3] = 0; this->v[2][4] = 0; this->v[2][5] = 0; 00527 this->v[3][0] = 0; this->v[3][1] = 0; this->v[3][2] = 0; this->v[3][3] = scale; this->v[3][4] = 0; this->v[3][5] = 0; 00528 this->v[4][0] = 0; this->v[4][1] = 0; this->v[4][2] = 0; this->v[4][3] = 0; this->v[4][4] = scale; this->v[4][5] = 0; 00529 this->v[5][0] = 0; this->v[5][1] = 0; this->v[5][2] = 0; this->v[5][3] = 0; this->v[5][4] = 0; this->v[5][5] = scale; 00530 } 00531 ML_CATCH_RETHROW; 00532 } 00534 00535 //------------------------------------------------------------------- 00537 00538 //------------------------------------------------------------------- 00539 00540 //-------------------------------------------------------------------- 00542 //-------------------------------------------------------------------- 00543 template <class DT> 00544 DT Tmat6<DT>::determinantLower5(const int col1, const int col2, const int col3, const int col4, const int col5) const 00545 { 00546 ML_TRACE_IN("Tmat6<DT>::determinantLower5()"); 00547 00548 DT determ = 0; 00549 ML_TRY 00550 { 00551 /* Determinant of 5x5 matrix with given entries */ 00552 determ = Tmat5<DT>(this->v[1][col1], this->v[1][col2], this->v[1][col3], this->v[1][col4], this->v[1][col5], 00553 this->v[2][col1], this->v[2][col2], this->v[2][col3], this->v[2][col4], this->v[2][col5], 00554 this->v[3][col1], this->v[3][col2], this->v[3][col3], this->v[3][col4], this->v[3][col5], 00555 this->v[4][col1], this->v[4][col2], this->v[4][col3], this->v[4][col4], this->v[4][col5], 00556 this->v[5][col1], this->v[5][col2], this->v[5][col3], this->v[5][col4], this->v[5][col5]).det(); 00557 } 00558 ML_CATCH_RETHROW; 00559 return determ; 00560 } 00561 00562 //-------------------------------------------------------------------- 00564 //-------------------------------------------------------------------- 00565 template <class DT> 00566 DT Tmat6<DT>::det() const 00567 { 00568 ML_TRACE_IN("Tmat6<DT>::det()"); 00569 00570 DT determ = 0; 00571 ML_TRY 00572 { 00573 determ = ( this->v[0][0] * determinantLower5(1, 2, 3, 4, 5) 00574 - this->v[0][1] * determinantLower5(0, 2, 3, 4, 5) 00575 + this->v[0][2] * determinantLower5(0, 1, 3, 4, 5) 00576 - this->v[0][3] * determinantLower5(0, 1, 2, 4, 5) 00577 + this->v[0][4] * determinantLower5(0, 1, 2, 3, 5) 00578 - this->v[0][5] * determinantLower5(0, 1, 2, 3, 4)); 00579 } 00580 ML_CATCH_RETHROW; 00581 return determ; 00582 } 00583 00584 //-------------------------------------------------------------------- 00592 //-------------------------------------------------------------------- 00593 template <class DT> 00594 Tmat6<DT> Tmat6<DT>::inverse(bool* isInvertible) const 00595 { 00596 ML_TRACE_IN("Tmat6<DT>::inverse( )"); 00597 00598 Tmat6<DT> retVal; 00599 ML_TRY 00600 { 00601 // Epsilon for comparison with 0 in inversion process. 00602 static const DT Epsilon = static_cast<DT>(10e-12); 00603 00604 // Use helper function from tools to invert the matrix. 00605 retVal = MLInverseMatHelper(*this, 00606 isInvertible, 00607 Epsilon, 00608 "Tmat6<DT> Tmat6<DT>::inverse(bool* isInvertible) const, matrix not invertable", 00609 getIdentity(), 00610 6); 00611 } 00612 ML_CATCH_RETHROW; 00613 return retVal; 00614 } 00615 00616 00617 //------------------------------------------------------------------- 00619 //------------------------------------------------------------------- 00620 template <class DT> 00621 Tmat6<DT> Tmat6<DT>::transpose() const 00622 { 00623 ML_TRACE_IN("Tmat6<DT>::transpose()"); 00624 00625 return Tmat6<DT>(Tvec6<DT>(this->v[0][0], this->v[1][0], this->v[2][0], this->v[3][0], this->v[4][0], this->v[5][0]), 00626 Tvec6<DT>(this->v[0][1], this->v[1][1], this->v[2][1], this->v[3][1], this->v[4][1], this->v[5][1]), 00627 Tvec6<DT>(this->v[0][2], this->v[1][2], this->v[2][2], this->v[3][2], this->v[4][2], this->v[5][2]), 00628 Tvec6<DT>(this->v[0][3], this->v[1][3], this->v[2][3], this->v[3][3], this->v[4][3], this->v[5][3]), 00629 Tvec6<DT>(this->v[0][4], this->v[1][4], this->v[2][4], this->v[3][4], this->v[4][4], this->v[5][4]), 00630 Tvec6<DT>(this->v[0][5], this->v[1][5], this->v[2][5], this->v[3][5], this->v[4][5], this->v[5][5])); 00631 } 00632 00633 //------------------------------------------------------------------- 00635 //------------------------------------------------------------------- 00636 template <class DT> 00637 Tmat6<DT> Tmat6<DT>::getIdentity() 00638 { 00639 ML_TRACE_IN("Tmat6<DT>::getIdentity()"); 00640 00641 return Tmat6<DT>(Tvec6<DT>(1,0,0,0,0,0), 00642 Tvec6<DT>(0,1,0,0,0,0), 00643 Tvec6<DT>(0,0,1,0,0,0), 00644 Tvec6<DT>(0,0,0,1,0,0), 00645 Tvec6<DT>(0,0,0,0,1,0), 00646 Tvec6<DT>(0,0,0,0,0,1)); 00647 } 00648 00649 //------------------------------------------------------------------- 00651 //------------------------------------------------------------------- 00652 template <class DT> 00653 const Tmat6<DT> & Tmat6<DT>::apply(MLDblFuncPtr fct) 00654 { 00655 ML_TRACE_IN("Tmat6<DT>::apply(MLDblFuncPtr fct)"); 00656 ML_TRY 00657 { 00658 this->v[0].apply(fct); 00659 this->v[1].apply(fct); 00660 this->v[2].apply(fct); 00661 this->v[3].apply(fct); 00662 this->v[4].apply(fct); 00663 this->v[5].apply(fct); 00664 } 00665 ML_CATCH_RETHROW; 00666 return *this; 00667 } 00668 00669 00671 #define _ML_MAT6_RC(i, j) a[i][0]*b[0][j] + a[i][1]*b[1][j] + a[i][2]*b[2][j] + \ 00672 a[i][3]*b[3][j] + a[i][4]*b[4][j] + a[i][5]*b[5][j] 00673 //------------------------------------------------------------------- 00675 //------------------------------------------------------------------- 00676 template <class DT> 00677 Tmat6<DT> operator*(const Tmat6<DT> &a, const Tmat6<DT> &b) 00678 { 00679 ML_TRACE_IN_TIME_CRITICAL("mlMmat6.h: operator*(const Tmat6<DT> &a, const Tmat6<DT> &b)"); 00680 00681 00682 return Tmat6<DT>(Tvec6<DT>(_ML_MAT6_RC(0,0), _ML_MAT6_RC(0,1), _ML_MAT6_RC(0,2), _ML_MAT6_RC(0,3), _ML_MAT6_RC(0,4), _ML_MAT6_RC(0,5)), 00683 Tvec6<DT>(_ML_MAT6_RC(1,0), _ML_MAT6_RC(1,1), _ML_MAT6_RC(1,2), _ML_MAT6_RC(1,3), _ML_MAT6_RC(1,4), _ML_MAT6_RC(1,5)), 00684 Tvec6<DT>(_ML_MAT6_RC(2,0), _ML_MAT6_RC(2,1), _ML_MAT6_RC(2,2), _ML_MAT6_RC(2,3), _ML_MAT6_RC(2,4), _ML_MAT6_RC(2,5)), 00685 Tvec6<DT>(_ML_MAT6_RC(3,0), _ML_MAT6_RC(3,1), _ML_MAT6_RC(3,2), _ML_MAT6_RC(3,3), _ML_MAT6_RC(3,4), _ML_MAT6_RC(3,5)), 00686 Tvec6<DT>(_ML_MAT6_RC(4,0), _ML_MAT6_RC(4,1), _ML_MAT6_RC(4,2), _ML_MAT6_RC(4,3), _ML_MAT6_RC(4,4), _ML_MAT6_RC(4,5)), 00687 Tvec6<DT>(_ML_MAT6_RC(5,0), _ML_MAT6_RC(5,1), _ML_MAT6_RC(5,2), _ML_MAT6_RC(5,3), _ML_MAT6_RC(5,4), _ML_MAT6_RC(5,5))); 00688 } 00689 #undef _ML_MAT6_RC 00690 00691 //------------------------------------------------------------------- 00693 //------------------------------------------------------------------- 00694 template <class DT> 00695 bool operator==(const Tmat6<DT> &a, const Tmat6<DT> &b) 00696 { 00697 ML_TRACE_IN_TIME_CRITICAL("mlMmat6.h: operator==(const Tmat6<DT> &a, const Tmat6<DT> &b)"); 00698 00699 return (a[0] == b[0]) && 00700 (a[1] == b[1]) && 00701 (a[2] == b[2]) && 00702 (a[3] == b[3]) && 00703 (a[4] == b[4]) && 00704 (a[5] == b[5]); 00705 } 00706 00707 //------------------------------------------------------------------- 00709 //------------------------------------------------------------------- 00710 template <class DT> 00711 bool operator!=(const Tmat6<DT> &a, const Tmat6<DT> &b) 00712 { 00713 ML_TRACE_IN_TIME_CRITICAL("mlMmat6.h: operator!=(const Tmat6<DT> &a, const Tmat6<DT> &b)"); 00714 00715 return !(a == b); 00716 } 00717 00719 #define _ML_MAT6_RC(i) a[i][0]*v[0] + a[i][1]*v[1] + a[i][2]*v[2] + a[i][3]*v[3] + a[i][4]*v[4] + a[i][5]*v[5] 00720 //----------------------------------------------------------------------------------- 00722 //----------------------------------------------------------------------------------- 00723 template <class DT> 00724 inline Tvec6<DT> operator*(const Tmat6<DT>& a, const Tvec6<DT>& v) 00725 { 00726 ML_TRACE_IN_TIME_CRITICAL("mlMmat6.h: operator*(const Tmat6<DT>& a, const Tvec6<DT>& v)"); 00727 00728 return Tvec6<DT>(_ML_MAT6_RC(0), _ML_MAT6_RC(1), _ML_MAT6_RC(2), _ML_MAT6_RC(3), _ML_MAT6_RC(4), _ML_MAT6_RC(5)); 00729 } 00730 #undef _ML_MAT6_RC 00731 00732 //----------------------------------------------------------------------------------- 00734 //----------------------------------------------------------------------------------- 00735 template <class DT> 00736 inline Tvec6<DT> operator*(const Tvec6<DT>& v, const Tmat6<DT>& a) 00737 { 00738 ML_TRACE_IN_TIME_CRITICAL("mlMmat6.h: operator*(const Tvec6<DT>& v, const Tmat6<DT>& a)"); 00739 00740 return a.transpose() * v; 00741 } 00743 00744 00745 //----------------------------------------------------------------------------------- 00747 00748 //----------------------------------------------------------------------------------- 00749 00751 typedef Tmat6<MLfloat> Matrix6f; 00753 typedef Tmat6<MLdouble> Matrix6d; 00755 typedef Tmat6<MLldouble> Matrix6ld; 00757 typedef Tmat6<MLdouble> Matrix6; 00759 00760 00761 #ifdef ML_DEPRECATED 00762 00764 00765 00769 ML_DEPRECATED typedef Tmat6<MLfloat> matf6; 00773 ML_DEPRECATED typedef Tmat6<MLdouble> matd6; 00777 ML_DEPRECATED typedef Tmat6<MLldouble> matld6; 00781 ML_DEPRECATED typedef Tmat6<MLdouble> mat6; 00783 00784 00785 #endif // ML_DEPRECATED 00786 00787 00788 00789 ML_LA_END_NAMESPACE 00790 00791 namespace std 00792 { 00793 //------------------------------------------------------------------- 00795 //------------------------------------------------------------------- 00796 template <class DT> 00797 inline std::ostream& operator<<(std::ostream& os, const ML_LA_NAMESPACE::Tmat6<DT> & m) 00798 { 00799 return os << m[0] << '\n' << m[1] << '\n' << m[2] << '\n' << m[3] << '\n' << m[4] << '\n' << m[5]; 00800 } 00801 00802 //------------------------------------------------------------------- 00804 //------------------------------------------------------------------- 00805 template <class DT> 00806 inline std::istream& operator>>(std::istream& is, ML_LA_NAMESPACE::Tmat6<DT> & m) 00807 { 00808 ML_LA_NAMESPACE::Tmat6<DT> m_tmp; 00809 00810 is >> m_tmp[0] >> m_tmp[1] >> m_tmp[2] >> m_tmp[3] >> m_tmp[4] >> m_tmp[5]; 00811 if (is){ m = m_tmp; } 00812 return is; 00813 } 00814 } 00815 00816 00817 #endif // __mlMatrix6_H 00818 00819