Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics_new/src/bullet/LinearMath/btMatrix3x3.h @ 2119

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

Merged physics branch into physics_new branch.
  • Property svn:eol-style set to native
File size: 15.1 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///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                btMatrix3x3 () {}
31               
32//              explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
33               
34                explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); }
35                /*
36                template <typename btScalar>
37                Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
38                {
39                        setEulerYPR(yaw, pitch, roll);
40                }
41                */
42                btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz,
43                                  const btScalar& yx, const btScalar& yy, const btScalar& yz,
44                                  const btScalar& zx, const btScalar& zy, const btScalar& zz)
45                { 
46                        setValue(xx, xy, xz, 
47                                         yx, yy, yz, 
48                                         zx, zy, zz);
49                }
50               
51                SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
52                {
53                        m_el[0] = other.m_el[0];
54                        m_el[1] = other.m_el[1];
55                        m_el[2] = other.m_el[2];
56                }
57
58                SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
59                {
60                        m_el[0] = other.m_el[0];
61                        m_el[1] = other.m_el[1];
62                        m_el[2] = other.m_el[2];
63                        return *this;
64                }
65
66                SIMD_FORCE_INLINE btVector3 getColumn(int i) const
67                {
68                        return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
69                }
70               
71
72
73                SIMD_FORCE_INLINE const btVector3& getRow(int i) const
74                {
75                        return m_el[i];
76                }
77
78
79                SIMD_FORCE_INLINE btVector3&  operator[](int i)
80                { 
81                        btFullAssert(0 <= i && i < 3);
82                        return m_el[i]; 
83                }
84               
85                SIMD_FORCE_INLINE const btVector3& operator[](int i) const
86                {
87                        btFullAssert(0 <= i && i < 3);
88                        return m_el[i]; 
89                }
90               
91                btMatrix3x3& operator*=(const btMatrix3x3& m); 
92               
93       
94        void setFromOpenGLSubMatrix(const btScalar *m)
95                {
96                        m_el[0].setValue(m[0],m[4],m[8]);
97                        m_el[1].setValue(m[1],m[5],m[9]);
98                        m_el[2].setValue(m[2],m[6],m[10]);
99
100                }
101
102                void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, 
103                                          const btScalar& yx, const btScalar& yy, const btScalar& yz, 
104                                          const btScalar& zx, const btScalar& zy, const btScalar& zz)
105                {
106                        m_el[0].setValue(xx,xy,xz);
107                        m_el[1].setValue(yx,yy,yz);
108                        m_el[2].setValue(zx,zy,zz);
109                }
110 
111                void setRotation(const btQuaternion& q) 
112                {
113                        btScalar d = q.length2();
114                        btFullAssert(d != btScalar(0.0));
115                        btScalar s = btScalar(2.0) / d;
116                        btScalar xs = q.x() * s,   ys = q.y() * s,   zs = q.z() * s;
117                        btScalar wx = q.w() * xs,  wy = q.w() * ys,  wz = q.w() * zs;
118                        btScalar xx = q.x() * xs,  xy = q.x() * ys,  xz = q.x() * zs;
119                        btScalar yy = q.y() * ys,  yz = q.y() * zs,  zz = q.z() * zs;
120                        setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
121                                         xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
122                                         xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
123                }
124               
125
126
127                void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) 
128                {
129
130                        btScalar cy(btCos(yaw)); 
131                        btScalar  sy(btSin(yaw)); 
132                        btScalar  cp(btCos(pitch)); 
133                        btScalar  sp(btSin(pitch)); 
134                        btScalar  cr(btCos(roll));
135                        btScalar  sr(btSin(roll));
136                        btScalar  cc = cy * cr; 
137                        btScalar  cs = cy * sr; 
138                        btScalar  sc = sy * cr; 
139                        btScalar  ss = sy * sr;
140                        setValue(cc - sp * ss, -cs - sp * sc, -sy * cp,
141                     cp * sr,       cp * cr,      -sp,
142                                         sc + sp * cs, -ss + sp * cc,  cy * cp);
143               
144                }
145
146        /**
147         * setEulerZYX
148         * @param euler a const reference to a btVector3 of euler angles
149         * These angles are used to produce a rotation matrix. The euler
150         * angles are applied in ZYX order. I.e a vector is first rotated
151         * about X then Y and then Z
152         **/
153       
154        void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) {
155                btScalar ci ( btCos(eulerX)); 
156                btScalar cj ( btCos(eulerY)); 
157                btScalar ch ( btCos(eulerZ)); 
158                btScalar si ( btSin(eulerX)); 
159                btScalar sj ( btSin(eulerY)); 
160                btScalar sh ( btSin(eulerZ)); 
161                btScalar cc = ci * ch; 
162                btScalar cs = ci * sh; 
163                btScalar sc = si * ch; 
164                btScalar ss = si * sh;
165               
166                setValue(cj * ch, sj * sc - cs, sj * cc + ss,
167                                 cj * sh, sj * ss + cc, sj * cs - sc, 
168                                 -sj,      cj * si,      cj * ci);
169        }
170
171                void setIdentity()
172                { 
173                        setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0), 
174                                         btScalar(0.0), btScalar(1.0), btScalar(0.0), 
175                                         btScalar(0.0), btScalar(0.0), btScalar(1.0)); 
176                }
177   
178                void getOpenGLSubMatrix(btScalar *m) const 
179                {
180                        m[0]  = btScalar(m_el[0].x()); 
181                        m[1]  = btScalar(m_el[1].x());
182                        m[2]  = btScalar(m_el[2].x());
183                        m[3]  = btScalar(0.0); 
184                        m[4]  = btScalar(m_el[0].y());
185                        m[5]  = btScalar(m_el[1].y());
186                        m[6]  = btScalar(m_el[2].y());
187                        m[7]  = btScalar(0.0); 
188                        m[8]  = btScalar(m_el[0].z()); 
189                        m[9]  = btScalar(m_el[1].z());
190                        m[10] = btScalar(m_el[2].z());
191                        m[11] = btScalar(0.0); 
192                }
193
194                void getRotation(btQuaternion& q) const
195                {
196                        btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
197                        btScalar temp[4];
198                       
199                        if (trace > btScalar(0.0)) 
200                        {
201                                btScalar s = btSqrt(trace + btScalar(1.0));
202                                temp[3]=(s * btScalar(0.5));
203                                s = btScalar(0.5) / s;
204                               
205                                temp[0]=((m_el[2].y() - m_el[1].z()) * s);
206                                temp[1]=((m_el[0].z() - m_el[2].x()) * s);
207                                temp[2]=((m_el[1].x() - m_el[0].y()) * s);
208                        } 
209                        else 
210                        {
211                                int i = m_el[0].x() < m_el[1].y() ? 
212                                        (m_el[1].y() < m_el[2].z() ? 2 : 1) :
213                                        (m_el[0].x() < m_el[2].z() ? 2 : 0); 
214                                int j = (i + 1) % 3; 
215                                int k = (i + 2) % 3;
216                               
217                                btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
218                                temp[i] = s * btScalar(0.5);
219                                s = btScalar(0.5) / s;
220                               
221                                temp[3] = (m_el[k][j] - m_el[j][k]) * s;
222                                temp[j] = (m_el[j][i] + m_el[i][j]) * s;
223                                temp[k] = (m_el[k][i] + m_el[i][k]) * s;
224                        }
225                        q.setValue(temp[0],temp[1],temp[2],temp[3]);
226                }
227       
228                void getEuler(btScalar& yaw, btScalar& pitch, btScalar& roll) const
229                {
230                       
231                        if (btScalar(m_el[1].z()) < btScalar(1))
232                        {
233                                if (btScalar(m_el[1].z()) > -btScalar(1))
234                                {
235                                        yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
236                                        pitch = btScalar(btAsin(-m_el[1].y()));
237                                        roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
238                                }
239                                else 
240                                {
241                                        yaw = btScalar(-btAtan2(-m_el[0].y(), m_el[0].z()));
242                                        pitch = SIMD_HALF_PI;
243                                        roll = btScalar(0.0);
244                                }
245                        }
246                        else
247                        {
248                                yaw = btScalar(btAtan2(-m_el[0].y(), m_el[0].z()));
249                                pitch = -SIMD_HALF_PI;
250                                roll = btScalar(0.0);
251                        }
252                }
253               
254
255       
256               
257                btMatrix3x3 scaled(const btVector3& s) const
258                {
259                        return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
260                                                                         m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
261                                                                         m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
262                }
263
264                btScalar            determinant() const;
265                btMatrix3x3 adjoint() const;
266                btMatrix3x3 absolute() const;
267                btMatrix3x3 transpose() const;
268                btMatrix3x3 inverse() const; 
269               
270                btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
271                btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
272               
273                SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const 
274                {
275                        return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
276                }
277                SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const 
278                {
279                        return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
280                }
281                SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const 
282                {
283                        return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
284                }
285               
286
287                ///diagonalizes this matrix by the Jacobi method. rot stores the rotation
288                ///from the coordinate system in which the matrix is diagonal to the original
289                ///coordinate system, i.e., old_this = rot * new_this * rot^T. The iteration
290                ///stops when all off-diagonal elements are less than the threshold multiplied
291                ///by the sum of the absolute values of the diagonal, or when maxSteps have
292                ///been executed. Note that this matrix is assumed to be symmetric.
293                void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
294                {
295                 rot.setIdentity();
296                 for (int step = maxSteps; step > 0; step--)
297                 {
298                        // find off-diagonal element [p][q] with largest magnitude
299                        int p = 0;
300                        int q = 1;
301                        int r = 2;
302                        btScalar max = btFabs(m_el[0][1]);
303                        btScalar v = btFabs(m_el[0][2]);
304                        if (v > max)
305                        {
306                           q = 2;
307                           r = 1;
308                           max = v;
309                        }
310                        v = btFabs(m_el[1][2]);
311                        if (v > max)
312                        {
313                           p = 1;
314                           q = 2;
315                           r = 0;
316                           max = v;
317                        }
318
319                        btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2]));
320                        if (max <= t)
321                        {
322                           if (max <= SIMD_EPSILON * t)
323                           {
324                                  return;
325                           }
326                           step = 1;
327                        }
328
329                        // compute Jacobi rotation J which leads to a zero for element [p][q]
330                        btScalar mpq = m_el[p][q];
331                        btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
332                        btScalar theta2 = theta * theta;
333                        btScalar cos;
334                        btScalar sin;
335                        if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON))
336                        {
337                           t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
338                                                                                : 1 / (theta - btSqrt(1 + theta2));
339                           cos = 1 / btSqrt(1 + t * t);
340                           sin = cos * t;
341                        }
342                        else
343                        {
344                           // approximation for large theta-value, i.e., a nearly diagonal matrix
345                           t = 1 / (theta * (2 + btScalar(0.5) / theta2));
346                           cos = 1 - btScalar(0.5) * t * t;
347                           sin = cos * t;
348                        }
349
350                        // apply rotation to matrix (this = J^T * this * J)
351                        m_el[p][q] = m_el[q][p] = 0;
352                        m_el[p][p] -= t * mpq;
353                        m_el[q][q] += t * mpq;
354                        btScalar mrp = m_el[r][p];
355                        btScalar mrq = m_el[r][q];
356                        m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
357                        m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
358
359                        // apply rotation to rot (rot = rot * J)
360                        for (int i = 0; i < 3; i++)
361                        {
362                           btVector3& row = rot[i];
363                           mrp = row[p];
364                           mrq = row[q];
365                           row[p] = cos * mrp - sin * mrq;
366                           row[q] = cos * mrq + sin * mrp;
367                        }
368                 }
369                }
370
371
372               
373        protected:
374                btScalar cofac(int r1, int c1, int r2, int c2) const 
375                {
376                        return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
377                }
378
379                btVector3 m_el[3];
380        };
381       
382        SIMD_FORCE_INLINE btMatrix3x3& 
383        btMatrix3x3::operator*=(const btMatrix3x3& m)
384        {
385                setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
386                                 m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
387                                 m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
388                return *this;
389        }
390       
391        SIMD_FORCE_INLINE btScalar
392        btMatrix3x3::determinant() const
393        { 
394                return triple((*this)[0], (*this)[1], (*this)[2]);
395        }
396       
397
398        SIMD_FORCE_INLINE btMatrix3x3
399        btMatrix3x3::absolute() const
400        {
401                return btMatrix3x3(
402                        btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
403                        btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
404                        btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
405        }
406
407        SIMD_FORCE_INLINE btMatrix3x3
408        btMatrix3x3::transpose() const 
409        {
410                return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
411                                                                 m_el[0].y(), m_el[1].y(), m_el[2].y(),
412                                                                 m_el[0].z(), m_el[1].z(), m_el[2].z());
413        }
414       
415        SIMD_FORCE_INLINE btMatrix3x3
416        btMatrix3x3::adjoint() const 
417        {
418                return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
419                                                                 cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
420                                                                 cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
421        }
422       
423        SIMD_FORCE_INLINE btMatrix3x3
424        btMatrix3x3::inverse() const
425        {
426                btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
427                btScalar det = (*this)[0].dot(co);
428                btFullAssert(det != btScalar(0.0));
429                btScalar s = btScalar(1.0) / det;
430                return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
431                                                                 co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
432                                                                 co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
433        }
434       
435        SIMD_FORCE_INLINE btMatrix3x3
436        btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
437        {
438                return btMatrix3x3(
439                        m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
440                        m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
441                        m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
442                        m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
443                        m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
444                        m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
445                        m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
446                        m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
447                        m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
448        }
449       
450        SIMD_FORCE_INLINE btMatrix3x3
451        btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
452        {
453                return btMatrix3x3(
454                        m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
455                        m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
456                        m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
457               
458        }
459
460        SIMD_FORCE_INLINE btVector3
461        operator*(const btMatrix3x3& m, const btVector3& v) 
462        {
463                return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
464        }
465       
466
467        SIMD_FORCE_INLINE btVector3
468        operator*(const btVector3& v, const btMatrix3x3& m)
469        {
470                return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
471        }
472
473        SIMD_FORCE_INLINE btMatrix3x3
474        operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
475        {
476                return btMatrix3x3(
477                        m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
478                        m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
479                        m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
480        }
481
482/*
483        SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
484    return btMatrix3x3(
485        m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
486        m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
487        m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
488        m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
489        m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
490        m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
491        m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
492        m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
493        m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
494}
495*/
496
497SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
498{
499   return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
500            m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
501            m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
502}
503
504#endif
Note: See TracBrowser for help on using the repository browser.