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 BT_MATRIX3x3_H
00017 #define BT_MATRIX3x3_H
00018 
00019 #include "btVector3.h"
00020 #include "btQuaternion.h"
00021 
00022 #ifdef BT_USE_DOUBLE_PRECISION
00023 #define btMatrix3x3Data btMatrix3x3DoubleData 
00024 #else
00025 #define btMatrix3x3Data btMatrix3x3FloatData
00026 #endif //BT_USE_DOUBLE_PRECISION
00027 
00028 
00031 class btMatrix3x3 {
00032 
00034         btVector3 m_el[3];
00035 
00036 public:
00038         btMatrix3x3 () {}
00039 
00040         //              explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
00041 
00043         explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); }
00044         /*
00045         template <typename btScalar>
00046         Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
00047         { 
00048         setEulerYPR(yaw, pitch, roll);
00049         }
00050         */
00052         btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz,
00053                 const btScalar& yx, const btScalar& yy, const btScalar& yz,
00054                 const btScalar& zx, const btScalar& zy, const btScalar& zz)
00055         { 
00056                 setValue(xx, xy, xz, 
00057                         yx, yy, yz, 
00058                         zx, zy, zz);
00059         }
00061         SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
00062         {
00063                 m_el[0] = other.m_el[0];
00064                 m_el[1] = other.m_el[1];
00065                 m_el[2] = other.m_el[2];
00066         }
00068         SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
00069         {
00070                 m_el[0] = other.m_el[0];
00071                 m_el[1] = other.m_el[1];
00072                 m_el[2] = other.m_el[2];
00073                 return *this;
00074         }
00075 
00078         SIMD_FORCE_INLINE btVector3 getColumn(int i) const
00079         {
00080                 return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
00081         }
00082 
00083 
00086         SIMD_FORCE_INLINE const btVector3& getRow(int i) const
00087         {
00088                 btFullAssert(0 <= i && i < 3);
00089                 return m_el[i];
00090         }
00091 
00094         SIMD_FORCE_INLINE btVector3&  operator[](int i)
00095         { 
00096                 btFullAssert(0 <= i && i < 3);
00097                 return m_el[i]; 
00098         }
00099 
00102         SIMD_FORCE_INLINE const btVector3& operator[](int i) const
00103         {
00104                 btFullAssert(0 <= i && i < 3);
00105                 return m_el[i]; 
00106         }
00107 
00111         btMatrix3x3& operator*=(const btMatrix3x3& m); 
00112 
00116         btMatrix3x3& operator+=(const btMatrix3x3& m); 
00117 
00121         btMatrix3x3& operator-=(const btMatrix3x3& m); 
00122 
00125         void setFromOpenGLSubMatrix(const btScalar *m)
00126         {
00127                 m_el[0].setValue(m[0],m[4],m[8]);
00128                 m_el[1].setValue(m[1],m[5],m[9]);
00129                 m_el[2].setValue(m[2],m[6],m[10]);
00130 
00131         }
00142         void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz, 
00143                 const btScalar& yx, const btScalar& yy, const btScalar& yz, 
00144                 const btScalar& zx, const btScalar& zy, const btScalar& zz)
00145         {
00146                 m_el[0].setValue(xx,xy,xz);
00147                 m_el[1].setValue(yx,yy,yz);
00148                 m_el[2].setValue(zx,zy,zz);
00149         }
00150 
00153         void setRotation(const btQuaternion& q) 
00154         {
00155                 btScalar d = q.length2();
00156                 btFullAssert(d != btScalar(0.0));
00157                 btScalar s = btScalar(2.0) / d;
00158                 btScalar xs = q.x() * s,   ys = q.y() * s,   zs = q.z() * s;
00159                 btScalar wx = q.w() * xs,  wy = q.w() * ys,  wz = q.w() * zs;
00160                 btScalar xx = q.x() * xs,  xy = q.x() * ys,  xz = q.x() * zs;
00161                 btScalar yy = q.y() * ys,  yz = q.y() * zs,  zz = q.z() * zs;
00162                 setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
00163                         xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
00164                         xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
00165         }
00166 
00167 
00173         void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) 
00174         {
00175                 setEulerZYX(roll, pitch, yaw);
00176         }
00177 
00187         void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) { 
00189                 btScalar ci ( btCos(eulerX)); 
00190                 btScalar cj ( btCos(eulerY)); 
00191                 btScalar ch ( btCos(eulerZ)); 
00192                 btScalar si ( btSin(eulerX)); 
00193                 btScalar sj ( btSin(eulerY)); 
00194                 btScalar sh ( btSin(eulerZ)); 
00195                 btScalar cc = ci * ch; 
00196                 btScalar cs = ci * sh; 
00197                 btScalar sc = si * ch; 
00198                 btScalar ss = si * sh;
00199 
00200                 setValue(cj * ch, sj * sc - cs, sj * cc + ss,
00201                         cj * sh, sj * ss + cc, sj * cs - sc, 
00202                         -sj,      cj * si,      cj * ci);
00203         }
00204 
00206         void setIdentity()
00207         { 
00208                 setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0), 
00209                         btScalar(0.0), btScalar(1.0), btScalar(0.0), 
00210                         btScalar(0.0), btScalar(0.0), btScalar(1.0)); 
00211         }
00212 
00213         static const btMatrix3x3&       getIdentity()
00214         {
00215                 static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0), 
00216                         btScalar(0.0), btScalar(1.0), btScalar(0.0), 
00217                         btScalar(0.0), btScalar(0.0), btScalar(1.0));
00218                 return identityMatrix;
00219         }
00220 
00223         void getOpenGLSubMatrix(btScalar *m) const 
00224         {
00225                 m[0]  = btScalar(m_el[0].x()); 
00226                 m[1]  = btScalar(m_el[1].x());
00227                 m[2]  = btScalar(m_el[2].x());
00228                 m[3]  = btScalar(0.0); 
00229                 m[4]  = btScalar(m_el[0].y());
00230                 m[5]  = btScalar(m_el[1].y());
00231                 m[6]  = btScalar(m_el[2].y());
00232                 m[7]  = btScalar(0.0); 
00233                 m[8]  = btScalar(m_el[0].z()); 
00234                 m[9]  = btScalar(m_el[1].z());
00235                 m[10] = btScalar(m_el[2].z());
00236                 m[11] = btScalar(0.0); 
00237         }
00238 
00241         void getRotation(btQuaternion& q) const
00242         {
00243                 btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
00244                 btScalar temp[4];
00245 
00246                 if (trace > btScalar(0.0)) 
00247                 {
00248                         btScalar s = btSqrt(trace + btScalar(1.0));
00249                         temp[3]=(s * btScalar(0.5));
00250                         s = btScalar(0.5) / s;
00251 
00252                         temp[0]=((m_el[2].y() - m_el[1].z()) * s);
00253                         temp[1]=((m_el[0].z() - m_el[2].x()) * s);
00254                         temp[2]=((m_el[1].x() - m_el[0].y()) * s);
00255                 } 
00256                 else 
00257                 {
00258                         int i = m_el[0].x() < m_el[1].y() ? 
00259                                 (m_el[1].y() < m_el[2].z() ? 2 : 1) :
00260                                 (m_el[0].x() < m_el[2].z() ? 2 : 0); 
00261                         int j = (i + 1) % 3;  
00262                         int k = (i + 2) % 3;
00263 
00264                         btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
00265                         temp[i] = s * btScalar(0.5);
00266                         s = btScalar(0.5) / s;
00267 
00268                         temp[3] = (m_el[k][j] - m_el[j][k]) * s;
00269                         temp[j] = (m_el[j][i] + m_el[i][j]) * s;
00270                         temp[k] = (m_el[k][i] + m_el[i][k]) * s;
00271                 }
00272                 q.setValue(temp[0],temp[1],temp[2],temp[3]);
00273         }
00274 
00279         void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const
00280         {
00281 
00282                 // first use the normal calculus
00283                 yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
00284                 pitch = btScalar(btAsin(-m_el[2].x()));
00285                 roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
00286 
00287                 // on pitch = +/-HalfPI
00288                 if (btFabs(pitch)==SIMD_HALF_PI)
00289                 {
00290                         if (yaw>0)
00291                                 yaw-=SIMD_PI;
00292                         else
00293                                 yaw+=SIMD_PI;
00294 
00295                         if (roll>0)
00296                                 roll-=SIMD_PI;
00297                         else
00298                                 roll+=SIMD_PI;
00299                 }
00300         };
00301 
00302 
00308         void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const
00309         {
00310                 struct Euler
00311                 {
00312                         btScalar yaw;
00313                         btScalar pitch;
00314                         btScalar roll;
00315                 };
00316 
00317                 Euler euler_out;
00318                 Euler euler_out2; //second solution
00319                 //get the pointer to the raw data
00320 
00321                 // Check that pitch is not at a singularity
00322                 if (btFabs(m_el[2].x()) >= 1)
00323                 {
00324                         euler_out.yaw = 0;
00325                         euler_out2.yaw = 0;
00326 
00327                         // From difference of angles formula
00328                         btScalar delta = btAtan2(m_el[0].x(),m_el[0].z());
00329                         if (m_el[2].x() > 0)  //gimbal locked up
00330                         {
00331                                 euler_out.pitch = SIMD_PI / btScalar(2.0);
00332                                 euler_out2.pitch = SIMD_PI / btScalar(2.0);
00333                                 euler_out.roll = euler_out.pitch + delta;
00334                                 euler_out2.roll = euler_out.pitch + delta;
00335                         }
00336                         else // gimbal locked down
00337                         {
00338                                 euler_out.pitch = -SIMD_PI / btScalar(2.0);
00339                                 euler_out2.pitch = -SIMD_PI / btScalar(2.0);
00340                                 euler_out.roll = -euler_out.pitch + delta;
00341                                 euler_out2.roll = -euler_out.pitch + delta;
00342                         }
00343                 }
00344                 else
00345                 {
00346                         euler_out.pitch = - btAsin(m_el[2].x());
00347                         euler_out2.pitch = SIMD_PI - euler_out.pitch;
00348 
00349                         euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch), 
00350                                 m_el[2].z()/btCos(euler_out.pitch));
00351                         euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch), 
00352                                 m_el[2].z()/btCos(euler_out2.pitch));
00353 
00354                         euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch), 
00355                                 m_el[0].x()/btCos(euler_out.pitch));
00356                         euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch), 
00357                                 m_el[0].x()/btCos(euler_out2.pitch));
00358                 }
00359 
00360                 if (solution_number == 1)
00361                 { 
00362                         yaw = euler_out.yaw; 
00363                         pitch = euler_out.pitch;
00364                         roll = euler_out.roll;
00365                 }
00366                 else
00367                 { 
00368                         yaw = euler_out2.yaw; 
00369                         pitch = euler_out2.pitch;
00370                         roll = euler_out2.roll;
00371                 }
00372         }
00373 
00377         btMatrix3x3 scaled(const btVector3& s) const
00378         {
00379                 return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
00380                         m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
00381                         m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
00382         }
00383 
00385         btScalar            determinant() const;
00387         btMatrix3x3 adjoint() const;
00389         btMatrix3x3 absolute() const;
00391         btMatrix3x3 transpose() const;
00393         btMatrix3x3 inverse() const; 
00394 
00395         btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
00396         btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
00397 
00398         SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const 
00399         {
00400                 return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
00401         }
00402         SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const 
00403         {
00404                 return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
00405         }
00406         SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const 
00407         {
00408                 return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
00409         }
00410 
00411 
00421         void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
00422         {
00423                 rot.setIdentity();
00424                 for (int step = maxSteps; step > 0; step--)
00425                 {
00426                         // find off-diagonal element [p][q] with largest magnitude
00427                         int p = 0;
00428                         int q = 1;
00429                         int r = 2;
00430                         btScalar max = btFabs(m_el[0][1]);
00431                         btScalar v = btFabs(m_el[0][2]);
00432                         if (v > max)
00433                         {
00434                                 q = 2;
00435                                 r = 1;
00436                                 max = v;
00437                         }
00438                         v = btFabs(m_el[1][2]);
00439                         if (v > max)
00440                         {
00441                                 p = 1;
00442                                 q = 2;
00443                                 r = 0;
00444                                 max = v;
00445                         }
00446 
00447                         btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2]));
00448                         if (max <= t)
00449                         {
00450                                 if (max <= SIMD_EPSILON * t)
00451                                 {
00452                                         return;
00453                                 }
00454                                 step = 1;
00455                         }
00456 
00457                         // compute Jacobi rotation J which leads to a zero for element [p][q] 
00458                         btScalar mpq = m_el[p][q];
00459                         btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
00460                         btScalar theta2 = theta * theta;
00461                         btScalar cos;
00462                         btScalar sin;
00463                         if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON))
00464                         {
00465                                 t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
00466                                         : 1 / (theta - btSqrt(1 + theta2));
00467                                 cos = 1 / btSqrt(1 + t * t);
00468                                 sin = cos * t;
00469                         }
00470                         else
00471                         {
00472                                 // approximation for large theta-value, i.e., a nearly diagonal matrix
00473                                 t = 1 / (theta * (2 + btScalar(0.5) / theta2));
00474                                 cos = 1 - btScalar(0.5) * t * t;
00475                                 sin = cos * t;
00476                         }
00477 
00478                         // apply rotation to matrix (this = J^T * this * J)
00479                         m_el[p][q] = m_el[q][p] = 0;
00480                         m_el[p][p] -= t * mpq;
00481                         m_el[q][q] += t * mpq;
00482                         btScalar mrp = m_el[r][p];
00483                         btScalar mrq = m_el[r][q];
00484                         m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
00485                         m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
00486 
00487                         // apply rotation to rot (rot = rot * J)
00488                         for (int i = 0; i < 3; i++)
00489                         {
00490                                 btVector3& row = rot[i];
00491                                 mrp = row[p];
00492                                 mrq = row[q];
00493                                 row[p] = cos * mrp - sin * mrq;
00494                                 row[q] = cos * mrq + sin * mrp;
00495                         }
00496                 }
00497         }
00498 
00499 
00500 
00501 
00509         btScalar cofac(int r1, int c1, int r2, int c2) const 
00510         {
00511                 return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
00512         }
00513 
00514         void    serialize(struct        btMatrix3x3Data& dataOut) const;
00515 
00516         void    serializeFloat(struct   btMatrix3x3FloatData& dataOut) const;
00517 
00518         void    deSerialize(const struct        btMatrix3x3Data& dataIn);
00519 
00520         void    deSerializeFloat(const struct   btMatrix3x3FloatData& dataIn);
00521 
00522         void    deSerializeDouble(const struct  btMatrix3x3DoubleData& dataIn);
00523 
00524 };
00525 
00526 
00527 SIMD_FORCE_INLINE btMatrix3x3& 
00528 btMatrix3x3::operator*=(const btMatrix3x3& m)
00529 {
00530         setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
00531                 m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
00532                 m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
00533         return *this;
00534 }
00535 
00536 SIMD_FORCE_INLINE btMatrix3x3& 
00537 btMatrix3x3::operator+=(const btMatrix3x3& m)
00538 {
00539         setValue(
00540                 m_el[0][0]+m.m_el[0][0], 
00541                 m_el[0][1]+m.m_el[0][1],
00542                 m_el[0][2]+m.m_el[0][2],
00543                 m_el[1][0]+m.m_el[1][0], 
00544                 m_el[1][1]+m.m_el[1][1],
00545                 m_el[1][2]+m.m_el[1][2],
00546                 m_el[2][0]+m.m_el[2][0], 
00547                 m_el[2][1]+m.m_el[2][1],
00548                 m_el[2][2]+m.m_el[2][2]);
00549         return *this;
00550 }
00551 
00552 SIMD_FORCE_INLINE btMatrix3x3
00553 operator*(const btMatrix3x3& m, const btScalar & k)
00554 {
00555         return btMatrix3x3(
00556                 m[0].x()*k,m[0].y()*k,m[0].z()*k,
00557                 m[1].x()*k,m[1].y()*k,m[1].z()*k,
00558                 m[2].x()*k,m[2].y()*k,m[2].z()*k);
00559 }
00560 
00561  SIMD_FORCE_INLINE btMatrix3x3 
00562 operator+(const btMatrix3x3& m1, const btMatrix3x3& m2)
00563 {
00564         return btMatrix3x3(
00565         m1[0][0]+m2[0][0], 
00566         m1[0][1]+m2[0][1],
00567         m1[0][2]+m2[0][2],
00568         m1[1][0]+m2[1][0], 
00569         m1[1][1]+m2[1][1],
00570         m1[1][2]+m2[1][2],
00571         m1[2][0]+m2[2][0], 
00572         m1[2][1]+m2[2][1],
00573         m1[2][2]+m2[2][2]);
00574 }
00575 
00576 SIMD_FORCE_INLINE btMatrix3x3 
00577 operator-(const btMatrix3x3& m1, const btMatrix3x3& m2)
00578 {
00579         return btMatrix3x3(
00580         m1[0][0]-m2[0][0], 
00581         m1[0][1]-m2[0][1],
00582         m1[0][2]-m2[0][2],
00583         m1[1][0]-m2[1][0], 
00584         m1[1][1]-m2[1][1],
00585         m1[1][2]-m2[1][2],
00586         m1[2][0]-m2[2][0], 
00587         m1[2][1]-m2[2][1],
00588         m1[2][2]-m2[2][2]);
00589 }
00590 
00591 
00592 SIMD_FORCE_INLINE btMatrix3x3& 
00593 btMatrix3x3::operator-=(const btMatrix3x3& m)
00594 {
00595         setValue(
00596         m_el[0][0]-m.m_el[0][0], 
00597         m_el[0][1]-m.m_el[0][1],
00598         m_el[0][2]-m.m_el[0][2],
00599         m_el[1][0]-m.m_el[1][0], 
00600         m_el[1][1]-m.m_el[1][1],
00601         m_el[1][2]-m.m_el[1][2],
00602         m_el[2][0]-m.m_el[2][0], 
00603         m_el[2][1]-m.m_el[2][1],
00604         m_el[2][2]-m.m_el[2][2]);
00605         return *this;
00606 }
00607 
00608 
00609 SIMD_FORCE_INLINE btScalar 
00610 btMatrix3x3::determinant() const
00611 { 
00612         return btTriple((*this)[0], (*this)[1], (*this)[2]);
00613 }
00614 
00615 
00616 SIMD_FORCE_INLINE btMatrix3x3 
00617 btMatrix3x3::absolute() const
00618 {
00619         return btMatrix3x3(
00620                 btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
00621                 btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
00622                 btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
00623 }
00624 
00625 SIMD_FORCE_INLINE btMatrix3x3 
00626 btMatrix3x3::transpose() const 
00627 {
00628         return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
00629                 m_el[0].y(), m_el[1].y(), m_el[2].y(),
00630                 m_el[0].z(), m_el[1].z(), m_el[2].z());
00631 }
00632 
00633 SIMD_FORCE_INLINE btMatrix3x3 
00634 btMatrix3x3::adjoint() const 
00635 {
00636         return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
00637                 cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
00638                 cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
00639 }
00640 
00641 SIMD_FORCE_INLINE btMatrix3x3 
00642 btMatrix3x3::inverse() const
00643 {
00644         btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
00645         btScalar det = (*this)[0].dot(co);
00646         btFullAssert(det != btScalar(0.0));
00647         btScalar s = btScalar(1.0) / det;
00648         return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
00649                 co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
00650                 co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
00651 }
00652 
00653 SIMD_FORCE_INLINE btMatrix3x3 
00654 btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
00655 {
00656         return btMatrix3x3(
00657                 m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
00658                 m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
00659                 m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
00660                 m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
00661                 m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
00662                 m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
00663                 m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
00664                 m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
00665                 m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
00666 }
00667 
00668 SIMD_FORCE_INLINE btMatrix3x3 
00669 btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
00670 {
00671         return btMatrix3x3(
00672                 m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
00673                 m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
00674                 m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
00675 
00676 }
00677 
00678 SIMD_FORCE_INLINE btVector3 
00679 operator*(const btMatrix3x3& m, const btVector3& v) 
00680 {
00681         return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
00682 }
00683 
00684 
00685 SIMD_FORCE_INLINE btVector3
00686 operator*(const btVector3& v, const btMatrix3x3& m)
00687 {
00688         return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
00689 }
00690 
00691 SIMD_FORCE_INLINE btMatrix3x3 
00692 operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
00693 {
00694         return btMatrix3x3(
00695                 m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
00696                 m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
00697                 m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
00698 }
00699 
00700 /*
00701 SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
00702 return btMatrix3x3(
00703 m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
00704 m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
00705 m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
00706 m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
00707 m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
00708 m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
00709 m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
00710 m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
00711 m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
00712 }
00713 */
00714 
00717 SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
00718 {
00719         return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
00720                 m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
00721                 m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
00722 }
00723 
00725 struct  btMatrix3x3FloatData
00726 {
00727         btVector3FloatData m_el[3];
00728 };
00729 
00731 struct  btMatrix3x3DoubleData
00732 {
00733         btVector3DoubleData m_el[3];
00734 };
00735 
00736 
00737         
00738 
00739 SIMD_FORCE_INLINE       void    btMatrix3x3::serialize(struct   btMatrix3x3Data& dataOut) const
00740 {
00741         for (int i=0;i<3;i++)
00742                 m_el[i].serialize(dataOut.m_el[i]);
00743 }
00744 
00745 SIMD_FORCE_INLINE       void    btMatrix3x3::serializeFloat(struct      btMatrix3x3FloatData& dataOut) const
00746 {
00747         for (int i=0;i<3;i++)
00748                 m_el[i].serializeFloat(dataOut.m_el[i]);
00749 }
00750 
00751 
00752 SIMD_FORCE_INLINE       void    btMatrix3x3::deSerialize(const struct   btMatrix3x3Data& dataIn)
00753 {
00754         for (int i=0;i<3;i++)
00755                 m_el[i].deSerialize(dataIn.m_el[i]);
00756 }
00757 
00758 SIMD_FORCE_INLINE       void    btMatrix3x3::deSerializeFloat(const struct      btMatrix3x3FloatData& dataIn)
00759 {
00760         for (int i=0;i<3;i++)
00761                 m_el[i].deSerializeFloat(dataIn.m_el[i]);
00762 }
00763 
00764 SIMD_FORCE_INLINE       void    btMatrix3x3::deSerializeDouble(const struct     btMatrix3x3DoubleData& dataIn)
00765 {
00766         for (int i=0;i<3;i++)
00767                 m_el[i].deSerializeDouble(dataIn.m_el[i]);
00768 }
00769 
00770 #endif //BT_MATRIX3x3_H
00771 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


bullet
Author(s): Erwin Coumans, ROS package maintained by Tully Foote
autogenerated on Wed Oct 31 2012 07:54:31