Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/kicklib/src/external/bullet/LinearMath/btMatrix3x3.h @ 8740

Last change on this file since 8740 was 7983, checked in by rgrieder, 14 years ago

Updated Bullet Physics Engine from v2.74 to v2.77

  • Property svn:eol-style set to native
File size: 20.6 KB
RevLine 
[1963]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
[7983]16#ifndef BT_MATRIX3x3_H
17#define BT_MATRIX3x3_H
[1963]18
19#include "btVector3.h"
20#include "btQuaternion.h"
21
[7983]22#ifdef BT_USE_DOUBLE_PRECISION
23#define btMatrix3x3Data btMatrix3x3DoubleData
24#else
25#define btMatrix3x3Data btMatrix3x3FloatData
26#endif //BT_USE_DOUBLE_PRECISION
[1963]27
28
[2430]29/**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3.
[7983]30* Make sure to only include a pure orthogonal matrix without scaling. */
[1963]31class btMatrix3x3 {
32
[7983]33        ///Data storage for the matrix, each vector is a row of the matrix
34        btVector3 m_el[3];
[1963]35
[7983]36public:
37        /** @brief No initializaion constructor */
38        btMatrix3x3 () {}
[1963]39
[7983]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 Set from a carray of btScalars
114        *  @param m A pointer to the beginning of an array of 9 btScalars */
[1963]115        void setFromOpenGLSubMatrix(const btScalar *m)
[7983]116        {
117                m_el[0].setValue(m[0],m[4],m[8]);
118                m_el[1].setValue(m[1],m[5],m[9]);
119                m_el[2].setValue(m[2],m[6],m[10]);
[1963]120
[7983]121        }
122        /** @brief Set the values of the matrix explicitly (row major)
123        *  @param xx Top left
124        *  @param xy Top Middle
125        *  @param xz Top Right
126        *  @param yx Middle Left
127        *  @param yy Middle Middle
128        *  @param yz Middle Right
129        *  @param zx Bottom Left
130        *  @param zy Bottom Middle
131        *  @param zz Bottom Right*/
132        void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, 
133                const btScalar& yx, const btScalar& yy, const btScalar& yz, 
134                const btScalar& zx, const btScalar& zy, const btScalar& zz)
135        {
136                m_el[0].setValue(xx,xy,xz);
137                m_el[1].setValue(yx,yy,yz);
138                m_el[2].setValue(zx,zy,zz);
139        }
[2430]140
[7983]141        /** @brief Set the matrix from a quaternion
142        *  @param q The Quaternion to match */ 
143        void setRotation(const btQuaternion& q) 
144        {
145                btScalar d = q.length2();
146                btFullAssert(d != btScalar(0.0));
147                btScalar s = btScalar(2.0) / d;
148                btScalar xs = q.x() * s,   ys = q.y() * s,   zs = q.z() * s;
149                btScalar wx = q.w() * xs,  wy = q.w() * ys,  wz = q.w() * zs;
150                btScalar xx = q.x() * xs,  xy = q.x() * ys,  xz = q.x() * zs;
151                btScalar yy = q.y() * ys,  yz = q.y() * zs,  zz = q.z() * zs;
152                setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
153                        xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
154                        xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
155        }
[1963]156
157
[7983]158        /** @brief Set the matrix from euler angles using YPR around YXZ respectively
159        *  @param yaw Yaw about Y axis
160        *  @param pitch Pitch about X axis
161        *  @param roll Roll about Z axis
162        */
163        void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) 
164        {
165                setEulerZYX(roll, pitch, yaw);
166        }
167
[2430]168        /** @brief Set the matrix from euler angles YPR around ZYX axes
[7983]169        * @param eulerX Roll about X axis
170        * @param eulerY Pitch around Y axis
171        * @param eulerZ Yaw aboud Z axis
172        *
173        * These angles are used to produce a rotation matrix. The euler
174        * angles are applied in ZYX order. I.e a vector is first rotated
175        * about X then Y and then Z
176        **/
[2430]177        void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) { 
[7983]178                ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code
[1963]179                btScalar ci ( btCos(eulerX)); 
180                btScalar cj ( btCos(eulerY)); 
181                btScalar ch ( btCos(eulerZ)); 
182                btScalar si ( btSin(eulerX)); 
183                btScalar sj ( btSin(eulerY)); 
184                btScalar sh ( btSin(eulerZ)); 
185                btScalar cc = ci * ch; 
186                btScalar cs = ci * sh; 
187                btScalar sc = si * ch; 
188                btScalar ss = si * sh;
[7983]189
[1963]190                setValue(cj * ch, sj * sc - cs, sj * cc + ss,
[7983]191                        cj * sh, sj * ss + cc, sj * cs - sc, 
192                        -sj,      cj * si,      cj * ci);
[1963]193        }
194
[7983]195        /**@brief Set the matrix to the identity */
196        void setIdentity()
197        { 
198                setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0), 
199                        btScalar(0.0), btScalar(1.0), btScalar(0.0), 
200                        btScalar(0.0), btScalar(0.0), btScalar(1.0)); 
201        }
[2882]202
[7983]203        static const btMatrix3x3&       getIdentity()
204        {
205                static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0), 
206                        btScalar(0.0), btScalar(1.0), btScalar(0.0), 
207                        btScalar(0.0), btScalar(0.0), btScalar(1.0));
208                return identityMatrix;
209        }
210
211        /**@brief Fill the values of the matrix into a 9 element array
212        * @param m The array to be filled */
213        void getOpenGLSubMatrix(btScalar *m) const 
214        {
215                m[0]  = btScalar(m_el[0].x()); 
216                m[1]  = btScalar(m_el[1].x());
217                m[2]  = btScalar(m_el[2].x());
218                m[3]  = btScalar(0.0); 
219                m[4]  = btScalar(m_el[0].y());
220                m[5]  = btScalar(m_el[1].y());
221                m[6]  = btScalar(m_el[2].y());
222                m[7]  = btScalar(0.0); 
223                m[8]  = btScalar(m_el[0].z()); 
224                m[9]  = btScalar(m_el[1].z());
225                m[10] = btScalar(m_el[2].z());
226                m[11] = btScalar(0.0); 
227        }
228
229        /**@brief Get the matrix represented as a quaternion
230        * @param q The quaternion which will be set */
231        void getRotation(btQuaternion& q) const
232        {
233                btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
234                btScalar temp[4];
235
236                if (trace > btScalar(0.0)) 
[2882]237                {
[7983]238                        btScalar s = btSqrt(trace + btScalar(1.0));
239                        temp[3]=(s * btScalar(0.5));
240                        s = btScalar(0.5) / s;
241
242                        temp[0]=((m_el[2].y() - m_el[1].z()) * s);
243                        temp[1]=((m_el[0].z() - m_el[2].x()) * s);
244                        temp[2]=((m_el[1].x() - m_el[0].y()) * s);
245                } 
246                else 
247                {
248                        int i = m_el[0].x() < m_el[1].y() ? 
249                                (m_el[1].y() < m_el[2].z() ? 2 : 1) :
250                                (m_el[0].x() < m_el[2].z() ? 2 : 0); 
251                        int j = (i + 1) % 3; 
252                        int k = (i + 2) % 3;
253
254                        btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
255                        temp[i] = s * btScalar(0.5);
256                        s = btScalar(0.5) / s;
257
258                        temp[3] = (m_el[k][j] - m_el[j][k]) * s;
259                        temp[j] = (m_el[j][i] + m_el[i][j]) * s;
260                        temp[k] = (m_el[k][i] + m_el[i][k]) * s;
[2882]261                }
[7983]262                q.setValue(temp[0],temp[1],temp[2],temp[3]);
263        }
[2882]264
[7983]265        /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
266        * @param yaw Yaw around Y axis
267        * @param pitch Pitch around X axis
268        * @param roll around Z axis */ 
269        void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const
270        {
271
272                // first use the normal calculus
273                yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
274                pitch = btScalar(btAsin(-m_el[2].x()));
275                roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
276
277                // on pitch = +/-HalfPI
278                if (btFabs(pitch)==SIMD_HALF_PI)
[1963]279                {
[7983]280                        if (yaw>0)
281                                yaw-=SIMD_PI;
282                        else
283                                yaw+=SIMD_PI;
284
285                        if (roll>0)
286                                roll-=SIMD_PI;
287                        else
288                                roll+=SIMD_PI;
[1963]289                }
[7983]290        };
[1963]291
[7983]292
293        /**@brief Get the matrix represented as euler angles around ZYX
294        * @param yaw Yaw around X axis
295        * @param pitch Pitch around Y axis
296        * @param roll around X axis
297        * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/       
298        void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const
299        {
300                struct Euler
[1963]301                {
[7983]302                        btScalar yaw;
303                        btScalar pitch;
304                        btScalar roll;
305                };
306
307                Euler euler_out;
308                Euler euler_out2; //second solution
309                //get the pointer to the raw data
310
311                // Check that pitch is not at a singularity
312                if (btFabs(m_el[2].x()) >= 1)
313                {
314                        euler_out.yaw = 0;
315                        euler_out2.yaw = 0;
316
317                        // From difference of angles formula
318                        btScalar delta = btAtan2(m_el[0].x(),m_el[0].z());
319                        if (m_el[2].x() > 0)  //gimbal locked up
[1963]320                        {
[7983]321                                euler_out.pitch = SIMD_PI / btScalar(2.0);
322                                euler_out2.pitch = SIMD_PI / btScalar(2.0);
323                                euler_out.roll = euler_out.pitch + delta;
324                                euler_out2.roll = euler_out.pitch + delta;
325                        }
326                        else // gimbal locked down
[1963]327                        {
[7983]328                                euler_out.pitch = -SIMD_PI / btScalar(2.0);
329                                euler_out2.pitch = -SIMD_PI / btScalar(2.0);
330                                euler_out.roll = -euler_out.pitch + delta;
331                                euler_out2.roll = -euler_out.pitch + delta;
[1963]332                        }
333                }
[7983]334                else
[1963]335                {
[7983]336                        euler_out.pitch = - btAsin(m_el[2].x());
337                        euler_out2.pitch = SIMD_PI - euler_out.pitch;
[2430]338
[7983]339                        euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch), 
340                                m_el[2].z()/btCos(euler_out.pitch));
341                        euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch), 
[2430]342                                m_el[2].z()/btCos(euler_out2.pitch));
343
[7983]344                        euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch), 
345                                m_el[0].x()/btCos(euler_out.pitch));
346                        euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch), 
347                                m_el[0].x()/btCos(euler_out2.pitch));
[1963]348                }
349
[7983]350                if (solution_number == 1)
351                { 
352                        yaw = euler_out.yaw; 
353                        pitch = euler_out.pitch;
354                        roll = euler_out.roll;
[1963]355                }
[7983]356                else
357                { 
358                        yaw = euler_out2.yaw; 
359                        pitch = euler_out2.pitch;
360                        roll = euler_out2.roll;
[1963]361                }
[7983]362        }
[1963]363
[7983]364        /**@brief Create a scaled copy of the matrix
365        * @param s Scaling vector The elements of the vector will scale each column */
366
367        btMatrix3x3 scaled(const btVector3& s) const
368        {
369                return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
370                        m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
371                        m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
372        }
373
374        /**@brief Return the determinant of the matrix */
375        btScalar            determinant() const;
376        /**@brief Return the adjoint of the matrix */
377        btMatrix3x3 adjoint() const;
378        /**@brief Return the matrix with all values non negative */
379        btMatrix3x3 absolute() const;
380        /**@brief Return the transpose of the matrix */
381        btMatrix3x3 transpose() const;
382        /**@brief Return the inverse of the matrix */
383        btMatrix3x3 inverse() const; 
384
385        btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
386        btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
387
388        SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const 
389        {
390                return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
391        }
392        SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const 
393        {
394                return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
395        }
396        SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const 
397        {
398                return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
399        }
400
401
402        /**@brief diagonalizes this matrix by the Jacobi method.
403        * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
404        * coordinate system, i.e., old_this = rot * new_this * rot^T.
405        * @param threshold See iteration
406        * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied
407        * by the sum of the absolute values of the diagonal, or when maxSteps have been executed.
408        *
409        * Note that this matrix is assumed to be symmetric.
410        */
411        void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
412        {
413                rot.setIdentity();
414                for (int step = maxSteps; step > 0; step--)
[1963]415                {
416                        // find off-diagonal element [p][q] with largest magnitude
417                        int p = 0;
418                        int q = 1;
419                        int r = 2;
420                        btScalar max = btFabs(m_el[0][1]);
421                        btScalar v = btFabs(m_el[0][2]);
422                        if (v > max)
423                        {
[7983]424                                q = 2;
425                                r = 1;
426                                max = v;
[1963]427                        }
428                        v = btFabs(m_el[1][2]);
429                        if (v > max)
430                        {
[7983]431                                p = 1;
432                                q = 2;
433                                r = 0;
434                                max = v;
[1963]435                        }
436
437                        btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2]));
438                        if (max <= t)
439                        {
[7983]440                                if (max <= SIMD_EPSILON * t)
441                                {
442                                        return;
443                                }
444                                step = 1;
[1963]445                        }
446
447                        // compute Jacobi rotation J which leads to a zero for element [p][q]
448                        btScalar mpq = m_el[p][q];
449                        btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
450                        btScalar theta2 = theta * theta;
451                        btScalar cos;
452                        btScalar sin;
453                        if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON))
454                        {
[7983]455                                t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
456                                        : 1 / (theta - btSqrt(1 + theta2));
457                                cos = 1 / btSqrt(1 + t * t);
458                                sin = cos * t;
[1963]459                        }
460                        else
461                        {
[7983]462                                // approximation for large theta-value, i.e., a nearly diagonal matrix
463                                t = 1 / (theta * (2 + btScalar(0.5) / theta2));
464                                cos = 1 - btScalar(0.5) * t * t;
465                                sin = cos * t;
[1963]466                        }
467
468                        // apply rotation to matrix (this = J^T * this * J)
469                        m_el[p][q] = m_el[q][p] = 0;
470                        m_el[p][p] -= t * mpq;
471                        m_el[q][q] += t * mpq;
472                        btScalar mrp = m_el[r][p];
473                        btScalar mrq = m_el[r][q];
474                        m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
475                        m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
476
477                        // apply rotation to rot (rot = rot * J)
478                        for (int i = 0; i < 3; i++)
479                        {
[7983]480                                btVector3& row = rot[i];
481                                mrp = row[p];
482                                mrq = row[q];
483                                row[p] = cos * mrp - sin * mrq;
484                                row[q] = cos * mrq + sin * mrp;
[1963]485                        }
486                }
[7983]487        }
[1963]488
489
490
491
[7983]492        /**@brief Calculate the matrix cofactor
493        * @param r1 The first row to use for calculating the cofactor
494        * @param c1 The first column to use for calculating the cofactor
495        * @param r1 The second row to use for calculating the cofactor
496        * @param c1 The second column to use for calculating the cofactor
497        * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
498        */
499        btScalar cofac(int r1, int c1, int r2, int c2) const 
[1963]500        {
[7983]501                return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
[1963]502        }
503
[7983]504        void    serialize(struct        btMatrix3x3Data& dataOut) const;
[1963]505
[7983]506        void    serializeFloat(struct   btMatrix3x3FloatData& dataOut) const;
[1963]507
[7983]508        void    deSerialize(const struct        btMatrix3x3Data& dataIn);
[1963]509
[7983]510        void    deSerializeFloat(const struct   btMatrix3x3FloatData& dataIn);
511
512        void    deSerializeDouble(const struct  btMatrix3x3DoubleData& dataIn);
513
514};
515
516
517SIMD_FORCE_INLINE btMatrix3x3& 
518btMatrix3x3::operator*=(const btMatrix3x3& m)
519{
520        setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
521                m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
522                m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
523        return *this;
524}
525
526SIMD_FORCE_INLINE btScalar
527btMatrix3x3::determinant() const
528{ 
529        return btTriple((*this)[0], (*this)[1], (*this)[2]);
530}
531
532
533SIMD_FORCE_INLINE btMatrix3x3
534btMatrix3x3::absolute() const
535{
536        return btMatrix3x3(
537                btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
538                btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
539                btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
540}
541
542SIMD_FORCE_INLINE btMatrix3x3
543btMatrix3x3::transpose() const 
544{
545        return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
546                m_el[0].y(), m_el[1].y(), m_el[2].y(),
547                m_el[0].z(), m_el[1].z(), m_el[2].z());
548}
549
550SIMD_FORCE_INLINE btMatrix3x3
551btMatrix3x3::adjoint() const 
552{
553        return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
554                cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
555                cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
556}
557
558SIMD_FORCE_INLINE btMatrix3x3
559btMatrix3x3::inverse() const
560{
561        btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
562        btScalar det = (*this)[0].dot(co);
563        btFullAssert(det != btScalar(0.0));
564        btScalar s = btScalar(1.0) / det;
565        return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
566                co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
567                co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
568}
569
570SIMD_FORCE_INLINE btMatrix3x3
571btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
572{
573        return btMatrix3x3(
574                m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
575                m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
576                m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
577                m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
578                m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
579                m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
580                m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
581                m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
582                m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
583}
584
585SIMD_FORCE_INLINE btMatrix3x3
586btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
587{
588        return btMatrix3x3(
589                m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
590                m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
591                m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
592
593}
594
595SIMD_FORCE_INLINE btVector3
596operator*(const btMatrix3x3& m, const btVector3& v) 
597{
598        return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
599}
600
601
602SIMD_FORCE_INLINE btVector3
603operator*(const btVector3& v, const btMatrix3x3& m)
604{
605        return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
606}
607
608SIMD_FORCE_INLINE btMatrix3x3
609operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
610{
611        return btMatrix3x3(
612                m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
613                m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
614                m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
615}
616
[1963]617/*
[7983]618SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
619return btMatrix3x3(
620m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
621m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
622m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
623m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
624m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
625m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
626m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
627m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
628m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
[1963]629}
630*/
631
[2430]632/**@brief Equality operator between two matrices
[7983]633* It will test all elements are equal.  */
[1963]634SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
635{
[7983]636        return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
637                m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
638                m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
[1963]639}
640
[7983]641///for serialization
642struct  btMatrix3x3FloatData
643{
644        btVector3FloatData m_el[3];
645};
646
647///for serialization
648struct  btMatrix3x3DoubleData
649{
650        btVector3DoubleData m_el[3];
651};
652
653
654       
655
656SIMD_FORCE_INLINE       void    btMatrix3x3::serialize(struct   btMatrix3x3Data& dataOut) const
657{
658        for (int i=0;i<3;i++)
659                m_el[i].serialize(dataOut.m_el[i]);
660}
661
662SIMD_FORCE_INLINE       void    btMatrix3x3::serializeFloat(struct      btMatrix3x3FloatData& dataOut) const
663{
664        for (int i=0;i<3;i++)
665                m_el[i].serializeFloat(dataOut.m_el[i]);
666}
667
668
669SIMD_FORCE_INLINE       void    btMatrix3x3::deSerialize(const struct   btMatrix3x3Data& dataIn)
670{
671        for (int i=0;i<3;i++)
672                m_el[i].deSerialize(dataIn.m_el[i]);
673}
674
675SIMD_FORCE_INLINE       void    btMatrix3x3::deSerializeFloat(const struct      btMatrix3x3FloatData& dataIn)
676{
677        for (int i=0;i<3;i++)
678                m_el[i].deSerializeFloat(dataIn.m_el[i]);
679}
680
681SIMD_FORCE_INLINE       void    btMatrix3x3::deSerializeDouble(const struct     btMatrix3x3DoubleData& dataIn)
682{
683        for (int i=0;i<3;i++)
684                m_el[i].deSerializeDouble(dataIn.m_el[i]);
685}
686
687#endif //BT_MATRIX3x3_H
688
Note: See TracBrowser for help on using the repository browser.