|
"L. Spiro Engine"
|
F:/My Projects/LSEngine/Modules/LSMathLib/Src/Matrix/LSMMatrix3x3Base.h
00001 00016 #ifndef __LSM_MATRIX3X3BASE_H__ 00017 #define __LSM_MATRIX3X3BASE_H__ 00018 00019 #include "../LSMMathLib.h" 00020 #include "../Vector/LSMVector3Base.h" 00021 00022 namespace lsm { 00023 00030 template <typename _tType, typename _tVector3Type> 00031 class CMatrix3x3Base { 00032 // All is public. This class has no secrets. 00033 public : 00034 // == Various constructors. 00035 LSE_CALLCTOR CMatrix3x3Base() { 00036 } 00037 LSE_CALLCTOR CMatrix3x3Base( const CMatrix3x3Base<_tType, _tVector3Type> &_m33bOther ) : 00038 _11( _m33bOther._11 ), _12( _m33bOther._12 ), _13( _m33bOther._13 ), 00039 _21( _m33bOther._21 ), _22( _m33bOther._22 ), _23( _m33bOther._23 ), 00040 _31( _m33bOther._31 ), _32( _m33bOther._32 ), _33( _m33bOther._33 ) { 00041 } 00042 LSE_CALLCTOR CMatrix3x3Base( const CMatrix3x3Base<_tType, _tVector3Type> &_m33bOther, LSBOOL _bTranspose ) { 00043 if ( !_bTranspose ) { 00044 _11 =_m33bOther._11; _12 =_m33bOther._12; _13 =_m33bOther._13; 00045 _21 =_m33bOther._21; _22 =_m33bOther._22; _23 =_m33bOther._23; 00046 _31 =_m33bOther._31; _32 =_m33bOther._32; _33 =_m33bOther._33; 00047 } 00048 else { 00049 _11 =_m33bOther._11; _12 =_m33bOther._21; _13 =_m33bOther._31; 00050 _21 =_m33bOther._12; _22 =_m33bOther._22; _23 =_m33bOther._32; 00051 _31 =_m33bOther._31; _32 =_m33bOther._32; _33 =_m33bOther._33; 00052 } 00053 } 00054 LSE_CALLCTOR CMatrix3x3Base( const _tType * _ptArray ) { 00055 _11 = _ptArray[0]; 00056 _12 = _ptArray[1]; 00057 _13 = _ptArray[2]; 00058 00059 _21 = _ptArray[3]; 00060 _22 = _ptArray[4]; 00061 _23 = _ptArray[5]; 00062 00063 _31 = _ptArray[6]; 00064 _32 = _ptArray[7]; 00065 _33 = _ptArray[8]; 00066 } 00067 00068 00069 // == Operators. 00078 _tType & LSE_FCALL operator () ( LSUINT32 _ui32Row, LSUINT32 _ui32Col ) { 00079 return reinterpret_cast<_tType *>(this)[(_ui32Row*3UL)+_ui32Col]; 00080 } 00081 00089 _tType LSE_FCALL operator () ( LSUINT32 _ui32Row, LSUINT32 _ui32Col ) const { 00090 return reinterpret_cast<const _tType *>(this)[(_ui32Row*3UL)+_ui32Col]; 00091 } 00092 00099 CMatrix3x3Base<_tType, _tVector3Type> LSE_FCALL operator * ( const CMatrix3x3Base<_tType, _tVector3Type> &_m33bOther ) const { 00100 CMatrix3x3Base<_tType, _tVector3Type> m33bOut; 00101 for ( LSUINT32 I = 3UL; I--; ) { 00102 const _tType * pfBufferIn = &reinterpret_cast<const _tType *>(this)[I*3]; 00103 _tType * pfBufferOut = &reinterpret_cast<_tType *>(&m33bOut)[I*3]; 00104 for ( LSUINT32 J = 3UL; J--; ) { 00105 _tType fSum = LSM_ZERO; 00106 for ( LSUINT32 K = 3UL; K--; ) { 00107 fSum += pfBufferIn[K] * _m33bOther( K, J ); 00108 } 00109 pfBufferOut[J] = fSum; 00110 } 00111 } 00112 00113 return m33bOut; 00114 } 00115 00122 _tVector3Type LSE_FCALL operator * ( const _tVector3Type &_v3bOther ) const { 00123 _tVector3Type v3bRet; 00124 v3bRet.x = (_11 * _v3bOther.x + _21 * _v3bOther.y + 00125 _31 * _v3bOther.z); 00126 v3bRet.y = (_12 * _v3bOther.x + _22 * _v3bOther.y + 00127 _32 * _v3bOther.z); 00128 v3bRet.z = (_13 * _v3bOther.x + _23 * _v3bOther.y + 00129 _33 * _v3bOther.z); 00130 return v3bRet; 00131 } 00132 00139 CMatrix3x3Base<_tType, _tVector3Type> LSE_FCALL operator * ( _tType _tValue ) const { 00140 CMatrix3x3Base<_tType, _tVector3Type> m33bRet; 00141 m33bRet._11 = _11 * _tValue; 00142 m33bRet._12 = _12 * _tValue; 00143 m33bRet._13 = _13 * _tValue; 00144 m33bRet._21 = _21 * _tValue; 00145 m33bRet._22 = _22 * _tValue; 00146 m33bRet._23 = _23 * _tValue; 00147 m33bRet._31 = _31 * _tValue; 00148 m33bRet._32 = _32 * _tValue; 00149 m33bRet._33 = _33 * _tValue; 00150 return m33bRet; 00151 } 00152 00159 CMatrix3x3Base<_tType, _tVector3Type> & LSE_FCALL 00160 operator *= ( _tType _tValue ) { 00161 _11 = _11 * _tValue; 00162 _12 = _12 * _tValue; 00163 _13 = _13 * _tValue; 00164 _21 = _21 * _tValue; 00165 _22 = _22 * _tValue; 00166 _23 = _23 * _tValue; 00167 _31 = _31 * _tValue; 00168 _32 = _32 * _tValue; 00169 _33 = _33 * _tValue; 00170 return (*this); 00171 } 00172 00179 CMatrix3x3Base<_tType, _tVector3Type> & LSE_FCALL 00180 operator += ( const CMatrix3x3Base<_tType, _tVector3Type> &_m33bOther ) { 00181 _11 += _m33bOther._11; 00182 _12 += _m33bOther._12; 00183 _13 += _m33bOther._13; 00184 _21 += _m33bOther._21; 00185 _22 += _m33bOther._22; 00186 _23 += _m33bOther._23; 00187 _31 += _m33bOther._31; 00188 _32 += _m33bOther._32; 00189 _33 += _m33bOther._33; 00190 return (*this); 00191 } 00192 00193 00194 // == Functions. 00200 CMatrix3x3Base<_tType, _tVector3Type> & LSE_FCALL 00201 Identity() { 00202 _11 = _22 = _33 = _tType( 1.0 ); 00203 _12 = _13 = _21 = _23 = _31 = _32 = _tType( 0.0 ); 00204 return (*this); 00205 } 00206 00212 CMatrix3x3Base<_tType, _tVector3Type> & LSE_FCALL 00213 Transpose() { 00214 CMathLib::Swap( _12, _21 ); 00215 CMathLib::Swap( _13, _31 ); 00216 CMathLib::Swap( _23, _32 ); 00217 return (*this); 00218 } 00219 00225 CMatrix3x3Base<_tType, _tVector3Type> Inverse() const { 00226 // Invert a 3-by-3 matrix using cofactors. 00227 CMatrix3x3Base<_tType, _tVector3Type> m33bInv; 00228 00229 m33bInv._11 = 00230 _22 * _33 - _23 * _32; 00231 m33bInv._12 = 00232 _13 * _32 - _12 * _33; 00233 m33bInv._13 = 00234 _12 * _23 - _13 * _22; 00235 m33bInv._21 = 00236 _23 * _31 - _21 * _33; 00237 m33bInv._22 = 00238 _11 * _33 - _13 * _31; 00239 m33bInv._23 = 00240 _13 * _21 - _11 * _23; 00241 m33bInv._31 = 00242 _21 * _32 - _22 * _31; 00243 m33bInv._32 = 00244 _12 * _31 - _11 * _32; 00245 m33bInv._33 = 00246 _11 * _22 - _12 * _21; 00247 00248 _tType fDet = 00249 _11 * m33bInv._11 + 00250 _12 * m33bInv._21 + 00251 _13 * m33bInv._31; 00252 00253 if ( CMathLib::Abs( static_cast<LSREAL>(fDet) ) > LSM_MAT_EPSILON ) { 00254 _tType fInvDet = _tType( 1.0 ) / fDet; 00255 m33bInv._11 *= fInvDet; 00256 m33bInv._12 *= fInvDet; 00257 m33bInv._13 *= fInvDet; 00258 m33bInv._21 *= fInvDet; 00259 m33bInv._22 *= fInvDet; 00260 m33bInv._23 *= fInvDet; 00261 m33bInv._31 *= fInvDet; 00262 m33bInv._32 *= fInvDet; 00263 m33bInv._33 *= fInvDet; 00264 } 00265 else { 00266 m33bInv.Identity(); 00267 } 00268 00269 return m33bInv; 00270 } 00271 00277 _tType LSE_FCALL Determ() const { 00278 CMatrix3x3Base<_tType, _tVector3Type> m33bInv; 00279 00280 m33bInv._11 = 00281 _22 * _33 - _23 * _32; 00282 m33bInv._12 = 00283 _13 * _32 - _12 * _33; 00284 m33bInv._13 = 00285 _12 * _23 - _13 * _22; 00286 m33bInv._21 = 00287 _23 * _31 - _21 * _33; 00288 m33bInv._22 = 00289 _11 * _33 - _13 * _31; 00290 m33bInv._23 = 00291 _13 * _21 - _11 * _23; 00292 m33bInv._31 = 00293 _21 * _32 - _22 * _31; 00294 m33bInv._32 = 00295 _12 * _31 - _11 * _32; 00296 m33bInv._33 = 00297 _11 * _22 - _12 * _21; 00298 00299 return _11 * m33bInv._11 + 00300 _12 * m33bInv._21 + 00301 _13 * m33bInv._31; 00302 } 00303 00310 LSVOID LSE_FCALL SetRow( LSUINT32 _ui32Row, const _tVector3Type &_v3bVec ) { 00311 LSE_UNALIGNED _tType * pfData = &_11; 00312 pfData += (_ui32Row * 3UL); 00313 00314 pfData[0] = _v3bVec.x; 00315 pfData[1] = _v3bVec.y; 00316 pfData[2] = _v3bVec.z; 00317 } 00318 00325 CMatrix3x3Base<_tType, _tVector3Type> & LSE_FCALL 00326 SetSkewSymmetric( const _tVector3Type &_v3bVec ) { 00327 _11 = _22 = _33 = 0; 00328 _21 = -_v3bVec.z; 00329 _31 = _v3bVec.y; 00330 _12 = _v3bVec.z; 00331 _32 = -_v3bVec.x; 00332 _13 = -_v3bVec.y; 00333 _23 = _v3bVec.x; 00334 return (*this); 00335 } 00336 00344 LSVOID LSE_FCALL GetYawPitchRoll( _tType &_tYaw, _tType &_tPitch, _tType &_tRoll ) const { 00345 _tYaw = CMathLib::ATan( static_cast<LSREAL>(_21), static_cast<LSREAL>(_11) ); 00346 _tPitch = CMathLib::ATan( static_cast<LSREAL>(-_31), CMathLib::Sqrt( static_cast<LSREAL>(_32 * _32 + _33 * _33) ) ); 00347 _tRoll = CMathLib::ATan( static_cast<LSREAL>(_32), static_cast<LSREAL>(_33) ); 00348 } 00349 00359 static CMatrix3x3Base<_tType, _tVector3Type> * LSE_FCALL 00360 MatrixMultiply( CMatrix3x3Base<_tType, _tVector3Type> &_m33bOut, const CMatrix3x3Base<_tType, _tVector3Type> &_m33bM1, const CMatrix3x3Base<_tType, _tVector3Type> &_m33bM2 ) { 00361 const CMatrix3x3Base<_tType, _tVector3Type> * LSE_RESTRICT pmSrc1 = &_m33bM1; 00362 const CMatrix3x3Base<_tType, _tVector3Type> * LSE_RESTRICT pmSrc2 = &_m33bM2; 00363 00364 for ( LSINT32 I = 3; --I >= 0; ) { 00365 const _tType * pfBufferIn = &reinterpret_cast<const _tType *>(pmSrc1)[I*3]; 00366 _tType * pfBufferOut = &reinterpret_cast<_tType *>(&_m33bOut)[I*3]; 00367 for ( LSINT32 J = 3; --J >= 0; ) { 00368 _tType tSum = LSM_ZERO; 00369 for ( LSINT32 K = 3; --K >= 0; ) { 00370 tSum += pfBufferIn[K] * (*pmSrc2)( K, J ); 00371 } 00372 pfBufferOut[J] = tSum; 00373 } 00374 } 00375 00376 return &_m33bOut; 00377 } 00378 00386 static LSVOID LSE_FCALL MultiplyVec3ByMat3x3( const CMatrix3x3Base<_tType, _tVector3Type> &_m33bMat, const _tVector3Type &_v3bIn, _tVector3Type &_v3bOut ) { 00387 const _tType * /*LSE_RESTRICT*/ pfIn = reinterpret_cast<const _tType *>(&_v3bIn); 00388 _v3bOut[0] = _m33bMat._11 * pfIn[0] + _m33bMat._21 * pfIn[1] + 00389 _m33bMat._31 * pfIn[2]; 00390 _v3bOut[1] = _m33bMat._12 * pfIn[0] + _m33bMat._22 * pfIn[1] + 00391 _m33bMat._32 * pfIn[2]; 00392 _v3bOut[2] = _m33bMat._13 * pfIn[0] + _m33bMat._23 * pfIn[1] + 00393 _m33bMat._33 * pfIn[2]; 00394 } 00395 00403 static LSVOID LSE_FCALL MultiplyVec3ByMat3x3Transpose( const CMatrix3x3Base<_tType, _tVector3Type> &_m33bMat, const _tVector3Type &_v3bIn, _tVector3Type &_v3bOut ) { 00404 const _tType * /*LSE_RESTRICT*/ pfIn = reinterpret_cast<const _tType *>(&_v3bIn); 00405 _v3bOut[0] = _m33bMat._11 * pfIn[0] + _m33bMat._12 * pfIn[1] + 00406 _m33bMat._13 * pfIn[2]; 00407 _v3bOut[1] = _m33bMat._21 * pfIn[0] + _m33bMat._22 * pfIn[1] + 00408 _m33bMat._23 * pfIn[2]; 00409 _v3bOut[2] = _m33bMat._31 * pfIn[0] + _m33bMat._32 * pfIn[1] + 00410 _m33bMat._33 * pfIn[2]; 00411 } 00412 00413 // == Members. 00414 #pragma pack( push, 1 ) 00415 _tType _11, _12, _13; // Vector 0. 00416 _tType _21, _22, _23; // Vector 1. 00417 _tType _31, _32, _33; // Vector 2. 00418 #pragma pack( pop ) 00419 }; 00420 00421 } // namespace lsm 00422 00423 #endif // __LSM_MATRIX3X3BASE_H__ 00424
