Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/trunk/src/external/bullet/LinearMath/btMatrix3x3.h @ 11029

Last change on this file since 11029 was 8393, checked in by rgrieder, 14 years ago

Updated Bullet from v2.77 to v2.78.
(I'm not going to make a branch for that since the update from 2.74 to 2.77 hasn't been tested that much either).

You will HAVE to do a complete RECOMPILE! I tested with MSVC and MinGW and they both threw linker errors at me.

  • Property svn:eol-style set to native
File size: 22.5 KB
Line 
1/*
2Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
3
4This software is provided 'as-is', without any express or implied warranty.
5In no event will the authors be held liable for any damages arising from the use of this software.
6Permission is granted to anyone to use this software for any purpose,
7including commercial applications, and to alter it and redistribute it freely,
8subject to the following restrictions:
9
101. 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.
112. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
123. 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. */
31class btMatrix3x3 {
32
33        ///Data storage for the matrix, each vector is a row of the matrix
34        btVector3 m_el[3];
35
36public:
37        /** @brief No initializaion constructor */
38        btMatrix3x3 () {}
39
40        //              explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
41
42        /**@brief Constructor from Quaternion */
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 */
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 */
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 */
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 */
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*/
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*/
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 */ 
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        */
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        **/
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 */
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
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 */
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 */
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 */ 
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
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
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        }
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        }
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        */
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        */
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
527SIMD_FORCE_INLINE btMatrix3x3& 
528btMatrix3x3::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
536SIMD_FORCE_INLINE btMatrix3x3& 
537btMatrix3x3::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
552SIMD_FORCE_INLINE btMatrix3x3
553operator*(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
562operator+(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
576SIMD_FORCE_INLINE btMatrix3x3
577operator-(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
592SIMD_FORCE_INLINE btMatrix3x3& 
593btMatrix3x3::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
609SIMD_FORCE_INLINE btScalar
610btMatrix3x3::determinant() const
611{ 
612        return btTriple((*this)[0], (*this)[1], (*this)[2]);
613}
614
615
616SIMD_FORCE_INLINE btMatrix3x3
617btMatrix3x3::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
625SIMD_FORCE_INLINE btMatrix3x3
626btMatrix3x3::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
633SIMD_FORCE_INLINE btMatrix3x3
634btMatrix3x3::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
641SIMD_FORCE_INLINE btMatrix3x3
642btMatrix3x3::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
653SIMD_FORCE_INLINE btMatrix3x3
654btMatrix3x3::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
668SIMD_FORCE_INLINE btMatrix3x3
669btMatrix3x3::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
678SIMD_FORCE_INLINE btVector3
679operator*(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
685SIMD_FORCE_INLINE btVector3
686operator*(const btVector3& v, const btMatrix3x3& m)
687{
688        return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
689}
690
691SIMD_FORCE_INLINE btMatrix3x3
692operator*(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/*
701SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
702return btMatrix3x3(
703m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
704m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
705m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
706m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
707m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
708m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
709m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
710m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
711m1[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.  */
717SIMD_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
725struct  btMatrix3x3FloatData
726{
727        btVector3FloatData m_el[3];
728};
729
730///for serialization
731struct  btMatrix3x3DoubleData
732{
733        btVector3DoubleData m_el[3];
734};
735
736
737       
738
739SIMD_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
745SIMD_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
752SIMD_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
758SIMD_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
764SIMD_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
Note: See TracBrowser for help on using the repository browser.