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