• Main Page
  • Related Pages
  • Modules
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

spl/math/Matrix.h

00001 /*
00002  *   This file is part of the Standard Portable Library (SPL).
00003  *
00004  *   SPL is free software: you can redistribute it and/or modify
00005  *   it under the terms of the GNU General Public License as published by
00006  *   the Free Software Foundation, either version 3 of the License, or
00007  *   (at your option) any later version.
00008  *
00009  *   SPL is distributed in the hope that it will be useful,
00010  *   but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  *   GNU General Public License for more details.
00013  *
00014  *   You should have received a copy of the GNU General Public License
00015  *   along with SPL.  If not, see <http://www.gnu.org/licenses/>.
00016  */
00017 #ifndef _matrix4_h
00018 #define _matrix4_h
00019 
00020 #include <spl/types.h>
00021 #include <spl/Debug.h>
00022 #include <spl/Memory.h>
00023 #include <spl/math/Vector.h>
00024 
00031 template<typename T, typename FLOATTYPE>
00032 class Matrix3 : public IMemoryValidate
00033 {
00034 protected:
00035         T m00, m01, m02;
00036         T m10, m11, m12;
00037         T m20, m21, m22;
00038 
00039 public:
00040         friend template Vector3<typename T, typename FLOATTYPE>;
00041 
00042         Matrix3()
00043         {
00044                 SetIdentity();
00045         }
00046 
00047         Matrix3(const Matrix3& m)
00048         :       m00(m.m00), m01(m.m01), m02(m.m02),
00049                 m10(m.m10), m11(m.m11), m12(m.m12),
00050                 m20(m.m20), m21(m.m21), m22(m.m22)
00051         {
00052         }
00053 
00054         Matrix3(Vector3<T,FLOATTYPE> *right, Vector3<T,FLOATTYPE> * up, Vector3<T,FLOATTYPE> * forward)
00055         {
00056                 m00 = right->X();
00057                 m10 = right->Y();
00058                 m20 = right->Z();
00059                 m01 = up->X();
00060                 m11 = up->Y();
00061                 m21 = up->Z();
00062                 m02 = forward->X();
00063                 m12 = forward->Y();
00064                 m22 = forward->Z();
00065         }
00066 
00068         static Matrix3<T,FLOATTYPE> ScaleMatrix(FLOATTYPE dx, FLOATTYPE dy, FLOATTYPE dz)
00069         {
00070                 Matrix3<T,FLOATTYPE> m;
00071                 m.m00 = dx;
00072                 m.m11 = dy;
00073                 m.m22 = dz;
00074                 return m;
00075         }
00076 
00078         static Matrix3<T,FLOATTYPE> ScaleMatrix(FLOATTYPE d)
00079         {
00080                 return ScaleMatrix(d, d, d);
00081         }
00082 
00083         void Shift(FLOATTYPE dx, FLOATTYPE dy, FLOATTYPE dz)
00084         {
00085                 Matrix3<T,FLOATTYPE> m = ShiftMatrix(dx, dy, dz);
00086                 Transform( &m );
00087         }
00088 
00089         void Scale(FLOATTYPE dx, FLOATTYPE dy, FLOATTYPE dz)
00090         {
00091                 Matrix3<T,FLOATTYPE> m = ScaleMatrix(dx, dy, dz);
00092                 Transform( &m );
00093         }
00094 
00095         void Scale(FLOATTYPE d)
00096         {
00097                 Matrix4<T,FLOATTYPE> m = ScaleMatrix(d);
00098                 Transform( &m );
00099         }
00100 
00101         void Rotate(FLOATTYPE dx, FLOATTYPE dy, FLOATTYPE dz)
00102         {
00103                 Matrix3<T,FLOATTYPE> m = RotateMatrix(dx, dy, dz);
00104                 Transform( &m );
00105         }
00106 
00107         void ScaleSelf(FLOATTYPE dx, FLOATTYPE dy, FLOATTYPE dz)
00108         {
00109                 Matrix3<T,FLOATTYPE> m = ScaleMatrix(dx, dy, dz);
00110                 PreTransform( &m );
00111         }
00112 
00113         void ScaleSelf(FLOATTYPE d)
00114         {
00115                 Matrix3<T,FLOATTYPE> m = ScaleMatrix(d);
00116                 PreTransform( &m );
00117         }
00118 
00119         void RotateSelf(FLOATTYPE dx, FLOATTYPE dy, FLOATTYPE dz)
00120         {
00121                 Matrix3<T,FLOATTYPE> m = RotateMatrix(dx, dy, dz);
00122                 PreTransform( &m );
00123         }
00124 
00126         static Matrix3<T,FLOATTYPE> RotateMatrix(FLOATTYPE dx, FLOATTYPE dy, FLOATTYPE dz)
00127         {
00128                 Matrix3<T,FLOATTYPE> out;
00129                 FLOATTYPE SIN;
00130                 FLOATTYPE COS;
00131 
00132                 if (dx != 0)
00133                 {
00134                         Matrix3<T,FLOATTYPE> m;
00135                         SIN = FastMath::Sin(dx);
00136                         COS = FastMath::Cos(dx);
00137                         m.m11 = (T)COS;
00138                         m.m12 = (T)SIN;
00139                         m.m21 = (T)-SIN;
00140                         m.m22 = (T)COS;
00141                         out.Transform(&m);
00142                 }
00143                 if (dy != 0)
00144                 {
00145                         Matrix3<T,FLOATTYPE> m;
00146                         SIN = FastMath::Sin(dy);
00147                         COS = FastMath::Cos(dy);
00148                         m.m00 = (T)COS;
00149                         m.m02 = (T)SIN;
00150                         m.m20 = (T)-SIN;
00151                         m.m22 = (T)COS;
00152                         out.Transform(&m);
00153                 }
00154                 if (dz != 0)
00155                 {
00156                         Matrix3<T,FLOATTYPE> m;
00157                         SIN = FastMath::Sin(dz);
00158                         COS = FastMath::Cos(dz);
00159                         m.m00 = (T)COS;
00160                         m.m01 = (T)SIN;
00161                         m.m10 = (T)-SIN;
00162                         m.m11 = (T)COS;
00163                         out.Transform(&m);
00164                 }
00165                 return out;
00166         }
00167 
00169         void Transform( Matrix3<T,FLOATTYPE> *n )
00170         {
00171                 Matrix3<T,FLOATTYPE> m = *this;
00172 
00173                 m00 = n->m00*m.m00 + n->m01*m.m10 + n->m02*m.m20;
00174                 m01 = n->m00*m.m01 + n->m01*m.m11 + n->m02*m.m21;
00175                 m02 = n->m00*m.m02 + n->m01*m.m12 + n->m02*m.m22;
00176                 m10 = n->m10*m.m00 + n->m11*m.m10 + n->m12*m.m20;
00177                 m11 = n->m10*m.m01 + n->m11*m.m11 + n->m12*m.m21;
00178                 m12 = n->m10*m.m02 + n->m11*m.m12 + n->m12*m.m22;
00179                 m20 = n->m20*m.m00 + n->m21*m.m10 + n->m22*m.m20;
00180                 m21 = n->m20*m.m01 + n->m21*m.m11 + n->m22*m.m21;
00181                 m22 = n->m20*m.m02 + n->m21*m.m12 + n->m22*m.m22;
00182         }
00183 
00185         void PreTransform( Matrix3<T,FLOATTYPE> *n )
00186         {
00187                 Matrix3<T,FLOATTYPE> m = *this;
00188 
00189                 m00 = m.m00*n->m00 + m.m01*n->m10 + m.m02*n->m20;
00190                 m01 = m.m00*n->m01 + m.m01*n->m11 + m.m02*n->m21;
00191                 m02 = m.m00*n->m02 + m.m01*n->m12 + m.m02*n->m22;
00192                 m10 = m.m10*n->m00 + m.m11*n->m10 + m.m12*n->m20;
00193                 m11 = m.m10*n->m01 + m.m11*n->m11 + m.m12*n->m21;
00194                 m12 = m.m10*n->m02 + m.m11*n->m12 + m.m12*n->m22;
00195                 m20 = m.m20*n->m00 + m.m21*n->m10 + m.m22*n->m20;
00196                 m21 = m.m20*n->m01 + m.m21*n->m11 + m.m22*n->m21;
00197                 m22 = m.m20*n->m02 + m.m21*n->m12 + m.m22*n->m22;
00198         }
00199 
00201         static Matrix3<T,FLOATTYPE> Multiply(Matrix3<T,FLOATTYPE> *m1, Matrix3<T,FLOATTYPE> *m2)
00202         {
00203                 Matrix3<T,FLOATTYPE> m;
00204 
00205                 m.m00 = m1->m00*m2->m00 + m1->m01*m2->m10 + m1->m02*m2->m20;
00206                 m.m01 = m1->m00*m2->m01 + m1->m01*m2->m11 + m1->m02*m2->m21;
00207                 m.m02 = m1->m00*m2->m02 + m1->m01*m2->m12 + m1->m02*m2->m22;
00208                 m.m10 = m1->m10*m2->m00 + m1->m11*m2->m10 + m1->m12*m2->m20;
00209                 m.m11 = m1->m10*m2->m01 + m1->m11*m2->m11 + m1->m12*m2->m21;
00210                 m.m12 = m1->m10*m2->m02 + m1->m11*m2->m12 + m1->m12*m2->m22;
00211                 m.m20 = m1->m20*m2->m00 + m1->m21*m2->m10 + m1->m22*m2->m20;
00212                 m.m21 = m1->m20*m2->m01 + m1->m21*m2->m11 + m1->m22*m2->m21;
00213                 m.m22 = m1->m20*m2->m02 + m1->m21*m2->m12 + m1->m22*m2->m22;
00214                 return m;
00215         }
00216 
00217         void MultEqual(Matrix3<T,FLOATTYPE> *m2)
00218         {
00219                 T m00, m01, m02;
00220                 T m10, m11, m12;
00221                 T m20, m21, m22;
00222 
00223                 mm00 = m00*m2->m00 + m01*m2->m10 + m02*m2->m20;
00224                 mm01 = m00*m2->m01 + m01*m2->m11 + m02*m2->m21;
00225                 mm02 = m00*m2->m02 + m01*m2->m12 + m02*m2->m22;
00226                 mm10 = m10*m2->m00 + m11*m2->m10 + m12*m2->m20;
00227                 mm11 = m10*m2->m01 + m11*m2->m11 + m12*m2->m21;
00228                 mm12 = m10*m2->m02 + m11*m2->m12 + m12*m2->m22;
00229                 mm20 = m20*m2->m00 + m21*m2->m10 + m22*m2->m20;
00230                 mm21 = m20*m2->m01 + m21*m2->m11 + m22*m2->m21;
00231                 mm22 = m20*m2->m02 + m21*m2->m12 + m22*m2->m22;
00232 
00233                 m00 = mm00; 
00234                 m01 = mm01; 
00235                 m02 = mm02; 
00236                 m10 = mm10; 
00237                 m11 = mm11; 
00238                 m12 = mm12; 
00239                 m20 = mm20; 
00240                 m21 = mm21; 
00241                 m22 = mm22; 
00242         }
00243 
00245         Matrix3<T,FLOATTYPE> Inverse()
00246         {
00247                 Matrix3<T,FLOATTYPE> m;
00248 
00249                 FLOATTYPE q1 = m12;  FLOATTYPE q6 = m10*m01;  FLOATTYPE q7 = m10*m21;  FLOATTYPE q8 = m02;
00250                 FLOATTYPE q13 = m20*m01;  FLOATTYPE q14 = m20*m11;  FLOATTYPE q21 = m02*m21;  
00251                 FLOATTYPE q25 = m01*m12;  FLOATTYPE q27 = m02*m11;  
00252                 FLOATTYPE q29 = m10*m22;  FLOATTYPE q31 = m20*m12;  
00253                 FLOATTYPE q35 = m00*m22;  FLOATTYPE q37 = m20*m02;  
00254                 FLOATTYPE q41 = m00*m12;  FLOATTYPE q43 = m10*m02;  
00255                 FLOATTYPE q45 = m00*m11;  FLOATTYPE q48 = m00*m21;
00256                 FLOATTYPE q49 = q45*m22-q48*q1-q6*m22+q7*q8;
00257                 FLOATTYPE q50 = q13*q1-q14*q8;
00258                 FLOATTYPE q51 = 1/(q49+q50);
00259 
00260                 m.m00 = (T)((m11*m22-m11-m21*m12+m21+m12-m22)*q51);
00261                 m.m01 = (T)(-(m01*m22-m01-q21)*q51);
00262                 m.m02 = (T)((q25-q27)*q51);
00263                 m.m10 = (T)(-(q29-q31)*q51);
00264                 m.m11 = (T)((q35-q37)*q51);
00265                 m.m12 = (T)(-(q41-q43)*q51);
00266                 m.m20 = (T)((q7-q14)*q51);
00267                 m.m21 = (T)(-(q48-q13)*q51);
00268                 m.m22 = (T)((q45-q6)*q51);
00269 
00270                 return m;
00271         }
00272 
00274         void SetIdentity()
00275         {
00276                 m00=1; m01=0; m02=0;
00277                 m10=0; m11=1; m12=0;
00278                 m20=0; m21=0; m22=1;
00279         }
00280         
00281         Matrix3& operator =(const Matrix3& m)
00282         {
00283                 m00 = m.m00; m01 = m.m01; m02 = m.m02;
00284                 m10 = m.m10; m11 = m.m11; m12 = m.m12;
00285                 m20 = m.m20; m21 = m.m21; m22 = m.m22;
00286                 return *this;
00287         }
00288 
00289         virtual void ValidateMem() const
00290         {
00291         }
00292 
00293         virtual void CheckMem() const
00294         {
00295         }
00296 };
00297 
00299 template<typename T, typename FLOATTYPE>
00300 class Matrix4 : public IMemoryValidate
00301 {
00302 protected:
00303         T m00, m01, m02, m03;
00304         T m10, m11, m12, m13;
00305         T m20, m21, m22, m23;
00306         T m30, m31, m32, m33;
00307 
00308 public:
00309         friend template Vector4<typename T, typename FLOATTYPE>;
00310 
00311         Matrix4()
00312         {
00313                 SetIdentity();
00314         }
00315 
00316         Matrix4(const Matrix4& m)
00317         : m00(m.m00), m01(m.m01), m02(m.m02), m03(m.m03),
00318           m10(m.m10), m11(m.m11), m12(m.m12), m13(m.m13),
00319           m20(m.m20), m21(m.m21), m22(m.m22), m23(m.m23),
00320           m30(m.m30), m31(m.m31), m32(m.m32), m33(m.m33)
00321         {
00322         }
00323 
00324         Matrix4(Vector4<T,FLOATTYPE> *right, Vector4<T,FLOATTYPE> * up, Vector4<T,FLOATTYPE> * forward)
00325         : m03(0), m13(0), m23(0), m33(1)
00326         {
00327                 m00 = right->X();
00328                 m10 = right->Y();
00329                 m20 = right->Z();
00330                 m30 = right->W();
00331                 m01 = up->X();
00332                 m11 = up->Y();
00333                 m21 = up->Z();
00334                 m31 = up->W();
00335                 m02 = forward->X();
00336                 m12 = forward->Y();
00337                 m22 = forward->Z();
00338                 m32 = forward->W();
00339         }
00340 
00342         static Matrix4<T,FLOATTYPE> ShiftMatrix(FLOATTYPE dx, FLOATTYPE dy, FLOATTYPE dz)
00343         {
00344                 Matrix4<T,FLOATTYPE> m;
00345                 m.m03=dx;
00346                 m.m13=dy;
00347                 m.m23=dz;
00348                 return m;
00349         }
00350 
00352         static Matrix4<T,FLOATTYPE> ScaleMatrix(FLOATTYPE dx, FLOATTYPE dy, FLOATTYPE dz)
00353         {
00354                 Matrix4<T,FLOATTYPE> m;
00355                 m.m00=dx;
00356                 m.m11=dy;
00357                 m.m22=dz;
00358                 return m;
00359         }
00360 
00362         static Matrix4<T,FLOATTYPE> ScaleMatrix(FLOATTYPE d)
00363         {
00364                 return ScaleMatrix(d, d, d);
00365         }
00366 
00367         void Shift(FLOATTYPE dx, FLOATTYPE dy, FLOATTYPE dz)
00368         {
00369                 Matrix4<T,FLOATTYPE> m = ShiftMatrix(dx, dy, dz);
00370                 Transform( &m );
00371         }
00372 
00373         void Scale(FLOATTYPE dx, FLOATTYPE dy, FLOATTYPE dz)
00374         {
00375                 Matrix4<T,FLOATTYPE> m = ScaleMatrix(dx, dy, dz);
00376                 Transform( &m );
00377         }
00378 
00379         void Scale(FLOATTYPE d)
00380         {
00381                 Matrix4<T,FLOATTYPE> m = ScaleMatrix(d);
00382                 Transform( &m );
00383         }
00384 
00385         void Rotate(FLOATTYPE dx, FLOATTYPE dy, FLOATTYPE dz)
00386         {
00387                 Matrix4<T,FLOATTYPE> m = RotateMatrix(dx, dy, dz);
00388                 Transform( &m );
00389         }
00390 
00391         void ScaleSelf(FLOATTYPE dx, FLOATTYPE dy, FLOATTYPE dz)
00392         {
00393                 Matrix4<T,FLOATTYPE> m = ScaleMatrix(dx, dy, dz);
00394                 PreTransform( &m );
00395         }
00396 
00397         void ScaleSelf(FLOATTYPE d)
00398         {
00399                 Matrix4<T,FLOATTYPE> m = ScaleMatrix(d);
00400                 PreTransform( &m );
00401         }
00402 
00403         void RotateSelf(FLOATTYPE dx, FLOATTYPE dy, FLOATTYPE dz)
00404         {
00405                 Matrix4<T,FLOATTYPE> m = RotateMatrix(dx, dy, dz);
00406                 PreTransform( &m );
00407         }
00408 
00410         static Matrix4<T,FLOATTYPE> RotateMatrix(FLOATTYPE dx, FLOATTYPE dy, FLOATTYPE dz)
00411         {
00412                 Matrix4<T,FLOATTYPE> out;
00413                 FLOATTYPE SIN;
00414                 FLOATTYPE COS;
00415 
00416                 if (dx != 0)
00417                 {
00418                         Matrix4<T,FLOATTYPE> m;
00419                         SIN = FastMath::Sin(dx);
00420                         COS = FastMath::Cos(dx);
00421                         m.m11 = COS;
00422                         m.m12 = SIN;
00423                         m.m21 = -SIN;
00424                         m.m22 = COS;
00425                         out.Transform(&m);
00426                 }
00427                 if (dy != 0)
00428                 {
00429                         Matrix4<T,FLOATTYPE> m;
00430                         SIN = FastMath::Sin(dy);
00431                         COS = FastMath::Cos(dy);
00432                         m.m00 = COS;
00433                         m.m02 = SIN;
00434                         m.m20 = -SIN;
00435                         m.m22 = COS;
00436                         out.Transform(&m);
00437                 }
00438                 if (dz != 0)
00439                 {
00440                         Matrix4<T,FLOATTYPE> m;
00441                         SIN = FastMath::Sin(dz);
00442                         COS = FastMath::Cos(dz);
00443                         m.m00 = COS;
00444                         m.m01 = SIN;
00445                         m.m10 = -SIN;
00446                         m.m11 = COS;
00447                         out.Transform(&m);
00448                 }
00449                 return out;
00450         }
00451 
00453         void Transform( Matrix4<T,FLOATTYPE> *n )
00454         {
00455                 Matrix4<T,FLOATTYPE> m = *this;
00456 
00457                 m00 = n->m00*m.m00 + n->m01*m.m10 + n->m02*m.m20;
00458                 m01 = n->m00*m.m01 + n->m01*m.m11 + n->m02*m.m21;
00459                 m02 = n->m00*m.m02 + n->m01*m.m12 + n->m02*m.m22;
00460                 m03 = n->m00*m.m03 + n->m01*m.m13 + n->m02*m.m23 + n->m03;
00461                 m10 = n->m10*m.m00 + n->m11*m.m10 + n->m12*m.m20;
00462                 m11 = n->m10*m.m01 + n->m11*m.m11 + n->m12*m.m21;
00463                 m12 = n->m10*m.m02 + n->m11*m.m12 + n->m12*m.m22;
00464                 m13 = n->m10*m.m03 + n->m11*m.m13 + n->m12*m.m23 + n->m13;
00465                 m20 = n->m20*m.m00 + n->m21*m.m10 + n->m22*m.m20;
00466                 m21 = n->m20*m.m01 + n->m21*m.m11 + n->m22*m.m21;
00467                 m22 = n->m20*m.m02 + n->m21*m.m12 + n->m22*m.m22;
00468                 m23 = n->m20*m.m03 + n->m21*m.m13 + n->m22*m.m23 + n->m23;
00469         }
00470 
00472         void PreTransform( Matrix4<T,FLOATTYPE> *n )
00473         {
00474                 Matrix4<T,FLOATTYPE> m = *this;
00475 
00476                 m00 = m.m00*n->m00 + m.m01*n->m10 + m.m02*n->m20;
00477                 m01 = m.m00*n->m01 + m.m01*n->m11 + m.m02*n->m21;
00478                 m02 = m.m00*n->m02 + m.m01*n->m12 + m.m02*n->m22;
00479                 m03 = m.m00*n->m03 + m.m01*n->m13 + m.m02*n->m23 + m.m03;
00480                 m10 = m.m10*n->m00 + m.m11*n->m10 + m.m12*n->m20;
00481                 m11 = m.m10*n->m01 + m.m11*n->m11 + m.m12*n->m21;
00482                 m12 = m.m10*n->m02 + m.m11*n->m12 + m.m12*n->m22;
00483                 m13 = m.m10*n->m03 + m.m11*n->m13 + m.m12*n->m23 + m.m13;
00484                 m20 = m.m20*n->m00 + m.m21*n->m10 + m.m22*n->m20;
00485                 m21 = m.m20*n->m01 + m.m21*n->m11 + m.m22*n->m21;
00486                 m22 = m.m20*n->m02 + m.m21*n->m12 + m.m22*n->m22;
00487                 m23 = m.m20*n->m03 + m.m21*n->m13 + m.m22*n->m23 + m.m23;
00488         }
00489 
00491         static Matrix4<T,FLOATTYPE> Multiply(Matrix4<T,FLOATTYPE> *m1, Matrix4<T,FLOATTYPE> *m2)
00492         {
00493                 Matrix4<T,FLOATTYPE> m;
00494 
00495                 m.m00 = m1->m00*m2->m00 + m1->m01*m2->m10 + m1->m02*m2->m20 + m1->m03*m2->m30;
00496                 m.m01 = m1->m00*m2->m01 + m1->m01*m2->m11 + m1->m02*m2->m21 + m1->m03*m2->m31;
00497                 m.m02 = m1->m00*m2->m02 + m1->m01*m2->m12 + m1->m02*m2->m22 + m1->m03*m2->m32;
00498                 m.m03 = m1->m00*m2->m03 + m1->m01*m2->m13 + m1->m02*m2->m23 + m1->m03*m2->m33;
00499                 m.m10 = m1->m10*m2->m00 + m1->m11*m2->m10 + m1->m12*m2->m20 + m1->m13*m2->m30;
00500                 m.m11 = m1->m10*m2->m01 + m1->m11*m2->m11 + m1->m12*m2->m21 + m1->m13*m2->m31;
00501                 m.m12 = m1->m10*m2->m02 + m1->m11*m2->m12 + m1->m12*m2->m22 + m1->m13*m2->m32;
00502                 m.m13 = m1->m10*m2->m03 + m1->m11*m2->m13 + m1->m12*m2->m23 + m1->m13*m2->m33;
00503                 m.m20 = m1->m20*m2->m00 + m1->m21*m2->m10 + m1->m22*m2->m20 + m1->m23*m2->m30;
00504                 m.m21 = m1->m20*m2->m01 + m1->m21*m2->m11 + m1->m22*m2->m21 + m1->m23*m2->m31;
00505                 m.m22 = m1->m20*m2->m02 + m1->m21*m2->m12 + m1->m22*m2->m22 + m1->m23*m2->m32;
00506                 m.m23 = m1->m20*m2->m03 + m1->m21*m2->m13 + m1->m22*m2->m23 + m1->m23*m2->m33;
00507                 m.m30 = m1->m30*m2->m00 + m1->m31*m2->m10 + m1->m32*m2->m20 + m1->m33*m2->m30;
00508                 m.m31 = m1->m30*m2->m01 + m1->m31*m2->m11 + m1->m32*m2->m21 + m1->m33*m2->m31;
00509                 m.m32 = m1->m30*m2->m02 + m1->m31*m2->m12 + m1->m32*m2->m22 + m1->m33*m2->m32;
00510                 m.m33 = m1->m30*m2->m03 + m1->m31*m2->m13 + m1->m32*m2->m23 + m1->m33*m2->m33;
00511                 return m;
00512         }
00513 
00514         void MultEqual( Matrix4<T,FLOATTYPE> *m2 )
00515         {
00516                 T mm00, mm01, mm02, mm03;
00517                 T mm10, mm11, mm12, mm13;
00518                 T mm20, mm21, mm22, mm23;
00519                 T mm30, mm31, mm32, mm33;
00520 
00521                 mm00 = m00*m2->m00 + m01*m2->m10 + m02*m2->m20 + m03*m2->m30;
00522                 mm01 = m00*m2->m01 + m01*m2->m11 + m02*m2->m21 + m03*m2->m31;
00523                 mm02 = m00*m2->m02 + m01*m2->m12 + m02*m2->m22 + m03*m2->m32;
00524                 mm03 = m00*m2->m03 + m01*m2->m13 + m02*m2->m23 + m03*m2->m33;
00525                 mm10 = m10*m2->m00 + m11*m2->m10 + m12*m2->m20 + m13*m2->m30;
00526                 mm11 = m10*m2->m01 + m11*m2->m11 + m12*m2->m21 + m13*m2->m31;
00527                 mm12 = m10*m2->m02 + m11*m2->m12 + m12*m2->m22 + m13*m2->m32;
00528                 mm13 = m10*m2->m03 + m11*m2->m13 + m12*m2->m23 + m13*m2->m33;
00529                 mm20 = m20*m2->m00 + m21*m2->m10 + m22*m2->m20 + m23*m2->m30;
00530                 mm21 = m20*m2->m01 + m21*m2->m11 + m22*m2->m21 + m23*m2->m31;
00531                 mm22 = m20*m2->m02 + m21*m2->m12 + m22*m2->m22 + m23*m2->m32;
00532                 mm23 = m20*m2->m03 + m21*m2->m13 + m22*m2->m23 + m23*m2->m33;
00533                 mm30 = m30*m2->m00 + m31*m2->m10 + m32*m2->m20 + m33*m2->m30;
00534                 mm31 = m30*m2->m01 + m31*m2->m11 + m32*m2->m21 + m33*m2->m31;
00535                 mm32 = m30*m2->m02 + m31*m2->m12 + m32*m2->m22 + m33*m2->m32;
00536                 mm33 = m30*m2->m03 + m31*m2->m13 + m32*m2->m23 + m33*m2->m33;
00537 
00538                 m00 = mm00; 
00539                 m01 = mm01; 
00540                 m02 = mm02; 
00541                 m03 = mm03;
00542                 m10 = mm10; 
00543                 m11 = mm11; 
00544                 m12 = mm12; 
00545                 m13 = mm13;
00546                 m20 = mm20; 
00547                 m21 = mm21; 
00548                 m22 = mm22; 
00549                 m23 = mm23;
00550                 m30 = mm30; 
00551                 m31 = mm31; 
00552                 m32 = mm32; 
00553                 m33 = mm33;
00554         }
00555 
00557         Matrix4<T,FLOATTYPE> Inverse()
00558         {
00559                 Matrix4<T,FLOATTYPE> m;
00560 
00561                 FLOATTYPE q1 = m12;  FLOATTYPE q6 = m10*m01;  FLOATTYPE q7 = m10*m21;  FLOATTYPE q8 = m02;
00562                 FLOATTYPE q13 = m20*m01;  FLOATTYPE q14 = m20*m11;  FLOATTYPE q21 = m02*m21;  FLOATTYPE q22 = m03*m21;
00563                 FLOATTYPE q25 = m01*m12;  FLOATTYPE q26 = m01*m13;  FLOATTYPE q27 = m02*m11;  FLOATTYPE q28 = m03*m11;
00564                 FLOATTYPE q29 = m10*m22;  FLOATTYPE q30 = m10*m23;  FLOATTYPE q31 = m20*m12;  FLOATTYPE q32 = m20*m13;
00565                 FLOATTYPE q35 = m00*m22;  FLOATTYPE q36 = m00*m23;  FLOATTYPE q37 = m20*m02;  FLOATTYPE q38 = m20*m03;
00566                 FLOATTYPE q41 = m00*m12;  FLOATTYPE q42 = m00*m13;  FLOATTYPE q43 = m10*m02;  FLOATTYPE q44 = m10*m03;
00567                 FLOATTYPE q45 = m00*m11;  FLOATTYPE q48 = m00*m21;
00568                 FLOATTYPE q49 = q45*m22-q48*q1-q6*m22+q7*q8;
00569                 FLOATTYPE q50 = q13*q1-q14*q8;
00570                 FLOATTYPE q51 = 1/(q49+q50);
00571 
00572                 m.m00 = (T)((m11*m22*m33-m11*m23*m32-m21*m12*m33+m21*m13*m32+m31*m12*m23-m31*m13*m22)*q51);
00573                 m.m01 = (T)(-(m01*m22*m33-m01*m23*m32-q21*m33+q22*m32)*q51);
00574                 m.m02 = (T)((q25*m33-q26*m32-q27*m33+q28*m32)*q51);
00575                 m.m03 = (T)(-(q25*m23-q26*m22-q27*m23+q28*m22+q21*m13-q22*m12)*q51);
00576                 m.m10 = (T)(-(q29*m33-q30*m32-q31*m33+q32*m32)*q51);
00577                 m.m11 = (T)((q35*m33-q36*m32-q37*m33+q38*m32)*q51);
00578                 m.m12 = (T)(-(q41*m33-q42*m32-q43*m33+q44*m32)*q51);
00579                 m.m13 = (T)((q41*m23-q42*m22-q43*m23+q44*m22+q37*m13-q38*m12)*q51);
00580                 m.m20 = (T)((q7*m33-q30*m31-q14*m33+q32*m31)*q51);
00581                 m.m21 = (T)(-(q48*m33-q36*m31-q13*m33+q38*m31)*q51);
00582                 m.m22 = (T)((q45*m33-q42*m31-q6*m33+q44*m31)*q51);
00583                 m.m23 = (T)(-(q45*m23-q42*m21-q6*m23+q44*m21+q13*m13-q38*m11)*q51);
00584 
00585                 return m;
00586         }
00587 
00589         void SetIdentity()
00590         {
00591                 m00=1; m01=0; m02=0; m03=0;
00592                 m10=0; m11=1; m12=0; m13=0;
00593                 m20=0; m21=0; m22=1; m23=0;
00594                 m30=0; m31=0; m32=0; m33=1;
00595         }
00596 
00597         Matrix4& operator =(const Matrix4& m)
00598         {
00599                 m00 = m.m00; m01 = m.m01; m02 = m.m02; m03 = m.m03;
00600                 m10 = m.m10; m11 = m.m11; m12 = m.m12; m13 = m.m13;
00601                 m20 = m.m20; m21 = m.m21; m22 = m.m22; m23 = m.m23;
00602                 m30 = m.m30; m31 = m.m31; m32 = m.m32; m33 = m.m33;
00603         }
00604 
00605         virtual void ValidateMem() const
00606         {
00607         }
00608 
00609         virtual void CheckMem() const
00610         {
00611         }
00612 };
00613 
00616 #endif