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


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Thu Jan 4 2018 03:35:29