← Back to branch summary

~siretart/ubuntu/utopic/blender/libav10

« back to all changes in this revision

Viewing changes to extern/bullet2/src/LinearMath/btMatrix3x3.h

  • Committer: Package Import Robot
  • Author(s): Matteo F. Vescovi
  • Date: 2012-07-23 08:54:18 UTC
  • mfrom: (14.2.16 sid)
  • mto: (14.2.19 sid)
  • mto: This revision was merged to the branch mainline in revision 42.
  • Revision ID: package-import@ubuntu.com-20120723085418-9foz30v6afaf5ffs
Tags: 2.63a-2
http://bugs.debian.org/658075
* debian/: Cycles support added (Closes: #658075)
  For now, this top feature has been enabled only
  on [any-amd64 any-i386] architectures because
  of OpenImageIO failing on all others
* debian/: scripts installation path changed
  from /usr/lib to /usr/share:
  + debian/patches/: patchset re-worked for path changing
  + debian/control: "Breaks" field added on yafaray-exporter

Show diffs side-by-side

added added

removed removed

Lines of Context:
extern/bullet2/src/LinearMath/btMatrix3x3.h
13
13
*/
14
14
 
15
15
 
16
 
#ifndef btMatrix3x3_H
17
 
#define btMatrix3x3_H
18
 
 
19
 
#include "btScalar.h"
 
16
#ifndef BT_MATRIX3x3_H
 
17
#define BT_MATRIX3x3_H
20
18
 
21
19
#include "btVector3.h"
22
20
#include "btQuaternion.h"
23
21
 
 
22
#ifdef BT_USE_DOUBLE_PRECISION
 
23
#define btMatrix3x3Data btMatrix3x3DoubleData 
 
24
#else
 
25
#define btMatrix3x3Data btMatrix3x3FloatData
 
26
#endif //BT_USE_DOUBLE_PRECISION
24
27
 
25
28
 
26
29
/**@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. */
 
30
* Make sure to only include a pure orthogonal matrix without scaling. */
28
31
class 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 */
 
32
 
 
33
        ///Data storage for the matrix, each vector is a row of the matrix
 
34
        btVector3 m_el[3];
 
35
 
 
36
public:
 
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*/
108
125
        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
 
                }
 
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
        }
160
177
 
161
178
        /** @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
 
         **/
 
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
        **/
170
187
        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
 
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
172
189
                btScalar ci ( btCos(eulerX)); 
173
190
                btScalar cj ( btCos(eulerY)); 
174
191
                btScalar ch ( btCos(eulerZ)); 
179
196
                btScalar cs = ci * sh; 
180
197
                btScalar sc = si * ch; 
181
198
                btScalar ss = si * sh;
182
 
                
 
199
 
183
200
                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
 
                        }
 
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;
283
315
                };
284
316
 
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), 
 
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), 
329
352
                                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
 
                 {
 
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
                {
403
426
                        // find off-diagonal element [p][q] with largest magnitude
404
427
                        int p = 0;
405
428
                        int q = 1;
408
431
                        btScalar v = btFabs(m_el[0][2]);
409
432
                        if (v > max)
410
433
                        {
411
 
                           q = 2;
412
 
                           r = 1;
413
 
                           max = v;
 
434
                                q = 2;
 
435
                                r = 1;
 
436
                                max = v;
414
437
                        }
415
438
                        v = btFabs(m_el[1][2]);
416
439
                        if (v > max)
417
440
                        {
418
 
                           p = 1;
419
 
                           q = 2;
420
 
                           r = 0;
421
 
                           max = v;
 
441
                                p = 1;
 
442
                                q = 2;
 
443
                                r = 0;
 
444
                                max = v;
422
445
                        }
423
446
 
424
447
                        btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2]));
425
448
                        if (max <= t)
426
449
                        {
427
 
                           if (max <= SIMD_EPSILON * t)
428
 
                           {
429
 
                                  return;
430
 
                           }
431
 
                           step = 1;
 
450
                                if (max <= SIMD_EPSILON * t)
 
451
                                {
 
452
                                        return;
 
453
                                }
 
454
                                step = 1;
432
455
                        }
433
456
 
434
457
                        // compute Jacobi rotation J which leads to a zero for element [p][q] 
439
462
                        btScalar sin;
440
463
                        if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON))
441
464
                        {
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;
 
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;
446
469
                        }
447
470
                        else
448
471
                        {
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;
 
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;
453
476
                        }
454
477
 
455
478
                        // apply rotation to matrix (this = J^T * this * J)
464
487
                        // apply rotation to rot (rot = rot * J)
465
488
                        for (int i = 0; i < 3; i++)
466
489
                        {
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;
 
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;
472
495
                        }
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
 
        }
 
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
 
 
527
SIMD_FORCE_INLINE btMatrix3x3& 
 
528
btMatrix3x3::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
 
 
536
SIMD_FORCE_INLINE btMatrix3x3& 
 
537
btMatrix3x3::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
 
 
552
SIMD_FORCE_INLINE btMatrix3x3
 
553
operator*(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 
 
562
operator+(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
 
 
576
SIMD_FORCE_INLINE btMatrix3x3 
 
577
operator-(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
 
 
592
SIMD_FORCE_INLINE btMatrix3x3& 
 
593
btMatrix3x3::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
 
 
609
SIMD_FORCE_INLINE btScalar 
 
610
btMatrix3x3::determinant() const
 
611
{ 
 
612
        return btTriple((*this)[0], (*this)[1], (*this)[2]);
 
613
}
 
614
 
 
615
 
 
616
SIMD_FORCE_INLINE btMatrix3x3 
 
617
btMatrix3x3::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
 
 
625
SIMD_FORCE_INLINE btMatrix3x3 
 
626
btMatrix3x3::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
 
 
633
SIMD_FORCE_INLINE btMatrix3x3 
 
634
btMatrix3x3::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
 
 
641
SIMD_FORCE_INLINE btMatrix3x3 
 
642
btMatrix3x3::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
 
 
653
SIMD_FORCE_INLINE btMatrix3x3 
 
654
btMatrix3x3::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
 
 
668
SIMD_FORCE_INLINE btMatrix3x3 
 
669
btMatrix3x3::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
 
 
678
SIMD_FORCE_INLINE btVector3 
 
679
operator*(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
 
 
685
SIMD_FORCE_INLINE btVector3
 
686
operator*(const btVector3& v, const btMatrix3x3& m)
 
687
{
 
688
        return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
 
689
}
 
690
 
 
691
SIMD_FORCE_INLINE btMatrix3x3 
 
692
operator*(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
}
593
699
 
594
700
/*
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]);
 
701
SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
 
702
return btMatrix3x3(
 
703
m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
 
704
m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
 
705
m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
 
706
m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
 
707
m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
 
708
m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
 
709
m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
 
710
m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
 
711
m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
606
712
}
607
713
*/
608
714
 
609
715
/**@brief Equality operator between two matrices
610
 
 * It will test all elements are equal.  */
 
716
* It will test all elements are equal.  */
611
717
SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
612
718
{
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
 
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
 
725
struct  btMatrix3x3FloatData
 
726
{
 
727
        btVector3FloatData m_el[3];
 
728
};
 
729
 
 
730
///for serialization
 
731
struct  btMatrix3x3DoubleData
 
732
{
 
733
        btVector3DoubleData m_el[3];
 
734
};
 
735
 
 
736
 
 
737
        
 
738
 
 
739
SIMD_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
 
 
745
SIMD_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
 
 
752
SIMD_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
 
 
758
SIMD_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
 
 
764
SIMD_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