Back to index

supertuxkart  0.5+dfsg1
btMatrix3x3.h
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
00003 
00004 This software is provided 'as-is', without any express or implied warranty.
00005 In no event will the authors be held liable for any damages arising from the use of this software.
00006 Permission is granted to anyone to use this software for any purpose, 
00007 including commercial applications, and to alter it and redistribute it freely, 
00008 subject to the following restrictions:
00009 
00010 1. 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.
00011 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00012 3. This notice may not be removed or altered from any source distribution.
00013 */
00014 
00015 
00016 #ifndef btMatrix3x3_H
00017 #define btMatrix3x3_H
00018 
00019 #include "btScalar.h"
00020 
00021 #include "btVector3.h"
00022 #include "btQuaternion.h"
00023 
00024 
00025 class btMatrix3x3 {
00026        public:
00027               btMatrix3x3 () {}
00028               
00029 //            explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
00030               
00031               explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); }
00032               /*
00033               template <typename btScalar>
00034               Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
00035               { 
00036                      setEulerYPR(yaw, pitch, roll);
00037               }
00038               */
00039               btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz,
00040                               const btScalar& yx, const btScalar& yy, const btScalar& yz,
00041                               const btScalar& zx, const btScalar& zy, const btScalar& zz)
00042               { 
00043                      setValue(xx, xy, xz, 
00044                                     yx, yy, yz, 
00045                                     zx, zy, zz);
00046               }
00047               
00048               SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
00049               {
00050                      m_el[0] = other.m_el[0];
00051                      m_el[1] = other.m_el[1];
00052                      m_el[2] = other.m_el[2];
00053               }
00054 
00055               SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
00056               {
00057                      m_el[0] = other.m_el[0];
00058                      m_el[1] = other.m_el[1];
00059                      m_el[2] = other.m_el[2];
00060                      return *this;
00061               }
00062 
00063               SIMD_FORCE_INLINE btVector3 getColumn(int i) const
00064               {
00065                      return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
00066               }
00067               
00068 
00069 
00070               SIMD_FORCE_INLINE const btVector3& getRow(int i) const
00071               {
00072                      return m_el[i];
00073               }
00074 
00075 
00076               SIMD_FORCE_INLINE btVector3&  operator[](int i)
00077               { 
00078                      btFullAssert(0 <= i && i < 3);
00079                      return m_el[i]; 
00080               }
00081               
00082               SIMD_FORCE_INLINE const btVector3& operator[](int i) const
00083               {
00084                      btFullAssert(0 <= i && i < 3);
00085                      return m_el[i]; 
00086               }
00087               
00088               btMatrix3x3& operator*=(const btMatrix3x3& m); 
00089               
00090        
00091        void setFromOpenGLSubMatrix(const btScalar *m)
00092               {
00093                      m_el[0].setValue(m[0],m[4],m[8]);
00094                      m_el[1].setValue(m[1],m[5],m[9]);
00095                      m_el[2].setValue(m[2],m[6],m[10]);
00096 
00097               }
00098 
00099               void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, 
00100                                      const btScalar& yx, const btScalar& yy, const btScalar& yz, 
00101                                      const btScalar& zx, const btScalar& zy, const btScalar& zz)
00102               {
00103                      m_el[0].setValue(xx,xy,xz);
00104                      m_el[1].setValue(yx,yy,yz);
00105                      m_el[2].setValue(zx,zy,zz);
00106               }
00107   
00108               void setRotation(const btQuaternion& q) 
00109               {
00110                      btScalar d = q.length2();
00111                      btFullAssert(d != btScalar(0.0));
00112                      btScalar s = btScalar(2.0) / d;
00113                      btScalar xs = q.x() * s,   ys = q.y() * s,   zs = q.z() * s;
00114                      btScalar wx = q.w() * xs,  wy = q.w() * ys,  wz = q.w() * zs;
00115                      btScalar xx = q.x() * xs,  xy = q.x() * ys,  xz = q.x() * zs;
00116                      btScalar yy = q.y() * ys,  yz = q.y() * zs,  zz = q.z() * zs;
00117                      setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
00118                                     xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
00119                                     xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
00120               }
00121               
00122 
00123 
00124               void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) 
00125               {
00126 
00127                      btScalar cy(btCos(yaw)); 
00128                      btScalar  sy(btSin(yaw)); 
00129                      btScalar  cp(btCos(pitch)); 
00130                      btScalar  sp(btSin(pitch)); 
00131                      btScalar  cr(btCos(roll));
00132                      btScalar  sr(btSin(roll));
00133                      btScalar  cc = cy * cr; 
00134                      btScalar  cs = cy * sr; 
00135                      btScalar  sc = sy * cr; 
00136                      btScalar  ss = sy * sr;
00137                      setValue(cc - sp * ss, -cs - sp * sc, -sy * cp,
00138                      cp * sr,       cp * cr,      -sp,
00139                                     sc + sp * cs, -ss + sp * cc,  cy * cp);
00140               
00141               }
00142 
00151        void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) {
00152               btScalar ci ( btCos(eulerX)); 
00153               btScalar cj ( btCos(eulerY)); 
00154               btScalar ch ( btCos(eulerZ)); 
00155               btScalar si ( btSin(eulerX)); 
00156               btScalar sj ( btSin(eulerY)); 
00157               btScalar sh ( btSin(eulerZ)); 
00158               btScalar cc = ci * ch; 
00159               btScalar cs = ci * sh; 
00160               btScalar sc = si * ch; 
00161               btScalar ss = si * sh;
00162               
00163               setValue(cj * ch, sj * sc - cs, sj * cc + ss,
00164                              cj * sh, sj * ss + cc, sj * cs - sc, 
00165                                     -sj,      cj * si,      cj * ci);
00166        }
00167 
00168               void setIdentity()
00169               { 
00170                      setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0), 
00171                                     btScalar(0.0), btScalar(1.0), btScalar(0.0), 
00172                                     btScalar(0.0), btScalar(0.0), btScalar(1.0)); 
00173               }
00174     
00175               void getOpenGLSubMatrix(btScalar *m) const 
00176               {
00177                      m[0]  = btScalar(m_el[0].x()); 
00178                      m[1]  = btScalar(m_el[1].x());
00179                      m[2]  = btScalar(m_el[2].x());
00180                      m[3]  = btScalar(0.0); 
00181                      m[4]  = btScalar(m_el[0].y());
00182                      m[5]  = btScalar(m_el[1].y());
00183                      m[6]  = btScalar(m_el[2].y());
00184                      m[7]  = btScalar(0.0); 
00185                      m[8]  = btScalar(m_el[0].z()); 
00186                      m[9]  = btScalar(m_el[1].z());
00187                      m[10] = btScalar(m_el[2].z());
00188                      m[11] = btScalar(0.0); 
00189               }
00190 
00191               void getRotation(btQuaternion& q) const
00192               {
00193                      btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
00194                      btScalar temp[4];
00195                      
00196                      if (trace > btScalar(0.0)) 
00197                      {
00198                             btScalar s = btSqrt(trace + btScalar(1.0));
00199                             temp[3]=(s * btScalar(0.5));
00200                             s = btScalar(0.5) / s;
00201                             
00202                             temp[0]=((m_el[2].y() - m_el[1].z()) * s);
00203                             temp[1]=((m_el[0].z() - m_el[2].x()) * s);
00204                             temp[2]=((m_el[1].x() - m_el[0].y()) * s);
00205                      } 
00206                      else 
00207                      {
00208                             int i = m_el[0].x() < m_el[1].y() ? 
00209                                    (m_el[1].y() < m_el[2].z() ? 2 : 1) :
00210                                    (m_el[0].x() < m_el[2].z() ? 2 : 0); 
00211                             int j = (i + 1) % 3;  
00212                             int k = (i + 2) % 3;
00213                             
00214                             btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
00215                             temp[i] = s * btScalar(0.5);
00216                             s = btScalar(0.5) / s;
00217                             
00218                             temp[3] = (m_el[k][j] - m_el[j][k]) * s;
00219                             temp[j] = (m_el[j][i] + m_el[i][j]) * s;
00220                             temp[k] = (m_el[k][i] + m_el[i][k]) * s;
00221                      }
00222                      q.setValue(temp[0],temp[1],temp[2],temp[3]);
00223               }
00224        
00225               void getEuler(btScalar& yaw, btScalar& pitch, btScalar& roll) const
00226               {
00227                      
00228                      if (btScalar(m_el[1].z()) < btScalar(1))
00229                      {
00230                             if (btScalar(m_el[1].z()) > -btScalar(1))
00231                             {
00232                                    yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
00233                                    pitch = btScalar(btAsin(-m_el[1].y()));
00234                                    roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
00235                             }
00236                             else 
00237                             {
00238                                    yaw = btScalar(-btAtan2(-m_el[0].y(), m_el[0].z()));
00239                                    pitch = SIMD_HALF_PI;
00240                                    roll = btScalar(0.0);
00241                             }
00242                      }
00243                      else
00244                      {
00245                             yaw = btScalar(btAtan2(-m_el[0].y(), m_el[0].z()));
00246                             pitch = -SIMD_HALF_PI;
00247                             roll = btScalar(0.0);
00248                      }
00249               }
00250               
00251 
00252        
00253               
00254               btMatrix3x3 scaled(const btVector3& s) const
00255               {
00256                      return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
00257                                                                 m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
00258                                                                 m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
00259               }
00260 
00261               btScalar            determinant() const;
00262               btMatrix3x3 adjoint() const;
00263               btMatrix3x3 absolute() const;
00264               btMatrix3x3 transpose() const;
00265               btMatrix3x3 inverse() const; 
00266               
00267               btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
00268               btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
00269               
00270               SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const 
00271               {
00272                      return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
00273               }
00274               SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const 
00275               {
00276                      return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
00277               }
00278               SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const 
00279               {
00280                      return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
00281               }
00282               
00283 
00284               
00285        protected:
00286               btScalar cofac(int r1, int c1, int r2, int c2) const 
00287               {
00288                      return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
00289               }
00290 
00291               btVector3 m_el[3];
00292        };
00293        
00294        SIMD_FORCE_INLINE btMatrix3x3& 
00295        btMatrix3x3::operator*=(const btMatrix3x3& m)
00296        {
00297               setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
00298                              m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
00299                              m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
00300               return *this;
00301        }
00302        
00303        SIMD_FORCE_INLINE btScalar 
00304        btMatrix3x3::determinant() const
00305        { 
00306               return triple((*this)[0], (*this)[1], (*this)[2]);
00307        }
00308        
00309 
00310        SIMD_FORCE_INLINE btMatrix3x3 
00311        btMatrix3x3::absolute() const
00312        {
00313               return btMatrix3x3(
00314                      btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
00315                      btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
00316                      btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
00317        }
00318 
00319        SIMD_FORCE_INLINE btMatrix3x3 
00320        btMatrix3x3::transpose() const 
00321        {
00322               return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
00323                                                          m_el[0].y(), m_el[1].y(), m_el[2].y(),
00324                                                          m_el[0].z(), m_el[1].z(), m_el[2].z());
00325        }
00326        
00327        SIMD_FORCE_INLINE btMatrix3x3 
00328        btMatrix3x3::adjoint() const 
00329        {
00330               return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
00331                                                          cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
00332                                                          cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
00333        }
00334        
00335        SIMD_FORCE_INLINE btMatrix3x3 
00336        btMatrix3x3::inverse() const
00337        {
00338               btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
00339               btScalar det = (*this)[0].dot(co);
00340               btFullAssert(det != btScalar(0.0));
00341               btScalar s = btScalar(1.0) / det;
00342               return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
00343                                                          co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
00344                                                          co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
00345        }
00346        
00347        SIMD_FORCE_INLINE btMatrix3x3 
00348        btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
00349        {
00350               return btMatrix3x3(
00351                      m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
00352                      m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
00353                      m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
00354                      m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
00355                      m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
00356                      m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
00357                      m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
00358                      m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
00359                      m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
00360        }
00361        
00362        SIMD_FORCE_INLINE btMatrix3x3 
00363        btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
00364        {
00365               return btMatrix3x3(
00366                      m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
00367                      m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
00368                      m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
00369               
00370        }
00371 
00372        SIMD_FORCE_INLINE btVector3 
00373        operator*(const btMatrix3x3& m, const btVector3& v) 
00374        {
00375               return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
00376        }
00377        
00378 
00379        SIMD_FORCE_INLINE btVector3
00380        operator*(const btVector3& v, const btMatrix3x3& m)
00381        {
00382               return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
00383        }
00384 
00385        SIMD_FORCE_INLINE btMatrix3x3 
00386        operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
00387        {
00388               return btMatrix3x3(
00389                      m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
00390                      m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
00391                      m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
00392        }
00393 
00394 /*
00395        SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
00396     return btMatrix3x3(
00397         m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
00398         m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
00399         m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
00400         m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
00401         m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
00402         m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
00403         m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
00404         m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
00405         m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
00406 }
00407 */
00408 
00409 
00410 #endif