2
Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/
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:
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.
16
#ifndef BT_MATRIX3x3_H
17
#define BT_MATRIX3x3_H
19
#include "btVector3.h"
20
#include "btQuaternion.h"
22
#ifdef BT_USE_DOUBLE_PRECISION
23
#define btMatrix3x3Data btMatrix3x3DoubleData
25
#define btMatrix3x3Data btMatrix3x3FloatData
26
#endif //BT_USE_DOUBLE_PRECISION
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. */
33
///Data storage for the matrix, each vector is a row of the matrix
37
/** @brief No initializaion constructor */
40
// explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
42
/**@brief Constructor from Quaternion */
43
explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); }
45
template <typename btScalar>
46
Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
48
setEulerYPR(yaw, pitch, roll);
51
/** @brief Constructor with row major formatting */
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)
60
/** @brief Copy constructor */
61
SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
63
m_el[0] = other.m_el[0];
64
m_el[1] = other.m_el[1];
65
m_el[2] = other.m_el[2];
67
/** @brief Assignment Operator */
68
SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
70
m_el[0] = other.m_el[0];
71
m_el[1] = other.m_el[1];
72
m_el[2] = other.m_el[2];
76
/** @brief Get a column of the matrix as a vector
77
* @param i Column number 0 indexed */
78
SIMD_FORCE_INLINE btVector3 getColumn(int i) const
80
return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
84
/** @brief Get a row of the matrix as a vector
85
* @param i Row number 0 indexed */
86
SIMD_FORCE_INLINE const btVector3& getRow(int i) const
88
btFullAssert(0 <= i && i < 3);
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)
96
btFullAssert(0 <= i && i < 3);
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
104
btFullAssert(0 <= i && i < 3);
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);
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);
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);
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*/
125
void setFromOpenGLSubMatrix(const btScalar *m)
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]);
132
/** @brief Set the values of the matrix explicitly (row major)
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*/
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)
146
m_el[0].setValue(xx,xy,xz);
147
m_el[1].setValue(yx,yy,yz);
148
m_el[2].setValue(zx,zy,zz);
151
/** @brief Set the matrix from a quaternion
152
* @param q The Quaternion to match */
153
void setRotation(const btQuaternion& q)
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));
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
173
void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
175
setEulerZYX(roll, pitch, yaw);
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
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
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;
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);
205
/**@brief Set the matrix to the identity */
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));
213
static const btMatrix3x3& getIdentity()
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;
221
/**@brief Fill the rotational part of an OpenGL matrix and clear the shear/perspective
222
* @param m The array to be filled */
223
void getOpenGLSubMatrix(btScalar *m) const
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);
239
/**@brief Get the matrix represented as a quaternion
240
* @param q The quaternion which will be set */
241
void getRotation(btQuaternion& q) const
243
btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
246
if (trace > btScalar(0.0))
248
btScalar s = btSqrt(trace + btScalar(1.0));
249
temp[3]=(s * btScalar(0.5));
250
s = btScalar(0.5) / s;
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);
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);
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;
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;
272
q.setValue(temp[0],temp[1],temp[2],temp[3]);
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 */
279
void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const
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()));
287
// on pitch = +/-HalfPI
288
if (btFabs(pitch)==SIMD_HALF_PI)
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
318
Euler euler_out2; //second solution
319
//get the pointer to the raw data
321
// Check that pitch is not at a singularity
322
if (btFabs(m_el[2].x()) >= 1)
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
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;
336
else // gimbal locked down
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;
346
euler_out.pitch = - btAsin(m_el[2].x());
347
euler_out2.pitch = SIMD_PI - euler_out.pitch;
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));
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));
360
if (solution_number == 1)
363
pitch = euler_out.pitch;
364
roll = euler_out.roll;
368
yaw = euler_out2.yaw;
369
pitch = euler_out2.pitch;
370
roll = euler_out2.roll;
374
/**@brief Create a scaled copy of the matrix
375
* @param s Scaling vector The elements of the vector will scale each column */
377
btMatrix3x3 scaled(const btVector3& s) const
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());
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;
395
btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
396
btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
398
SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const
400
return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
402
SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const
404
return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
406
SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const
408
return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
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.
419
* Note that this matrix is assumed to be symmetric.
421
void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
424
for (int step = maxSteps; step > 0; step--)
426
// find off-diagonal element [p][q] with largest magnitude
430
btScalar max = btFabs(m_el[0][1]);
431
btScalar v = btFabs(m_el[0][2]);
438
v = btFabs(m_el[1][2]);
447
btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2]));
450
if (max <= SIMD_EPSILON * t)
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;
463
if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON))
465
t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
466
: 1 / (theta - btSqrt(1 + theta2));
467
cos = 1 / btSqrt(1 + t * t);
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;
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;
487
// apply rotation to rot (rot = rot * J)
488
for (int i = 0; i < 3; i++)
490
btVector3& row = rot[i];
493
row[p] = cos * mrp - sin * mrq;
494
row[q] = cos * mrq + sin * mrp;
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
509
btScalar cofac(int r1, int c1, int r2, int c2) const
511
return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
514
void serialize(struct btMatrix3x3Data& dataOut) const;
516
void serializeFloat(struct btMatrix3x3FloatData& dataOut) const;
518
void deSerialize(const struct btMatrix3x3Data& dataIn);
520
void deSerializeFloat(const struct btMatrix3x3FloatData& dataIn);
522
void deSerializeDouble(const struct btMatrix3x3DoubleData& dataIn);
527
SIMD_FORCE_INLINE btMatrix3x3&
528
btMatrix3x3::operator*=(const btMatrix3x3& m)
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]));
536
SIMD_FORCE_INLINE btMatrix3x3&
537
btMatrix3x3::operator+=(const btMatrix3x3& m)
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]);
552
SIMD_FORCE_INLINE btMatrix3x3
553
operator*(const btMatrix3x3& m, const btScalar & k)
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);
561
SIMD_FORCE_INLINE btMatrix3x3
562
operator+(const btMatrix3x3& m1, const btMatrix3x3& m2)
576
SIMD_FORCE_INLINE btMatrix3x3
577
operator-(const btMatrix3x3& m1, const btMatrix3x3& m2)
592
SIMD_FORCE_INLINE btMatrix3x3&
593
btMatrix3x3::operator-=(const btMatrix3x3& m)
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]);
609
SIMD_FORCE_INLINE btScalar
610
btMatrix3x3::determinant() const
612
return btTriple((*this)[0], (*this)[1], (*this)[2]);
616
SIMD_FORCE_INLINE btMatrix3x3
617
btMatrix3x3::absolute() const
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()));
625
SIMD_FORCE_INLINE btMatrix3x3
626
btMatrix3x3::transpose() const
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());
633
SIMD_FORCE_INLINE btMatrix3x3
634
btMatrix3x3::adjoint() const
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));
641
SIMD_FORCE_INLINE btMatrix3x3
642
btMatrix3x3::inverse() const
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);
653
SIMD_FORCE_INLINE btMatrix3x3
654
btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
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());
668
SIMD_FORCE_INLINE btMatrix3x3
669
btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
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]));
678
SIMD_FORCE_INLINE btVector3
679
operator*(const btMatrix3x3& m, const btVector3& v)
681
return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
685
SIMD_FORCE_INLINE btVector3
686
operator*(const btVector3& v, const btMatrix3x3& m)
688
return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
691
SIMD_FORCE_INLINE btMatrix3x3
692
operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
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]));
701
SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
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]);
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)
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] );
725
struct btMatrix3x3FloatData
727
btVector3FloatData m_el[3];
731
struct btMatrix3x3DoubleData
733
btVector3DoubleData m_el[3];
739
SIMD_FORCE_INLINE void btMatrix3x3::serialize(struct btMatrix3x3Data& dataOut) const
741
for (int i=0;i<3;i++)
742
m_el[i].serialize(dataOut.m_el[i]);
745
SIMD_FORCE_INLINE void btMatrix3x3::serializeFloat(struct btMatrix3x3FloatData& dataOut) const
747
for (int i=0;i<3;i++)
748
m_el[i].serializeFloat(dataOut.m_el[i]);
752
SIMD_FORCE_INLINE void btMatrix3x3::deSerialize(const struct btMatrix3x3Data& dataIn)
754
for (int i=0;i<3;i++)
755
m_el[i].deSerialize(dataIn.m_el[i]);
758
SIMD_FORCE_INLINE void btMatrix3x3::deSerializeFloat(const struct btMatrix3x3FloatData& dataIn)
760
for (int i=0;i<3;i++)
761
m_el[i].deSerializeFloat(dataIn.m_el[i]);
764
SIMD_FORCE_INLINE void btMatrix3x3::deSerializeDouble(const struct btMatrix3x3DoubleData& dataIn)
766
for (int i=0;i<3;i++)
767
m_el[i].deSerializeDouble(dataIn.m_el[i]);
770
#endif //BT_MATRIX3x3_H
added
removed
tests/bullet/src/LinearMath/btMatrix3x3.h
