Colobot
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Pages
matrix.h
Go to the documentation of this file.
1 // * This file is part of the COLOBOT source code
2 // * Copyright (C) 2012, Polish Portal of Colobot (PPC)
3 // *
4 // * This program is free software: you can redistribute it and/or modify
5 // * it under the terms of the GNU General Public License as published by
6 // * the Free Software Foundation, either version 3 of the License, or
7 // * (at your option) any later version.
8 // *
9 // * This program is distributed in the hope that it will be useful,
10 // * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 // * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 // * GNU General Public License for more details.
13 // *
14 // * You should have received a copy of the GNU General Public License
15 // * along with this program. If not, see http://www.gnu.org/licenses/.
16 
22 #pragma once
23 
24 
25 #include "math/const.h"
26 #include "math/func.h"
27 #include "math/vector.h"
28 
29 
30 #include <cmath>
31 #include <cassert>
32 
33 
34 // Math module namespace
35 namespace Math {
36 
63 struct Matrix
64 {
66  float m[16];
67 
69  inline Matrix()
70  {
71  LoadIdentity();
72  }
73 
75 
76  inline explicit Matrix(const float (&_m)[16])
77  {
78  for (int i = 0; i < 16; ++i)
79  m[i] = _m[i];
80  }
81 
83 
87  inline explicit Matrix(const float (&_m)[4][4])
88  {
89  for (int c = 0; c < 4; ++c)
90  {
91  for (int r = 0; r < 4; ++r)
92  {
93  m[4*c+r] = _m[r][c];
94  }
95  }
96  }
97 
99 
104  inline void Set(int row, int col, float value)
105  {
106  m[(col-1)*4+(row-1)] = value;
107  }
108 
110 
115  inline float Get(int row, int col)
116  {
117  return m[(col-1)*4+(row-1)];
118  }
119 
121  inline void LoadZero()
122  {
123  for (int i = 0; i < 16; ++i)
124  m[i] = 0.0f;
125  }
126 
128  inline void LoadIdentity()
129  {
130  LoadZero();
131  /* (1,1) */ m[0 ] = 1.0f;
132  /* (2,2) */ m[5 ] = 1.0f;
133  /* (3,3) */ m[10] = 1.0f;
134  /* (4,4) */ m[15] = 1.0f;
135  }
136 
138  inline float* Array()
139  {
140  return reinterpret_cast<float*>(this);
141  }
142 
144  inline void Transpose()
145  {
146  /* (2,1) <-> (1,2) */ Swap(m[1 ], m[4 ]);
147  /* (3,1) <-> (1,3) */ Swap(m[2 ], m[8 ]);
148  /* (4,1) <-> (1,4) */ Swap(m[3 ], m[12]);
149  /* (3,2) <-> (2,3) */ Swap(m[6 ], m[9 ]);
150  /* (4,2) <-> (2,4) */ Swap(m[7 ], m[13]);
151  /* (4,3) <-> (3,4) */ Swap(m[11], m[14]);
152  }
153 
155 
156  inline float Det() const
157  {
158  float result = 0.0f;
159  for (int i = 0; i < 4; ++i)
160  {
161  result += m[i] * Cofactor(i, 0);
162  }
163  return result;
164  }
165 
167 
172  inline float Cofactor(int r, int c) const
173  {
174  assert(r >= 0 && r <= 3);
175  assert(c >= 0 && c <= 3);
176 
177  float result = 0.0f;
178 
179  /* That looks horrible, I know. But it's fast :) */
180 
181  switch (4*r + c)
182  {
183  // r=0, c=0
184  /* 05 09 13
185  06 10 14
186  07 11 15 */
187  case 0:
188  result = + m[5 ] * (m[10] * m[15] - m[14] * m[11])
189  - m[9 ] * (m[6 ] * m[15] - m[14] * m[7 ])
190  + m[13] * (m[6 ] * m[11] - m[10] * m[7 ]);
191  break;
192 
193  // r=0, c=1
194  /* 01 09 13
195  02 10 14
196  03 11 15 */
197  case 1:
198  result = - m[1 ] * (m[10] * m[15] - m[14] * m[11])
199  + m[9 ] * (m[2 ] * m[15] - m[14] * m[3 ])
200  - m[13] * (m[2 ] * m[11] - m[10] * m[3 ]);
201  break;
202 
203  // r=0, c=2
204  /* 01 05 13
205  02 06 14
206  03 07 15 */
207  case 2:
208  result = + m[1 ] * (m[6 ] * m[15] - m[14] * m[7 ])
209  - m[5 ] * (m[2 ] * m[15] - m[14] * m[3 ])
210  + m[13] * (m[2 ] * m[7 ] - m[6 ] * m[3 ]);
211  break;
212 
213  // r=0, c=3
214  /* 01 05 09
215  02 06 10
216  03 07 11 */
217  case 3:
218  result = - m[1 ] * (m[6 ] * m[11] - m[10] * m[7 ])
219  + m[5 ] * (m[2 ] * m[11] - m[10] * m[3 ])
220  - m[9 ] * (m[2 ] * m[7 ] - m[6 ] * m[3 ]);
221  break;
222 
223  // r=1, c=0
224  /* 04 08 12
225  06 10 14
226  07 11 15 */
227  case 4:
228  result = - m[4 ] * (m[10] * m[15] - m[14] * m[11])
229  + m[8 ] * (m[6 ] * m[15] - m[14] * m[7 ])
230  - m[12] * (m[6 ] * m[11] - m[10] * m[7 ]);
231  break;
232 
233  // r=1, c=1
234  /* 00 08 12
235  02 10 14
236  03 11 15 */
237  case 5:
238  result = + m[0 ] * (m[10] * m[15] - m[14] * m[11])
239  - m[8 ] * (m[2 ] * m[15] - m[14] * m[3 ])
240  + m[12] * (m[2 ] * m[11] - m[10] * m[3 ]);
241  break;
242 
243  // r=1, c=2
244  /* 00 04 12
245  02 06 14
246  03 07 15 */
247  case 6:
248  result = - m[0 ] * (m[6 ] * m[15] - m[14] * m[7 ])
249  + m[4 ] * (m[2 ] * m[15] - m[14] * m[3 ])
250  - m[12] * (m[2 ] * m[7 ] - m[6 ] * m[3 ]);
251  break;
252 
253  // r=1, c=3
254  /* 00 04 08
255  02 06 10
256  03 07 11 */
257  case 7:
258  result = + m[0 ] * (m[6 ] * m[11] - m[10] * m[7 ])
259  - m[4 ] * (m[2 ] * m[11] - m[10] * m[3 ])
260  + m[8 ] * (m[2 ] * m[7 ] - m[6 ] * m[3 ]);
261  break;
262 
263  // r=2, c=0
264  /* 04 08 12
265  05 09 13
266  07 11 15 */
267  case 8:
268  result = + m[4 ] * (m[9 ] * m[15] - m[13] * m[11])
269  - m[8 ] * (m[5 ] * m[15] - m[13] * m[7 ])
270  + m[12] * (m[5 ] * m[11] - m[9 ] * m[7 ]);
271  break;
272 
273  // r=2, c=1
274  /* 00 08 12
275  01 09 13
276  03 11 15 */
277  case 9:
278  result = - m[0 ] * (m[9 ] * m[15] - m[13] * m[11])
279  + m[8 ] * (m[1 ] * m[15] - m[13] * m[3 ])
280  - m[12] * (m[1 ] * m[11] - m[9 ] * m[3 ]);
281  break;
282 
283  // r=2, c=2
284  /* 00 04 12
285  01 05 13
286  03 07 15 */
287  case 10:
288  result = + m[0 ] * (m[5 ] * m[15] - m[13] * m[7 ])
289  - m[4 ] * (m[1 ] * m[15] - m[13] * m[3 ])
290  + m[12] * (m[1 ] * m[7 ] - m[5 ] * m[3 ]);
291  break;
292 
293  // r=2, c=3
294  /* 00 04 08
295  01 05 09
296  03 07 11 */
297  case 11:
298  result = - m[0 ] * (m[5 ] * m[11] - m[9 ] * m[7 ])
299  + m[4 ] * (m[1 ] * m[11] - m[9 ] * m[3 ])
300  - m[8 ] * (m[1 ] * m[7 ] - m[5 ] * m[3 ]);
301  break;
302 
303  // r=3, c=0
304  /* 04 08 12
305  05 09 13
306  06 10 14 */
307  case 12:
308  result = - m[4 ] * (m[9 ] * m[14] - m[13] * m[10])
309  + m[8 ] * (m[5 ] * m[14] - m[13] * m[6 ])
310  - m[12] * (m[5 ] * m[10] - m[9 ] * m[6 ]);
311  break;
312 
313  // r=3, c=1
314  /* 00 08 12
315  01 09 13
316  02 10 14 */
317  case 13:
318  result = + m[0 ] * (m[9 ] * m[14] - m[13] * m[10])
319  - m[8 ] * (m[1 ] * m[14] - m[13] * m[2 ])
320  + m[12] * (m[1 ] * m[10] - m[9 ] * m[2 ]);
321  break;
322 
323  // r=3, c=2
324  /* 00 04 12
325  01 05 13
326  02 06 14 */
327  case 14:
328  result = - m[0 ] * (m[5 ] * m[14] - m[13] * m[6 ])
329  + m[4 ] * (m[1 ] * m[14] - m[13] * m[2 ])
330  - m[12] * (m[1 ] * m[6 ] - m[5 ] * m[2 ]);
331  break;
332 
333  // r=3, c=3
334  /* 00 04 08
335  01 05 09
336  02 06 10 */
337  case 15:
338  result = + m[0 ] * (m[5 ] * m[10] - m[9 ] * m[6 ])
339  - m[4 ] * (m[1 ] * m[10] - m[9 ] * m[2 ])
340  + m[8 ] * (m[1 ] * m[6 ] - m[5 ] * m[2 ]);
341  break;
342 
343  default:
344  break;
345  }
346 
347  return result;
348  }
349 
351 
355  inline Matrix Inverse() const
356  {
357  float d = Det();
358  assert(! IsZero(d));
359 
360  float result[16] = { 0.0f };
361 
362  for (int r = 0; r < 4; ++r)
363  {
364  for (int c = 0; c < 4; ++c)
365  {
366  // Already transposed!
367  result[4*r+c] = (1.0f / d) * Cofactor(r, c);
368  }
369  }
370 
371  return Matrix(result);
372  }
373 
375 
379  inline Matrix Multiply(const Matrix &right) const
380  {
381  float result[16] = { 0.0f };
382 
383  for (int c = 0; c < 4; ++c)
384  {
385  for (int r = 0; r < 4; ++r)
386  {
387  result[4*c+r] = 0.0f;
388  for (int i = 0; i < 4; ++i)
389  {
390  result[4*c+r] += m[4*i+r] * right.m[4*c+i];
391  }
392  }
393  }
394 
395  return Matrix(result);
396  }
397 }; // struct Matrix
398 
400 inline bool MatricesEqual(const Matrix &m1, const Matrix &m2,
401  float tolerance = TOLERANCE)
402 {
403  for (int i = 0; i < 16; ++i)
404  {
405  if (! IsEqual(m1.m[i], m2.m[i], tolerance))
406  return false;
407  }
408 
409  return true;
410 }
411 
414 {
415  Math::Matrix result = m;
416  result.Transpose();
417  return result;
418 }
419 
421 
424 inline Math::Matrix MultiplyMatrices(const Math::Matrix &left, const Math::Matrix &right)
425 {
426  return left.Multiply(right);
427 }
428 
430 
442 inline Math::Vector MatrixVectorMultiply(const Math::Matrix &m, const Math::Vector &v, bool wDivide = false)
443 {
444  float x = v.x * m.m[0 ] + v.y * m.m[4 ] + v.z * m.m[8 ] + m.m[12];
445  float y = v.x * m.m[1 ] + v.y * m.m[5 ] + v.z * m.m[9 ] + m.m[13];
446  float z = v.x * m.m[2 ] + v.y * m.m[6 ] + v.z * m.m[10] + m.m[14];
447 
448  if (!wDivide)
449  return Math::Vector(x, y, z);
450 
451  float w = v.x * m.m[3 ] + v.y * m.m[7 ] + v.z * m.m[11] + m.m[15];
452 
453  if (IsZero(w))
454  return Math::Vector(x, y, z);
455 
456  x /= w;
457  y /= w;
458  z /= w;
459 
460  return Math::Vector(x, y, z);
461 }
462 
463 
464 } // namespace Math
465 
void LoadIdentity()
Loads the identity matrix.
Definition: matrix.h:128
void Transpose()
Transposes the matrix.
Definition: matrix.h:144
const float TOLERANCE
Tolerance level – minimum accepted float value.
Definition: const.h:33
bool IsZero(float a, float tolerance=Math::TOLERANCE)
Compares a to zero within tolerance.
Definition: func.h:44
void LoadZero()
Loads the zero matrix.
Definition: matrix.h:121
bool MatricesEqual(const Matrix &m1, const Matrix &m2, float tolerance=TOLERANCE)
Checks if two matrices are equal within given tolerance.
Definition: matrix.h:400
void Set(int row, int col, float value)
Sets value in given row and col.
Definition: matrix.h:104
Matrix(const float(&_m)[16])
Creates the matrix from 1D array.
Definition: matrix.h:76
4x4 matrix
Definition: matrix.h:63
float x
X - 1st coord.
Definition: vector.h:52
Math::Matrix MultiplyMatrices(const Math::Matrix &left, const Math::Matrix &right)
Convenience function for multiplying a matrix.
Definition: matrix.h:424
Matrix()
Creates the indentity matrix.
Definition: matrix.h:69
float Get(int row, int col)
Returns the value in given row and col.
Definition: matrix.h:115
Math::Matrix Transpose(const Math::Matrix &m)
Convenience function for getting transposed matrix.
Definition: matrix.h:413
bool IsEqual(float a, float b, float tolerance=Math::TOLERANCE)
Compares a and b within tolerance.
Definition: func.h:38
Math::Vector MatrixVectorMultiply(const Math::Matrix &m, const Math::Vector &v, bool wDivide=false)
Calculates the result of multiplying m * v.
Definition: matrix.h:442
float Cofactor(int r, int c) const
Calculates the cofactor of the matrix.
Definition: matrix.h:172
void Swap(int &a, int &b)
Swaps two integers.
Definition: func.h:102
float z
Z - 3rd coord.
Definition: vector.h:56
Constants used in math functions.
Matrix Inverse() const
Calculates the inverse matrix.
Definition: matrix.h:355
Vector struct and related functions.
Common math functions.
float * Array()
Returns the struct cast to float* array; use with care!
Definition: matrix.h:138
3D (3x1) vector
Definition: vector.h:49
float Det() const
Calculates the determinant of the matrix.
Definition: matrix.h:156
Matrix(const float(&_m)[4][4])
Creates the matrix from 2D array.
Definition: matrix.h:87
Matrix Multiply(const Matrix &right) const
Calculates the multiplication of this matrix * given matrix.
Definition: matrix.h:379
float y
Y - 2nd coord.
Definition: vector.h:54
float m[16]
Matrix values in column-major order.
Definition: matrix.h:66