1 /*
2 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
3
4 This software is provided 'as-is', without any express or implied warranty.
5 In no event will the authors be held liable for any damages arising from the use of this software.
6 Permission is granted to anyone to use this software for any purpose,
7 including commercial applications, and to alter it and redistribute it freely,
8 subject to the following restrictions:
9
10 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
12 3. This notice may not be removed or altered from any source distribution.
13 */
14
15
16 #ifndef BT_MATRIX3x3_H
17 #define BT_MATRIX3x3_H
18
19 #include "btVector3.h"
20 #include "btQuaternion.h"
21
22 #ifdef BT_USE_DOUBLE_PRECISION
23 #define btMatrix3x3Data btMatrix3x3DoubleData
24 #else
25 #define btMatrix3x3Data btMatrix3x3FloatData
26 #endif //BT_USE_DOUBLE_PRECISION
27
28
29 /**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3.
30 * Make sure to only include a pure orthogonal matrix without scaling. */
31 class btMatrix3x3 {
32
33 ///Data storage for the matrix, each vector is a row of the matrix
34 btVector3 m_el[3];
35
36 public:
37 /** @brief No initializaion constructor */
btMatrix3x3()38 btMatrix3x3 () {}
39
40 // explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
41
42 /**@brief Constructor from Quaternion */
btMatrix3x3(const btQuaternion & q)43 explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); }
44 /*
45 template <typename btScalar>
46 Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
47 {
48 setEulerYPR(yaw, pitch, roll);
49 }
50 */
51 /** @brief Constructor with row major formatting */
btMatrix3x3(const btScalar & xx,const btScalar & xy,const btScalar & xz,const btScalar & yx,const btScalar & yy,const btScalar & yz,const btScalar & zx,const btScalar & zy,const btScalar & zz)52 btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz,
53 const btScalar& yx, const btScalar& yy, const btScalar& yz,
54 const btScalar& zx, const btScalar& zy, const btScalar& zz)
55 {
56 setValue(xx, xy, xz,
57 yx, yy, yz,
58 zx, zy, zz);
59 }
60 /** @brief Copy constructor */
btMatrix3x3(const btMatrix3x3 & other)61 SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
62 {
63 m_el[0] = other.m_el[0];
64 m_el[1] = other.m_el[1];
65 m_el[2] = other.m_el[2];
66 }
67 /** @brief Assignment Operator */
68 SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
69 {
70 m_el[0] = other.m_el[0];
71 m_el[1] = other.m_el[1];
72 m_el[2] = other.m_el[2];
73 return *this;
74 }
75
76 /** @brief Get a column of the matrix as a vector
77 * @param i Column number 0 indexed */
getColumn(int i)78 SIMD_FORCE_INLINE btVector3 getColumn(int i) const
79 {
80 return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
81 }
82
83
84 /** @brief Get a row of the matrix as a vector
85 * @param i Row number 0 indexed */
getRow(int i)86 SIMD_FORCE_INLINE const btVector3& getRow(int i) const
87 {
88 btFullAssert(0 <= i && i < 3);
89 return m_el[i];
90 }
91
92 /** @brief Get a mutable reference to a row of the matrix as a vector
93 * @param i Row number 0 indexed */
94 SIMD_FORCE_INLINE btVector3& operator[](int i)
95 {
96 btFullAssert(0 <= i && i < 3);
97 return m_el[i];
98 }
99
100 /** @brief Get a const reference to a row of the matrix as a vector
101 * @param i Row number 0 indexed */
102 SIMD_FORCE_INLINE const btVector3& operator[](int i) const
103 {
104 btFullAssert(0 <= i && i < 3);
105 return m_el[i];
106 }
107
108 /** @brief Multiply by the target matrix on the right
109 * @param m Rotation matrix to be applied
110 * Equivilant to this = this * m */
111 btMatrix3x3& operator*=(const btMatrix3x3& m);
112
113 /** @brief Adds by the target matrix on the right
114 * @param m matrix to be applied
115 * Equivilant to this = this + m */
116 btMatrix3x3& operator+=(const btMatrix3x3& m);
117
118 /** @brief Substractss by the target matrix on the right
119 * @param m matrix to be applied
120 * Equivilant to this = this - m */
121 btMatrix3x3& operator-=(const btMatrix3x3& m);
122
123 /** @brief Set from the rotational part of a 4x4 OpenGL matrix
124 * @param m A pointer to the beginning of the array of scalars*/
setFromOpenGLSubMatrix(const btScalar * m)125 void setFromOpenGLSubMatrix(const btScalar *m)
126 {
127 m_el[0].setValue(m[0],m[4],m[8]);
128 m_el[1].setValue(m[1],m[5],m[9]);
129 m_el[2].setValue(m[2],m[6],m[10]);
130
131 }
132 /** @brief Set the values of the matrix explicitly (row major)
133 * @param xx Top left
134 * @param xy Top Middle
135 * @param xz Top Right
136 * @param yx Middle Left
137 * @param yy Middle Middle
138 * @param yz Middle Right
139 * @param zx Bottom Left
140 * @param zy Bottom Middle
141 * @param zz Bottom Right*/
setValue(const btScalar & xx,const btScalar & xy,const btScalar & xz,const btScalar & yx,const btScalar & yy,const btScalar & yz,const btScalar & zx,const btScalar & zy,const btScalar & zz)142 void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz,
143 const btScalar& yx, const btScalar& yy, const btScalar& yz,
144 const btScalar& zx, const btScalar& zy, const btScalar& zz)
145 {
146 m_el[0].setValue(xx,xy,xz);
147 m_el[1].setValue(yx,yy,yz);
148 m_el[2].setValue(zx,zy,zz);
149 }
150
151 /** @brief Set the matrix from a quaternion
152 * @param q The Quaternion to match */
setRotation(const btQuaternion & q)153 void setRotation(const btQuaternion& q)
154 {
155 btScalar d = q.length2();
156 btFullAssert(d != btScalar(0.0));
157 btScalar s = btScalar(2.0) / d;
158 btScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s;
159 btScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs;
160 btScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs;
161 btScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs;
162 setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
163 xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
164 xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
165 }
166
167
168 /** @brief Set the matrix from euler angles using YPR around YXZ respectively
169 * @param yaw Yaw about Y axis
170 * @param pitch Pitch about X axis
171 * @param roll Roll about Z axis
172 */
setEulerYPR(const btScalar & yaw,const btScalar & pitch,const btScalar & roll)173 void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
174 {
175 setEulerZYX(roll, pitch, yaw);
176 }
177
178 /** @brief Set the matrix from euler angles YPR around ZYX axes
179 * @param eulerX Roll about X axis
180 * @param eulerY Pitch around Y axis
181 * @param eulerZ Yaw aboud Z axis
182 *
183 * These angles are used to produce a rotation matrix. The euler
184 * angles are applied in ZYX order. I.e a vector is first rotated
185 * about X then Y and then Z
186 **/
setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ)187 void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) {
188 ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code
189 btScalar ci ( btCos(eulerX));
190 btScalar cj ( btCos(eulerY));
191 btScalar ch ( btCos(eulerZ));
192 btScalar si ( btSin(eulerX));
193 btScalar sj ( btSin(eulerY));
194 btScalar sh ( btSin(eulerZ));
195 btScalar cc = ci * ch;
196 btScalar cs = ci * sh;
197 btScalar sc = si * ch;
198 btScalar ss = si * sh;
199
200 setValue(cj * ch, sj * sc - cs, sj * cc + ss,
201 cj * sh, sj * ss + cc, sj * cs - sc,
202 -sj, cj * si, cj * ci);
203 }
204
205 /**@brief Set the matrix to the identity */
setIdentity()206 void setIdentity()
207 {
208 setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0),
209 btScalar(0.0), btScalar(1.0), btScalar(0.0),
210 btScalar(0.0), btScalar(0.0), btScalar(1.0));
211 }
212
getIdentity()213 static const btMatrix3x3& getIdentity()
214 {
215 static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0),
216 btScalar(0.0), btScalar(1.0), btScalar(0.0),
217 btScalar(0.0), btScalar(0.0), btScalar(1.0));
218 return identityMatrix;
219 }
220
221 /**@brief Fill the rotational part of an OpenGL matrix and clear the shear/perspective
222 * @param m The array to be filled */
getOpenGLSubMatrix(btScalar * m)223 void getOpenGLSubMatrix(btScalar *m) const
224 {
225 m[0] = btScalar(m_el[0].x());
226 m[1] = btScalar(m_el[1].x());
227 m[2] = btScalar(m_el[2].x());
228 m[3] = btScalar(0.0);
229 m[4] = btScalar(m_el[0].y());
230 m[5] = btScalar(m_el[1].y());
231 m[6] = btScalar(m_el[2].y());
232 m[7] = btScalar(0.0);
233 m[8] = btScalar(m_el[0].z());
234 m[9] = btScalar(m_el[1].z());
235 m[10] = btScalar(m_el[2].z());
236 m[11] = btScalar(0.0);
237 }
238
239 /**@brief Get the matrix represented as a quaternion
240 * @param q The quaternion which will be set */
getRotation(btQuaternion & q)241 void getRotation(btQuaternion& q) const
242 {
243 btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
244 btScalar temp[4];
245
246 if (trace > btScalar(0.0))
247 {
248 btScalar s = btSqrt(trace + btScalar(1.0));
249 temp[3]=(s * btScalar(0.5));
250 s = btScalar(0.5) / s;
251
252 temp[0]=((m_el[2].y() - m_el[1].z()) * s);
253 temp[1]=((m_el[0].z() - m_el[2].x()) * s);
254 temp[2]=((m_el[1].x() - m_el[0].y()) * s);
255 }
256 else
257 {
258 int i = m_el[0].x() < m_el[1].y() ?
259 (m_el[1].y() < m_el[2].z() ? 2 : 1) :
260 (m_el[0].x() < m_el[2].z() ? 2 : 0);
261 int j = (i + 1) % 3;
262 int k = (i + 2) % 3;
263
264 btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
265 temp[i] = s * btScalar(0.5);
266 s = btScalar(0.5) / s;
267
268 temp[3] = (m_el[k][j] - m_el[j][k]) * s;
269 temp[j] = (m_el[j][i] + m_el[i][j]) * s;
270 temp[k] = (m_el[k][i] + m_el[i][k]) * s;
271 }
272 q.setValue(temp[0],temp[1],temp[2],temp[3]);
273 }
274
275 /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
276 * @param yaw Yaw around Y axis
277 * @param pitch Pitch around X axis
278 * @param roll around Z axis */
getEulerYPR(btScalar & yaw,btScalar & pitch,btScalar & roll)279 void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const
280 {
281
282 // first use the normal calculus
283 yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
284 pitch = btScalar(btAsin(-m_el[2].x()));
285 roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
286
287 // on pitch = +/-HalfPI
288 if (btFabs(pitch)==SIMD_HALF_PI)
289 {
290 if (yaw>0)
291 yaw-=SIMD_PI;
292 else
293 yaw+=SIMD_PI;
294
295 if (roll>0)
296 roll-=SIMD_PI;
297 else
298 roll+=SIMD_PI;
299 }
300 };
301
302
303 /**@brief Get the matrix represented as euler angles around ZYX
304 * @param yaw Yaw around X axis
305 * @param pitch Pitch around Y axis
306 * @param roll around X axis
307 * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/
308 void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const
309 {
310 struct Euler
311 {
312 btScalar yaw;
313 btScalar pitch;
314 btScalar roll;
315 };
316
317 Euler euler_out;
318 Euler euler_out2; //second solution
319 //get the pointer to the raw data
320
321 // Check that pitch is not at a singularity
322 if (btFabs(m_el[2].x()) >= 1)
323 {
324 euler_out.yaw = 0;
325 euler_out2.yaw = 0;
326
327 // From difference of angles formula
328 btScalar delta = btAtan2(m_el[0].x(),m_el[0].z());
329 if (m_el[2].x() > 0) //gimbal locked up
330 {
331 euler_out.pitch = SIMD_PI / btScalar(2.0);
332 euler_out2.pitch = SIMD_PI / btScalar(2.0);
333 euler_out.roll = euler_out.pitch + delta;
334 euler_out2.roll = euler_out.pitch + delta;
335 }
336 else // gimbal locked down
337 {
338 euler_out.pitch = -SIMD_PI / btScalar(2.0);
339 euler_out2.pitch = -SIMD_PI / btScalar(2.0);
340 euler_out.roll = -euler_out.pitch + delta;
341 euler_out2.roll = -euler_out.pitch + delta;
342 }
343 }
344 else
345 {
346 euler_out.pitch = - btAsin(m_el[2].x());
347 euler_out2.pitch = SIMD_PI - euler_out.pitch;
348
349 euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch),
350 m_el[2].z()/btCos(euler_out.pitch));
351 euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch),
352 m_el[2].z()/btCos(euler_out2.pitch));
353
354 euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch),
355 m_el[0].x()/btCos(euler_out.pitch));
356 euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch),
357 m_el[0].x()/btCos(euler_out2.pitch));
358 }
359
360 if (solution_number == 1)
361 {
362 yaw = euler_out.yaw;
363 pitch = euler_out.pitch;
364 roll = euler_out.roll;
365 }
366 else
367 {
368 yaw = euler_out2.yaw;
369 pitch = euler_out2.pitch;
370 roll = euler_out2.roll;
371 }
372 }
373
374 /**@brief Create a scaled copy of the matrix
375 * @param s Scaling vector The elements of the vector will scale each column */
376
scaled(const btVector3 & s)377 btMatrix3x3 scaled(const btVector3& s) const
378 {
379 return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
380 m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
381 m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
382 }
383
384 /**@brief Return the determinant of the matrix */
385 btScalar determinant() const;
386 /**@brief Return the adjoint of the matrix */
387 btMatrix3x3 adjoint() const;
388 /**@brief Return the matrix with all values non negative */
389 btMatrix3x3 absolute() const;
390 /**@brief Return the transpose of the matrix */
391 btMatrix3x3 transpose() const;
392 /**@brief Return the inverse of the matrix */
393 btMatrix3x3 inverse() const;
394
395 btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
396 btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
397
tdotx(const btVector3 & v)398 SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const
399 {
400 return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
401 }
tdoty(const btVector3 & v)402 SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const
403 {
404 return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
405 }
tdotz(const btVector3 & v)406 SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const
407 {
408 return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
409 }
410
411
412 /**@brief diagonalizes this matrix by the Jacobi method.
413 * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
414 * coordinate system, i.e., old_this = rot * new_this * rot^T.
415 * @param threshold See iteration
416 * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied
417 * by the sum of the absolute values of the diagonal, or when maxSteps have been executed.
418 *
419 * Note that this matrix is assumed to be symmetric.
420 */
diagonalize(btMatrix3x3 & rot,btScalar threshold,int maxSteps)421 void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
422 {
423 rot.setIdentity();
424 for (int step = maxSteps; step > 0; step--)
425 {
426 // find off-diagonal element [p][q] with largest magnitude
427 int p = 0;
428 int q = 1;
429 int r = 2;
430 btScalar max = btFabs(m_el[0][1]);
431 btScalar v = btFabs(m_el[0][2]);
432 if (v > max)
433 {
434 q = 2;
435 r = 1;
436 max = v;
437 }
438 v = btFabs(m_el[1][2]);
439 if (v > max)
440 {
441 p = 1;
442 q = 2;
443 r = 0;
444 max = v;
445 }
446
447 btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2]));
448 if (max <= t)
449 {
450 if (max <= SIMD_EPSILON * t)
451 {
452 return;
453 }
454 step = 1;
455 }
456
457 // compute Jacobi rotation J which leads to a zero for element [p][q]
458 btScalar mpq = m_el[p][q];
459 btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
460 btScalar theta2 = theta * theta;
461 btScalar cos;
462 btScalar sin;
463 if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON))
464 {
465 t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
466 : 1 / (theta - btSqrt(1 + theta2));
467 cos = 1 / btSqrt(1 + t * t);
468 sin = cos * t;
469 }
470 else
471 {
472 // approximation for large theta-value, i.e., a nearly diagonal matrix
473 t = 1 / (theta * (2 + btScalar(0.5) / theta2));
474 cos = 1 - btScalar(0.5) * t * t;
475 sin = cos * t;
476 }
477
478 // apply rotation to matrix (this = J^T * this * J)
479 m_el[p][q] = m_el[q][p] = 0;
480 m_el[p][p] -= t * mpq;
481 m_el[q][q] += t * mpq;
482 btScalar mrp = m_el[r][p];
483 btScalar mrq = m_el[r][q];
484 m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
485 m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
486
487 // apply rotation to rot (rot = rot * J)
488 for (int i = 0; i < 3; i++)
489 {
490 btVector3& row = rot[i];
491 mrp = row[p];
492 mrq = row[q];
493 row[p] = cos * mrp - sin * mrq;
494 row[q] = cos * mrq + sin * mrp;
495 }
496 }
497 }
498
499
500
501
502 /**@brief Calculate the matrix cofactor
503 * @param r1 The first row to use for calculating the cofactor
504 * @param c1 The first column to use for calculating the cofactor
505 * @param r1 The second row to use for calculating the cofactor
506 * @param c1 The second column to use for calculating the cofactor
507 * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
508 */
cofac(int r1,int c1,int r2,int c2)509 btScalar cofac(int r1, int c1, int r2, int c2) const
510 {
511 return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
512 }
513
514 void serialize(struct btMatrix3x3Data& dataOut) const;
515
516 void serializeFloat(struct btMatrix3x3FloatData& dataOut) const;
517
518 void deSerialize(const struct btMatrix3x3Data& dataIn);
519
520 void deSerializeFloat(const struct btMatrix3x3FloatData& dataIn);
521
522 void deSerializeDouble(const struct btMatrix3x3DoubleData& dataIn);
523
524 };
525
526
527 SIMD_FORCE_INLINE btMatrix3x3&
528 btMatrix3x3::operator*=(const btMatrix3x3& m)
529 {
530 setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
531 m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
532 m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
533 return *this;
534 }
535
536 SIMD_FORCE_INLINE btMatrix3x3&
537 btMatrix3x3::operator+=(const btMatrix3x3& m)
538 {
539 setValue(
540 m_el[0][0]+m.m_el[0][0],
541 m_el[0][1]+m.m_el[0][1],
542 m_el[0][2]+m.m_el[0][2],
543 m_el[1][0]+m.m_el[1][0],
544 m_el[1][1]+m.m_el[1][1],
545 m_el[1][2]+m.m_el[1][2],
546 m_el[2][0]+m.m_el[2][0],
547 m_el[2][1]+m.m_el[2][1],
548 m_el[2][2]+m.m_el[2][2]);
549 return *this;
550 }
551
552 SIMD_FORCE_INLINE btMatrix3x3
553 operator*(const btMatrix3x3& m, const btScalar & k)
554 {
555 return btMatrix3x3(
556 m[0].x()*k,m[0].y()*k,m[0].z()*k,
557 m[1].x()*k,m[1].y()*k,m[1].z()*k,
558 m[2].x()*k,m[2].y()*k,m[2].z()*k);
559 }
560
561 SIMD_FORCE_INLINE btMatrix3x3
562 operator+(const btMatrix3x3& m1, const btMatrix3x3& m2)
563 {
564 return btMatrix3x3(
565 m1[0][0]+m2[0][0],
566 m1[0][1]+m2[0][1],
567 m1[0][2]+m2[0][2],
568 m1[1][0]+m2[1][0],
569 m1[1][1]+m2[1][1],
570 m1[1][2]+m2[1][2],
571 m1[2][0]+m2[2][0],
572 m1[2][1]+m2[2][1],
573 m1[2][2]+m2[2][2]);
574 }
575
576 SIMD_FORCE_INLINE btMatrix3x3
577 operator-(const btMatrix3x3& m1, const btMatrix3x3& m2)
578 {
579 return btMatrix3x3(
580 m1[0][0]-m2[0][0],
581 m1[0][1]-m2[0][1],
582 m1[0][2]-m2[0][2],
583 m1[1][0]-m2[1][0],
584 m1[1][1]-m2[1][1],
585 m1[1][2]-m2[1][2],
586 m1[2][0]-m2[2][0],
587 m1[2][1]-m2[2][1],
588 m1[2][2]-m2[2][2]);
589 }
590
591
592 SIMD_FORCE_INLINE btMatrix3x3&
593 btMatrix3x3::operator-=(const btMatrix3x3& m)
594 {
595 setValue(
596 m_el[0][0]-m.m_el[0][0],
597 m_el[0][1]-m.m_el[0][1],
598 m_el[0][2]-m.m_el[0][2],
599 m_el[1][0]-m.m_el[1][0],
600 m_el[1][1]-m.m_el[1][1],
601 m_el[1][2]-m.m_el[1][2],
602 m_el[2][0]-m.m_el[2][0],
603 m_el[2][1]-m.m_el[2][1],
604 m_el[2][2]-m.m_el[2][2]);
605 return *this;
606 }
607
608
609 SIMD_FORCE_INLINE btScalar
determinant()610 btMatrix3x3::determinant() const
611 {
612 return btTriple((*this)[0], (*this)[1], (*this)[2]);
613 }
614
615
616 SIMD_FORCE_INLINE btMatrix3x3
absolute()617 btMatrix3x3::absolute() const
618 {
619 return btMatrix3x3(
620 btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
621 btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
622 btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
623 }
624
625 SIMD_FORCE_INLINE btMatrix3x3
transpose()626 btMatrix3x3::transpose() const
627 {
628 return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
629 m_el[0].y(), m_el[1].y(), m_el[2].y(),
630 m_el[0].z(), m_el[1].z(), m_el[2].z());
631 }
632
633 SIMD_FORCE_INLINE btMatrix3x3
adjoint()634 btMatrix3x3::adjoint() const
635 {
636 return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
637 cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
638 cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
639 }
640
641 SIMD_FORCE_INLINE btMatrix3x3
inverse()642 btMatrix3x3::inverse() const
643 {
644 btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
645 btScalar det = (*this)[0].dot(co);
646 btFullAssert(det != btScalar(0.0));
647 btScalar s = btScalar(1.0) / det;
648 return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
649 co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
650 co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
651 }
652
653 SIMD_FORCE_INLINE btMatrix3x3
transposeTimes(const btMatrix3x3 & m)654 btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
655 {
656 return btMatrix3x3(
657 m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
658 m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
659 m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
660 m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
661 m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
662 m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
663 m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
664 m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
665 m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
666 }
667
668 SIMD_FORCE_INLINE btMatrix3x3
timesTranspose(const btMatrix3x3 & m)669 btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
670 {
671 return btMatrix3x3(
672 m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
673 m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
674 m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
675
676 }
677
678 SIMD_FORCE_INLINE btVector3
679 operator*(const btMatrix3x3& m, const btVector3& v)
680 {
681 return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
682 }
683
684
685 SIMD_FORCE_INLINE btVector3
686 operator*(const btVector3& v, const btMatrix3x3& m)
687 {
688 return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
689 }
690
691 SIMD_FORCE_INLINE btMatrix3x3
692 operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
693 {
694 return btMatrix3x3(
695 m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
696 m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
697 m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
698 }
699
700 /*
701 SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
702 return btMatrix3x3(
703 m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
704 m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
705 m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
706 m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
707 m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
708 m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
709 m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
710 m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
711 m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
712 }
713 */
714
715 /**@brief Equality operator between two matrices
716 * It will test all elements are equal. */
717 SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
718 {
719 return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
720 m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
721 m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
722 }
723
724 ///for serialization
725 struct btMatrix3x3FloatData
726 {
727 btVector3FloatData m_el[3];
728 };
729
730 ///for serialization
731 struct btMatrix3x3DoubleData
732 {
733 btVector3DoubleData m_el[3];
734 };
735
736
737
738
serialize(struct btMatrix3x3Data & dataOut)739 SIMD_FORCE_INLINE void btMatrix3x3::serialize(struct btMatrix3x3Data& dataOut) const
740 {
741 for (int i=0;i<3;i++)
742 m_el[i].serialize(dataOut.m_el[i]);
743 }
744
serializeFloat(struct btMatrix3x3FloatData & dataOut)745 SIMD_FORCE_INLINE void btMatrix3x3::serializeFloat(struct btMatrix3x3FloatData& dataOut) const
746 {
747 for (int i=0;i<3;i++)
748 m_el[i].serializeFloat(dataOut.m_el[i]);
749 }
750
751
deSerialize(const struct btMatrix3x3Data & dataIn)752 SIMD_FORCE_INLINE void btMatrix3x3::deSerialize(const struct btMatrix3x3Data& dataIn)
753 {
754 for (int i=0;i<3;i++)
755 m_el[i].deSerialize(dataIn.m_el[i]);
756 }
757
deSerializeFloat(const struct btMatrix3x3FloatData & dataIn)758 SIMD_FORCE_INLINE void btMatrix3x3::deSerializeFloat(const struct btMatrix3x3FloatData& dataIn)
759 {
760 for (int i=0;i<3;i++)
761 m_el[i].deSerializeFloat(dataIn.m_el[i]);
762 }
763
deSerializeDouble(const struct btMatrix3x3DoubleData & dataIn)764 SIMD_FORCE_INLINE void btMatrix3x3::deSerializeDouble(const struct btMatrix3x3DoubleData& dataIn)
765 {
766 for (int i=0;i<3;i++)
767 m_el[i].deSerializeDouble(dataIn.m_el[i]);
768 }
769
770 #endif //BT_MATRIX3x3_H
771
772