Mercurial > fife-parpg

comparison engine/core/util/math/matrix.h @ 0:4a0efb7baf70

Find changesets by keywords (author, files, the commit message), revision number or hash, or revset expression.
* Datasets becomes the new trunk and retires after that :-)
author mvbarracuda@33b003aa-7bff-0310-803a-e67f0ece8222
date Sun, 29 Jun 2008 18:44:17 +0000
parents
children 90005975cdbb
comparison
equal deleted inserted replaced
-1:000000000000 0:4a0efb7baf70
1 /***************************************************************************
2 * Copyright (C) 2005-2008 by the FIFE team *
3 * http://www.fifengine.de *
4 * This file is part of FIFE. *
5 * *
6 * FIFE is free software; you can redistribute it and/or modify *
7 * it under the terms of the GNU General Public License as published by *
8 * the Free Software Foundation; either version 2 of the License, or *
9 * (at your option) any later version. *
10 * *
11 * This program is distributed in the hope that it will be useful, *
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of *
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
14 * GNU General Public License for more details. *
15 * *
16 * You should have received a copy of the GNU General Public License *
17 * along with this program; if not, write to the *
18 * Free Software Foundation, Inc., *
19 * 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA *
20 ***************************************************************************/
21 /***************************************************************************
22 * Includes some heavy copying from mathgl-pp project *
23 * (http://sourceforge.net/projects/mathgl-pp/) *
24 ***************************************************************************/
25
26 #ifndef FIFE_UTIL_MATRIX_H
27 #define FIFE_UTIL_MATRIX_H
28
29 // Standard C++ library includes
30 #include <cassert>
31 #include <iostream>
32
33 // Platform specific includes
34
35 // 3rd party library includes
36
37 // FIFE includes
38 // These includes are split up in two parts, separated by one empty line
39 // First block: files included from the FIFE root src directory
40 // Second block: files included from the same folder
41 #include "util/base/fife_stdint.h"
42 #include "util/structures/point.h"
43
44 #include "fife_math.h"
45
46 namespace FIFE {
47
48
49 /** Minimal matrix class to assist in view 3d calculations
50 */
51 template <typename T>
52 class Matrix {
53 public:
54 Matrix<T>() {}
55 template <typename U> friend class Matrix;
56 template <typename U> Matrix<T>(const Matrix<U>& mat) {
57 memmove(m, mat.m, 16*sizeof(T));
58 }
59 ~Matrix() {}
60
61 /** Adjoint method inverse, constant time inversion implementation
62 */
63 Matrix inverse() const {
64 Matrix ret(adjoint());
65
66 T determinant = m0*ret[0] + m1*ret[4] + m2*ret[8] + m3*ret[12];
67 assert(determinant!=0 && "Singular matrix has no inverse");
68
69 ret/=determinant;
70 return ret;
71 }
72
73 /** Divide this matrix by a scalar
74 */
75 inline Matrix& operator/= (T val) {
76 for (register unsigned i = 0; i < 16; ++i)
77 m[i] /= val;
78 return *this;
79 }
80
81 /** Get the adjoint matrix
82 */
83 Matrix adjoint() const {
84 Matrix ret;
85
86 ret[0] = cofactorm0();
87 ret[1] = -cofactorm4();
88 ret[2] = cofactorm8();
89 ret[3] = -cofactorm12();
90
91 ret[4] = -cofactorm1();
92 ret[5] = cofactorm5();
93 ret[6] = -cofactorm9();
94 ret[7] = cofactorm13();
95
96 ret[8] = cofactorm2();
97 ret[9] = -cofactorm6();
98 ret[10] = cofactorm10();
99 ret[11] = -cofactorm14();
100
101 ret[12] = -cofactorm3();
102 ret[13] = cofactorm7();
103 ret[14] = -cofactorm11();
104 ret[15] = cofactorm15();
105
106 return ret;
107 }
108
109
110 /** Make this a rotation matrix
111 */
112
113 inline Matrix& loadRotate(T angle, T x, T y, T z) {
114 register T magSqr = x*x + y*y + z*z;
115 if (magSqr != 1.0) {
116 register T mag = sqrt(magSqr);
117 x/=mag;
118 y/=mag;
119 z/=mag;
120 }
121 T c = cos(angle*M_PI/180);
122 T s = sin(angle*M_PI/180);
123 m0 = x*x*(1-c)+c;
124 m1 = y*x*(1-c)+z*s;
125 m2 = z*x*(1-c)-y*s;
126 m3 = 0;
127
128 m4 = x*y*(1-c)-z*s;
129 m5 = y*y*(1-c)+c;
130 m6 = z*y*(1-c)+x*s;
131 m7 = 0;
132
133 m8 = x*z*(1-c)+y*s;
134 m9 = y*z*(1-c)-x*s;
135 m10 = z*z*(1-c)+c;
136 m11 = 0;
137
138 m12 = 0;
139 m13 = 0;
140 m14 = 0;
141 m15 = 1;
142
143 return *this;
144 }
145
146 /** Apply scale into this matrix
147 */
148 inline Matrix& applyScale(T x, T y, T z) {
149 static Matrix<T> temp;
150 temp.loadScale(x,y,z);
151 *this = temp.mult4by4(*this);
152 return *this;
153 }
154
155 /** Make this a scale matrix
156 */
157 inline Matrix& loadScale(T x, T y, T z = 1) {
158 m0 = x;
159 m4 = 0;
160 m8 = 0;
161 m12 = 0;
162 m1 = 0;
163 m5 = y;
164 m9 = 0;
165 m13 = 0;
166 m2 = 0;
167 m6 = 0;
168 m10 = z;
169 m14 = 0;
170 m3 = 0;
171 m7 = 0;
172 m11 = 0;
173 m15 = 1;
174
175 return *this;
176 }
177
178 /** Apply translation into this matrix
179 */
180 inline Matrix& applyTranslate(T x, T y, T z) {
181 static Matrix<T> temp;
182 temp.loadTranslate(x,y,z);
183 *this = temp.mult4by4(*this);
184 return *this;
185 }
186
187 /** Make this a translation matrix
188 */
189 inline Matrix& loadTranslate( const T x, const T y, const T z) {
190 m0 = 1;
191 m4 = 0;
192 m8 = 0;
193 m12 = x;
194 m1 = 0;
195 m5 = 1;
196 m9 = 0;
197 m13 = y;
198 m2 = 0;
199 m6 = 0;
200 m10 = 1;
201 m14 = z;
202 m3 = 0;
203 m7 = 0;
204 m11 = 0;
205 m15 = 1;
206
207 return *this;
208 }
209
210 /** Transform given point using this matrix
211 */
212 inline PointType3D<T> operator* (const PointType3D<T>& vec) {
213 PointType3D<T> ret(T(0));
214 for (register unsigned j = 0; j < 3; ++j)
215 for (register unsigned i = 0; i < 3; ++i)
216 ret.val[j] += vec.val[i]*m[j+i*4]; //scale and rotate disregarding w scaling
217
218 for (register unsigned i = 0; i < 3; ++i)
219 ret.val[i] += m[i+3*4]; //translate
220
221 //do w scaling
222 T w = m[15];
223 for (register unsigned i = 0; i < 3; ++i)
224 w += vec.val[i]*m[3+i*4];
225
226 register T resip = 1/w;
227
228 for (register unsigned i = 0; i < 3; ++i)
229 ret[i] *= resip;
230
231 return ret;
232 }
233
234 /** Direct access to the matrix elements, just remember they are in column major format!!
235 */
236 inline T& operator[] (int ind) {
237 assert(ind > -1 && ind < 16);
238 return m[ind];
239 }
240
241 /** Apply the matrix dot product to this matrix
242 */
243 inline Matrix& mult3by3(const Matrix& mat) {
244 Matrix temp(*this);
245 m0 = temp.m0*mat.m0+temp.m4*mat.m1+temp.m8*mat.m2;
246 m4 = temp.m0*mat.m4+temp.m4*mat.m5+temp.m8*mat.m6;
247 m8 = temp.m0*mat.m8+temp.m4*mat.m9+temp.m8*mat.m10;
248
249 m1 = temp.m1*mat.m0+temp.m5*mat.m1+temp.m9*mat.m2;
250 m5 = temp.m1*mat.m4+temp.m5*mat.m5+temp.m9*mat.m6;
251 m9 = temp.m1*mat.m8+temp.m5*mat.m9+temp.m9*mat.m10;
252
253 m2 = temp.m2*mat.m0+temp.m6*mat.m1+temp.m10*mat.m2;
254 m6 = temp.m2*mat.m4+temp.m6*mat.m5+temp.m10*mat.m6;
255 m10 = temp.m2*mat.m8+temp.m6*mat.m9+temp.m10*mat.m10;
256
257 m3 = temp.m3*mat.m0+temp.m7*mat.m1+temp.m11*mat.m2;
258 m7 = temp.m3*mat.m4+temp.m7*mat.m5+temp.m11*mat.m6;
259 m11 = temp.m3*mat.m8+temp.m7*mat.m9+temp.m11*mat.m10;
260 return *this;
261 }
262
263 /** this->Rmult4by4(temp) == [temp] X [*this] **/
264 /** also equal to temp->mult4by4(*this) **/
265 inline Matrix<T>& Rmult4by4(const Matrix<T>& mat) {
266 Matrix temp(*this);
267
268 m0 = mat.m0*temp.m0+mat.m4*temp.m1+mat.m8*temp.m2+mat.m12*temp.m3;
269 m4 = mat.m0*temp.m4+mat.m4*temp.m5+mat.m8*temp.m6+mat.m12*temp.m7;
270 m8 = mat.m0*temp.m8+mat.m4*temp.m9+mat.m8*temp.m10+mat.m12*temp.m11;
271 m12 = mat.m0*temp.m12+mat.m4*temp.m13+mat.m8*temp.m14+mat.m12*temp.m15;
272
273 m1 = mat.m1*temp.m0 + mat.m5*temp.m1 + mat.m9*temp.m2+mat.m13*temp.m3;
274 m5 = mat.m1*temp.m4 + mat.m5*temp.m5 + mat.m9*temp.m6+mat.m13*temp.m7;
275 m9 = mat.m1*temp.m8 + mat.m5*temp.m9 + mat.m9*temp.m10+mat.m13*temp.m11;
276 m13 = mat.m1*temp.m12+ mat.m5*temp.m13 + mat.m9*temp.m14+mat.m13*temp.m15;
277
278 m2 = mat.m2*temp.m0+mat.m6*temp.m1+mat.m10*temp.m2+mat.m14*temp.m3;
279 m6 = mat.m2*temp.m4+mat.m6*temp.m5+mat.m10*temp.m6+mat.m14*temp.m7;
280 m10 = mat.m2*temp.m8+mat.m6*temp.m9+mat.m10*temp.m10+mat.m14*temp.m11;
281 m14 = mat.m2*temp.m12+mat.m6*temp.m13+mat.m10*temp.m14+mat.m14*temp.m15;
282
283 m3 = mat.m3*temp.m0+mat.m7*temp.m1+mat.m11*temp.m2+mat.m15*temp.m3;
284 m7 = mat.m3*temp.m4+mat.m7*temp.m5+mat.m11*temp.m6+mat.m15*temp.m7;
285 m11 = mat.m3*temp.m8+mat.m7*temp.m9+mat.m11*temp.m10+mat.m15*temp.m11;
286 m15 = mat.m3*temp.m12+mat.m7*temp.m13+mat.m11*temp.m14+mat.m15*temp.m15;
287 return *this;
288 }
289
290
291 inline Matrix<T>& mult4by4(const Matrix<T>& mat) {
292 Matrix temp(*this);
293
294 m0 = temp.m0*mat.m0+temp.m4*mat.m1+temp.m8*mat.m2+temp.m12*mat.m3;
295 m4 = temp.m0*mat.m4+temp.m4*mat.m5+temp.m8*mat.m6+temp.m12*mat.m7;
296 m8 = temp.m0*mat.m8+temp.m4*mat.m9+temp.m8*mat.m10+temp.m12*mat.m11;
297 m12 = temp.m0*mat.m12+temp.m4*mat.m13+temp.m8*mat.m14+temp.m12*mat.m15;
298
299 m1 = temp.m1*mat.m0 + temp.m5*mat.m1 + temp.m9*mat.m2+temp.m13*mat.m3;
300 m5 = temp.m1*mat.m4 + temp.m5*mat.m5 + temp.m9*mat.m6+temp.m13*mat.m7;
301 m9 = temp.m1*mat.m8 + temp.m5*mat.m9 + temp.m9*mat.m10+temp.m13*mat.m11;
302 m13 = temp.m1*mat.m12+ temp.m5*mat.m13 + temp.m9*mat.m14+temp.m13*mat.m15;
303
304 m2 = temp.m2*mat.m0+temp.m6*mat.m1+temp.m10*mat.m2+temp.m14*mat.m3;
305 m6 = temp.m2*mat.m4+temp.m6*mat.m5+temp.m10*mat.m6+temp.m14*mat.m7;
306 m10 = temp.m2*mat.m8+temp.m6*mat.m9+temp.m10*mat.m10+temp.m14*mat.m11;
307 m14 = temp.m2*mat.m12+temp.m6*mat.m13+temp.m10*mat.m14+temp.m14*mat.m15;
308
309 m3 = temp.m3*mat.m0+temp.m7*mat.m1+temp.m11*mat.m2+temp.m15*mat.m3;
310 m7 = temp.m3*mat.m4+temp.m7*mat.m5+temp.m11*mat.m6+temp.m15*mat.m7;
311 m11 = temp.m3*mat.m8+temp.m7*mat.m9+temp.m11*mat.m10+temp.m15*mat.m11;
312 m15 = temp.m3*mat.m12+temp.m7*mat.m13+temp.m11*mat.m14+temp.m15*mat.m15;
313 return *this;
314 }
315
316 Matrix& applyRotate(T angle, T x, T y, T z) {
317 static Matrix<T> temp;
318 temp.loadRotate(angle,x,y,z);
319 *this = temp.mult4by4(*this);
320 return *this;
321 }
322
323
324 private:
325 #define cofactor_maker(f1,mj1,mi1, f2,mj2,mi2, f3,mj3,mi3) \
326 f1*(mj1*mi1-mj2*mi3) + f2*(mj2*mi2-mj3*mi1) + f3*(mj3*mi3-mj1*mi2)
327
328 inline T cofactorm0() const {
329 return cofactor_maker(m5,m10,m15, m6,m11,m13, m7,m9,m14);
330 }
331 inline T cofactorm1() const {
332 return cofactor_maker(m6,m11,m12, m7,m8,m14, m4,m10,m15);
333 }
334 inline T cofactorm2() const {
335 return cofactor_maker(m7,m8,m13, m4,m9,m15, m5,m11,m12);
336 }
337 inline T cofactorm3() const {
338 return cofactor_maker(m4,m9,m14, m5,m10,m12, m6,m8,m13);
339 }
340 inline T cofactorm4() const {
341 return cofactor_maker(m9,m14,m3, m10,m15,m1, m11,m13,m2);
342 }
343 inline T cofactorm5() const {
344 return cofactor_maker(m10,m15,m0, m11,m12,m2, m8,m14,m3);
345 }
346 inline T cofactorm6() const {
347 return cofactor_maker(m11,m12,m1, m8,m13,m3, m9,m15,m0);
348 }
349 inline T cofactorm7() const {
350 return cofactor_maker(m8,m13,m2, m9,m14,m0, m10,m12,m1);
351 }
352 inline T cofactorm8() const {
353 return cofactor_maker(m13,m2,m7, m14,m3,m5, m15,m1,m6);
354 }
355 inline T cofactorm9() const {
356 return cofactor_maker(m14,m13,m4, m15,m0,m6, m12,m2,m7);
357 }
358 inline T cofactorm10() const {
359 return cofactor_maker(m15,m0,m5, m12,m1,m7, m13,m3,m4);
360 }
361 inline T cofactorm11() const {
362 return cofactor_maker(m12,m1,m6, m13,m2,m4, m14,m0,m5);
363 }
364 inline T cofactorm12() const {
365 return cofactor_maker(m1,m6,m11, m2,m7,m9, m3,m5,m10);
366 }
367 inline T cofactorm13() const {
368 return cofactor_maker(m2,m7,m8, m3,m4,m10, m10,m6,m11);
369 }
370 inline T cofactorm14() const {
371 return cofactor_maker(m3,m4,m9, m0,m5,m11, m1,m7,m8);
372 }
373 inline T cofactorm15() const {
374 return cofactor_maker(m0,m5,m10, m1,m6,m8, m2,m4,m9);
375 }
376
377 union {
378 T m[16];
379 struct {
380 T m0,m1,m2,m3,m4,m5,m6,m7,m8,m9,m10,m11,m12,m13,m14,m15;
381 };
382 };
383 };
384
385 typedef Matrix<double> DoubleMatrix;
386 typedef Matrix<int> IntMatrix;
387
388 /** Print coords of the Matrix to a stream
389 */
390 template<typename T>
391 std::ostream& operator<<(std::ostream& os, Matrix<T>& m) {
392 return os << "\n|" << m[0] << "," << m[4] << "," << m[8] << "," << m[12] << "|\n" << \
393 "|" << m[1] << "," << m[5] << "," << m[9] << "," << m[13] << "|\n" << \
394 "|" << m[2] << "," << m[6] << "," << m[10] << "," << m[14] << "|\n" << \
395 "|" << m[3] << "," << m[7] << "," << m[11] << "," << m[15] << "|\n";
396 }
397
398
399 }
400
401 #endif