Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

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

Last change on this file since 2969 was 2882, checked in by rgrieder, 16 years ago

Update from Bullet 2.73 to 2.74.
  • Property svn:eol-style set to native
File size: 20.0 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
196                static const btMatrix3x3&       getIdentity()
197                {
198                        static const btMatrix3x3 identityMatrix(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                        return identityMatrix;
202                }
203
204  /**@brief Fill the values of the matrix into a 9 element array
205   * @param m The array to be filled */
206                void getOpenGLSubMatrix(btScalar *m) const 
207                {
208                        m[0]  = btScalar(m_el[0].x()); 
209                        m[1]  = btScalar(m_el[1].x());
210                        m[2]  = btScalar(m_el[2].x());
211                        m[3]  = btScalar(0.0); 
212                        m[4]  = btScalar(m_el[0].y());
213                        m[5]  = btScalar(m_el[1].y());
214                        m[6]  = btScalar(m_el[2].y());
215                        m[7]  = btScalar(0.0); 
216                        m[8]  = btScalar(m_el[0].z()); 
217                        m[9]  = btScalar(m_el[1].z());
218                        m[10] = btScalar(m_el[2].z());
219                        m[11] = btScalar(0.0); 
220                }
221
222  /**@brief Get the matrix represented as a quaternion
223   * @param q The quaternion which will be set */
224                void getRotation(btQuaternion& q) const
225                {
226                        btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
227                        btScalar temp[4];
228                       
229                        if (trace > btScalar(0.0)) 
230                        {
231                                btScalar s = btSqrt(trace + btScalar(1.0));
232                                temp[3]=(s * btScalar(0.5));
233                                s = btScalar(0.5) / s;
234                               
235                                temp[0]=((m_el[2].y() - m_el[1].z()) * s);
236                                temp[1]=((m_el[0].z() - m_el[2].x()) * s);
237                                temp[2]=((m_el[1].x() - m_el[0].y()) * s);
238                        } 
239                        else 
240                        {
241                                int i = m_el[0].x() < m_el[1].y() ? 
242                                        (m_el[1].y() < m_el[2].z() ? 2 : 1) :
243                                        (m_el[0].x() < m_el[2].z() ? 2 : 0); 
244                                int j = (i + 1) % 3; 
245                                int k = (i + 2) % 3;
246                               
247                                btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
248                                temp[i] = s * btScalar(0.5);
249                                s = btScalar(0.5) / s;
250                               
251                                temp[3] = (m_el[k][j] - m_el[j][k]) * s;
252                                temp[j] = (m_el[j][i] + m_el[i][j]) * s;
253                                temp[k] = (m_el[k][i] + m_el[i][k]) * s;
254                        }
255                        q.setValue(temp[0],temp[1],temp[2],temp[3]);
256                }
257
258  /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
259   * @param yaw Yaw around Y axis
260   * @param pitch Pitch around X axis
261   * @param roll around Z axis */       
262                void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const
263                {
264                       
265                        // first use the normal calculus
266                        yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
267                        pitch = btScalar(btAsin(-m_el[2].x()));
268                        roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
269
270                        // on pitch = +/-HalfPI
271                        if (btFabs(pitch)==SIMD_HALF_PI)
272                        {
273                                if (yaw>0)
274                                        yaw-=SIMD_PI;
275                                else
276                                        yaw+=SIMD_PI;
277
278                                if (roll>0)
279                                        roll-=SIMD_PI;
280                                else
281                                        roll+=SIMD_PI;
282                        }
283                };
284
285
286  /**@brief Get the matrix represented as euler angles around ZYX
287   * @param yaw Yaw around X axis
288   * @param pitch Pitch around Y axis
289   * @param roll around X axis
290   * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/   
291  void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const
292  {
293    struct Euler{btScalar yaw, pitch, roll;};
294    Euler euler_out;
295    Euler euler_out2; //second solution
296    //get the pointer to the raw data
297   
298    // Check that pitch is not at a singularity
299    if (btFabs(m_el[2].x()) >= 1)
300    {
301      euler_out.yaw = 0;
302      euler_out2.yaw = 0;
303       
304      // From difference of angles formula
305      btScalar delta = btAtan2(m_el[0].x(),m_el[0].z());
306      if (m_el[2].x() > 0)  //gimbal locked up
307      {
308        euler_out.pitch = SIMD_PI / btScalar(2.0);
309        euler_out2.pitch = SIMD_PI / btScalar(2.0);
310        euler_out.roll = euler_out.pitch + delta;
311        euler_out2.roll = euler_out.pitch + delta;
312      }
313      else // gimbal locked down
314      {
315        euler_out.pitch = -SIMD_PI / btScalar(2.0);
316        euler_out2.pitch = -SIMD_PI / btScalar(2.0);
317        euler_out.roll = -euler_out.pitch + delta;
318        euler_out2.roll = -euler_out.pitch + delta;
319      }
320    }
321    else
322    {
323      euler_out.pitch = - btAsin(m_el[2].x());
324      euler_out2.pitch = SIMD_PI - euler_out.pitch;
325       
326      euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch), 
327                               m_el[2].z()/btCos(euler_out.pitch));
328      euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch), 
329                                m_el[2].z()/btCos(euler_out2.pitch));
330       
331      euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch), 
332                              m_el[0].x()/btCos(euler_out.pitch));
333      euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch), 
334                               m_el[0].x()/btCos(euler_out2.pitch));
335    }
336   
337    if (solution_number == 1)
338    { 
339                yaw = euler_out.yaw; 
340                pitch = euler_out.pitch;
341                roll = euler_out.roll;
342    }
343    else
344    { 
345                yaw = euler_out2.yaw; 
346                pitch = euler_out2.pitch;
347                roll = euler_out2.roll;
348    }
349  }
350
351  /**@brief Create a scaled copy of the matrix
352   * @param s Scaling vector The elements of the vector will scale each column */
353               
354                btMatrix3x3 scaled(const btVector3& s) const
355                {
356                        return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
357                                                                         m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
358                                                                         m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
359                }
360
361  /**@brief Return the determinant of the matrix */
362                btScalar            determinant() const;
363  /**@brief Return the adjoint of the matrix */
364                btMatrix3x3 adjoint() const;
365  /**@brief Return the matrix with all values non negative */
366                btMatrix3x3 absolute() const;
367  /**@brief Return the transpose of the matrix */
368                btMatrix3x3 transpose() const;
369  /**@brief Return the inverse of the matrix */
370                btMatrix3x3 inverse() const; 
371               
372                btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
373                btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
374               
375                SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const 
376                {
377                        return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
378                }
379                SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const 
380                {
381                        return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
382                }
383                SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const 
384                {
385                        return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
386                }
387               
388
389  /**@brief diagonalizes this matrix by the Jacobi method.
390   * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
391   * coordinate system, i.e., old_this = rot * new_this * rot^T.
392   * @param threshold See iteration
393   * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied
394   * by the sum of the absolute values of the diagonal, or when maxSteps have been executed.
395   *
396   * Note that this matrix is assumed to be symmetric.
397   */
398                void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
399                {
400                 rot.setIdentity();
401                 for (int step = maxSteps; step > 0; step--)
402                 {
403                        // find off-diagonal element [p][q] with largest magnitude
404                        int p = 0;
405                        int q = 1;
406                        int r = 2;
407                        btScalar max = btFabs(m_el[0][1]);
408                        btScalar v = btFabs(m_el[0][2]);
409                        if (v > max)
410                        {
411                           q = 2;
412                           r = 1;
413                           max = v;
414                        }
415                        v = btFabs(m_el[1][2]);
416                        if (v > max)
417                        {
418                           p = 1;
419                           q = 2;
420                           r = 0;
421                           max = v;
422                        }
423
424                        btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2]));
425                        if (max <= t)
426                        {
427                           if (max <= SIMD_EPSILON * t)
428                           {
429                                  return;
430                           }
431                           step = 1;
432                        }
433
434                        // compute Jacobi rotation J which leads to a zero for element [p][q]
435                        btScalar mpq = m_el[p][q];
436                        btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
437                        btScalar theta2 = theta * theta;
438                        btScalar cos;
439                        btScalar sin;
440                        if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON))
441                        {
442                           t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
443                                                                                : 1 / (theta - btSqrt(1 + theta2));
444                           cos = 1 / btSqrt(1 + t * t);
445                           sin = cos * t;
446                        }
447                        else
448                        {
449                           // approximation for large theta-value, i.e., a nearly diagonal matrix
450                           t = 1 / (theta * (2 + btScalar(0.5) / theta2));
451                           cos = 1 - btScalar(0.5) * t * t;
452                           sin = cos * t;
453                        }
454
455                        // apply rotation to matrix (this = J^T * this * J)
456                        m_el[p][q] = m_el[q][p] = 0;
457                        m_el[p][p] -= t * mpq;
458                        m_el[q][q] += t * mpq;
459                        btScalar mrp = m_el[r][p];
460                        btScalar mrq = m_el[r][q];
461                        m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
462                        m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
463
464                        // apply rotation to rot (rot = rot * J)
465                        for (int i = 0; i < 3; i++)
466                        {
467                           btVector3& row = rot[i];
468                           mrp = row[p];
469                           mrq = row[q];
470                           row[p] = cos * mrp - sin * mrq;
471                           row[q] = cos * mrq + sin * mrp;
472                        }
473                 }
474                }
475
476
477               
478        protected:
479  /**@brief Calculate the matrix cofactor
480   * @param r1 The first row to use for calculating the cofactor
481   * @param c1 The first column to use for calculating the cofactor
482   * @param r1 The second row to use for calculating the cofactor
483   * @param c1 The second column to use for calculating the cofactor
484   * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
485   */
486                btScalar cofac(int r1, int c1, int r2, int c2) const 
487                {
488                        return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
489                }
490  ///Data storage for the matrix, each vector is a row of the matrix
491                btVector3 m_el[3];
492        };
493       
494        SIMD_FORCE_INLINE btMatrix3x3& 
495        btMatrix3x3::operator*=(const btMatrix3x3& m)
496        {
497                setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
498                                 m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
499                                 m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
500                return *this;
501        }
502       
503        SIMD_FORCE_INLINE btScalar
504        btMatrix3x3::determinant() const
505        { 
506                return triple((*this)[0], (*this)[1], (*this)[2]);
507        }
508       
509
510        SIMD_FORCE_INLINE btMatrix3x3
511        btMatrix3x3::absolute() const
512        {
513                return btMatrix3x3(
514                        btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
515                        btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
516                        btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
517        }
518
519        SIMD_FORCE_INLINE btMatrix3x3
520        btMatrix3x3::transpose() const 
521        {
522                return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
523                                                                 m_el[0].y(), m_el[1].y(), m_el[2].y(),
524                                                                 m_el[0].z(), m_el[1].z(), m_el[2].z());
525        }
526       
527        SIMD_FORCE_INLINE btMatrix3x3
528        btMatrix3x3::adjoint() const 
529        {
530                return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
531                                                                 cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
532                                                                 cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
533        }
534       
535        SIMD_FORCE_INLINE btMatrix3x3
536        btMatrix3x3::inverse() const
537        {
538                btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
539                btScalar det = (*this)[0].dot(co);
540                btFullAssert(det != btScalar(0.0));
541                btScalar s = btScalar(1.0) / det;
542                return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
543                                                                 co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
544                                                                 co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
545        }
546       
547        SIMD_FORCE_INLINE btMatrix3x3
548        btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
549        {
550                return btMatrix3x3(
551                        m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
552                        m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
553                        m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
554                        m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
555                        m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
556                        m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
557                        m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
558                        m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
559                        m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
560        }
561       
562        SIMD_FORCE_INLINE btMatrix3x3
563        btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
564        {
565                return btMatrix3x3(
566                        m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
567                        m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
568                        m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
569               
570        }
571
572        SIMD_FORCE_INLINE btVector3
573        operator*(const btMatrix3x3& m, const btVector3& v) 
574        {
575                return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
576        }
577       
578
579        SIMD_FORCE_INLINE btVector3
580        operator*(const btVector3& v, const btMatrix3x3& m)
581        {
582                return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
583        }
584
585        SIMD_FORCE_INLINE btMatrix3x3
586        operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
587        {
588                return btMatrix3x3(
589                        m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
590                        m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
591                        m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
592        }
593
594/*
595        SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
596    return btMatrix3x3(
597        m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
598        m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
599        m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
600        m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
601        m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
602        m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
603        m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
604        m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
605        m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
606}
607*/
608
609/**@brief Equality operator between two matrices
610 * It will test all elements are equal.  */
611SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
612{
613   return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
614            m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
615            m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
616}
617
618#endif
Note: See TracBrowser for help on using the repository browser.