00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026 #ifndef FIFE_UTIL_MATRIX_H
00027 #define FIFE_UTIL_MATRIX_H
00028
00029
00030 #include <cassert>
00031 #include <iostream>
00032
00033
00034
00035
00036
00037
00038
00039
00040
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 = Math<T>::Cos(angle*Math<T>::pi()/180);
00122 T s = Math<T>::Sin(angle*Math<T>::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 inline const T& operator[] (int ind) const {
00227 assert(ind > -1 && ind < 16);
00228 return m[ind];
00229 }
00230
00233 inline Matrix& mult3by3(const Matrix& mat) {
00234 Matrix temp(*this);
00235 m0 = temp.m0*mat.m0+temp.m4*mat.m1+temp.m8*mat.m2;
00236 m4 = temp.m0*mat.m4+temp.m4*mat.m5+temp.m8*mat.m6;
00237 m8 = temp.m0*mat.m8+temp.m4*mat.m9+temp.m8*mat.m10;
00238
00239 m1 = temp.m1*mat.m0+temp.m5*mat.m1+temp.m9*mat.m2;
00240 m5 = temp.m1*mat.m4+temp.m5*mat.m5+temp.m9*mat.m6;
00241 m9 = temp.m1*mat.m8+temp.m5*mat.m9+temp.m9*mat.m10;
00242
00243 m2 = temp.m2*mat.m0+temp.m6*mat.m1+temp.m10*mat.m2;
00244 m6 = temp.m2*mat.m4+temp.m6*mat.m5+temp.m10*mat.m6;
00245 m10 = temp.m2*mat.m8+temp.m6*mat.m9+temp.m10*mat.m10;
00246
00247 m3 = temp.m3*mat.m0+temp.m7*mat.m1+temp.m11*mat.m2;
00248 m7 = temp.m3*mat.m4+temp.m7*mat.m5+temp.m11*mat.m6;
00249 m11 = temp.m3*mat.m8+temp.m7*mat.m9+temp.m11*mat.m10;
00250 return *this;
00251 }
00252
00255 inline Matrix<T>& Rmult4by4(const Matrix<T>& mat) {
00256 Matrix temp(*this);
00257
00258 m0 = mat.m0*temp.m0+mat.m4*temp.m1+mat.m8*temp.m2+mat.m12*temp.m3;
00259 m4 = mat.m0*temp.m4+mat.m4*temp.m5+mat.m8*temp.m6+mat.m12*temp.m7;
00260 m8 = mat.m0*temp.m8+mat.m4*temp.m9+mat.m8*temp.m10+mat.m12*temp.m11;
00261 m12 = mat.m0*temp.m12+mat.m4*temp.m13+mat.m8*temp.m14+mat.m12*temp.m15;
00262
00263 m1 = mat.m1*temp.m0 + mat.m5*temp.m1 + mat.m9*temp.m2+mat.m13*temp.m3;
00264 m5 = mat.m1*temp.m4 + mat.m5*temp.m5 + mat.m9*temp.m6+mat.m13*temp.m7;
00265 m9 = mat.m1*temp.m8 + mat.m5*temp.m9 + mat.m9*temp.m10+mat.m13*temp.m11;
00266 m13 = mat.m1*temp.m12+ mat.m5*temp.m13 + mat.m9*temp.m14+mat.m13*temp.m15;
00267
00268 m2 = mat.m2*temp.m0+mat.m6*temp.m1+mat.m10*temp.m2+mat.m14*temp.m3;
00269 m6 = mat.m2*temp.m4+mat.m6*temp.m5+mat.m10*temp.m6+mat.m14*temp.m7;
00270 m10 = mat.m2*temp.m8+mat.m6*temp.m9+mat.m10*temp.m10+mat.m14*temp.m11;
00271 m14 = mat.m2*temp.m12+mat.m6*temp.m13+mat.m10*temp.m14+mat.m14*temp.m15;
00272
00273 m3 = mat.m3*temp.m0+mat.m7*temp.m1+mat.m11*temp.m2+mat.m15*temp.m3;
00274 m7 = mat.m3*temp.m4+mat.m7*temp.m5+mat.m11*temp.m6+mat.m15*temp.m7;
00275 m11 = mat.m3*temp.m8+mat.m7*temp.m9+mat.m11*temp.m10+mat.m15*temp.m11;
00276 m15 = mat.m3*temp.m12+mat.m7*temp.m13+mat.m11*temp.m14+mat.m15*temp.m15;
00277 return *this;
00278 }
00279
00280
00281 inline Matrix<T>& mult4by4(const Matrix<T>& mat) {
00282 Matrix temp(*this);
00283
00284 m0 = temp.m0*mat.m0+temp.m4*mat.m1+temp.m8*mat.m2+temp.m12*mat.m3;
00285 m4 = temp.m0*mat.m4+temp.m4*mat.m5+temp.m8*mat.m6+temp.m12*mat.m7;
00286 m8 = temp.m0*mat.m8+temp.m4*mat.m9+temp.m8*mat.m10+temp.m12*mat.m11;
00287 m12 = temp.m0*mat.m12+temp.m4*mat.m13+temp.m8*mat.m14+temp.m12*mat.m15;
00288
00289 m1 = temp.m1*mat.m0 + temp.m5*mat.m1 + temp.m9*mat.m2+temp.m13*mat.m3;
00290 m5 = temp.m1*mat.m4 + temp.m5*mat.m5 + temp.m9*mat.m6+temp.m13*mat.m7;
00291 m9 = temp.m1*mat.m8 + temp.m5*mat.m9 + temp.m9*mat.m10+temp.m13*mat.m11;
00292 m13 = temp.m1*mat.m12+ temp.m5*mat.m13 + temp.m9*mat.m14+temp.m13*mat.m15;
00293
00294 m2 = temp.m2*mat.m0+temp.m6*mat.m1+temp.m10*mat.m2+temp.m14*mat.m3;
00295 m6 = temp.m2*mat.m4+temp.m6*mat.m5+temp.m10*mat.m6+temp.m14*mat.m7;
00296 m10 = temp.m2*mat.m8+temp.m6*mat.m9+temp.m10*mat.m10+temp.m14*mat.m11;
00297 m14 = temp.m2*mat.m12+temp.m6*mat.m13+temp.m10*mat.m14+temp.m14*mat.m15;
00298
00299 m3 = temp.m3*mat.m0+temp.m7*mat.m1+temp.m11*mat.m2+temp.m15*mat.m3;
00300 m7 = temp.m3*mat.m4+temp.m7*mat.m5+temp.m11*mat.m6+temp.m15*mat.m7;
00301 m11 = temp.m3*mat.m8+temp.m7*mat.m9+temp.m11*mat.m10+temp.m15*mat.m11;
00302 m15 = temp.m3*mat.m12+temp.m7*mat.m13+temp.m11*mat.m14+temp.m15*mat.m15;
00303 return *this;
00304 }
00305
00306 Matrix& applyRotate(T angle, T x, T y, T z) {
00307 static Matrix<T> temp;
00308 temp.loadRotate(angle,x,y,z);
00309 *this = temp.mult4by4(*this);
00310 return *this;
00311 }
00312
00313
00314 private:
00315 #define cofactor_maker(f1,mj1,mi1, f2,mj2,mi2, f3,mj3,mi3) \
00316 f1*(mj1*mi1-mj2*mi3) + f2*(mj2*mi2-mj3*mi1) + f3*(mj3*mi3-mj1*mi2)
00317
00318 inline T cofactorm0() const {
00319 return cofactor_maker(m5,m10,m15, m6,m11,m13, m7,m9,m14);
00320 }
00321 inline T cofactorm1() const {
00322 return cofactor_maker(m6,m11,m12, m7,m8,m14, m4,m10,m15);
00323 }
00324 inline T cofactorm2() const {
00325 return cofactor_maker(m7,m8,m13, m4,m9,m15, m5,m11,m12);
00326 }
00327 inline T cofactorm3() const {
00328 return cofactor_maker(m4,m9,m14, m5,m10,m12, m6,m8,m13);
00329 }
00330 inline T cofactorm4() const {
00331 return cofactor_maker(m9,m14,m3, m10,m15,m1, m11,m13,m2);
00332 }
00333 inline T cofactorm5() const {
00334 return cofactor_maker(m10,m15,m0, m11,m12,m2, m8,m14,m3);
00335 }
00336 inline T cofactorm6() const {
00337 return cofactor_maker(m11,m12,m1, m8,m13,m3, m9,m15,m0);
00338 }
00339 inline T cofactorm7() const {
00340 return cofactor_maker(m8,m13,m2, m9,m14,m0, m10,m12,m1);
00341 }
00342 inline T cofactorm8() const {
00343 return cofactor_maker(m13,m2,m7, m14,m3,m5, m15,m1,m6);
00344 }
00345 inline T cofactorm9() const {
00346 return cofactor_maker(m14,m13,m4, m15,m0,m6, m12,m2,m7);
00347 }
00348 inline T cofactorm10() const {
00349 return cofactor_maker(m15,m0,m5, m12,m1,m7, m13,m3,m4);
00350 }
00351 inline T cofactorm11() const {
00352 return cofactor_maker(m12,m1,m6, m13,m2,m4, m14,m0,m5);
00353 }
00354 inline T cofactorm12() const {
00355 return cofactor_maker(m1,m6,m11, m2,m7,m9, m3,m5,m10);
00356 }
00357 inline T cofactorm13() const {
00358 return cofactor_maker(m2,m7,m8, m3,m4,m10, m10,m6,m11);
00359 }
00360 inline T cofactorm14() const {
00361 return cofactor_maker(m3,m4,m9, m0,m5,m11, m1,m7,m8);
00362 }
00363 inline T cofactorm15() const {
00364 return cofactor_maker(m0,m5,m10, m1,m6,m8, m2,m4,m9);
00365 }
00366
00367 union {
00368 T m[16];
00369 struct {
00370 T m0,m1,m2,m3,m4,m5,m6,m7,m8,m9,m10,m11,m12,m13,m14,m15;
00371 };
00372 };
00373 };
00374
00375 typedef Matrix<double> DoubleMatrix;
00376 typedef Matrix<int> IntMatrix;
00377
00380 template<typename T>
00381 std::ostream& operator<<(std::ostream& os, const Matrix<T>& m) {
00382
00383 return os << "\n|" << m[0] << "," << m[4] << "," << m[8] << "," << m[12] << "|\n" << \
00384 "|" << m[1] << "," << m[5] << "," << m[9] << "," << m[13] << "|\n" << \
00385 "|" << m[2] << "," << m[6] << "," << m[10] << "," << m[14] << "|\n" << \
00386 "|" << m[3] << "," << m[7] << "," << m[11] << "," << m[15] << "|\n";
00387 }
00388
00389
00390 }
00391
00392 #endif