FIFE 2008.0
|
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 = 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