1 /// @file Built-In Matrix-Vector functions
2 #ifndef IDMATVEC_HPP_
3 #define IDMATVEC_HPP_
4
5 #include <cstdlib>
6
7 #include "../IDConfig.hpp"
8 #define BT_ID_HAVE_MAT3X
9
10 namespace btInverseDynamics
11 {
12 class vec3;
13 class vecx;
14 class mat33;
15 class matxx;
16 class mat3x;
17
18 /// This is a very basic implementation to enable stand-alone use of the library.
19 /// The implementation is not really optimized and misses many features that you would
20 /// want from a "fully featured" linear math library.
21 class vec3
22 {
23 public:
operator ()(int i)24 idScalar& operator()(int i) { return m_data[i]; }
operator ()(int i) const25 const idScalar& operator()(int i) const { return m_data[i]; }
size() const26 const int size() const { return 3; }
27 const vec3& operator=(const vec3& rhs);
28 const vec3& operator+=(const vec3& b);
29 const vec3& operator-=(const vec3& b);
30 vec3 cross(const vec3& b) const;
31 idScalar dot(const vec3& b) const;
32
33 friend vec3 operator*(const mat33& a, const vec3& b);
34 friend vec3 operator*(const vec3& a, const idScalar& s);
35 friend vec3 operator*(const idScalar& s, const vec3& a);
36
37 friend vec3 operator+(const vec3& a, const vec3& b);
38 friend vec3 operator-(const vec3& a, const vec3& b);
39 friend vec3 operator/(const vec3& a, const idScalar& s);
40
41 private:
42 idScalar m_data[3];
43 };
44
45 class mat33
46 {
47 public:
operator ()(int i,int j)48 idScalar& operator()(int i, int j) { return m_data[3 * i + j]; }
operator ()(int i,int j) const49 const idScalar& operator()(int i, int j) const { return m_data[3 * i + j]; }
50 const mat33& operator=(const mat33& rhs);
51 mat33 transpose() const;
52 const mat33& operator+=(const mat33& b);
53 const mat33& operator-=(const mat33& b);
54
55 friend mat33 operator*(const mat33& a, const mat33& b);
56 friend vec3 operator*(const mat33& a, const vec3& b);
57 friend mat33 operator*(const mat33& a, const idScalar& s);
58 friend mat33 operator*(const idScalar& s, const mat33& a);
59 friend mat33 operator+(const mat33& a, const mat33& b);
60 friend mat33 operator-(const mat33& a, const mat33& b);
61 friend mat33 operator/(const mat33& a, const idScalar& s);
62
63 private:
64 // layout is [0,1,2;3,4,5;6,7,8]
65 idScalar m_data[9];
66 };
67
68 class vecx
69 {
70 public:
vecx(int size)71 vecx(int size) : m_size(size)
72 {
73 m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * size));
74 }
~vecx()75 ~vecx() { idFree(m_data); }
76 const vecx& operator=(const vecx& rhs);
operator ()(int i)77 idScalar& operator()(int i) { return m_data[i]; }
operator ()(int i) const78 const idScalar& operator()(int i) const { return m_data[i]; }
size() const79 const int& size() const { return m_size; }
80
81 friend vecx operator*(const vecx& a, const idScalar& s);
82 friend vecx operator*(const idScalar& s, const vecx& a);
83
84 friend vecx operator+(const vecx& a, const vecx& b);
85 friend vecx operator-(const vecx& a, const vecx& b);
86 friend vecx operator/(const vecx& a, const idScalar& s);
87
88 private:
89 int m_size;
90 idScalar* m_data;
91 };
92
93 class matxx
94 {
95 public:
matxx()96 matxx()
97 {
98 m_data = 0x0;
99 m_cols = 0;
100 m_rows = 0;
101 }
matxx(int rows,int cols)102 matxx(int rows, int cols) : m_rows(rows), m_cols(cols)
103 {
104 m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * rows * cols));
105 }
~matxx()106 ~matxx() { idFree(m_data); }
operator ()(int row,int col)107 idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; }
operator ()(int row,int col) const108 const idScalar& operator()(int row, int col) const { return m_data[row * m_cols + col]; }
rows() const109 const int& rows() const { return m_rows; }
cols() const110 const int& cols() const { return m_cols; }
111
112 private:
113 int m_rows;
114 int m_cols;
115 idScalar* m_data;
116 };
117
118 class mat3x
119 {
120 public:
mat3x()121 mat3x()
122 {
123 m_data = 0x0;
124 m_cols = 0;
125 }
mat3x(const mat3x & rhs)126 mat3x(const mat3x& rhs)
127 {
128 m_cols = rhs.m_cols;
129 allocate();
130 *this = rhs;
131 }
mat3x(int rows,int cols)132 mat3x(int rows, int cols) : m_cols(cols)
133 {
134 allocate();
135 };
operator =(const mat3x & rhs)136 void operator=(const mat3x& rhs)
137 {
138 if (m_cols != rhs.m_cols)
139 {
140 bt_id_error_message("size missmatch, cols= %d but rhs.cols= %d\n", cols(), rhs.cols());
141 abort();
142 }
143 for (int i = 0; i < 3 * m_cols; i++)
144 {
145 m_data[i] = rhs.m_data[i];
146 }
147 }
148
~mat3x()149 ~mat3x()
150 {
151 free();
152 }
operator ()(int row,int col)153 idScalar& operator()(int row, int col) { return m_data[row * m_cols + col]; }
operator ()(int row,int col) const154 const idScalar& operator()(int row, int col) const { return m_data[row * m_cols + col]; }
rows() const155 int rows() const { return m_rows; }
cols() const156 const int& cols() const { return m_cols; }
resize(int rows,int cols)157 void resize(int rows, int cols)
158 {
159 m_cols = cols;
160 free();
161 allocate();
162 }
setZero()163 void setZero()
164 {
165 memset(m_data, 0x0, sizeof(idScalar) * m_rows * m_cols);
166 }
167 // avoid operators that would allocate -- use functions sub/add/mul in IDMath.hpp instead
168 private:
allocate()169 void allocate() { m_data = static_cast<idScalar*>(idMalloc(sizeof(idScalar) * m_rows * m_cols)); }
free()170 void free() { idFree(m_data); }
171 enum
172 {
173 m_rows = 3
174 };
175 int m_cols;
176 idScalar* m_data;
177 };
178
resize(mat3x & m,idArrayIdx size)179 inline void resize(mat3x& m, idArrayIdx size)
180 {
181 m.resize(3, size);
182 m.setZero();
183 }
184
185 //////////////////////////////////////////////////
186 // Implementations
operator =(const vec3 & rhs)187 inline const vec3& vec3::operator=(const vec3& rhs)
188 {
189 if (&rhs != this)
190 {
191 memcpy(m_data, rhs.m_data, 3 * sizeof(idScalar));
192 }
193 return *this;
194 }
195
cross(const vec3 & b) const196 inline vec3 vec3::cross(const vec3& b) const
197 {
198 vec3 result;
199 result.m_data[0] = m_data[1] * b.m_data[2] - m_data[2] * b.m_data[1];
200 result.m_data[1] = m_data[2] * b.m_data[0] - m_data[0] * b.m_data[2];
201 result.m_data[2] = m_data[0] * b.m_data[1] - m_data[1] * b.m_data[0];
202
203 return result;
204 }
205
dot(const vec3 & b) const206 inline idScalar vec3::dot(const vec3& b) const
207 {
208 return m_data[0] * b.m_data[0] + m_data[1] * b.m_data[1] + m_data[2] * b.m_data[2];
209 }
210
operator =(const mat33 & rhs)211 inline const mat33& mat33::operator=(const mat33& rhs)
212 {
213 if (&rhs != this)
214 {
215 memcpy(m_data, rhs.m_data, 9 * sizeof(idScalar));
216 }
217 return *this;
218 }
transpose() const219 inline mat33 mat33::transpose() const
220 {
221 mat33 result;
222 result.m_data[0] = m_data[0];
223 result.m_data[1] = m_data[3];
224 result.m_data[2] = m_data[6];
225 result.m_data[3] = m_data[1];
226 result.m_data[4] = m_data[4];
227 result.m_data[5] = m_data[7];
228 result.m_data[6] = m_data[2];
229 result.m_data[7] = m_data[5];
230 result.m_data[8] = m_data[8];
231
232 return result;
233 }
234
operator *(const mat33 & a,const mat33 & b)235 inline mat33 operator*(const mat33& a, const mat33& b)
236 {
237 mat33 result;
238 result.m_data[0] =
239 a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[3] + a.m_data[2] * b.m_data[6];
240 result.m_data[1] =
241 a.m_data[0] * b.m_data[1] + a.m_data[1] * b.m_data[4] + a.m_data[2] * b.m_data[7];
242 result.m_data[2] =
243 a.m_data[0] * b.m_data[2] + a.m_data[1] * b.m_data[5] + a.m_data[2] * b.m_data[8];
244 result.m_data[3] =
245 a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[3] + a.m_data[5] * b.m_data[6];
246 result.m_data[4] =
247 a.m_data[3] * b.m_data[1] + a.m_data[4] * b.m_data[4] + a.m_data[5] * b.m_data[7];
248 result.m_data[5] =
249 a.m_data[3] * b.m_data[2] + a.m_data[4] * b.m_data[5] + a.m_data[5] * b.m_data[8];
250 result.m_data[6] =
251 a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[3] + a.m_data[8] * b.m_data[6];
252 result.m_data[7] =
253 a.m_data[6] * b.m_data[1] + a.m_data[7] * b.m_data[4] + a.m_data[8] * b.m_data[7];
254 result.m_data[8] =
255 a.m_data[6] * b.m_data[2] + a.m_data[7] * b.m_data[5] + a.m_data[8] * b.m_data[8];
256
257 return result;
258 }
259
operator +=(const mat33 & b)260 inline const mat33& mat33::operator+=(const mat33& b)
261 {
262 for (int i = 0; i < 9; i++)
263 {
264 m_data[i] += b.m_data[i];
265 }
266
267 return *this;
268 }
269
operator -=(const mat33 & b)270 inline const mat33& mat33::operator-=(const mat33& b)
271 {
272 for (int i = 0; i < 9; i++)
273 {
274 m_data[i] -= b.m_data[i];
275 }
276 return *this;
277 }
278
operator *(const mat33 & a,const vec3 & b)279 inline vec3 operator*(const mat33& a, const vec3& b)
280 {
281 vec3 result;
282
283 result.m_data[0] =
284 a.m_data[0] * b.m_data[0] + a.m_data[1] * b.m_data[1] + a.m_data[2] * b.m_data[2];
285 result.m_data[1] =
286 a.m_data[3] * b.m_data[0] + a.m_data[4] * b.m_data[1] + a.m_data[5] * b.m_data[2];
287 result.m_data[2] =
288 a.m_data[6] * b.m_data[0] + a.m_data[7] * b.m_data[1] + a.m_data[8] * b.m_data[2];
289
290 return result;
291 }
292
operator +=(const vec3 & b)293 inline const vec3& vec3::operator+=(const vec3& b)
294 {
295 for (int i = 0; i < 3; i++)
296 {
297 m_data[i] += b.m_data[i];
298 }
299 return *this;
300 }
301
operator -=(const vec3 & b)302 inline const vec3& vec3::operator-=(const vec3& b)
303 {
304 for (int i = 0; i < 3; i++)
305 {
306 m_data[i] -= b.m_data[i];
307 }
308 return *this;
309 }
310
operator *(const mat33 & a,const idScalar & s)311 inline mat33 operator*(const mat33& a, const idScalar& s)
312 {
313 mat33 result;
314 for (int i = 0; i < 9; i++)
315 {
316 result.m_data[i] = a.m_data[i] * s;
317 }
318 return result;
319 }
320
operator *(const idScalar & s,const mat33 & a)321 inline mat33 operator*(const idScalar& s, const mat33& a) { return a * s; }
322
operator *(const vec3 & a,const idScalar & s)323 inline vec3 operator*(const vec3& a, const idScalar& s)
324 {
325 vec3 result;
326 for (int i = 0; i < 3; i++)
327 {
328 result.m_data[i] = a.m_data[i] * s;
329 }
330 return result;
331 }
operator *(const idScalar & s,const vec3 & a)332 inline vec3 operator*(const idScalar& s, const vec3& a) { return a * s; }
333
operator +(const mat33 & a,const mat33 & b)334 inline mat33 operator+(const mat33& a, const mat33& b)
335 {
336 mat33 result;
337 for (int i = 0; i < 9; i++)
338 {
339 result.m_data[i] = a.m_data[i] + b.m_data[i];
340 }
341 return result;
342 }
operator +(const vec3 & a,const vec3 & b)343 inline vec3 operator+(const vec3& a, const vec3& b)
344 {
345 vec3 result;
346 for (int i = 0; i < 3; i++)
347 {
348 result.m_data[i] = a.m_data[i] + b.m_data[i];
349 }
350 return result;
351 }
352
operator -(const mat33 & a,const mat33 & b)353 inline mat33 operator-(const mat33& a, const mat33& b)
354 {
355 mat33 result;
356 for (int i = 0; i < 9; i++)
357 {
358 result.m_data[i] = a.m_data[i] - b.m_data[i];
359 }
360 return result;
361 }
operator -(const vec3 & a,const vec3 & b)362 inline vec3 operator-(const vec3& a, const vec3& b)
363 {
364 vec3 result;
365 for (int i = 0; i < 3; i++)
366 {
367 result.m_data[i] = a.m_data[i] - b.m_data[i];
368 }
369 return result;
370 }
371
operator /(const mat33 & a,const idScalar & s)372 inline mat33 operator/(const mat33& a, const idScalar& s)
373 {
374 mat33 result;
375 for (int i = 0; i < 9; i++)
376 {
377 result.m_data[i] = a.m_data[i] / s;
378 }
379 return result;
380 }
381
operator /(const vec3 & a,const idScalar & s)382 inline vec3 operator/(const vec3& a, const idScalar& s)
383 {
384 vec3 result;
385 for (int i = 0; i < 3; i++)
386 {
387 result.m_data[i] = a.m_data[i] / s;
388 }
389 return result;
390 }
391
operator =(const vecx & rhs)392 inline const vecx& vecx::operator=(const vecx& rhs)
393 {
394 if (size() != rhs.size())
395 {
396 bt_id_error_message("size missmatch, size()= %d but rhs.size()= %d\n", size(), rhs.size());
397 abort();
398 }
399 if (&rhs != this)
400 {
401 memcpy(m_data, rhs.m_data, rhs.size() * sizeof(idScalar));
402 }
403 return *this;
404 }
operator *(const vecx & a,const idScalar & s)405 inline vecx operator*(const vecx& a, const idScalar& s)
406 {
407 vecx result(a.size());
408 for (int i = 0; i < result.size(); i++)
409 {
410 result.m_data[i] = a.m_data[i] * s;
411 }
412 return result;
413 }
operator *(const idScalar & s,const vecx & a)414 inline vecx operator*(const idScalar& s, const vecx& a) { return a * s; }
operator +(const vecx & a,const vecx & b)415 inline vecx operator+(const vecx& a, const vecx& b)
416 {
417 vecx result(a.size());
418 // TODO: error handling for a.size() != b.size()??
419 if (a.size() != b.size())
420 {
421 bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
422 abort();
423 }
424 for (int i = 0; i < a.size(); i++)
425 {
426 result.m_data[i] = a.m_data[i] + b.m_data[i];
427 }
428
429 return result;
430 }
operator -(const vecx & a,const vecx & b)431 inline vecx operator-(const vecx& a, const vecx& b)
432 {
433 vecx result(a.size());
434 // TODO: error handling for a.size() != b.size()??
435 if (a.size() != b.size())
436 {
437 bt_id_error_message("size missmatch. a.size()= %d, b.size()= %d\n", a.size(), b.size());
438 abort();
439 }
440 for (int i = 0; i < a.size(); i++)
441 {
442 result.m_data[i] = a.m_data[i] - b.m_data[i];
443 }
444 return result;
445 }
operator /(const vecx & a,const idScalar & s)446 inline vecx operator/(const vecx& a, const idScalar& s)
447 {
448 vecx result(a.size());
449 for (int i = 0; i < result.size(); i++)
450 {
451 result.m_data[i] = a.m_data[i] / s;
452 }
453
454 return result;
455 }
456
operator *(const mat3x & a,const vecx & b)457 inline vec3 operator*(const mat3x& a, const vecx& b)
458 {
459 vec3 result;
460 if (a.cols() != b.size())
461 {
462 bt_id_error_message("size missmatch. a.cols()= %d, b.size()= %d\n", a.cols(), b.size());
463 abort();
464 }
465 result(0) = 0.0;
466 result(1) = 0.0;
467 result(2) = 0.0;
468 for (int i = 0; i < b.size(); i++)
469 {
470 for (int k = 0; k < 3; k++)
471 {
472 result(k) += a(k, i) * b(i);
473 }
474 }
475 return result;
476 }
477
setMatxxElem(const idArrayIdx row,const idArrayIdx col,const idScalar val,matxx * m)478 inline void setMatxxElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, matxx* m)
479 {
480 (*m)(row, col) = val;
481 }
482
setMat3xElem(const idArrayIdx row,const idArrayIdx col,const idScalar val,mat3x * m)483 inline void setMat3xElem(const idArrayIdx row, const idArrayIdx col, const idScalar val, mat3x* m)
484 {
485 (*m)(row, col) = val;
486 }
487
488 } // namespace btInverseDynamics
489 #endif
490