src/LinearMath/btMatrix3x3.h

   1 /*
   2 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
   3
   4 This software is provided 'as-is', without any express or implied warranty.
   5 In no event will the authors be held liable for any damages arising from the use of this software.
   6 Permission is granted to anyone to use this software for any purpose,
   7 including commercial applications, and to alter it and redistribute it freely,
   8 subject to the following restrictions:
   9
  10 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.
  11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  12 3. This notice may not be removed or altered from any source distribution.
  13 */
  14
  15
  16 #ifndef BT_MATRIX3x3_H
  17 #define BT_MATRIX3x3_H
  18
  19 #include "btVector3.h"
  20 #include "btQuaternion.h"
  21
  22 #ifdef BT_USE_DOUBLE_PRECISION
  23 #define btMatrix3x3Data btMatrix3x3DoubleData
  24 #else
  25 #define btMatrix3x3Data btMatrix3x3FloatData
  26 #endif //BT_USE_DOUBLE_PRECISION
  27
  28
  29 /**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3.
  30 * Make sure to only include a pure orthogonal matrix without scaling. */
  31 class btMatrix3x3 {
  32
  33         ///Data storage for the matrix, each vector is a row of the matrix
  34         btVector3 m_el[3];
  35
  36 public:
  37         /** @brief No initializaion constructor */
  38         btMatrix3x3 () {}
  39
  40         //              explicit btMatrix3x3(const btScalar *m) { setFromOpenGLSubMatrix(m); }
  41
  42         /**@brief Constructor from Quaternion */
  43         explicit btMatrix3x3(const btQuaternion& q) { setRotation(q); }
  44         /*
  45         template <typename btScalar>
  46         Matrix3x3(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
  47         {
  48         setEulerYPR(yaw, pitch, roll);
  49         }
  50         */
  51         /** @brief Constructor with row major formatting */
  52         btMatrix3x3(const btScalar& xx, const btScalar& xy, const btScalar& xz,
  53                 const btScalar& yx, const btScalar& yy, const btScalar& yz,
  54                 const btScalar& zx, const btScalar& zy, const btScalar& zz)
  55         {
  56                 setValue(xx, xy, xz,
  57                         yx, yy, yz,
  58                         zx, zy, zz);
  59         }
  60         /** @brief Copy constructor */
  61         SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
  62         {
  63                 m_el[0] = other.m_el[0];
  64                 m_el[1] = other.m_el[1];
  65                 m_el[2] = other.m_el[2];
  66         }
  67         /** @brief Assignment Operator */
  68         SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
  69         {
  70                 m_el[0] = other.m_el[0];
  71                 m_el[1] = other.m_el[1];
  72                 m_el[2] = other.m_el[2];
  73                 return *this;
  74         }
  75
  76         /** @brief Get a column of the matrix as a vector
  77         *  @param i Column number 0 indexed */
  78         SIMD_FORCE_INLINE btVector3 getColumn(int i) const
  79         {
  80                 return btVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
  81         }
  82
  83
  84         /** @brief Get a row of the matrix as a vector
  85         *  @param i Row number 0 indexed */
  86         SIMD_FORCE_INLINE const btVector3& getRow(int i) const
  87         {
  88                 btFullAssert(0 <= i && i < 3);
  89                 return m_el[i];
  90         }
  91
  92         /** @brief Get a mutable reference to a row of the matrix as a vector
  93         *  @param i Row number 0 indexed */
  94         SIMD_FORCE_INLINE btVector3&  operator[](int i)
  95         {
  96                 btFullAssert(0 <= i && i < 3);
  97                 return m_el[i];
  98         }
  99
 100         /** @brief Get a const reference to a row of the matrix as a vector
 101         *  @param i Row number 0 indexed */
 102         SIMD_FORCE_INLINE const btVector3& operator[](int i) const
 103         {
 104                 btFullAssert(0 <= i && i < 3);
 105                 return m_el[i];
 106         }
 107
 108         /** @brief Multiply by the target matrix on the right
 109         *  @param m Rotation matrix to be applied
 110         * Equivilant to this = this * m */
 111         btMatrix3x3& operator*=(const btMatrix3x3& m);
 112
 113         /** @brief Adds by the target matrix on the right
 114         *  @param m matrix to be applied
 115         * Equivilant to this = this + m */
 116         btMatrix3x3& operator+=(const btMatrix3x3& m);
 117
 118         /** @brief Substractss by the target matrix on the right
 119         *  @param m matrix to be applied
 120         * Equivilant to this = this - m */
 121         btMatrix3x3& operator-=(const btMatrix3x3& m);
 122
 123         /** @brief Set from the rotational part of a 4x4 OpenGL matrix
 124         *  @param m A pointer to the beginning of the array of scalars*/
 125         void setFromOpenGLSubMatrix(const btScalar *m)
 126         {
 127                 m_el[0].setValue(m[0],m[4],m[8]);
 128                 m_el[1].setValue(m[1],m[5],m[9]);
 129                 m_el[2].setValue(m[2],m[6],m[10]);
 130
 131         }
 132         /** @brief Set the values of the matrix explicitly (row major)
 133         *  @param xx Top left
 134         *  @param xy Top Middle
 135         *  @param xz Top Right
 136         *  @param yx Middle Left
 137         *  @param yy Middle Middle
 138         *  @param yz Middle Right
 139         *  @param zx Bottom Left
 140         *  @param zy Bottom Middle
 141         *  @param zz Bottom Right*/
 142         void setValue(const btScalar& xx, const btScalar& xy, const btScalar& xz,
 143                 const btScalar& yx, const btScalar& yy, const btScalar& yz,
 144                 const btScalar& zx, const btScalar& zy, const btScalar& zz)
 145         {
 146                 m_el[0].setValue(xx,xy,xz);
 147                 m_el[1].setValue(yx,yy,yz);
 148                 m_el[2].setValue(zx,zy,zz);
 149         }
 150
 151         /** @brief Set the matrix from a quaternion
 152         *  @param q The Quaternion to match */
 153         void setRotation(const btQuaternion& q)
 154         {
 155                 btScalar d = q.length2();
 156                 btFullAssert(d != btScalar(0.0));
 157                 btScalar s = btScalar(2.0) / d;
 158                 btScalar xs = q.x() * s,   ys = q.y() * s,   zs = q.z() * s;
 159                 btScalar wx = q.w() * xs,  wy = q.w() * ys,  wz = q.w() * zs;
 160                 btScalar xx = q.x() * xs,  xy = q.x() * ys,  xz = q.x() * zs;
 161                 btScalar yy = q.y() * ys,  yz = q.y() * zs,  zz = q.z() * zs;
 162                 setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
 163                         xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
 164                         xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
 165         }
 166
 167
 168         /** @brief Set the matrix from euler angles using YPR around YXZ respectively
 169         *  @param yaw Yaw about Y axis
 170         *  @param pitch Pitch about X axis
 171         *  @param roll Roll about Z axis
 172         */
 173         void setEulerYPR(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
 174         {
 175                 setEulerZYX(roll, pitch, yaw);
 176         }
 177
 178         /** @brief Set the matrix from euler angles YPR around ZYX axes
 179         * @param eulerX Roll about X axis
 180         * @param eulerY Pitch around Y axis
 181         * @param eulerZ Yaw aboud Z axis
 182         *
 183         * These angles are used to produce a rotation matrix. The euler
 184         * angles are applied in ZYX order. I.e a vector is first rotated
 185         * about X then Y and then Z
 186         **/
 187         void setEulerZYX(btScalar eulerX,btScalar eulerY,btScalar eulerZ) {
 188                 ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code
 189                 btScalar ci ( btCos(eulerX));
 190                 btScalar cj ( btCos(eulerY));
 191                 btScalar ch ( btCos(eulerZ));
 192                 btScalar si ( btSin(eulerX));
 193                 btScalar sj ( btSin(eulerY));
 194                 btScalar sh ( btSin(eulerZ));
 195                 btScalar cc = ci * ch;
 196                 btScalar cs = ci * sh;
 197                 btScalar sc = si * ch;
 198                 btScalar ss = si * sh;
 199
 200                 setValue(cj * ch, sj * sc - cs, sj * cc + ss,
 201                         cj * sh, sj * ss + cc, sj * cs - sc,
 202                         -sj,      cj * si,      cj * ci);
 203         }
 204
 205         /**@brief Set the matrix to the identity */
 206         void setIdentity()
 207         {
 208                 setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0),
 209                         btScalar(0.0), btScalar(1.0), btScalar(0.0),
 210                         btScalar(0.0), btScalar(0.0), btScalar(1.0));
 211         }
 212
 213         static const btMatrix3x3&       getIdentity()
 214         {
 215                 static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0),
 216                         btScalar(0.0), btScalar(1.0), btScalar(0.0),
 217                         btScalar(0.0), btScalar(0.0), btScalar(1.0));
 218                 return identityMatrix;
 219         }
 220
 221         /**@brief Fill the rotational part of an OpenGL matrix and clear the shear/perspective
 222         * @param m The array to be filled */
 223         void getOpenGLSubMatrix(btScalar *m) const
 224         {
 225                 m[0]  = btScalar(m_el[0].x());
 226                 m[1]  = btScalar(m_el[1].x());
 227                 m[2]  = btScalar(m_el[2].x());
 228                 m[3]  = btScalar(0.0);
 229                 m[4]  = btScalar(m_el[0].y());
 230                 m[5]  = btScalar(m_el[1].y());
 231                 m[6]  = btScalar(m_el[2].y());
 232                 m[7]  = btScalar(0.0);
 233                 m[8]  = btScalar(m_el[0].z());
 234                 m[9]  = btScalar(m_el[1].z());
 235                 m[10] = btScalar(m_el[2].z());
 236                 m[11] = btScalar(0.0);
 237         }
 238
 239         /**@brief Get the matrix represented as a quaternion
 240         * @param q The quaternion which will be set */
 241         void getRotation(btQuaternion& q) const
 242         {
 243                 btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
 244                 btScalar temp[4];
 245
 246                 if (trace > btScalar(0.0))
 247                 {
 248                         btScalar s = btSqrt(trace + btScalar(1.0));
 249                         temp[3]=(s * btScalar(0.5));
 250                         s = btScalar(0.5) / s;
 251
 252                         temp[0]=((m_el[2].y() - m_el[1].z()) * s);
 253                         temp[1]=((m_el[0].z() - m_el[2].x()) * s);
 254                         temp[2]=((m_el[1].x() - m_el[0].y()) * s);
 255                 }
 256                 else
 257                 {
 258                         int i = m_el[0].x() < m_el[1].y() ?
 259                                 (m_el[1].y() < m_el[2].z() ? 2 : 1) :
 260                                 (m_el[0].x() < m_el[2].z() ? 2 : 0);
 261                         int j = (i + 1) % 3;
 262                         int k = (i + 2) % 3;
 263
 264                         btScalar s = btSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0));
 265                         temp[i] = s * btScalar(0.5);
 266                         s = btScalar(0.5) / s;
 267
 268                         temp[3] = (m_el[k][j] - m_el[j][k]) * s;
 269                         temp[j] = (m_el[j][i] + m_el[i][j]) * s;
 270                         temp[k] = (m_el[k][i] + m_el[i][k]) * s;
 271                 }
 272                 q.setValue(temp[0],temp[1],temp[2],temp[3]);
 273         }
 274
 275         /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
 276         * @param yaw Yaw around Y axis
 277         * @param pitch Pitch around X axis
 278         * @param roll around Z axis */
 279         void getEulerYPR(btScalar& yaw, btScalar& pitch, btScalar& roll) const
 280         {
 281
 282                 // first use the normal calculus
 283                 yaw = btScalar(btAtan2(m_el[1].x(), m_el[0].x()));
 284                 pitch = btScalar(btAsin(-m_el[2].x()));
 285                 roll = btScalar(btAtan2(m_el[2].y(), m_el[2].z()));
 286
 287                 // on pitch = +/-HalfPI
 288                 if (btFabs(pitch)==SIMD_HALF_PI)
 289                 {
 290                         if (yaw>0)
 291                                 yaw-=SIMD_PI;
 292                         else
 293                                 yaw+=SIMD_PI;
 294
 295                         if (roll>0)
 296                                 roll-=SIMD_PI;
 297                         else
 298                                 roll+=SIMD_PI;
 299                 }
 300         };
 301
 302
 303         /**@brief Get the matrix represented as euler angles around ZYX
 304         * @param yaw Yaw around X axis
 305         * @param pitch Pitch around Y axis
 306         * @param roll around X axis
 307         * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/
 308         void getEulerZYX(btScalar& yaw, btScalar& pitch, btScalar& roll, unsigned int solution_number = 1) const
 309         {
 310                 struct Euler
 311                 {
 312                         btScalar yaw;
 313                         btScalar pitch;
 314                         btScalar roll;
 315                 };
 316
 317                 Euler euler_out;
 318                 Euler euler_out2; //second solution
 319                 //get the pointer to the raw data
 320
 321                 // Check that pitch is not at a singularity
 322                 if (btFabs(m_el[2].x()) >= 1)
 323                 {
 324                         euler_out.yaw = 0;
 325                         euler_out2.yaw = 0;
 326
 327                         // From difference of angles formula
 328                         btScalar delta = btAtan2(m_el[0].x(),m_el[0].z());
 329                         if (m_el[2].x() > 0)  //gimbal locked up
 330                         {
 331                                 euler_out.pitch = SIMD_PI / btScalar(2.0);
 332                                 euler_out2.pitch = SIMD_PI / btScalar(2.0);
 333                                 euler_out.roll = euler_out.pitch + delta;
 334                                 euler_out2.roll = euler_out.pitch + delta;
 335                         }
 336                         else // gimbal locked down
 337                         {
 338                                 euler_out.pitch = -SIMD_PI / btScalar(2.0);
 339                                 euler_out2.pitch = -SIMD_PI / btScalar(2.0);
 340                                 euler_out.roll = -euler_out.pitch + delta;
 341                                 euler_out2.roll = -euler_out.pitch + delta;
 342                         }
 343                 }
 344                 else
 345                 {
 346                         euler_out.pitch = - btAsin(m_el[2].x());
 347                         euler_out2.pitch = SIMD_PI - euler_out.pitch;
 348
 349                         euler_out.roll = btAtan2(m_el[2].y()/btCos(euler_out.pitch),
 350                                 m_el[2].z()/btCos(euler_out.pitch));
 351                         euler_out2.roll = btAtan2(m_el[2].y()/btCos(euler_out2.pitch),
 352                                 m_el[2].z()/btCos(euler_out2.pitch));
 353
 354                         euler_out.yaw = btAtan2(m_el[1].x()/btCos(euler_out.pitch),
 355                                 m_el[0].x()/btCos(euler_out.pitch));
 356                         euler_out2.yaw = btAtan2(m_el[1].x()/btCos(euler_out2.pitch),
 357                                 m_el[0].x()/btCos(euler_out2.pitch));
 358                 }
 359
 360                 if (solution_number == 1)
 361                 {
 362                         yaw = euler_out.yaw;
 363                         pitch = euler_out.pitch;
 364                         roll = euler_out.roll;
 365                 }
 366                 else
 367                 {
 368                         yaw = euler_out2.yaw;
 369                         pitch = euler_out2.pitch;
 370                         roll = euler_out2.roll;
 371                 }
 372         }
 373
 374         /**@brief Create a scaled copy of the matrix
 375         * @param s Scaling vector The elements of the vector will scale each column */
 376
 377         btMatrix3x3 scaled(const btVector3& s) const
 378         {
 379                 return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
 380                         m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
 381                         m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
 382         }
 383
 384         /**@brief Return the determinant of the matrix */
 385         btScalar            determinant() const;
 386         /**@brief Return the adjoint of the matrix */
 387         btMatrix3x3 adjoint() const;
 388         /**@brief Return the matrix with all values non negative */
 389         btMatrix3x3 absolute() const;
 390         /**@brief Return the transpose of the matrix */
 391         btMatrix3x3 transpose() const;
 392         /**@brief Return the inverse of the matrix */
 393         btMatrix3x3 inverse() const;
 394
 395         btMatrix3x3 transposeTimes(const btMatrix3x3& m) const;
 396         btMatrix3x3 timesTranspose(const btMatrix3x3& m) const;
 397
 398         SIMD_FORCE_INLINE btScalar tdotx(const btVector3& v) const
 399         {
 400                 return m_el[0].x() * v.x() + m_el[1].x() * v.y() + m_el[2].x() * v.z();
 401         }
 402         SIMD_FORCE_INLINE btScalar tdoty(const btVector3& v) const
 403         {
 404                 return m_el[0].y() * v.x() + m_el[1].y() * v.y() + m_el[2].y() * v.z();
 405         }
 406         SIMD_FORCE_INLINE btScalar tdotz(const btVector3& v) const
 407         {
 408                 return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z();
 409         }
 410
 411
 412         /**@brief diagonalizes this matrix by the Jacobi method.
 413         * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
 414         * coordinate system, i.e., old_this = rot * new_this * rot^T.
 415         * @param threshold See iteration
 416         * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied
 417         * by the sum of the absolute values of the diagonal, or when maxSteps have been executed.
 418         *
 419         * Note that this matrix is assumed to be symmetric.
 420         */
 421         void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps)
 422         {
 423                 rot.setIdentity();
 424                 for (int step = maxSteps; step > 0; step--)
 425                 {
 426                         // find off-diagonal element [p][q] with largest magnitude
 427                         int p = 0;
 428                         int q = 1;
 429                         int r = 2;
 430                         btScalar max = btFabs(m_el[0][1]);
 431                         btScalar v = btFabs(m_el[0][2]);
 432                         if (v > max)
 433                         {
 434                                 q = 2;
 435                                 r = 1;
 436                                 max = v;
 437                         }
 438                         v = btFabs(m_el[1][2]);
 439                         if (v > max)
 440                         {
 441                                 p = 1;
 442                                 q = 2;
 443                                 r = 0;
 444                                 max = v;
 445                         }
 446
 447                         btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2]));
 448                         if (max <= t)
 449                         {
 450                                 if (max <= SIMD_EPSILON * t)
 451                                 {
 452                                         return;
 453                                 }
 454                                 step = 1;
 455                         }
 456
 457                         // compute Jacobi rotation J which leads to a zero for element [p][q]
 458                         btScalar mpq = m_el[p][q];
 459                         btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
 460                         btScalar theta2 = theta * theta;
 461                         btScalar cos;
 462                         btScalar sin;
 463                         if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON))
 464                         {
 465                                 t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2))
 466                                         : 1 / (theta - btSqrt(1 + theta2));
 467                                 cos = 1 / btSqrt(1 + t * t);
 468                                 sin = cos * t;
 469                         }
 470                         else
 471                         {
 472                                 // approximation for large theta-value, i.e., a nearly diagonal matrix
 473                                 t = 1 / (theta * (2 + btScalar(0.5) / theta2));
 474                                 cos = 1 - btScalar(0.5) * t * t;
 475                                 sin = cos * t;
 476                         }
 477
 478                         // apply rotation to matrix (this = J^T * this * J)
 479                         m_el[p][q] = m_el[q][p] = 0;
 480                         m_el[p][p] -= t * mpq;
 481                         m_el[q][q] += t * mpq;
 482                         btScalar mrp = m_el[r][p];
 483                         btScalar mrq = m_el[r][q];
 484                         m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
 485                         m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
 486
 487                         // apply rotation to rot (rot = rot * J)
 488                         for (int i = 0; i < 3; i++)
 489                         {
 490                                 btVector3& row = rot[i];
 491                                 mrp = row[p];
 492                                 mrq = row[q];
 493                                 row[p] = cos * mrp - sin * mrq;
 494                                 row[q] = cos * mrq + sin * mrp;
 495                         }
 496                 }
 497         }
 498
 499
 500
 501
 502         /**@brief Calculate the matrix cofactor
 503         * @param r1 The first row to use for calculating the cofactor
 504         * @param c1 The first column to use for calculating the cofactor
 505         * @param r1 The second row to use for calculating the cofactor
 506         * @param c1 The second column to use for calculating the cofactor
 507         * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
 508         */
 509         btScalar cofac(int r1, int c1, int r2, int c2) const
 510         {
 511                 return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
 512         }
 513
 514         void    serialize(struct        btMatrix3x3Data& dataOut) const;
 515
 516         void    serializeFloat(struct   btMatrix3x3FloatData& dataOut) const;
 517
 518         void    deSerialize(const struct        btMatrix3x3Data& dataIn);
 519
 520         void    deSerializeFloat(const struct   btMatrix3x3FloatData& dataIn);
 521
 522         void    deSerializeDouble(const struct  btMatrix3x3DoubleData& dataIn);
 523
 524 };
 525
 526
 527 SIMD_FORCE_INLINE btMatrix3x3&
 528 btMatrix3x3::operator*=(const btMatrix3x3& m)
 529 {
 530         setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
 531                 m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
 532                 m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
 533         return *this;
 534 }
 535
 536 SIMD_FORCE_INLINE btMatrix3x3&
 537 btMatrix3x3::operator+=(const btMatrix3x3& m)
 538 {
 539         setValue(
 540                 m_el[0][0]+m.m_el[0][0],
 541                 m_el[0][1]+m.m_el[0][1],
 542                 m_el[0][2]+m.m_el[0][2],
 543                 m_el[1][0]+m.m_el[1][0],
 544                 m_el[1][1]+m.m_el[1][1],
 545                 m_el[1][2]+m.m_el[1][2],
 546                 m_el[2][0]+m.m_el[2][0],
 547                 m_el[2][1]+m.m_el[2][1],
 548                 m_el[2][2]+m.m_el[2][2]);
 549         return *this;
 550 }
 551
 552 SIMD_FORCE_INLINE btMatrix3x3
 553 operator*(const btMatrix3x3& m, const btScalar & k)
 554 {
 555         return btMatrix3x3(
 556                 m[0].x()*k,m[0].y()*k,m[0].z()*k,
 557                 m[1].x()*k,m[1].y()*k,m[1].z()*k,
 558                 m[2].x()*k,m[2].y()*k,m[2].z()*k);
 559 }
 560
 561  SIMD_FORCE_INLINE btMatrix3x3
 562 operator+(const btMatrix3x3& m1, const btMatrix3x3& m2)
 563 {
 564         return btMatrix3x3(
 565         m1[0][0]+m2[0][0],
 566         m1[0][1]+m2[0][1],
 567         m1[0][2]+m2[0][2],
 568         m1[1][0]+m2[1][0],
 569         m1[1][1]+m2[1][1],
 570         m1[1][2]+m2[1][2],
 571         m1[2][0]+m2[2][0],
 572         m1[2][1]+m2[2][1],
 573         m1[2][2]+m2[2][2]);
 574 }
 575
 576 SIMD_FORCE_INLINE btMatrix3x3
 577 operator-(const btMatrix3x3& m1, const btMatrix3x3& m2)
 578 {
 579         return btMatrix3x3(
 580         m1[0][0]-m2[0][0],
 581         m1[0][1]-m2[0][1],
 582         m1[0][2]-m2[0][2],
 583         m1[1][0]-m2[1][0],
 584         m1[1][1]-m2[1][1],
 585         m1[1][2]-m2[1][2],
 586         m1[2][0]-m2[2][0],
 587         m1[2][1]-m2[2][1],
 588         m1[2][2]-m2[2][2]);
 589 }
 590
 591
 592 SIMD_FORCE_INLINE btMatrix3x3&
 593 btMatrix3x3::operator-=(const btMatrix3x3& m)
 594 {
 595         setValue(
 596         m_el[0][0]-m.m_el[0][0],
 597         m_el[0][1]-m.m_el[0][1],
 598         m_el[0][2]-m.m_el[0][2],
 599         m_el[1][0]-m.m_el[1][0],
 600         m_el[1][1]-m.m_el[1][1],
 601         m_el[1][2]-m.m_el[1][2],
 602         m_el[2][0]-m.m_el[2][0],
 603         m_el[2][1]-m.m_el[2][1],
 604         m_el[2][2]-m.m_el[2][2]);
 605         return *this;
 606 }
 607
 608
 609 SIMD_FORCE_INLINE btScalar
 610 btMatrix3x3::determinant() const
 611 {
 612         return btTriple((*this)[0], (*this)[1], (*this)[2]);
 613 }
 614
 615
 616 SIMD_FORCE_INLINE btMatrix3x3
 617 btMatrix3x3::absolute() const
 618 {
 619         return btMatrix3x3(
 620                 btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
 621                 btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
 622                 btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
 623 }
 624
 625 SIMD_FORCE_INLINE btMatrix3x3
 626 btMatrix3x3::transpose() const
 627 {
 628         return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
 629                 m_el[0].y(), m_el[1].y(), m_el[2].y(),
 630                 m_el[0].z(), m_el[1].z(), m_el[2].z());
 631 }
 632
 633 SIMD_FORCE_INLINE btMatrix3x3
 634 btMatrix3x3::adjoint() const
 635 {
 636         return btMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
 637                 cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
 638                 cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
 639 }
 640
 641 SIMD_FORCE_INLINE btMatrix3x3
 642 btMatrix3x3::inverse() const
 643 {
 644         btVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
 645         btScalar det = (*this)[0].dot(co);
 646         btFullAssert(det != btScalar(0.0));
 647         btScalar s = btScalar(1.0) / det;
 648         return btMatrix3x3(co.x() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
 649                 co.y() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
 650                 co.z() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
 651 }
 652
 653 SIMD_FORCE_INLINE btMatrix3x3
 654 btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
 655 {
 656         return btMatrix3x3(
 657                 m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
 658                 m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
 659                 m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
 660                 m_el[0].y() * m[0].x() + m_el[1].y() * m[1].x() + m_el[2].y() * m[2].x(),
 661                 m_el[0].y() * m[0].y() + m_el[1].y() * m[1].y() + m_el[2].y() * m[2].y(),
 662                 m_el[0].y() * m[0].z() + m_el[1].y() * m[1].z() + m_el[2].y() * m[2].z(),
 663                 m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
 664                 m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
 665                 m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
 666 }
 667
 668 SIMD_FORCE_INLINE btMatrix3x3
 669 btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
 670 {
 671         return btMatrix3x3(
 672                 m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
 673                 m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
 674                 m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
 675
 676 }
 677
 678 SIMD_FORCE_INLINE btVector3
 679 operator*(const btMatrix3x3& m, const btVector3& v)
 680 {
 681         return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
 682 }
 683
 684
 685 SIMD_FORCE_INLINE btVector3
 686 operator*(const btVector3& v, const btMatrix3x3& m)
 687 {
 688         return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
 689 }
 690
 691 SIMD_FORCE_INLINE btMatrix3x3
 692 operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
 693 {
 694         return btMatrix3x3(
 695                 m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
 696                 m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
 697                 m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
 698 }
 699
 700 /*
 701 SIMD_FORCE_INLINE btMatrix3x3 btMultTransposeLeft(const btMatrix3x3& m1, const btMatrix3x3& m2) {
 702 return btMatrix3x3(
 703 m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
 704 m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
 705 m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
 706 m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
 707 m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
 708 m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
 709 m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
 710 m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
 711 m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
 712 }
 713 */
 714
 715 /**@brief Equality operator between two matrices
 716 * It will test all elements are equal.  */
 717 SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
 718 {
 719         return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
 720                 m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
 721                 m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
 722 }
 723
 724 ///for serialization
 725 struct  btMatrix3x3FloatData
 726 {
 727         btVector3FloatData m_el[3];
 728 };
 729
 730 ///for serialization
 731 struct  btMatrix3x3DoubleData
 732 {
 733         btVector3DoubleData m_el[3];
 734 };
 735
 736
 737
 738
 739 SIMD_FORCE_INLINE       void    btMatrix3x3::serialize(struct   btMatrix3x3Data& dataOut) const
 740 {
 741         for (int i=0;i<3;i++)
 742                 m_el[i].serialize(dataOut.m_el[i]);
 743 }
 744
 745 SIMD_FORCE_INLINE       void    btMatrix3x3::serializeFloat(struct      btMatrix3x3FloatData& dataOut) const
 746 {
 747         for (int i=0;i<3;i++)
 748                 m_el[i].serializeFloat(dataOut.m_el[i]);
 749 }
 750
 751
 752 SIMD_FORCE_INLINE       void    btMatrix3x3::deSerialize(const struct   btMatrix3x3Data& dataIn)
 753 {
 754         for (int i=0;i<3;i++)
 755                 m_el[i].deSerialize(dataIn.m_el[i]);
 756 }
 757
 758 SIMD_FORCE_INLINE       void    btMatrix3x3::deSerializeFloat(const struct      btMatrix3x3FloatData& dataIn)
 759 {
 760         for (int i=0;i<3;i++)
 761                 m_el[i].deSerializeFloat(dataIn.m_el[i]);
 762 }
 763
 764 SIMD_FORCE_INLINE       void    btMatrix3x3::deSerializeDouble(const struct     btMatrix3x3DoubleData& dataIn)
 765 {
 766         for (int i=0;i<3;i++)
 767                 m_el[i].deSerializeDouble(dataIn.m_el[i]);
 768 }
 769
 770 #endif //BT_MATRIX3x3_H
 771