Matrix3x3.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 TF_MATRIX3x3_H
00017 #define TF_MATRIX3x3_H
00018 
00019 #include "Vector3.h"
00020 #include "Quaternion.h"
00021 
00022 namespace tf
00023 {
00024 
00025 
00026 #define Matrix3x3Data   Matrix3x3DoubleData 
00027 
00028 
00031 class Matrix3x3 {
00032 
00034         Vector3 m_el[3];
00035 
00036 public:
00038         Matrix3x3 () {}
00039 
00040         //              explicit Matrix3x3(const tfScalar *m) { setFromOpenGLSubMatrix(m); }
00041 
00043         explicit Matrix3x3(const Quaternion& q) { setRotation(q); }
00044         /*
00045         template <typename tfScalar>
00046         Matrix3x3(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll)
00047         { 
00048         setEulerYPR(yaw, pitch, roll);
00049         }
00050         */
00052         Matrix3x3(const tfScalar& xx, const tfScalar& xy, const tfScalar& xz,
00053                 const tfScalar& yx, const tfScalar& yy, const tfScalar& yz,
00054                 const tfScalar& zx, const tfScalar& zy, const tfScalar& zz)
00055         { 
00056                 setValue(xx, xy, xz, 
00057                         yx, yy, yz, 
00058                         zx, zy, zz);
00059         }
00061         TFSIMD_FORCE_INLINE Matrix3x3 (const Matrix3x3& 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         }
00067 
00068 
00070         TFSIMD_FORCE_INLINE Matrix3x3& operator=(const Matrix3x3& other)
00071         {
00072                 m_el[0] = other.m_el[0];
00073                 m_el[1] = other.m_el[1];
00074                 m_el[2] = other.m_el[2];
00075                 return *this;
00076         }
00077 
00078 
00081         TFSIMD_FORCE_INLINE Vector3 getColumn(int i) const
00082         {
00083                 return Vector3(m_el[0][i],m_el[1][i],m_el[2][i]);
00084         }
00085 
00086 
00089         TFSIMD_FORCE_INLINE const Vector3& getRow(int i) const
00090         {
00091                 tfFullAssert(0 <= i && i < 3);
00092                 return m_el[i];
00093         }
00094 
00097         TFSIMD_FORCE_INLINE Vector3&  operator[](int i)
00098         { 
00099                 tfFullAssert(0 <= i && i < 3);
00100                 return m_el[i]; 
00101         }
00102 
00105         TFSIMD_FORCE_INLINE const Vector3& operator[](int i) const
00106         {
00107                 tfFullAssert(0 <= i && i < 3);
00108                 return m_el[i]; 
00109         }
00110 
00114         Matrix3x3& operator*=(const Matrix3x3& m); 
00115 
00118         void setFromOpenGLSubMatrix(const tfScalar *m)
00119         {
00120                 m_el[0].setValue(m[0],m[4],m[8]);
00121                 m_el[1].setValue(m[1],m[5],m[9]);
00122                 m_el[2].setValue(m[2],m[6],m[10]);
00123 
00124         }
00135         void setValue(const tfScalar& xx, const tfScalar& xy, const tfScalar& xz, 
00136                 const tfScalar& yx, const tfScalar& yy, const tfScalar& yz, 
00137                 const tfScalar& zx, const tfScalar& zy, const tfScalar& zz)
00138         {
00139                 m_el[0].setValue(xx,xy,xz);
00140                 m_el[1].setValue(yx,yy,yz);
00141                 m_el[2].setValue(zx,zy,zz);
00142         }
00143 
00146         void setRotation(const Quaternion& q) 
00147         {
00148                 tfScalar d = q.length2();
00149                 tfFullAssert(d != tfScalar(0.0));
00150                 tfScalar s = tfScalar(2.0) / d;
00151                 tfScalar xs = q.x() * s,   ys = q.y() * s,   zs = q.z() * s;
00152                 tfScalar wx = q.w() * xs,  wy = q.w() * ys,  wz = q.w() * zs;
00153                 tfScalar xx = q.x() * xs,  xy = q.x() * ys,  xz = q.x() * zs;
00154                 tfScalar yy = q.y() * ys,  yz = q.y() * zs,  zz = q.z() * zs;
00155                 setValue(tfScalar(1.0) - (yy + zz), xy - wz, xz + wy,
00156                         xy + wz, tfScalar(1.0) - (xx + zz), yz - wx,
00157                         xz - wy, yz + wx, tfScalar(1.0) - (xx + yy));
00158         }
00159 
00160 
00166         void setEulerZYX(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll) __attribute__((deprecated))
00167         {
00168                 setEulerYPR(yaw, pitch, roll);
00169         }
00170 
00180         void setEulerYPR(tfScalar eulerZ, tfScalar eulerY,tfScalar eulerX)  { 
00181                 tfScalar ci ( tfCos(eulerX)); 
00182                 tfScalar cj ( tfCos(eulerY)); 
00183                 tfScalar ch ( tfCos(eulerZ)); 
00184                 tfScalar si ( tfSin(eulerX)); 
00185                 tfScalar sj ( tfSin(eulerY)); 
00186                 tfScalar sh ( tfSin(eulerZ)); 
00187                 tfScalar cc = ci * ch; 
00188                 tfScalar cs = ci * sh; 
00189                 tfScalar sc = si * ch; 
00190                 tfScalar ss = si * sh;
00191 
00192                 setValue(cj * ch, sj * sc - cs, sj * cc + ss,
00193                         cj * sh, sj * ss + cc, sj * cs - sc, 
00194                         -sj,      cj * si,      cj * ci);
00195         }
00196 
00203         void setRPY(tfScalar roll, tfScalar pitch,tfScalar yaw) { 
00204                setEulerYPR(yaw, pitch, roll);
00205         }
00206 
00208         void setIdentity()
00209         { 
00210                 setValue(tfScalar(1.0), tfScalar(0.0), tfScalar(0.0), 
00211                         tfScalar(0.0), tfScalar(1.0), tfScalar(0.0), 
00212                         tfScalar(0.0), tfScalar(0.0), tfScalar(1.0)); 
00213         }
00214 
00215         static const Matrix3x3& getIdentity()
00216         {
00217                 static const Matrix3x3 identityMatrix(tfScalar(1.0), tfScalar(0.0), tfScalar(0.0), 
00218                         tfScalar(0.0), tfScalar(1.0), tfScalar(0.0), 
00219                         tfScalar(0.0), tfScalar(0.0), tfScalar(1.0));
00220                 return identityMatrix;
00221         }
00222 
00225         void getOpenGLSubMatrix(tfScalar *m) const 
00226         {
00227                 m[0]  = tfScalar(m_el[0].x()); 
00228                 m[1]  = tfScalar(m_el[1].x());
00229                 m[2]  = tfScalar(m_el[2].x());
00230                 m[3]  = tfScalar(0.0); 
00231                 m[4]  = tfScalar(m_el[0].y());
00232                 m[5]  = tfScalar(m_el[1].y());
00233                 m[6]  = tfScalar(m_el[2].y());
00234                 m[7]  = tfScalar(0.0); 
00235                 m[8]  = tfScalar(m_el[0].z()); 
00236                 m[9]  = tfScalar(m_el[1].z());
00237                 m[10] = tfScalar(m_el[2].z());
00238                 m[11] = tfScalar(0.0); 
00239         }
00240 
00243         void getRotation(Quaternion& q) const
00244         {
00245                 tfScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
00246                 tfScalar temp[4];
00247 
00248                 if (trace > tfScalar(0.0)) 
00249                 {
00250                         tfScalar s = tfSqrt(trace + tfScalar(1.0));
00251                         temp[3]=(s * tfScalar(0.5));
00252                         s = tfScalar(0.5) / s;
00253 
00254                         temp[0]=((m_el[2].y() - m_el[1].z()) * s);
00255                         temp[1]=((m_el[0].z() - m_el[2].x()) * s);
00256                         temp[2]=((m_el[1].x() - m_el[0].y()) * s);
00257                 } 
00258                 else 
00259                 {
00260                         int i = m_el[0].x() < m_el[1].y() ? 
00261                                 (m_el[1].y() < m_el[2].z() ? 2 : 1) :
00262                                 (m_el[0].x() < m_el[2].z() ? 2 : 0); 
00263                         int j = (i + 1) % 3;  
00264                         int k = (i + 2) % 3;
00265 
00266                         tfScalar s = tfSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + tfScalar(1.0));
00267                         temp[i] = s * tfScalar(0.5);
00268                         s = tfScalar(0.5) / s;
00269 
00270                         temp[3] = (m_el[k][j] - m_el[j][k]) * s;
00271                         temp[j] = (m_el[j][i] + m_el[i][j]) * s;
00272                         temp[k] = (m_el[k][i] + m_el[i][k]) * s;
00273                 }
00274                 q.setValue(temp[0],temp[1],temp[2],temp[3]);
00275         }
00276 
00282         __attribute__((deprecated)) void getEulerZYX(tfScalar& yaw, tfScalar& pitch, tfScalar& roll, unsigned int solution_number = 1) const
00283         {
00284                 getEulerYPR(yaw, pitch, roll, solution_number);
00285         };
00286 
00287 
00292         void getEulerYPR(tfScalar& yaw, tfScalar& pitch, tfScalar& roll, unsigned int solution_number = 1) const
00293         {
00294                 struct Euler
00295                 {
00296                         tfScalar yaw;
00297                         tfScalar pitch;
00298                         tfScalar roll;
00299                 };
00300 
00301                 Euler euler_out;
00302                 Euler euler_out2; //second solution
00303                 //get the pointer to the raw data
00304 
00305                 // Check that pitch is not at a singularity
00306                 // Check that pitch is not at a singularity
00307                 if (tfFabs(m_el[2].x()) >= 1)
00308                 {
00309                         euler_out.yaw = 0;
00310                         euler_out2.yaw = 0;
00311         
00312                         // From difference of angles formula
00313                         if (m_el[2].x() < 0)  //gimbal locked down
00314                         {
00315                           tfScalar delta = tfAtan2(m_el[0].y(),m_el[0].z());
00316                                 euler_out.pitch = TFSIMD_PI / tfScalar(2.0);
00317                                 euler_out2.pitch = TFSIMD_PI / tfScalar(2.0);
00318                                 euler_out.roll = delta;
00319                                 euler_out2.roll = delta;
00320                         }
00321                         else // gimbal locked up
00322                         {
00323                           tfScalar delta = tfAtan2(-m_el[0].y(),-m_el[0].z());
00324                                 euler_out.pitch = -TFSIMD_PI / tfScalar(2.0);
00325                                 euler_out2.pitch = -TFSIMD_PI / tfScalar(2.0);
00326                                 euler_out.roll = delta;
00327                                 euler_out2.roll = delta;
00328                         }
00329                 }
00330                 else
00331                 {
00332                         euler_out.pitch = - tfAsin(m_el[2].x());
00333                         euler_out2.pitch = TFSIMD_PI - euler_out.pitch;
00334 
00335                         euler_out.roll = tfAtan2(m_el[2].y()/tfCos(euler_out.pitch), 
00336                                 m_el[2].z()/tfCos(euler_out.pitch));
00337                         euler_out2.roll = tfAtan2(m_el[2].y()/tfCos(euler_out2.pitch), 
00338                                 m_el[2].z()/tfCos(euler_out2.pitch));
00339 
00340                         euler_out.yaw = tfAtan2(m_el[1].x()/tfCos(euler_out.pitch), 
00341                                 m_el[0].x()/tfCos(euler_out.pitch));
00342                         euler_out2.yaw = tfAtan2(m_el[1].x()/tfCos(euler_out2.pitch), 
00343                                 m_el[0].x()/tfCos(euler_out2.pitch));
00344                 }
00345 
00346                 if (solution_number == 1)
00347                 { 
00348                         yaw = euler_out.yaw; 
00349                         pitch = euler_out.pitch;
00350                         roll = euler_out.roll;
00351                 }
00352                 else
00353                 { 
00354                         yaw = euler_out2.yaw; 
00355                         pitch = euler_out2.pitch;
00356                         roll = euler_out2.roll;
00357                 }
00358         }
00359 
00365         void getRPY(tfScalar& roll, tfScalar& pitch, tfScalar& yaw, unsigned int solution_number = 1) const
00366         {
00367         getEulerYPR(yaw, pitch, roll, solution_number);
00368         }
00369 
00373         Matrix3x3 scaled(const Vector3& s) const
00374         {
00375                 return Matrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
00376                         m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
00377                         m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
00378         }
00379 
00381         tfScalar            determinant() const;
00383         Matrix3x3 adjoint() const;
00385         Matrix3x3 absolute() const;
00387         Matrix3x3 transpose() const;
00389         Matrix3x3 inverse() const; 
00390 
00391         Matrix3x3 transposeTimes(const Matrix3x3& m) const;
00392         Matrix3x3 timesTranspose(const Matrix3x3& m) const;
00393 
00394         TFSIMD_FORCE_INLINE tfScalar tdotx(const Vector3& v) const 
00395         {
00396                 return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
00397         }
00398         TFSIMD_FORCE_INLINE tfScalar tdoty(const Vector3& v) const 
00399         {
00400                 return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
00401         }
00402         TFSIMD_FORCE_INLINE tfScalar tdotz(const Vector3& v) const 
00403         {
00404                 return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
00405         }
00406 
00407 
00417         void diagonalize(Matrix3x3& rot, tfScalar threshold, int maxSteps)
00418         {
00419                 rot.setIdentity();
00420                 for (int step = maxSteps; step > 0; step--)
00421                 {
00422                         // find off-diagonal element [p][q] with largest magnitude
00423                         int p = 0;
00424                         int q = 1;
00425                         int r = 2;
00426                         tfScalar max = tfFabs(m_el[0][1]);
00427                         tfScalar v = tfFabs(m_el[0][2]);
00428                         if (v > max)
00429                         {
00430                                 q = 2;
00431                                 r = 1;
00432                                 max = v;
00433                         }
00434                         v = tfFabs(m_el[1][2]);
00435                         if (v > max)
00436                         {
00437                                 p = 1;
00438                                 q = 2;
00439                                 r = 0;
00440                                 max = v;
00441                         }
00442 
00443                         tfScalar t = threshold * (tfFabs(m_el[0][0]) + tfFabs(m_el[1][1]) + tfFabs(m_el[2][2]));
00444                         if (max <= t)
00445                         {
00446                                 if (max <= TFSIMD_EPSILON * t)
00447                                 {
00448                                         return;
00449                                 }
00450                                 step = 1;
00451                         }
00452 
00453                         // compute Jacobi rotation J which leads to a zero for element [p][q] 
00454                         tfScalar mpq = m_el[p][q];
00455                         tfScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
00456                         tfScalar theta2 = theta * theta;
00457                         tfScalar cos;
00458                         tfScalar sin;
00459                         if (theta2 * theta2 < tfScalar(10 / TFSIMD_EPSILON))
00460                         {
00461                                 t = (theta >= 0) ? 1 / (theta + tfSqrt(1 + theta2))
00462                                         : 1 / (theta - tfSqrt(1 + theta2));
00463                                 cos = 1 / tfSqrt(1 + t * t);
00464                                 sin = cos * t;
00465                         }
00466                         else
00467                         {
00468                                 // approximation for large theta-value, i.e., a nearly diagonal matrix
00469                                 t = 1 / (theta * (2 + tfScalar(0.5) / theta2));
00470                                 cos = 1 - tfScalar(0.5) * t * t;
00471                                 sin = cos * t;
00472                         }
00473 
00474                         // apply rotation to matrix (this = J^T * this * J)
00475                         m_el[p][q] = m_el[q][p] = 0;
00476                         m_el[p][p] -= t * mpq;
00477                         m_el[q][q] += t * mpq;
00478                         tfScalar mrp = m_el[r][p];
00479                         tfScalar mrq = m_el[r][q];
00480                         m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
00481                         m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
00482 
00483                         // apply rotation to rot (rot = rot * J)
00484                         for (int i = 0; i < 3; i++)
00485                         {
00486                                 Vector3& row = rot[i];
00487                                 mrp = row[p];
00488                                 mrq = row[q];
00489                                 row[p] = cos * mrp - sin * mrq;
00490                                 row[q] = cos * mrq + sin * mrp;
00491                         }
00492                 }
00493         }
00494 
00495 
00496 
00497 
00505         tfScalar cofac(int r1, int c1, int r2, int c2) const 
00506         {
00507                 return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
00508         }
00509 
00510         void    serialize(struct        Matrix3x3Data& dataOut) const;
00511 
00512         void    serializeFloat(struct   Matrix3x3FloatData& dataOut) const;
00513 
00514         void    deSerialize(const struct        Matrix3x3Data& dataIn);
00515 
00516         void    deSerializeFloat(const struct   Matrix3x3FloatData& dataIn);
00517 
00518         void    deSerializeDouble(const struct  Matrix3x3DoubleData& dataIn);
00519 
00520 };
00521 
00522 
00523 TFSIMD_FORCE_INLINE Matrix3x3& 
00524 Matrix3x3::operator*=(const Matrix3x3& m)
00525 {
00526         setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
00527                 m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
00528                 m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
00529         return *this;
00530 }
00531 
00532 TFSIMD_FORCE_INLINE tfScalar 
00533 Matrix3x3::determinant() const
00534 { 
00535         return tfTriple((*this)[0], (*this)[1], (*this)[2]);
00536 }
00537 
00538 
00539 TFSIMD_FORCE_INLINE Matrix3x3 
00540 Matrix3x3::absolute() const
00541 {
00542         return Matrix3x3(
00543                 tfFabs(m_el[0].x()), tfFabs(m_el[0].y()), tfFabs(m_el[0].z()),
00544                 tfFabs(m_el[1].x()), tfFabs(m_el[1].y()), tfFabs(m_el[1].z()),
00545                 tfFabs(m_el[2].x()), tfFabs(m_el[2].y()), tfFabs(m_el[2].z()));
00546 }
00547 
00548 TFSIMD_FORCE_INLINE Matrix3x3 
00549 Matrix3x3::transpose() const 
00550 {
00551         return Matrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
00552                 m_el[0].y(), m_el[1].y(), m_el[2].y(),
00553                 m_el[0].z(), m_el[1].z(), m_el[2].z());
00554 }
00555 
00556 TFSIMD_FORCE_INLINE Matrix3x3 
00557 Matrix3x3::adjoint() const 
00558 {
00559         return Matrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
00560                 cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
00561                 cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
00562 }
00563 
00564 TFSIMD_FORCE_INLINE Matrix3x3 
00565 Matrix3x3::inverse() const
00566 {
00567         Vector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
00568         tfScalar det = (*this)[0].dot(co);
00569         tfFullAssert(det != tfScalar(0.0));
00570         tfScalar s = tfScalar(1.0) / det;
00571         return Matrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
00572                 co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
00573                 co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
00574 }
00575 
00576 TFSIMD_FORCE_INLINE Matrix3x3 
00577 Matrix3x3::transposeTimes(const Matrix3x3& m) const
00578 {
00579         return Matrix3x3(
00580                 m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
00581                 m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
00582                 m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
00583                 m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
00584                 m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
00585                 m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
00586                 m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
00587                 m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
00588                 m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
00589 }
00590 
00591 TFSIMD_FORCE_INLINE Matrix3x3 
00592 Matrix3x3::timesTranspose(const Matrix3x3& m) const
00593 {
00594         return Matrix3x3(
00595                 m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
00596                 m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
00597                 m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
00598 
00599 }
00600 
00601 TFSIMD_FORCE_INLINE Vector3 
00602 operator*(const Matrix3x3& m, const Vector3& v) 
00603 {
00604         return Vector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
00605 }
00606 
00607 
00608 TFSIMD_FORCE_INLINE Vector3
00609 operator*(const Vector3& v, const Matrix3x3& m)
00610 {
00611         return Vector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
00612 }
00613 
00614 TFSIMD_FORCE_INLINE Matrix3x3 
00615 operator*(const Matrix3x3& m1, const Matrix3x3& m2)
00616 {
00617         return Matrix3x3(
00618                 m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
00619                 m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
00620                 m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
00621 }
00622 
00623 /*
00624 TFSIMD_FORCE_INLINE Matrix3x3 tfMultTransposeLeft(const Matrix3x3& m1, const Matrix3x3& m2) {
00625 return Matrix3x3(
00626 m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
00627 m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
00628 m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
00629 m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
00630 m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
00631 m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
00632 m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
00633 m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
00634 m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
00635 }
00636 */
00637 
00640 TFSIMD_FORCE_INLINE bool operator==(const Matrix3x3& m1, const Matrix3x3& m2)
00641 {
00642         return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
00643                 m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
00644                 m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
00645 }
00646 
00648 struct  Matrix3x3FloatData
00649 {
00650         Vector3FloatData m_el[3];
00651 };
00652 
00654 struct  Matrix3x3DoubleData
00655 {
00656         Vector3DoubleData m_el[3];
00657 };
00658 
00659 
00660         
00661 
00662 TFSIMD_FORCE_INLINE     void    Matrix3x3::serialize(struct     Matrix3x3Data& dataOut) const
00663 {
00664         for (int i=0;i<3;i++)
00665                 m_el[i].serialize(dataOut.m_el[i]);
00666 }
00667 
00668 TFSIMD_FORCE_INLINE     void    Matrix3x3::serializeFloat(struct        Matrix3x3FloatData& dataOut) const
00669 {
00670         for (int i=0;i<3;i++)
00671                 m_el[i].serializeFloat(dataOut.m_el[i]);
00672 }
00673 
00674 
00675 TFSIMD_FORCE_INLINE     void    Matrix3x3::deSerialize(const struct     Matrix3x3Data& dataIn)
00676 {
00677         for (int i=0;i<3;i++)
00678                 m_el[i].deSerialize(dataIn.m_el[i]);
00679 }
00680 
00681 TFSIMD_FORCE_INLINE     void    Matrix3x3::deSerializeFloat(const struct        Matrix3x3FloatData& dataIn)
00682 {
00683         for (int i=0;i<3;i++)
00684                 m_el[i].deSerializeFloat(dataIn.m_el[i]);
00685 }
00686 
00687 TFSIMD_FORCE_INLINE     void    Matrix3x3::deSerializeDouble(const struct       Matrix3x3DoubleData& dataIn)
00688 {
00689         for (int i=0;i<3;i++)
00690                 m_el[i].deSerializeDouble(dataIn.m_el[i]);
00691 }
00692 
00693 }
00694 
00695 #endif //TF_MATRIX3x3_H
00696 


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Aug 11 2017 02:21:55