matrix.h

00001 /***************************************************************************
00002  *   Copyright (C) 2005-2008 by the FIFE team                              *
00003  *   http://www.fifengine.de                                               *
00004  *   This file is part of FIFE.                                            *
00005  *                                                                         *
00006  *   FIFE is free software; you can redistribute it and/or                 *
00007  *   modify it under the terms of the GNU Lesser General Public            *
00008  *   License as published by the Free Software Foundation; either          *
00009  *   version 2.1 of the License, or (at your option) any later version.    *
00010  *                                                                         *
00011  *   This library is distributed in the hope that it will be useful,       *
00012  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00013  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00014  *   Lesser General Public License for more details.                       *
00015  *                                                                         *
00016  *   You should have received a copy of the GNU Lesser General Public      *
00017  *   License along with this library; if not, write to the                 *
00018  *   Free Software Foundation, Inc.,                                       *
00019  *   51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA          *
00020  ***************************************************************************/
00021 /***************************************************************************
00022  * Includes some heavy copying from mathgl-pp project                      *
00023  * (http://sourceforge.net/projects/mathgl-pp/)                            *
00024  ***************************************************************************/
00025 
00026 #ifndef FIFE_UTIL_MATRIX_H
00027 #define FIFE_UTIL_MATRIX_H
00028 
00029 // Standard C++ library includes
00030 #include <cassert>
00031 #include <iostream>
00032 
00033 // Platform specific includes
00034 
00035 // 3rd party library includes
00036 
00037 // FIFE includes
00038 // These includes are split up in two parts, separated by one empty line
00039 // First block: files included from the FIFE root src directory
00040 // Second block: files included from the same folder
00041 #include "util/base/fife_stdint.h"
00042 #include "util/structures/point.h"
00043 
00044 #include "fife_math.h"
00045 
00046 namespace FIFE {
00047 
00048 
00051     template <typename T>
00052     class Matrix {
00053         public:
00054             Matrix<T>() {}
00055             template <typename U> friend class Matrix;
00056             template <typename U> Matrix<T>(const Matrix<U>& mat) {
00057                 memmove(m, mat.m, 16*sizeof(T));
00058             }
00059             ~Matrix() {}
00060 
00063             Matrix inverse() const {
00064                 Matrix ret(adjoint());
00065 
00066                 T determinant = m0*ret[0] + m1*ret[4] + m2*ret[8] + m3*ret[12];
00067                 assert(determinant!=0 && "Singular matrix has no inverse");
00068 
00069                 ret/=determinant;
00070                 return ret;
00071             }
00072 
00075             inline Matrix& operator/= (T val) {
00076                 for (register unsigned i = 0; i < 16; ++i)
00077                     m[i] /= val;
00078                 return *this;
00079             }
00080 
00083             Matrix adjoint() const {
00084                 Matrix ret;
00085 
00086                 ret[0] = cofactorm0();
00087                 ret[1] = -cofactorm4();
00088                 ret[2] = cofactorm8();
00089                 ret[3] = -cofactorm12();
00090 
00091                 ret[4] = -cofactorm1();
00092                 ret[5] = cofactorm5();
00093                 ret[6] = -cofactorm9();
00094                 ret[7] = cofactorm13();
00095 
00096                 ret[8] = cofactorm2();
00097                 ret[9] = -cofactorm6();
00098                 ret[10] = cofactorm10();
00099                 ret[11] = -cofactorm14();
00100 
00101                 ret[12] = -cofactorm3();
00102                 ret[13] = cofactorm7();
00103                 ret[14] = -cofactorm11();
00104                 ret[15] = cofactorm15();
00105 
00106                 return ret;
00107             }
00108 
00109 
00113             inline Matrix& loadRotate(T angle, T x, T y, T z) {
00114                 register T magSqr = x*x + y*y + z*z;
00115                 if (magSqr != 1.0) {
00116                     register T mag = sqrt(magSqr);
00117                     x/=mag;
00118                     y/=mag;
00119                     z/=mag;
00120                 }
00121                 T c = cos(angle*M_PI/180);
00122                 T s = sin(angle*M_PI/180);
00123                 m0 = x*x*(1-c)+c;
00124                 m1 = y*x*(1-c)+z*s;
00125                 m2 = z*x*(1-c)-y*s;
00126                 m3 = 0;
00127 
00128                 m4 = x*y*(1-c)-z*s;
00129                 m5 = y*y*(1-c)+c;
00130                 m6 = z*y*(1-c)+x*s;
00131                 m7 = 0;
00132 
00133                 m8 = x*z*(1-c)+y*s;
00134                 m9 = y*z*(1-c)-x*s;
00135                 m10 = z*z*(1-c)+c;
00136                 m11 = 0;
00137 
00138                 m12 = 0;
00139                 m13 = 0;
00140                 m14 = 0;
00141                 m15 = 1;
00142 
00143                 return *this;
00144             }
00145 
00148             inline Matrix& applyScale(T x, T y, T z) {
00149                 static Matrix<T> temp;
00150                 temp.loadScale(x,y,z);
00151                 *this = temp.mult4by4(*this);
00152                 return  *this;
00153             }
00154 
00157             inline Matrix& loadScale(T x, T y, T z = 1) {
00158                 m0 = x;
00159                 m4 = 0;
00160                 m8  = 0;
00161                 m12 = 0;
00162                 m1 = 0;
00163                 m5 = y;
00164                 m9  = 0;
00165                 m13 = 0;
00166                 m2 = 0;
00167                 m6 = 0;
00168                 m10 = z;
00169                 m14 = 0;
00170                 m3 = 0;
00171                 m7 = 0;
00172                 m11 = 0;
00173                 m15 = 1;
00174 
00175                 return *this;
00176             }
00177 
00180             inline Matrix& applyTranslate(T x, T y, T z) {
00181                 static Matrix<T> temp;
00182                 temp.loadTranslate(x,y,z);
00183                 *this = temp.mult4by4(*this);
00184                 return  *this;
00185             }
00186 
00189             inline Matrix& loadTranslate( const T x, const T y, const T z) {
00190                 m0 = 1;
00191                 m4 = 0;
00192                 m8  = 0;
00193                 m12 = x;
00194                 m1 = 0;
00195                 m5 = 1;
00196                 m9  = 0;
00197                 m13 = y;
00198                 m2 = 0;
00199                 m6 = 0;
00200                 m10 = 1;
00201                 m14 = z;
00202                 m3 = 0;
00203                 m7 = 0;
00204                 m11 = 0;
00205                 m15 = 1;
00206 
00207                 return *this;
00208             }
00209 
00212             inline PointType3D<T> operator* (const PointType3D<T>& vec) {
00213                 return PointType3D<T> (
00214                     vec.x * m0 + vec.y * m4 + vec.z * m8 + m12,
00215                     vec.x * m1 + vec.y * m5 + vec.z * m9 + m13,
00216                     vec.x * m2 + vec.y * m6 + vec.z * m10 + m14
00217                 );
00218             }
00219 
00222             inline T& operator[] (int ind) {
00223                 assert(ind > -1 && ind < 16);
00224                 return m[ind];
00225             }
00226 
00229             inline Matrix& mult3by3(const Matrix& mat) {
00230                 Matrix temp(*this);
00231                 m0 = temp.m0*mat.m0+temp.m4*mat.m1+temp.m8*mat.m2;
00232                 m4 = temp.m0*mat.m4+temp.m4*mat.m5+temp.m8*mat.m6;
00233                 m8 = temp.m0*mat.m8+temp.m4*mat.m9+temp.m8*mat.m10;
00234 
00235                 m1 = temp.m1*mat.m0+temp.m5*mat.m1+temp.m9*mat.m2;
00236                 m5 = temp.m1*mat.m4+temp.m5*mat.m5+temp.m9*mat.m6;
00237                 m9 = temp.m1*mat.m8+temp.m5*mat.m9+temp.m9*mat.m10;
00238 
00239                 m2 = temp.m2*mat.m0+temp.m6*mat.m1+temp.m10*mat.m2;
00240                 m6 = temp.m2*mat.m4+temp.m6*mat.m5+temp.m10*mat.m6;
00241                 m10 = temp.m2*mat.m8+temp.m6*mat.m9+temp.m10*mat.m10;
00242 
00243                 m3 = temp.m3*mat.m0+temp.m7*mat.m1+temp.m11*mat.m2;
00244                 m7 = temp.m3*mat.m4+temp.m7*mat.m5+temp.m11*mat.m6;
00245                 m11 = temp.m3*mat.m8+temp.m7*mat.m9+temp.m11*mat.m10;
00246                 return *this;
00247             }
00248 
00251             inline Matrix<T>& Rmult4by4(const Matrix<T>& mat) {
00252                 Matrix temp(*this);
00253 
00254                 m0 = mat.m0*temp.m0+mat.m4*temp.m1+mat.m8*temp.m2+mat.m12*temp.m3;
00255                 m4 = mat.m0*temp.m4+mat.m4*temp.m5+mat.m8*temp.m6+mat.m12*temp.m7;
00256                 m8 = mat.m0*temp.m8+mat.m4*temp.m9+mat.m8*temp.m10+mat.m12*temp.m11;
00257                 m12 = mat.m0*temp.m12+mat.m4*temp.m13+mat.m8*temp.m14+mat.m12*temp.m15;
00258 
00259                 m1 =  mat.m1*temp.m0 + mat.m5*temp.m1  + mat.m9*temp.m2+mat.m13*temp.m3;
00260                 m5 =  mat.m1*temp.m4 + mat.m5*temp.m5  + mat.m9*temp.m6+mat.m13*temp.m7;
00261                 m9 =  mat.m1*temp.m8 + mat.m5*temp.m9  + mat.m9*temp.m10+mat.m13*temp.m11;
00262                 m13 = mat.m1*temp.m12+ mat.m5*temp.m13 + mat.m9*temp.m14+mat.m13*temp.m15;
00263 
00264                 m2 = mat.m2*temp.m0+mat.m6*temp.m1+mat.m10*temp.m2+mat.m14*temp.m3;
00265                 m6 = mat.m2*temp.m4+mat.m6*temp.m5+mat.m10*temp.m6+mat.m14*temp.m7;
00266                 m10 = mat.m2*temp.m8+mat.m6*temp.m9+mat.m10*temp.m10+mat.m14*temp.m11;
00267                 m14 = mat.m2*temp.m12+mat.m6*temp.m13+mat.m10*temp.m14+mat.m14*temp.m15;
00268 
00269                 m3 = mat.m3*temp.m0+mat.m7*temp.m1+mat.m11*temp.m2+mat.m15*temp.m3;
00270                 m7 = mat.m3*temp.m4+mat.m7*temp.m5+mat.m11*temp.m6+mat.m15*temp.m7;
00271                 m11 = mat.m3*temp.m8+mat.m7*temp.m9+mat.m11*temp.m10+mat.m15*temp.m11;
00272                 m15 = mat.m3*temp.m12+mat.m7*temp.m13+mat.m11*temp.m14+mat.m15*temp.m15;
00273                 return *this;
00274             }
00275 
00276 
00277             inline Matrix<T>& mult4by4(const Matrix<T>& mat) {
00278                 Matrix temp(*this);
00279 
00280                 m0 = temp.m0*mat.m0+temp.m4*mat.m1+temp.m8*mat.m2+temp.m12*mat.m3;
00281                 m4 = temp.m0*mat.m4+temp.m4*mat.m5+temp.m8*mat.m6+temp.m12*mat.m7;
00282                 m8 = temp.m0*mat.m8+temp.m4*mat.m9+temp.m8*mat.m10+temp.m12*mat.m11;
00283                 m12 = temp.m0*mat.m12+temp.m4*mat.m13+temp.m8*mat.m14+temp.m12*mat.m15;
00284 
00285                 m1 =  temp.m1*mat.m0 + temp.m5*mat.m1  + temp.m9*mat.m2+temp.m13*mat.m3;
00286                 m5 =  temp.m1*mat.m4 + temp.m5*mat.m5  + temp.m9*mat.m6+temp.m13*mat.m7;
00287                 m9 =  temp.m1*mat.m8 + temp.m5*mat.m9  + temp.m9*mat.m10+temp.m13*mat.m11;
00288                 m13 = temp.m1*mat.m12+ temp.m5*mat.m13 + temp.m9*mat.m14+temp.m13*mat.m15;
00289 
00290                 m2 = temp.m2*mat.m0+temp.m6*mat.m1+temp.m10*mat.m2+temp.m14*mat.m3;
00291                 m6 = temp.m2*mat.m4+temp.m6*mat.m5+temp.m10*mat.m6+temp.m14*mat.m7;
00292                 m10 = temp.m2*mat.m8+temp.m6*mat.m9+temp.m10*mat.m10+temp.m14*mat.m11;
00293                 m14 = temp.m2*mat.m12+temp.m6*mat.m13+temp.m10*mat.m14+temp.m14*mat.m15;
00294 
00295                 m3 = temp.m3*mat.m0+temp.m7*mat.m1+temp.m11*mat.m2+temp.m15*mat.m3;
00296                 m7 = temp.m3*mat.m4+temp.m7*mat.m5+temp.m11*mat.m6+temp.m15*mat.m7;
00297                 m11 = temp.m3*mat.m8+temp.m7*mat.m9+temp.m11*mat.m10+temp.m15*mat.m11;
00298                 m15 = temp.m3*mat.m12+temp.m7*mat.m13+temp.m11*mat.m14+temp.m15*mat.m15;
00299                 return *this;
00300             }
00301 
00302             Matrix& applyRotate(T angle, T x, T y, T z) {
00303                 static Matrix<T> temp;
00304                 temp.loadRotate(angle,x,y,z);
00305                 *this = temp.mult4by4(*this);
00306                 return  *this;
00307             }
00308 
00309 
00310         private:
00311 #define cofactor_maker(f1,mj1,mi1, f2,mj2,mi2, f3,mj3,mi3) \
00312         f1*(mj1*mi1-mj2*mi3) + f2*(mj2*mi2-mj3*mi1) + f3*(mj3*mi3-mj1*mi2)
00313 
00314             inline T cofactorm0() const {
00315                 return cofactor_maker(m5,m10,m15, m6,m11,m13, m7,m9,m14);
00316             }
00317             inline T cofactorm1() const {
00318                 return cofactor_maker(m6,m11,m12, m7,m8,m14, m4,m10,m15);
00319             }
00320             inline T cofactorm2() const {
00321                 return cofactor_maker(m7,m8,m13, m4,m9,m15, m5,m11,m12);
00322             }
00323             inline T cofactorm3() const {
00324                 return cofactor_maker(m4,m9,m14, m5,m10,m12, m6,m8,m13);
00325             }
00326             inline T cofactorm4() const {
00327                 return cofactor_maker(m9,m14,m3, m10,m15,m1, m11,m13,m2);
00328             }
00329             inline T cofactorm5() const {
00330                 return cofactor_maker(m10,m15,m0, m11,m12,m2, m8,m14,m3);
00331             }
00332             inline T cofactorm6() const {
00333                 return cofactor_maker(m11,m12,m1, m8,m13,m3, m9,m15,m0);
00334             }
00335             inline T cofactorm7() const {
00336                 return cofactor_maker(m8,m13,m2, m9,m14,m0, m10,m12,m1);
00337             }
00338             inline T cofactorm8() const {
00339                 return cofactor_maker(m13,m2,m7, m14,m3,m5, m15,m1,m6);
00340             }
00341             inline T cofactorm9() const {
00342                 return cofactor_maker(m14,m13,m4, m15,m0,m6, m12,m2,m7);
00343             }
00344             inline T cofactorm10() const {
00345                 return cofactor_maker(m15,m0,m5, m12,m1,m7, m13,m3,m4);
00346             }
00347             inline T cofactorm11() const {
00348                 return cofactor_maker(m12,m1,m6, m13,m2,m4, m14,m0,m5);
00349             }
00350             inline T cofactorm12() const {
00351                 return cofactor_maker(m1,m6,m11, m2,m7,m9, m3,m5,m10);
00352             }
00353             inline T cofactorm13() const {
00354                 return cofactor_maker(m2,m7,m8, m3,m4,m10, m10,m6,m11);
00355             }
00356             inline T cofactorm14() const {
00357                 return cofactor_maker(m3,m4,m9, m0,m5,m11, m1,m7,m8);
00358             }
00359             inline T cofactorm15() const {
00360                 return cofactor_maker(m0,m5,m10, m1,m6,m8, m2,m4,m9);
00361             }
00362 
00363             union {
00364                 T m[16];
00365                 struct {
00366                     T m0,m1,m2,m3,m4,m5,m6,m7,m8,m9,m10,m11,m12,m13,m14,m15;
00367                 };
00368             };
00369     };
00370 
00371     typedef Matrix<double> DoubleMatrix;
00372     typedef Matrix<int> IntMatrix;
00373 
00376     template<typename T>
00377     std::ostream& operator<<(std::ostream& os, Matrix<T>& m) {
00378         return os << "\n|" << m[0] << "," << m[4] << "," << m[8] << ","  << m[12] << "|\n" << \
00379                        "|" << m[1] << "," << m[5] << "," << m[9] << ","  << m[13] << "|\n" << \
00380                        "|" << m[2] << "," << m[6] << "," << m[10] << "," << m[14] << "|\n" << \
00381                        "|" << m[3] << "," << m[7] << "," << m[11] << "," << m[15] << "|\n";
00382     }
00383 
00384 
00385 }
00386 
00387 #endif