extern/bullet/LinearMath/SimdMatrix3x3.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 SimdMatrix3x3_H
  17 #define SimdMatrix3x3_H
  18
  19 #include "SimdScalar.h"
  20
  21 #include "SimdVector3.h"
  22 #include "SimdQuaternion.h"
  23
  24
  25 class SimdMatrix3x3 {
  26         public:
  27                 SimdMatrix3x3 () {}
  28
  29 //              explicit SimdMatrix3x3(const SimdScalar *m) { setFromOpenGLSubMatrix(m); }
  30
  31                 explicit SimdMatrix3x3(const SimdQuaternion& q) { setRotation(q); }
  32                 /*
  33                 template <typename SimdScalar>
  34                 Matrix3x3(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll)
  35                 {
  36                         setEulerYPR(yaw, pitch, roll);
  37                 }
  38                 */
  39                 SimdMatrix3x3(const SimdScalar& xx, const SimdScalar& xy, const SimdScalar& xz,
  40                                   const SimdScalar& yx, const SimdScalar& yy, const SimdScalar& yz,
  41                                   const SimdScalar& zx, const SimdScalar& zy, const SimdScalar& zz)
  42                 {
  43                         setValue(xx, xy, xz,
  44                                          yx, yy, yz,
  45                                          zx, zy, zz);
  46                 }
  47
  48                 SimdVector3 getColumn(int i) const
  49                 {
  50                         return SimdVector3(m_el[0][i],m_el[1][i],m_el[2][i]);
  51                 }
  52
  53                 const SimdVector3& getRow(int i) const
  54                 {
  55                         return m_el[i];
  56                 }
  57
  58
  59                 SIMD_FORCE_INLINE SimdVector3&  operator[](int i)
  60                 {
  61                         assert(0 <= i && i < 3);
  62                         return m_el[i];
  63                 }
  64
  65                 const SimdVector3& operator[](int i) const
  66                 {
  67                         assert(0 <= i && i < 3);
  68                         return m_el[i];
  69                 }
  70
  71                 SimdMatrix3x3& operator*=(const SimdMatrix3x3& m);
  72
  73
  74         void setFromOpenGLSubMatrix(const SimdScalar *m)
  75                 {
  76                         m_el[0][0] = (m[0]);
  77                         m_el[1][0] = (m[1]);
  78                         m_el[2][0] = (m[2]);
  79                         m_el[0][1] = (m[4]);
  80                         m_el[1][1] = (m[5]);
  81                         m_el[2][1] = (m[6]);
  82                         m_el[0][2] = (m[8]);
  83                         m_el[1][2] = (m[9]);
  84                         m_el[2][2] = (m[10]);
  85                 }
  86
  87                 void setValue(const SimdScalar& xx, const SimdScalar& xy, const SimdScalar& xz,
  88                                           const SimdScalar& yx, const SimdScalar& yy, const SimdScalar& yz,
  89                                           const SimdScalar& zx, const SimdScalar& zy, const SimdScalar& zz)
  90                 {
  91                         m_el[0][0] = SimdScalar(xx);
  92                         m_el[0][1] = SimdScalar(xy);
  93                         m_el[0][2] = SimdScalar(xz);
  94                         m_el[1][0] = SimdScalar(yx);
  95                         m_el[1][1] = SimdScalar(yy);
  96                         m_el[1][2] = SimdScalar(yz);
  97                         m_el[2][0] = SimdScalar(zx);
  98                         m_el[2][1] = SimdScalar(zy);
  99                         m_el[2][2] = SimdScalar(zz);
 100                 }
 101
 102                 void setRotation(const SimdQuaternion& q)
 103                 {
 104                         SimdScalar d = q.length2();
 105                         assert(d != SimdScalar(0.0));
 106                         SimdScalar s = SimdScalar(2.0) / d;
 107                         SimdScalar xs = q[0] * s,   ys = q[1] * s,   zs = q[2] * s;
 108                         SimdScalar wx = q[3] * xs,  wy = q[3] * ys,  wz = q[3] * zs;
 109                         SimdScalar xx = q[0] * xs,  xy = q[0] * ys,  xz = q[0] * zs;
 110                         SimdScalar yy = q[1] * ys,  yz = q[1] * zs,  zz = q[2] * zs;
 111                         setValue(SimdScalar(1.0) - (yy + zz), xy - wz, xz + wy,
 112                                          xy + wz, SimdScalar(1.0) - (xx + zz), yz - wx,
 113                                          xz - wy, yz + wx, SimdScalar(1.0) - (xx + yy));
 114                 }
 115
 116
 117
 118                 void setEulerYPR(const SimdScalar& yaw, const SimdScalar& pitch, const SimdScalar& roll)
 119                 {
 120
 121                         SimdScalar cy(SimdCos(yaw));
 122                         SimdScalar  sy(SimdSin(yaw));
 123                         SimdScalar  cp(SimdCos(pitch));
 124                         SimdScalar  sp(SimdSin(pitch));
 125                         SimdScalar  cr(SimdCos(roll));
 126                         SimdScalar  sr(SimdSin(roll));
 127                         SimdScalar  cc = cy * cr;
 128                         SimdScalar  cs = cy * sr;
 129                         SimdScalar  sc = sy * cr;
 130                         SimdScalar  ss = sy * sr;
 131                         setValue(cc - sp * ss, -cs - sp * sc, -sy * cp,
 132                      cp * sr,       cp * cr,      -sp,
 133                                          sc + sp * cs, -ss + sp * cc,  cy * cp);
 134
 135                 }
 136
 137         /**
 138          * setEulerZYX
 139          * @param euler a const reference to a SimdVector3 of euler angles
 140          * These angles are used to produce a rotation matrix. The euler
 141          * angles are applied in ZYX order. I.e a vector is first rotated
 142          * about X then Y and then Z
 143          **/
 144
 145         void setEulerZYX(SimdScalar eulerX,SimdScalar eulerY,SimdScalar eulerZ) {
 146                 SimdScalar ci ( SimdCos(eulerX));
 147                 SimdScalar cj ( SimdCos(eulerY));
 148                 SimdScalar ch ( SimdCos(eulerZ));
 149                 SimdScalar si ( SimdSin(eulerX));
 150                 SimdScalar sj ( SimdSin(eulerY));
 151                 SimdScalar sh ( SimdSin(eulerZ));
 152                 SimdScalar cc = ci * ch;
 153                 SimdScalar cs = ci * sh;
 154                 SimdScalar sc = si * ch;
 155                 SimdScalar ss = si * sh;
 156
 157                 setValue(cj * ch, sj * sc - cs, sj * cc + ss,
 158                                  cj * sh, sj * ss + cc, sj * cs - sc,
 159                                  -sj,      cj * si,      cj * ci);
 160         }
 161
 162                 void setIdentity()
 163                 {
 164                         setValue(SimdScalar(1.0), SimdScalar(0.0), SimdScalar(0.0),
 165                                          SimdScalar(0.0), SimdScalar(1.0), SimdScalar(0.0),
 166                                          SimdScalar(0.0), SimdScalar(0.0), SimdScalar(1.0));
 167                 }
 168
 169                 void getOpenGLSubMatrix(SimdScalar *m) const
 170                 {
 171                         m[0]  = SimdScalar(m_el[0][0]);
 172                         m[1]  = SimdScalar(m_el[1][0]);
 173                         m[2]  = SimdScalar(m_el[2][0]);
 174                         m[3]  = SimdScalar(0.0);
 175                         m[4]  = SimdScalar(m_el[0][1]);
 176                         m[5]  = SimdScalar(m_el[1][1]);
 177                         m[6]  = SimdScalar(m_el[2][1]);
 178                         m[7]  = SimdScalar(0.0);
 179                         m[8]  = SimdScalar(m_el[0][2]);
 180                         m[9]  = SimdScalar(m_el[1][2]);
 181                         m[10] = SimdScalar(m_el[2][2]);
 182                         m[11] = SimdScalar(0.0);
 183                 }
 184
 185                 void getRotation(SimdQuaternion& q) const
 186                 {
 187                         SimdScalar trace = m_el[0][0] + m_el[1][1] + m_el[2][2];
 188
 189                         if (trace > SimdScalar(0.0))
 190                         {
 191                                 SimdScalar s = SimdSqrt(trace + SimdScalar(1.0));
 192                                 q[3] = s * SimdScalar(0.5);
 193                                 s = SimdScalar(0.5) / s;
 194
 195                                 q[0] = (m_el[2][1] - m_el[1][2]) * s;
 196                                 q[1] = (m_el[0][2] - m_el[2][0]) * s;
 197                                 q[2] = (m_el[1][0] - m_el[0][1]) * s;
 198                         }
 199                         else
 200                         {
 201                                 int i = m_el[0][0] < m_el[1][1] ?
 202                                         (m_el[1][1] < m_el[2][2] ? 2 : 1) :
 203                                         (m_el[0][0] < m_el[2][2] ? 2 : 0);
 204                                 int j = (i + 1) % 3;
 205                                 int k = (i + 2) % 3;
 206
 207                                 SimdScalar s = SimdSqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + SimdScalar(1.0));
 208                                 q[i] = s * SimdScalar(0.5);
 209                                 s = SimdScalar(0.5) / s;
 210
 211                                 q[3] = (m_el[k][j] - m_el[j][k]) * s;
 212                                 q[j] = (m_el[j][i] + m_el[i][j]) * s;
 213                                 q[k] = (m_el[k][i] + m_el[i][k]) * s;
 214                         }
 215                 }
 216
 217
 218
 219                 void getEuler(SimdScalar& yaw, SimdScalar& pitch, SimdScalar& roll) const
 220                 {
 221                         pitch = SimdScalar(SimdAsin(-m_el[2][0]));
 222                         if (pitch < SIMD_2_PI)
 223                         {
 224                                 if (pitch > SIMD_2_PI)
 225                                 {
 226                                         yaw = SimdScalar(SimdAtan2(m_el[1][0], m_el[0][0]));
 227                                         roll = SimdScalar(SimdAtan2(m_el[2][1], m_el[2][2]));
 228                                 }
 229                                 else
 230                                 {
 231                                         yaw = SimdScalar(-SimdAtan2(-m_el[0][1], m_el[0][2]));
 232                                         roll = SimdScalar(0.0);
 233                                 }
 234                         }
 235                         else
 236                         {
 237                                 yaw = SimdScalar(SimdAtan2(-m_el[0][1], m_el[0][2]));
 238                                 roll = SimdScalar(0.0);
 239                         }
 240                 }
 241
 242                 SimdVector3 getScaling() const
 243                 {
 244                         return SimdVector3(m_el[0][0] * m_el[0][0] + m_el[1][0] * m_el[1][0] + m_el[2][0] * m_el[2][0],
 245                                                                    m_el[0][1] * m_el[0][1] + m_el[1][1] * m_el[1][1] + m_el[2][1] * m_el[2][1],
 246                                                                    m_el[0][2] * m_el[0][2] + m_el[1][2] * m_el[1][2] + m_el[2][2] * m_el[2][2]);
 247                 }
 248
 249
 250                 SimdMatrix3x3 scaled(const SimdVector3& s) const
 251                 {
 252                         return SimdMatrix3x3(m_el[0][0] * s[0], m_el[0][1] * s[1], m_el[0][2] * s[2],
 253                                                                          m_el[1][0] * s[0], m_el[1][1] * s[1], m_el[1][2] * s[2],
 254                                                                          m_el[2][0] * s[0], m_el[2][1] * s[1], m_el[2][2] * s[2]);
 255                 }
 256
 257                 SimdScalar            determinant() const;
 258                 SimdMatrix3x3 adjoint() const;
 259                 SimdMatrix3x3 absolute() const;
 260                 SimdMatrix3x3 transpose() const;
 261                 SimdMatrix3x3 inverse() const;
 262
 263                 SimdMatrix3x3 transposeTimes(const SimdMatrix3x3& m) const;
 264                 SimdMatrix3x3 timesTranspose(const SimdMatrix3x3& m) const;
 265
 266                 SimdScalar tdot(int c, const SimdVector3& v) const
 267                 {
 268                         return m_el[0][c] * v[0] + m_el[1][c] * v[1] + m_el[2][c] * v[2];
 269                 }
 270
 271         protected:
 272                 SimdScalar cofac(int r1, int c1, int r2, int c2) const
 273                 {
 274                         return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
 275                 }
 276
 277                 SimdVector3 m_el[3];
 278         };
 279
 280         SIMD_FORCE_INLINE SimdMatrix3x3&
 281         SimdMatrix3x3::operator*=(const SimdMatrix3x3& m)
 282         {
 283                 setValue(m.tdot(0, m_el[0]), m.tdot(1, m_el[0]), m.tdot(2, m_el[0]),
 284                                  m.tdot(0, m_el[1]), m.tdot(1, m_el[1]), m.tdot(2, m_el[1]),
 285                                  m.tdot(0, m_el[2]), m.tdot(1, m_el[2]), m.tdot(2, m_el[2]));
 286                 return *this;
 287         }
 288
 289         SIMD_FORCE_INLINE SimdScalar
 290         SimdMatrix3x3::determinant() const
 291         {
 292                 return triple((*this)[0], (*this)[1], (*this)[2]);
 293         }
 294
 295
 296         SIMD_FORCE_INLINE SimdMatrix3x3
 297         SimdMatrix3x3::absolute() const
 298         {
 299                 return SimdMatrix3x3(
 300                         SimdFabs(m_el[0][0]), SimdFabs(m_el[0][1]), SimdFabs(m_el[0][2]),
 301                         SimdFabs(m_el[1][0]), SimdFabs(m_el[1][1]), SimdFabs(m_el[1][2]),
 302                         SimdFabs(m_el[2][0]), SimdFabs(m_el[2][1]), SimdFabs(m_el[2][2]));
 303         }
 304
 305         SIMD_FORCE_INLINE SimdMatrix3x3
 306         SimdMatrix3x3::transpose() const
 307         {
 308                 return SimdMatrix3x3(m_el[0][0], m_el[1][0], m_el[2][0],
 309                                                                  m_el[0][1], m_el[1][1], m_el[2][1],
 310                                                                  m_el[0][2], m_el[1][2], m_el[2][2]);
 311         }
 312
 313         SIMD_FORCE_INLINE SimdMatrix3x3
 314         SimdMatrix3x3::adjoint() const
 315         {
 316                 return SimdMatrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
 317                                                                  cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
 318                                                                  cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
 319         }
 320
 321         SIMD_FORCE_INLINE SimdMatrix3x3
 322         SimdMatrix3x3::inverse() const
 323         {
 324                 SimdVector3 co(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
 325                 SimdScalar det = (*this)[0].dot(co);
 326                 assert(det != SimdScalar(0.0f));
 327                 SimdScalar s = SimdScalar(1.0f) / det;
 328                 return SimdMatrix3x3(co[0] * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
 329                                                                  co[1] * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
 330                                                                  co[2] * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
 331         }
 332
 333         SIMD_FORCE_INLINE SimdMatrix3x3
 334         SimdMatrix3x3::transposeTimes(const SimdMatrix3x3& m) const
 335         {
 336                 return SimdMatrix3x3(
 337                         m_el[0][0] * m[0][0] + m_el[1][0] * m[1][0] + m_el[2][0] * m[2][0],
 338                         m_el[0][0] * m[0][1] + m_el[1][0] * m[1][1] + m_el[2][0] * m[2][1],
 339                         m_el[0][0] * m[0][2] + m_el[1][0] * m[1][2] + m_el[2][0] * m[2][2],
 340                         m_el[0][1] * m[0][0] + m_el[1][1] * m[1][0] + m_el[2][1] * m[2][0],
 341                         m_el[0][1] * m[0][1] + m_el[1][1] * m[1][1] + m_el[2][1] * m[2][1],
 342                         m_el[0][1] * m[0][2] + m_el[1][1] * m[1][2] + m_el[2][1] * m[2][2],
 343                         m_el[0][2] * m[0][0] + m_el[1][2] * m[1][0] + m_el[2][2] * m[2][0],
 344                         m_el[0][2] * m[0][1] + m_el[1][2] * m[1][1] + m_el[2][2] * m[2][1],
 345                         m_el[0][2] * m[0][2] + m_el[1][2] * m[1][2] + m_el[2][2] * m[2][2]);
 346         }
 347
 348         SIMD_FORCE_INLINE SimdMatrix3x3
 349         SimdMatrix3x3::timesTranspose(const SimdMatrix3x3& m) const
 350         {
 351                 return SimdMatrix3x3(
 352                         m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
 353                         m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
 354                         m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
 355
 356         }
 357
 358         SIMD_FORCE_INLINE SimdVector3
 359         operator*(const SimdMatrix3x3& m, const SimdVector3& v)
 360         {
 361                 return SimdVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
 362         }
 363
 364
 365         SIMD_FORCE_INLINE SimdVector3
 366         operator*(const SimdVector3& v, const SimdMatrix3x3& m)
 367         {
 368                 return SimdVector3(m.tdot(0, v), m.tdot(1, v), m.tdot(2, v));
 369         }
 370
 371         SIMD_FORCE_INLINE SimdMatrix3x3
 372         operator*(const SimdMatrix3x3& m1, const SimdMatrix3x3& m2)
 373         {
 374                 return SimdMatrix3x3(
 375                         m2.tdot(0, m1[0]), m2.tdot(1, m1[0]), m2.tdot(2, m1[0]),
 376                         m2.tdot(0, m1[1]), m2.tdot(1, m1[1]), m2.tdot(2, m1[1]),
 377                         m2.tdot(0, m1[2]), m2.tdot(1, m1[2]), m2.tdot(2, m1[2]));
 378         }
 379
 380
 381         SIMD_FORCE_INLINE SimdMatrix3x3 SimdMultTransposeLeft(const SimdMatrix3x3& m1, const SimdMatrix3x3& m2) {
 382     return SimdMatrix3x3(
 383         m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
 384         m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
 385         m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
 386         m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
 387         m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
 388         m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
 389         m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
 390         m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
 391         m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
 392 }
 393
 394
 395 #endif