Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/netp2/src/bullet/LinearMath/btMatrix3x3.h @ 2837

Last change on this file since 2837 was 2662, checked in by rgrieder, 16 years ago

Merged presentation branch back to trunk.

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