Colobot
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Pages
matrix.h
Go to the documentation of this file.
1 /*
2  * This file is part of the Colobot: Gold Edition source code
3  * Copyright (C) 2001-2014, Daniel Roux, EPSITEC SA & TerranovaTeam
4  * http://epsiteс.ch; http://colobot.info; http://github.com/colobot
5  *
6  * This program 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 3 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.
14  * See the 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, see http://gnu.org/licenses
18  */
19 
25 #pragma once
26 
27 
28 #include "math/const.h"
29 #include "math/func.h"
30 #include "math/vector.h"
31 
32 
33 #include <cmath>
34 #include <cassert>
35 
36 
37 // Math module namespace
38 namespace Math {
39 
66 struct Matrix
67 {
69  float m[16];
70 
72  inline Matrix()
73  {
74  LoadIdentity();
75  }
76 
78 
79  inline explicit Matrix(const float (&_m)[16])
80  {
81  for (int i = 0; i < 16; ++i)
82  m[i] = _m[i];
83  }
84 
86 
90  inline explicit Matrix(const float (&_m)[4][4])
91  {
92  for (int c = 0; c < 4; ++c)
93  {
94  for (int r = 0; r < 4; ++r)
95  {
96  m[4*c+r] = _m[r][c];
97  }
98  }
99  }
100 
102 
107  inline void Set(int row, int col, float value)
108  {
109  m[(col-1)*4+(row-1)] = value;
110  }
111 
113 
118  inline float Get(int row, int col)
119  {
120  return m[(col-1)*4+(row-1)];
121  }
122 
124  inline void LoadZero()
125  {
126  for (int i = 0; i < 16; ++i)
127  m[i] = 0.0f;
128  }
129 
131  inline void LoadIdentity()
132  {
133  LoadZero();
134  /* (1,1) */ m[0 ] = 1.0f;
135  /* (2,2) */ m[5 ] = 1.0f;
136  /* (3,3) */ m[10] = 1.0f;
137  /* (4,4) */ m[15] = 1.0f;
138  }
139 
141  inline float* Array()
142  {
143  return reinterpret_cast<float*>(this);
144  }
145 
147  inline void Transpose()
148  {
149  /* (2,1) <-> (1,2) */ Swap(m[1 ], m[4 ]);
150  /* (3,1) <-> (1,3) */ Swap(m[2 ], m[8 ]);
151  /* (4,1) <-> (1,4) */ Swap(m[3 ], m[12]);
152  /* (3,2) <-> (2,3) */ Swap(m[6 ], m[9 ]);
153  /* (4,2) <-> (2,4) */ Swap(m[7 ], m[13]);
154  /* (4,3) <-> (3,4) */ Swap(m[11], m[14]);
155  }
156 
158 
159  inline float Det() const
160  {
161  float result = 0.0f;
162  for (int i = 0; i < 4; ++i)
163  {
164  result += m[i] * Cofactor(i, 0);
165  }
166  return result;
167  }
168 
170 
175  inline float Cofactor(int r, int c) const
176  {
177  assert(r >= 0 && r <= 3);
178  assert(c >= 0 && c <= 3);
179 
180  float result = 0.0f;
181 
182  /* That looks horrible, I know. But it's fast :) */
183 
184  switch (4*r + c)
185  {
186  // r=0, c=0
187  /* 05 09 13
188  06 10 14
189  07 11 15 */
190  case 0:
191  result = + m[5 ] * (m[10] * m[15] - m[14] * m[11])
192  - m[9 ] * (m[6 ] * m[15] - m[14] * m[7 ])
193  + m[13] * (m[6 ] * m[11] - m[10] * m[7 ]);
194  break;
195 
196  // r=0, c=1
197  /* 01 09 13
198  02 10 14
199  03 11 15 */
200  case 1:
201  result = - m[1 ] * (m[10] * m[15] - m[14] * m[11])
202  + m[9 ] * (m[2 ] * m[15] - m[14] * m[3 ])
203  - m[13] * (m[2 ] * m[11] - m[10] * m[3 ]);
204  break;
205 
206  // r=0, c=2
207  /* 01 05 13
208  02 06 14
209  03 07 15 */
210  case 2:
211  result = + m[1 ] * (m[6 ] * m[15] - m[14] * m[7 ])
212  - m[5 ] * (m[2 ] * m[15] - m[14] * m[3 ])
213  + m[13] * (m[2 ] * m[7 ] - m[6 ] * m[3 ]);
214  break;
215 
216  // r=0, c=3
217  /* 01 05 09
218  02 06 10
219  03 07 11 */
220  case 3:
221  result = - m[1 ] * (m[6 ] * m[11] - m[10] * m[7 ])
222  + m[5 ] * (m[2 ] * m[11] - m[10] * m[3 ])
223  - m[9 ] * (m[2 ] * m[7 ] - m[6 ] * m[3 ]);
224  break;
225 
226  // r=1, c=0
227  /* 04 08 12
228  06 10 14
229  07 11 15 */
230  case 4:
231  result = - m[4 ] * (m[10] * m[15] - m[14] * m[11])
232  + m[8 ] * (m[6 ] * m[15] - m[14] * m[7 ])
233  - m[12] * (m[6 ] * m[11] - m[10] * m[7 ]);
234  break;
235 
236  // r=1, c=1
237  /* 00 08 12
238  02 10 14
239  03 11 15 */
240  case 5:
241  result = + m[0 ] * (m[10] * m[15] - m[14] * m[11])
242  - m[8 ] * (m[2 ] * m[15] - m[14] * m[3 ])
243  + m[12] * (m[2 ] * m[11] - m[10] * m[3 ]);
244  break;
245 
246  // r=1, c=2
247  /* 00 04 12
248  02 06 14
249  03 07 15 */
250  case 6:
251  result = - m[0 ] * (m[6 ] * m[15] - m[14] * m[7 ])
252  + m[4 ] * (m[2 ] * m[15] - m[14] * m[3 ])
253  - m[12] * (m[2 ] * m[7 ] - m[6 ] * m[3 ]);
254  break;
255 
256  // r=1, c=3
257  /* 00 04 08
258  02 06 10
259  03 07 11 */
260  case 7:
261  result = + m[0 ] * (m[6 ] * m[11] - m[10] * m[7 ])
262  - m[4 ] * (m[2 ] * m[11] - m[10] * m[3 ])
263  + m[8 ] * (m[2 ] * m[7 ] - m[6 ] * m[3 ]);
264  break;
265 
266  // r=2, c=0
267  /* 04 08 12
268  05 09 13
269  07 11 15 */
270  case 8:
271  result = + m[4 ] * (m[9 ] * m[15] - m[13] * m[11])
272  - m[8 ] * (m[5 ] * m[15] - m[13] * m[7 ])
273  + m[12] * (m[5 ] * m[11] - m[9 ] * m[7 ]);
274  break;
275 
276  // r=2, c=1
277  /* 00 08 12
278  01 09 13
279  03 11 15 */
280  case 9:
281  result = - m[0 ] * (m[9 ] * m[15] - m[13] * m[11])
282  + m[8 ] * (m[1 ] * m[15] - m[13] * m[3 ])
283  - m[12] * (m[1 ] * m[11] - m[9 ] * m[3 ]);
284  break;
285 
286  // r=2, c=2
287  /* 00 04 12
288  01 05 13
289  03 07 15 */
290  case 10:
291  result = + m[0 ] * (m[5 ] * m[15] - m[13] * m[7 ])
292  - m[4 ] * (m[1 ] * m[15] - m[13] * m[3 ])
293  + m[12] * (m[1 ] * m[7 ] - m[5 ] * m[3 ]);
294  break;
295 
296  // r=2, c=3
297  /* 00 04 08
298  01 05 09
299  03 07 11 */
300  case 11:
301  result = - m[0 ] * (m[5 ] * m[11] - m[9 ] * m[7 ])
302  + m[4 ] * (m[1 ] * m[11] - m[9 ] * m[3 ])
303  - m[8 ] * (m[1 ] * m[7 ] - m[5 ] * m[3 ]);
304  break;
305 
306  // r=3, c=0
307  /* 04 08 12
308  05 09 13
309  06 10 14 */
310  case 12:
311  result = - m[4 ] * (m[9 ] * m[14] - m[13] * m[10])
312  + m[8 ] * (m[5 ] * m[14] - m[13] * m[6 ])
313  - m[12] * (m[5 ] * m[10] - m[9 ] * m[6 ]);
314  break;
315 
316  // r=3, c=1
317  /* 00 08 12
318  01 09 13
319  02 10 14 */
320  case 13:
321  result = + m[0 ] * (m[9 ] * m[14] - m[13] * m[10])
322  - m[8 ] * (m[1 ] * m[14] - m[13] * m[2 ])
323  + m[12] * (m[1 ] * m[10] - m[9 ] * m[2 ]);
324  break;
325 
326  // r=3, c=2
327  /* 00 04 12
328  01 05 13
329  02 06 14 */
330  case 14:
331  result = - m[0 ] * (m[5 ] * m[14] - m[13] * m[6 ])
332  + m[4 ] * (m[1 ] * m[14] - m[13] * m[2 ])
333  - m[12] * (m[1 ] * m[6 ] - m[5 ] * m[2 ]);
334  break;
335 
336  // r=3, c=3
337  /* 00 04 08
338  01 05 09
339  02 06 10 */
340  case 15:
341  result = + m[0 ] * (m[5 ] * m[10] - m[9 ] * m[6 ])
342  - m[4 ] * (m[1 ] * m[10] - m[9 ] * m[2 ])
343  + m[8 ] * (m[1 ] * m[6 ] - m[5 ] * m[2 ]);
344  break;
345 
346  default:
347  break;
348  }
349 
350  return result;
351  }
352 
354 
358  inline Matrix Inverse() const
359  {
360  float d = Det();
361  assert(! IsZero(d));
362 
363  float result[16] = { 0.0f };
364 
365  for (int r = 0; r < 4; ++r)
366  {
367  for (int c = 0; c < 4; ++c)
368  {
369  // Already transposed!
370  result[4*r+c] = (1.0f / d) * Cofactor(r, c);
371  }
372  }
373 
374  return Matrix(result);
375  }
376 
378 
382  inline Matrix Multiply(const Matrix &right) const
383  {
384  float result[16] = { 0.0f };
385 
386  for (int c = 0; c < 4; ++c)
387  {
388  for (int r = 0; r < 4; ++r)
389  {
390  result[4*c+r] = 0.0f;
391  for (int i = 0; i < 4; ++i)
392  {
393  result[4*c+r] += m[4*i+r] * right.m[4*c+i];
394  }
395  }
396  }
397 
398  return Matrix(result);
399  }
400 }; // struct Matrix
401 
403 inline bool MatricesEqual(const Matrix &m1, const Matrix &m2,
404  float tolerance = TOLERANCE)
405 {
406  for (int i = 0; i < 16; ++i)
407  {
408  if (! IsEqual(m1.m[i], m2.m[i], tolerance))
409  return false;
410  }
411 
412  return true;
413 }
414 
417 {
418  Math::Matrix result = m;
419  result.Transpose();
420  return result;
421 }
422 
424 
427 inline Math::Matrix MultiplyMatrices(const Math::Matrix &left, const Math::Matrix &right)
428 {
429  return left.Multiply(right);
430 }
431 
433 
445 inline Math::Vector MatrixVectorMultiply(const Math::Matrix &m, const Math::Vector &v, bool wDivide = false)
446 {
447  float x = v.x * m.m[0 ] + v.y * m.m[4 ] + v.z * m.m[8 ] + m.m[12];
448  float y = v.x * m.m[1 ] + v.y * m.m[5 ] + v.z * m.m[9 ] + m.m[13];
449  float z = v.x * m.m[2 ] + v.y * m.m[6 ] + v.z * m.m[10] + m.m[14];
450 
451  if (!wDivide)
452  return Math::Vector(x, y, z);
453 
454  float w = v.x * m.m[3 ] + v.y * m.m[7 ] + v.z * m.m[11] + m.m[15];
455 
456  if (IsZero(w))
457  return Math::Vector(x, y, z);
458 
459  x /= w;
460  y /= w;
461  z /= w;
462 
463  return Math::Vector(x, y, z);
464 }
465 
466 
467 } // namespace Math
468 
void LoadIdentity()
Loads the identity matrix.
Definition: matrix.h:131
void Transpose()
Transposes the matrix.
Definition: matrix.h:147
const float TOLERANCE
Tolerance level – minimum accepted float value.
Definition: const.h:36
bool IsZero(float a, float tolerance=Math::TOLERANCE)
Compares a to zero within tolerance.
Definition: func.h:46
void LoadZero()
Loads the zero matrix.
Definition: matrix.h:124
bool MatricesEqual(const Matrix &m1, const Matrix &m2, float tolerance=TOLERANCE)
Checks if two matrices are equal within given tolerance.
Definition: matrix.h:403
void Set(int row, int col, float value)
Sets value in given row and col.
Definition: matrix.h:107
Matrix(const float(&_m)[16])
Creates the matrix from 1D array.
Definition: matrix.h:79
4x4 matrix
Definition: matrix.h:66
float x
X - 1st coord.
Definition: vector.h:55
Math::Matrix MultiplyMatrices(const Math::Matrix &left, const Math::Matrix &right)
Convenience function for multiplying a matrix.
Definition: matrix.h:427
Matrix()
Creates the indentity matrix.
Definition: matrix.h:72
float Get(int row, int col)
Returns the value in given row and col.
Definition: matrix.h:118
Math::Matrix Transpose(const Math::Matrix &m)
Convenience function for getting transposed matrix.
Definition: matrix.h:416
bool IsEqual(float a, float b, float tolerance=Math::TOLERANCE)
Compares a and b within tolerance.
Definition: func.h:40
Math::Vector MatrixVectorMultiply(const Math::Matrix &m, const Math::Vector &v, bool wDivide=false)
Calculates the result of multiplying m * v.
Definition: matrix.h:445
float Cofactor(int r, int c) const
Calculates the cofactor of the matrix.
Definition: matrix.h:175
Namespace for (new) math code.
Definition: const.h:32
void Swap(int &a, int &b)
Swaps two integers.
Definition: func.h:104
float z
Z - 3rd coord.
Definition: vector.h:59
Constants used in math functions.
Matrix Inverse() const
Calculates the inverse matrix.
Definition: matrix.h:358
Vector struct and related functions.
Common math functions.
float * Array()
Returns the struct cast to float* array; use with care!
Definition: matrix.h:141
3D (3x1) vector
Definition: vector.h:52
float Det() const
Calculates the determinant of the matrix.
Definition: matrix.h:159
Matrix(const float(&_m)[4][4])
Creates the matrix from 2D array.
Definition: matrix.h:90
Matrix Multiply(const Matrix &right) const
Calculates the multiplication of this matrix * given matrix.
Definition: matrix.h:382
float y
Y - 2nd coord.
Definition: vector.h:57
float m[16]
Matrix values in column-major order.
Definition: matrix.h:69