dali-physics/third-party/bullet3/src/Bullet3Common/b3Matrix3x3.h

   1 /*
   2 Copyright (c) 2003-2013 Gino van den Bergen / Erwin Coumans  http://bulletphysics.org
   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 #ifndef B3_MATRIX3x3_H
  16 #define B3_MATRIX3x3_H
  17
  18 #include "b3Vector3.h"
  19 #include "b3Quaternion.h"
  20 #include <stdio.h>
  21
  22 #ifdef B3_USE_SSE
  23 //const __m128 B3_ATTRIBUTE_ALIGNED16(b3v2220) = {2.0f, 2.0f, 2.0f, 0.0f};
  24 const __m128 B3_ATTRIBUTE_ALIGNED16(b3vMPPP) = {-0.0f, +0.0f, +0.0f, +0.0f};
  25 #endif
  26
  27 #if defined(B3_USE_SSE) || defined(B3_USE_NEON)
  28 const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3v1000) = {1.0f, 0.0f, 0.0f, 0.0f};
  29 const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3v0100) = {0.0f, 1.0f, 0.0f, 0.0f};
  30 const b3SimdFloat4 B3_ATTRIBUTE_ALIGNED16(b3v0010) = {0.0f, 0.0f, 1.0f, 0.0f};
  31 #endif
  32
  33 #ifdef B3_USE_DOUBLE_PRECISION
  34 #define b3Matrix3x3Data b3Matrix3x3DoubleData
  35 #else
  36 #define b3Matrix3x3Data b3Matrix3x3FloatData
  37 #endif  //B3_USE_DOUBLE_PRECISION
  38
  39 /**@brief The b3Matrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with b3Quaternion, b3Transform and b3Vector3.
  40 * Make sure to only include a pure orthogonal matrix without scaling. */
  41 B3_ATTRIBUTE_ALIGNED16(class)
  42 b3Matrix3x3
  43 {
  44         ///Data storage for the matrix, each vector is a row of the matrix
  45         b3Vector3 m_el[3];
  46
  47 public:
  48         /** @brief No initializaion constructor */
  49         b3Matrix3x3() {}
  50
  51         //              explicit b3Matrix3x3(const b3Scalar *m) { setFromOpenGLSubMatrix(m); }
  52
  53         /**@brief Constructor from Quaternion */
  54         explicit b3Matrix3x3(const b3Quaternion& q) { setRotation(q); }
  55         /*
  56         template <typename b3Scalar>
  57         Matrix3x3(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
  58         {
  59         setEulerYPR(yaw, pitch, roll);
  60         }
  61         */
  62         /** @brief Constructor with row major formatting */
  63         b3Matrix3x3(const b3Scalar& xx, const b3Scalar& xy, const b3Scalar& xz,
  64                                 const b3Scalar& yx, const b3Scalar& yy, const b3Scalar& yz,
  65                                 const b3Scalar& zx, const b3Scalar& zy, const b3Scalar& zz)
  66         {
  67                 setValue(xx, xy, xz,
  68                                  yx, yy, yz,
  69                                  zx, zy, zz);
  70         }
  71
  72 #if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
  73         B3_FORCE_INLINE b3Matrix3x3(const b3SimdFloat4 v0, const b3SimdFloat4 v1, const b3SimdFloat4 v2)
  74         {
  75                 m_el[0].mVec128 = v0;
  76                 m_el[1].mVec128 = v1;
  77                 m_el[2].mVec128 = v2;
  78         }
  79
  80         B3_FORCE_INLINE b3Matrix3x3(const b3Vector3& v0, const b3Vector3& v1, const b3Vector3& v2)
  81         {
  82                 m_el[0] = v0;
  83                 m_el[1] = v1;
  84                 m_el[2] = v2;
  85         }
  86
  87         // Copy constructor
  88         B3_FORCE_INLINE b3Matrix3x3(const b3Matrix3x3& rhs)
  89         {
  90                 m_el[0].mVec128 = rhs.m_el[0].mVec128;
  91                 m_el[1].mVec128 = rhs.m_el[1].mVec128;
  92                 m_el[2].mVec128 = rhs.m_el[2].mVec128;
  93         }
  94
  95         // Assignment Operator
  96         B3_FORCE_INLINE b3Matrix3x3& operator=(const b3Matrix3x3& m)
  97         {
  98                 m_el[0].mVec128 = m.m_el[0].mVec128;
  99                 m_el[1].mVec128 = m.m_el[1].mVec128;
 100                 m_el[2].mVec128 = m.m_el[2].mVec128;
 101
 102                 return *this;
 103         }
 104
 105 #else
 106
 107         /** @brief Copy constructor */
 108         B3_FORCE_INLINE b3Matrix3x3(const b3Matrix3x3& other)
 109         {
 110                 m_el[0] = other.m_el[0];
 111                 m_el[1] = other.m_el[1];
 112                 m_el[2] = other.m_el[2];
 113         }
 114
 115         /** @brief Assignment Operator */
 116         B3_FORCE_INLINE b3Matrix3x3& operator=(const b3Matrix3x3& other)
 117         {
 118                 m_el[0] = other.m_el[0];
 119                 m_el[1] = other.m_el[1];
 120                 m_el[2] = other.m_el[2];
 121                 return *this;
 122         }
 123
 124 #endif
 125
 126         /** @brief Get a column of the matrix as a vector
 127         *  @param i Column number 0 indexed */
 128         B3_FORCE_INLINE b3Vector3 getColumn(int i) const
 129         {
 130                 return b3MakeVector3(m_el[0][i], m_el[1][i], m_el[2][i]);
 131         }
 132
 133         /** @brief Get a row of the matrix as a vector
 134         *  @param i Row number 0 indexed */
 135         B3_FORCE_INLINE const b3Vector3& getRow(int i) const
 136         {
 137                 b3FullAssert(0 <= i && i < 3);
 138                 return m_el[i];
 139         }
 140
 141         /** @brief Get a mutable reference to a row of the matrix as a vector
 142         *  @param i Row number 0 indexed */
 143         B3_FORCE_INLINE b3Vector3& operator[](int i)
 144         {
 145                 b3FullAssert(0 <= i && i < 3);
 146                 return m_el[i];
 147         }
 148
 149         /** @brief Get a const reference to a row of the matrix as a vector
 150         *  @param i Row number 0 indexed */
 151         B3_FORCE_INLINE const b3Vector3& operator[](int i) const
 152         {
 153                 b3FullAssert(0 <= i && i < 3);
 154                 return m_el[i];
 155         }
 156
 157         /** @brief Multiply by the target matrix on the right
 158         *  @param m Rotation matrix to be applied
 159         * Equivilant to this = this * m */
 160         b3Matrix3x3& operator*=(const b3Matrix3x3& m);
 161
 162         /** @brief Adds by the target matrix on the right
 163         *  @param m matrix to be applied
 164         * Equivilant to this = this + m */
 165         b3Matrix3x3& operator+=(const b3Matrix3x3& m);
 166
 167         /** @brief Substractss by the target matrix on the right
 168         *  @param m matrix to be applied
 169         * Equivilant to this = this - m */
 170         b3Matrix3x3& operator-=(const b3Matrix3x3& m);
 171
 172         /** @brief Set from the rotational part of a 4x4 OpenGL matrix
 173         *  @param m A pointer to the beginning of the array of scalars*/
 174         void setFromOpenGLSubMatrix(const b3Scalar* m)
 175         {
 176                 m_el[0].setValue(m[0], m[4], m[8]);
 177                 m_el[1].setValue(m[1], m[5], m[9]);
 178                 m_el[2].setValue(m[2], m[6], m[10]);
 179         }
 180         /** @brief Set the values of the matrix explicitly (row major)
 181         *  @param xx Top left
 182         *  @param xy Top Middle
 183         *  @param xz Top Right
 184         *  @param yx Middle Left
 185         *  @param yy Middle Middle
 186         *  @param yz Middle Right
 187         *  @param zx Bottom Left
 188         *  @param zy Bottom Middle
 189         *  @param zz Bottom Right*/
 190         void setValue(const b3Scalar& xx, const b3Scalar& xy, const b3Scalar& xz,
 191                                   const b3Scalar& yx, const b3Scalar& yy, const b3Scalar& yz,
 192                                   const b3Scalar& zx, const b3Scalar& zy, const b3Scalar& zz)
 193         {
 194                 m_el[0].setValue(xx, xy, xz);
 195                 m_el[1].setValue(yx, yy, yz);
 196                 m_el[2].setValue(zx, zy, zz);
 197         }
 198
 199         /** @brief Set the matrix from a quaternion
 200         *  @param q The Quaternion to match */
 201         void setRotation(const b3Quaternion& q)
 202         {
 203                 b3Scalar d = q.length2();
 204                 b3FullAssert(d != b3Scalar(0.0));
 205                 b3Scalar s = b3Scalar(2.0) / d;
 206
 207 #if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
 208                 __m128 vs, Q = q.get128();
 209                 __m128i Qi = b3CastfTo128i(Q);
 210                 __m128 Y, Z;
 211                 __m128 V1, V2, V3;
 212                 __m128 V11, V21, V31;
 213                 __m128 NQ = _mm_xor_ps(Q, b3vMzeroMask);
 214                 __m128i NQi = b3CastfTo128i(NQ);
 215
 216                 V1 = b3CastiTo128f(_mm_shuffle_epi32(Qi, B3_SHUFFLE(1, 0, 2, 3)));  // Y X Z W
 217                 V2 = _mm_shuffle_ps(NQ, Q, B3_SHUFFLE(0, 0, 1, 3));                 // -X -X  Y  W
 218                 V3 = b3CastiTo128f(_mm_shuffle_epi32(Qi, B3_SHUFFLE(2, 1, 0, 3)));  // Z Y X W
 219                 V1 = _mm_xor_ps(V1, b3vMPPP);                                       //  change the sign of the first element
 220
 221                 V11 = b3CastiTo128f(_mm_shuffle_epi32(Qi, B3_SHUFFLE(1, 1, 0, 3)));  // Y Y X W
 222                 V21 = _mm_unpackhi_ps(Q, Q);                                         //  Z  Z  W  W
 223                 V31 = _mm_shuffle_ps(Q, NQ, B3_SHUFFLE(0, 2, 0, 3));                 //  X  Z -X -W
 224
 225                 V2 = V2 * V1;   //
 226                 V1 = V1 * V11;  //
 227                 V3 = V3 * V31;  //
 228
 229                 V11 = _mm_shuffle_ps(NQ, Q, B3_SHUFFLE(2, 3, 1, 3));                //  -Z -W  Y  W
 230                 V11 = V11 * V21;                                                    //
 231                 V21 = _mm_xor_ps(V21, b3vMPPP);                                     //  change the sign of the first element
 232                 V31 = _mm_shuffle_ps(Q, NQ, B3_SHUFFLE(3, 3, 1, 3));                //   W  W -Y -W
 233                 V31 = _mm_xor_ps(V31, b3vMPPP);                                     //  change the sign of the first element
 234                 Y = b3CastiTo128f(_mm_shuffle_epi32(NQi, B3_SHUFFLE(3, 2, 0, 3)));  // -W -Z -X -W
 235                 Z = b3CastiTo128f(_mm_shuffle_epi32(Qi, B3_SHUFFLE(1, 0, 1, 3)));   //  Y  X  Y  W
 236
 237                 vs = _mm_load_ss(&s);
 238                 V21 = V21 * Y;
 239                 V31 = V31 * Z;
 240
 241                 V1 = V1 + V11;
 242                 V2 = V2 + V21;
 243                 V3 = V3 + V31;
 244
 245                 vs = b3_splat3_ps(vs, 0);
 246                 //      s ready
 247                 V1 = V1 * vs;
 248                 V2 = V2 * vs;
 249                 V3 = V3 * vs;
 250
 251                 V1 = V1 + b3v1000;
 252                 V2 = V2 + b3v0100;
 253                 V3 = V3 + b3v0010;
 254
 255                 m_el[0] = b3MakeVector3(V1);
 256                 m_el[1] = b3MakeVector3(V2);
 257                 m_el[2] = b3MakeVector3(V3);
 258 #else
 259                 b3Scalar xs = q.getX() * s, ys = q.getY() * s, zs = q.getZ() * s;
 260                 b3Scalar wx = q.getW() * xs, wy = q.getW() * ys, wz = q.getW() * zs;
 261                 b3Scalar xx = q.getX() * xs, xy = q.getX() * ys, xz = q.getX() * zs;
 262                 b3Scalar yy = q.getY() * ys, yz = q.getY() * zs, zz = q.getZ() * zs;
 263                 setValue(
 264                         b3Scalar(1.0) - (yy + zz), xy - wz, xz + wy,
 265                         xy + wz, b3Scalar(1.0) - (xx + zz), yz - wx,
 266                         xz - wy, yz + wx, b3Scalar(1.0) - (xx + yy));
 267 #endif
 268         }
 269
 270         /** @brief Set the matrix from euler angles using YPR around YXZ respectively
 271         *  @param yaw Yaw about Y axis
 272         *  @param pitch Pitch about X axis
 273         *  @param roll Roll about Z axis
 274         */
 275         void setEulerYPR(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll)
 276         {
 277                 setEulerZYX(roll, pitch, yaw);
 278         }
 279
 280         /** @brief Set the matrix from euler angles YPR around ZYX axes
 281         * @param eulerX Roll about X axis
 282         * @param eulerY Pitch around Y axis
 283         * @param eulerZ Yaw aboud Z axis
 284         *
 285         * These angles are used to produce a rotation matrix. The euler
 286         * angles are applied in ZYX order. I.e a vector is first rotated
 287         * about X then Y and then Z
 288         **/
 289         void setEulerZYX(b3Scalar eulerX, b3Scalar eulerY, b3Scalar eulerZ)
 290         {
 291                 ///@todo proposed to reverse this since it's labeled zyx but takes arguments xyz and it will match all other parts of the code
 292                 b3Scalar ci(b3Cos(eulerX));
 293                 b3Scalar cj(b3Cos(eulerY));
 294                 b3Scalar ch(b3Cos(eulerZ));
 295                 b3Scalar si(b3Sin(eulerX));
 296                 b3Scalar sj(b3Sin(eulerY));
 297                 b3Scalar sh(b3Sin(eulerZ));
 298                 b3Scalar cc = ci * ch;
 299                 b3Scalar cs = ci * sh;
 300                 b3Scalar sc = si * ch;
 301                 b3Scalar ss = si * sh;
 302
 303                 setValue(cj * ch, sj * sc - cs, sj * cc + ss,
 304                                  cj * sh, sj * ss + cc, sj * cs - sc,
 305                                  -sj, cj * si, cj * ci);
 306         }
 307
 308         /**@brief Set the matrix to the identity */
 309         void setIdentity()
 310         {
 311 #if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
 312                 m_el[0] = b3MakeVector3(b3v1000);
 313                 m_el[1] = b3MakeVector3(b3v0100);
 314                 m_el[2] = b3MakeVector3(b3v0010);
 315 #else
 316                 setValue(b3Scalar(1.0), b3Scalar(0.0), b3Scalar(0.0),
 317                                  b3Scalar(0.0), b3Scalar(1.0), b3Scalar(0.0),
 318                                  b3Scalar(0.0), b3Scalar(0.0), b3Scalar(1.0));
 319 #endif
 320         }
 321
 322         static const b3Matrix3x3& getIdentity()
 323         {
 324 #if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
 325                 static const b3Matrix3x3
 326                         identityMatrix(b3v1000, b3v0100, b3v0010);
 327 #else
 328                 static const b3Matrix3x3
 329                         identityMatrix(
 330                                 b3Scalar(1.0), b3Scalar(0.0), b3Scalar(0.0),
 331                                 b3Scalar(0.0), b3Scalar(1.0), b3Scalar(0.0),
 332                                 b3Scalar(0.0), b3Scalar(0.0), b3Scalar(1.0));
 333 #endif
 334                 return identityMatrix;
 335         }
 336
 337         /**@brief Fill the rotational part of an OpenGL matrix and clear the shear/perspective
 338         * @param m The array to be filled */
 339         void getOpenGLSubMatrix(b3Scalar * m) const
 340         {
 341 #if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
 342                 __m128 v0 = m_el[0].mVec128;
 343                 __m128 v1 = m_el[1].mVec128;
 344                 __m128 v2 = m_el[2].mVec128;  //  x2 y2 z2 w2
 345                 __m128* vm = (__m128*)m;
 346                 __m128 vT;
 347
 348                 v2 = _mm_and_ps(v2, b3vFFF0fMask);  //  x2 y2 z2 0
 349
 350                 vT = _mm_unpackhi_ps(v0, v1);  //       z0 z1 * *
 351                 v0 = _mm_unpacklo_ps(v0, v1);  //       x0 x1 y0 y1
 352
 353                 v1 = _mm_shuffle_ps(v0, v2, B3_SHUFFLE(2, 3, 1, 3));                    // y0 y1 y2 0
 354                 v0 = _mm_shuffle_ps(v0, v2, B3_SHUFFLE(0, 1, 0, 3));                    // x0 x1 x2 0
 355                 v2 = b3CastdTo128f(_mm_move_sd(b3CastfTo128d(v2), b3CastfTo128d(vT)));  // z0 z1 z2 0
 356
 357                 vm[0] = v0;
 358                 vm[1] = v1;
 359                 vm[2] = v2;
 360 #elif defined(B3_USE_NEON)
 361                 // note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions.
 362                 static const uint32x2_t zMask = (const uint32x2_t){-1, 0};
 363                 float32x4_t* vm = (float32x4_t*)m;
 364                 float32x4x2_t top = vtrnq_f32(m_el[0].mVec128, m_el[1].mVec128);               // {x0 x1 z0 z1}, {y0 y1 w0 w1}
 365                 float32x2x2_t bl = vtrn_f32(vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f));  // {x2  0 }, {y2 0}
 366                 float32x4_t v0 = vcombine_f32(vget_low_f32(top.val[0]), bl.val[0]);
 367                 float32x4_t v1 = vcombine_f32(vget_low_f32(top.val[1]), bl.val[1]);
 368                 float32x2_t q = (float32x2_t)vand_u32((uint32x2_t)vget_high_f32(m_el[2].mVec128), zMask);
 369                 float32x4_t v2 = vcombine_f32(vget_high_f32(top.val[0]), q);  // z0 z1 z2  0
 370
 371                 vm[0] = v0;
 372                 vm[1] = v1;
 373                 vm[2] = v2;
 374 #else
 375                 m[0] = b3Scalar(m_el[0].getX());
 376                 m[1] = b3Scalar(m_el[1].getX());
 377                 m[2] = b3Scalar(m_el[2].getX());
 378                 m[3] = b3Scalar(0.0);
 379                 m[4] = b3Scalar(m_el[0].getY());
 380                 m[5] = b3Scalar(m_el[1].getY());
 381                 m[6] = b3Scalar(m_el[2].getY());
 382                 m[7] = b3Scalar(0.0);
 383                 m[8] = b3Scalar(m_el[0].getZ());
 384                 m[9] = b3Scalar(m_el[1].getZ());
 385                 m[10] = b3Scalar(m_el[2].getZ());
 386                 m[11] = b3Scalar(0.0);
 387 #endif
 388         }
 389
 390         /**@brief Get the matrix represented as a quaternion
 391         * @param q The quaternion which will be set */
 392         void getRotation(b3Quaternion & q) const
 393         {
 394 #if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
 395                 b3Scalar trace = m_el[0].getX() + m_el[1].getY() + m_el[2].getZ();
 396                 b3Scalar s, x;
 397
 398                 union {
 399                         b3SimdFloat4 vec;
 400                         b3Scalar f[4];
 401                 } temp;
 402
 403                 if (trace > b3Scalar(0.0))
 404                 {
 405                         x = trace + b3Scalar(1.0);
 406
 407                         temp.f[0] = m_el[2].getY() - m_el[1].getZ();
 408                         temp.f[1] = m_el[0].getZ() - m_el[2].getX();
 409                         temp.f[2] = m_el[1].getX() - m_el[0].getY();
 410                         temp.f[3] = x;
 411                         //temp.f[3]= s * b3Scalar(0.5);
 412                 }
 413                 else
 414                 {
 415                         int i, j, k;
 416                         if (m_el[0].getX() < m_el[1].getY())
 417                         {
 418                                 if (m_el[1].getY() < m_el[2].getZ())
 419                                 {
 420                                         i = 2;
 421                                         j = 0;
 422                                         k = 1;
 423                                 }
 424                                 else
 425                                 {
 426                                         i = 1;
 427                                         j = 2;
 428                                         k = 0;
 429                                 }
 430                         }
 431                         else
 432                         {
 433                                 if (m_el[0].getX() < m_el[2].getZ())
 434                                 {
 435                                         i = 2;
 436                                         j = 0;
 437                                         k = 1;
 438                                 }
 439                                 else
 440                                 {
 441                                         i = 0;
 442                                         j = 1;
 443                                         k = 2;
 444                                 }
 445                         }
 446
 447                         x = m_el[i][i] - m_el[j][j] - m_el[k][k] + b3Scalar(1.0);
 448
 449                         temp.f[3] = (m_el[k][j] - m_el[j][k]);
 450                         temp.f[j] = (m_el[j][i] + m_el[i][j]);
 451                         temp.f[k] = (m_el[k][i] + m_el[i][k]);
 452                         temp.f[i] = x;
 453                         //temp.f[i] = s * b3Scalar(0.5);
 454                 }
 455
 456                 s = b3Sqrt(x);
 457                 q.set128(temp.vec);
 458                 s = b3Scalar(0.5) / s;
 459
 460                 q *= s;
 461 #else
 462                 b3Scalar trace = m_el[0].getX() + m_el[1].getY() + m_el[2].getZ();
 463
 464                 b3Scalar temp[4];
 465
 466                 if (trace > b3Scalar(0.0))
 467                 {
 468                         b3Scalar s = b3Sqrt(trace + b3Scalar(1.0));
 469                         temp[3] = (s * b3Scalar(0.5));
 470                         s = b3Scalar(0.5) / s;
 471
 472                         temp[0] = ((m_el[2].getY() - m_el[1].getZ()) * s);
 473                         temp[1] = ((m_el[0].getZ() - m_el[2].getX()) * s);
 474                         temp[2] = ((m_el[1].getX() - m_el[0].getY()) * s);
 475                 }
 476                 else
 477                 {
 478                         int i = m_el[0].getX() < m_el[1].getY() ? (m_el[1].getY() < m_el[2].getZ() ? 2 : 1) : (m_el[0].getX() < m_el[2].getZ() ? 2 : 0);
 479                         int j = (i + 1) % 3;
 480                         int k = (i + 2) % 3;
 481
 482                         b3Scalar s = b3Sqrt(m_el[i][i] - m_el[j][j] - m_el[k][k] + b3Scalar(1.0));
 483                         temp[i] = s * b3Scalar(0.5);
 484                         s = b3Scalar(0.5) / s;
 485
 486                         temp[3] = (m_el[k][j] - m_el[j][k]) * s;
 487                         temp[j] = (m_el[j][i] + m_el[i][j]) * s;
 488                         temp[k] = (m_el[k][i] + m_el[i][k]) * s;
 489                 }
 490                 q.setValue(temp[0], temp[1], temp[2], temp[3]);
 491 #endif
 492         }
 493
 494         /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
 495         * @param yaw Yaw around Y axis
 496         * @param pitch Pitch around X axis
 497         * @param roll around Z axis */
 498         void getEulerYPR(b3Scalar & yaw, b3Scalar & pitch, b3Scalar & roll) const
 499         {
 500                 // first use the normal calculus
 501                 yaw = b3Scalar(b3Atan2(m_el[1].getX(), m_el[0].getX()));
 502                 pitch = b3Scalar(b3Asin(-m_el[2].getX()));
 503                 roll = b3Scalar(b3Atan2(m_el[2].getY(), m_el[2].getZ()));
 504
 505                 // on pitch = +/-HalfPI
 506                 if (b3Fabs(pitch) == B3_HALF_PI)
 507                 {
 508                         if (yaw > 0)
 509                                 yaw -= B3_PI;
 510                         else
 511                                 yaw += B3_PI;
 512
 513                         if (roll > 0)
 514                                 roll -= B3_PI;
 515                         else
 516                                 roll += B3_PI;
 517                 }
 518         };
 519
 520         /**@brief Get the matrix represented as euler angles around ZYX
 521         * @param yaw Yaw around X axis
 522         * @param pitch Pitch around Y axis
 523         * @param roll around X axis
 524         * @param solution_number Which solution of two possible solutions ( 1 or 2) are possible values*/
 525         void getEulerZYX(b3Scalar & yaw, b3Scalar & pitch, b3Scalar & roll, unsigned int solution_number = 1) const
 526         {
 527                 struct Euler
 528                 {
 529                         b3Scalar yaw;
 530                         b3Scalar pitch;
 531                         b3Scalar roll;
 532                 };
 533
 534                 Euler euler_out;
 535                 Euler euler_out2;  //second solution
 536                 //get the pointer to the raw data
 537
 538                 // Check that pitch is not at a singularity
 539                 if (b3Fabs(m_el[2].getX()) >= 1)
 540                 {
 541                         euler_out.yaw = 0;
 542                         euler_out2.yaw = 0;
 543
 544                         // From difference of angles formula
 545                         b3Scalar delta = b3Atan2(m_el[0].getX(), m_el[0].getZ());
 546                         if (m_el[2].getX() > 0)  //gimbal locked up
 547                         {
 548                                 euler_out.pitch = B3_PI / b3Scalar(2.0);
 549                                 euler_out2.pitch = B3_PI / b3Scalar(2.0);
 550                                 euler_out.roll = euler_out.pitch + delta;
 551                                 euler_out2.roll = euler_out.pitch + delta;
 552                         }
 553                         else  // gimbal locked down
 554                         {
 555                                 euler_out.pitch = -B3_PI / b3Scalar(2.0);
 556                                 euler_out2.pitch = -B3_PI / b3Scalar(2.0);
 557                                 euler_out.roll = -euler_out.pitch + delta;
 558                                 euler_out2.roll = -euler_out.pitch + delta;
 559                         }
 560                 }
 561                 else
 562                 {
 563                         euler_out.pitch = -b3Asin(m_el[2].getX());
 564                         euler_out2.pitch = B3_PI - euler_out.pitch;
 565
 566                         euler_out.roll = b3Atan2(m_el[2].getY() / b3Cos(euler_out.pitch),
 567                                                                          m_el[2].getZ() / b3Cos(euler_out.pitch));
 568                         euler_out2.roll = b3Atan2(m_el[2].getY() / b3Cos(euler_out2.pitch),
 569                                                                           m_el[2].getZ() / b3Cos(euler_out2.pitch));
 570
 571                         euler_out.yaw = b3Atan2(m_el[1].getX() / b3Cos(euler_out.pitch),
 572                                                                         m_el[0].getX() / b3Cos(euler_out.pitch));
 573                         euler_out2.yaw = b3Atan2(m_el[1].getX() / b3Cos(euler_out2.pitch),
 574                                                                          m_el[0].getX() / b3Cos(euler_out2.pitch));
 575                 }
 576
 577                 if (solution_number == 1)
 578                 {
 579                         yaw = euler_out.yaw;
 580                         pitch = euler_out.pitch;
 581                         roll = euler_out.roll;
 582                 }
 583                 else
 584                 {
 585                         yaw = euler_out2.yaw;
 586                         pitch = euler_out2.pitch;
 587                         roll = euler_out2.roll;
 588                 }
 589         }
 590
 591         /**@brief Create a scaled copy of the matrix
 592         * @param s Scaling vector The elements of the vector will scale each column */
 593
 594         b3Matrix3x3 scaled(const b3Vector3& s) const
 595         {
 596 #if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
 597                 return b3Matrix3x3(m_el[0] * s, m_el[1] * s, m_el[2] * s);
 598 #else
 599                 return b3Matrix3x3(
 600                         m_el[0].getX() * s.getX(), m_el[0].getY() * s.getY(), m_el[0].getZ() * s.getZ(),
 601                         m_el[1].getX() * s.getX(), m_el[1].getY() * s.getY(), m_el[1].getZ() * s.getZ(),
 602                         m_el[2].getX() * s.getX(), m_el[2].getY() * s.getY(), m_el[2].getZ() * s.getZ());
 603 #endif
 604         }
 605
 606         /**@brief Return the determinant of the matrix */
 607         b3Scalar determinant() const;
 608         /**@brief Return the adjoint of the matrix */
 609         b3Matrix3x3 adjoint() const;
 610         /**@brief Return the matrix with all values non negative */
 611         b3Matrix3x3 absolute() const;
 612         /**@brief Return the transpose of the matrix */
 613         b3Matrix3x3 transpose() const;
 614         /**@brief Return the inverse of the matrix */
 615         b3Matrix3x3 inverse() const;
 616
 617         b3Matrix3x3 transposeTimes(const b3Matrix3x3& m) const;
 618         b3Matrix3x3 timesTranspose(const b3Matrix3x3& m) const;
 619
 620         B3_FORCE_INLINE b3Scalar tdotx(const b3Vector3& v) const
 621         {
 622                 return m_el[0].getX() * v.getX() + m_el[1].getX() * v.getY() + m_el[2].getX() * v.getZ();
 623         }
 624         B3_FORCE_INLINE b3Scalar tdoty(const b3Vector3& v) const
 625         {
 626                 return m_el[0].getY() * v.getX() + m_el[1].getY() * v.getY() + m_el[2].getY() * v.getZ();
 627         }
 628         B3_FORCE_INLINE b3Scalar tdotz(const b3Vector3& v) const
 629         {
 630                 return m_el[0].getZ() * v.getX() + m_el[1].getZ() * v.getY() + m_el[2].getZ() * v.getZ();
 631         }
 632
 633         /**@brief diagonalizes this matrix by the Jacobi method.
 634         * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original
 635         * coordinate system, i.e., old_this = rot * new_this * rot^T.
 636         * @param threshold See iteration
 637         * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied
 638         * by the sum of the absolute values of the diagonal, or when maxSteps have been executed.
 639         *
 640         * Note that this matrix is assumed to be symmetric.
 641         */
 642         void diagonalize(b3Matrix3x3 & rot, b3Scalar threshold, int maxSteps)
 643         {
 644                 rot.setIdentity();
 645                 for (int step = maxSteps; step > 0; step--)
 646                 {
 647                         // find off-diagonal element [p][q] with largest magnitude
 648                         int p = 0;
 649                         int q = 1;
 650                         int r = 2;
 651                         b3Scalar max = b3Fabs(m_el[0][1]);
 652                         b3Scalar v = b3Fabs(m_el[0][2]);
 653                         if (v > max)
 654                         {
 655                                 q = 2;
 656                                 r = 1;
 657                                 max = v;
 658                         }
 659                         v = b3Fabs(m_el[1][2]);
 660                         if (v > max)
 661                         {
 662                                 p = 1;
 663                                 q = 2;
 664                                 r = 0;
 665                                 max = v;
 666                         }
 667
 668                         b3Scalar t = threshold * (b3Fabs(m_el[0][0]) + b3Fabs(m_el[1][1]) + b3Fabs(m_el[2][2]));
 669                         if (max <= t)
 670                         {
 671                                 if (max <= B3_EPSILON * t)
 672                                 {
 673                                         return;
 674                                 }
 675                                 step = 1;
 676                         }
 677
 678                         // compute Jacobi rotation J which leads to a zero for element [p][q]
 679                         b3Scalar mpq = m_el[p][q];
 680                         b3Scalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq);
 681                         b3Scalar theta2 = theta * theta;
 682                         b3Scalar cos;
 683                         b3Scalar sin;
 684                         if (theta2 * theta2 < b3Scalar(10 / B3_EPSILON))
 685                         {
 686                                 t = (theta >= 0) ? 1 / (theta + b3Sqrt(1 + theta2))
 687                                                                  : 1 / (theta - b3Sqrt(1 + theta2));
 688                                 cos = 1 / b3Sqrt(1 + t * t);
 689                                 sin = cos * t;
 690                         }
 691                         else
 692                         {
 693                                 // approximation for large theta-value, i.e., a nearly diagonal matrix
 694                                 t = 1 / (theta * (2 + b3Scalar(0.5) / theta2));
 695                                 cos = 1 - b3Scalar(0.5) * t * t;
 696                                 sin = cos * t;
 697                         }
 698
 699                         // apply rotation to matrix (this = J^T * this * J)
 700                         m_el[p][q] = m_el[q][p] = 0;
 701                         m_el[p][p] -= t * mpq;
 702                         m_el[q][q] += t * mpq;
 703                         b3Scalar mrp = m_el[r][p];
 704                         b3Scalar mrq = m_el[r][q];
 705                         m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq;
 706                         m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp;
 707
 708                         // apply rotation to rot (rot = rot * J)
 709                         for (int i = 0; i < 3; i++)
 710                         {
 711                                 b3Vector3& row = rot[i];
 712                                 mrp = row[p];
 713                                 mrq = row[q];
 714                                 row[p] = cos * mrp - sin * mrq;
 715                                 row[q] = cos * mrq + sin * mrp;
 716                         }
 717                 }
 718         }
 719
 720         /**@brief Calculate the matrix cofactor
 721         * @param r1 The first row to use for calculating the cofactor
 722         * @param c1 The first column to use for calculating the cofactor
 723         * @param r1 The second row to use for calculating the cofactor
 724         * @param c1 The second column to use for calculating the cofactor
 725         * See http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) for more details
 726         */
 727         b3Scalar cofac(int r1, int c1, int r2, int c2) const
 728         {
 729                 return m_el[r1][c1] * m_el[r2][c2] - m_el[r1][c2] * m_el[r2][c1];
 730         }
 731
 732         void serialize(struct b3Matrix3x3Data & dataOut) const;
 733
 734         void serializeFloat(struct b3Matrix3x3FloatData & dataOut) const;
 735
 736         void deSerialize(const struct b3Matrix3x3Data& dataIn);
 737
 738         void deSerializeFloat(const struct b3Matrix3x3FloatData& dataIn);
 739
 740         void deSerializeDouble(const struct b3Matrix3x3DoubleData& dataIn);
 741 };
 742
 743 B3_FORCE_INLINE b3Matrix3x3&
 744 b3Matrix3x3::operator*=(const b3Matrix3x3& m)
 745 {
 746 #if defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)
 747         __m128 rv00, rv01, rv02;
 748         __m128 rv10, rv11, rv12;
 749         __m128 rv20, rv21, rv22;
 750         __m128 mv0, mv1, mv2;
 751
 752         rv02 = m_el[0].mVec128;
 753         rv12 = m_el[1].mVec128;
 754         rv22 = m_el[2].mVec128;
 755
 756         mv0 = _mm_and_ps(m[0].mVec128, b3vFFF0fMask);
 757         mv1 = _mm_and_ps(m[1].mVec128, b3vFFF0fMask);
 758         mv2 = _mm_and_ps(m[2].mVec128, b3vFFF0fMask);
 759
 760         // rv0
 761         rv00 = b3_splat_ps(rv02, 0);
 762         rv01 = b3_splat_ps(rv02, 1);
 763         rv02 = b3_splat_ps(rv02, 2);
 764
 765         rv00 = _mm_mul_ps(rv00, mv0);
 766         rv01 = _mm_mul_ps(rv01, mv1);
 767         rv02 = _mm_mul_ps(rv02, mv2);
 768
 769         // rv1
 770         rv10 = b3_splat_ps(rv12, 0);
 771         rv11 = b3_splat_ps(rv12, 1);
 772         rv12 = b3_splat_ps(rv12, 2);
 773
 774         rv10 = _mm_mul_ps(rv10, mv0);
 775         rv11 = _mm_mul_ps(rv11, mv1);
 776         rv12 = _mm_mul_ps(rv12, mv2);
 777
 778         // rv2
 779         rv20 = b3_splat_ps(rv22, 0);
 780         rv21 = b3_splat_ps(rv22, 1);
 781         rv22 = b3_splat_ps(rv22, 2);
 782
 783         rv20 = _mm_mul_ps(rv20, mv0);
 784         rv21 = _mm_mul_ps(rv21, mv1);
 785         rv22 = _mm_mul_ps(rv22, mv2);
 786
 787         rv00 = _mm_add_ps(rv00, rv01);
 788         rv10 = _mm_add_ps(rv10, rv11);
 789         rv20 = _mm_add_ps(rv20, rv21);
 790
 791         m_el[0].mVec128 = _mm_add_ps(rv00, rv02);
 792         m_el[1].mVec128 = _mm_add_ps(rv10, rv12);
 793         m_el[2].mVec128 = _mm_add_ps(rv20, rv22);
 794
 795 #elif defined(B3_USE_NEON)
 796
 797         float32x4_t rv0, rv1, rv2;
 798         float32x4_t v0, v1, v2;
 799         float32x4_t mv0, mv1, mv2;
 800
 801         v0 = m_el[0].mVec128;
 802         v1 = m_el[1].mVec128;
 803         v2 = m_el[2].mVec128;
 804
 805         mv0 = (float32x4_t)vandq_s32((int32x4_t)m[0].mVec128, b3vFFF0Mask);
 806         mv1 = (float32x4_t)vandq_s32((int32x4_t)m[1].mVec128, b3vFFF0Mask);
 807         mv2 = (float32x4_t)vandq_s32((int32x4_t)m[2].mVec128, b3vFFF0Mask);
 808
 809         rv0 = vmulq_lane_f32(mv0, vget_low_f32(v0), 0);
 810         rv1 = vmulq_lane_f32(mv0, vget_low_f32(v1), 0);
 811         rv2 = vmulq_lane_f32(mv0, vget_low_f32(v2), 0);
 812
 813         rv0 = vmlaq_lane_f32(rv0, mv1, vget_low_f32(v0), 1);
 814         rv1 = vmlaq_lane_f32(rv1, mv1, vget_low_f32(v1), 1);
 815         rv2 = vmlaq_lane_f32(rv2, mv1, vget_low_f32(v2), 1);
 816
 817         rv0 = vmlaq_lane_f32(rv0, mv2, vget_high_f32(v0), 0);
 818         rv1 = vmlaq_lane_f32(rv1, mv2, vget_high_f32(v1), 0);
 819         rv2 = vmlaq_lane_f32(rv2, mv2, vget_high_f32(v2), 0);
 820
 821         m_el[0].mVec128 = rv0;
 822         m_el[1].mVec128 = rv1;
 823         m_el[2].mVec128 = rv2;
 824 #else
 825         setValue(
 826                 m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
 827                 m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
 828                 m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
 829 #endif
 830         return *this;
 831 }
 832
 833 B3_FORCE_INLINE b3Matrix3x3&
 834 b3Matrix3x3::operator+=(const b3Matrix3x3& m)
 835 {
 836 #if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
 837         m_el[0].mVec128 = m_el[0].mVec128 + m.m_el[0].mVec128;
 838         m_el[1].mVec128 = m_el[1].mVec128 + m.m_el[1].mVec128;
 839         m_el[2].mVec128 = m_el[2].mVec128 + m.m_el[2].mVec128;
 840 #else
 841         setValue(
 842                 m_el[0][0] + m.m_el[0][0],
 843                 m_el[0][1] + m.m_el[0][1],
 844                 m_el[0][2] + m.m_el[0][2],
 845                 m_el[1][0] + m.m_el[1][0],
 846                 m_el[1][1] + m.m_el[1][1],
 847                 m_el[1][2] + m.m_el[1][2],
 848                 m_el[2][0] + m.m_el[2][0],
 849                 m_el[2][1] + m.m_el[2][1],
 850                 m_el[2][2] + m.m_el[2][2]);
 851 #endif
 852         return *this;
 853 }
 854
 855 B3_FORCE_INLINE b3Matrix3x3
 856 operator*(const b3Matrix3x3& m, const b3Scalar& k)
 857 {
 858 #if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
 859         __m128 vk = b3_splat_ps(_mm_load_ss((float*)&k), 0x80);
 860         return b3Matrix3x3(
 861                 _mm_mul_ps(m[0].mVec128, vk),
 862                 _mm_mul_ps(m[1].mVec128, vk),
 863                 _mm_mul_ps(m[2].mVec128, vk));
 864 #elif defined(B3_USE_NEON)
 865         return b3Matrix3x3(
 866                 vmulq_n_f32(m[0].mVec128, k),
 867                 vmulq_n_f32(m[1].mVec128, k),
 868                 vmulq_n_f32(m[2].mVec128, k));
 869 #else
 870         return b3Matrix3x3(
 871                 m[0].getX() * k, m[0].getY() * k, m[0].getZ() * k,
 872                 m[1].getX() * k, m[1].getY() * k, m[1].getZ() * k,
 873                 m[2].getX() * k, m[2].getY() * k, m[2].getZ() * k);
 874 #endif
 875 }
 876
 877 B3_FORCE_INLINE b3Matrix3x3
 878 operator+(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
 879 {
 880 #if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
 881         return b3Matrix3x3(
 882                 m1[0].mVec128 + m2[0].mVec128,
 883                 m1[1].mVec128 + m2[1].mVec128,
 884                 m1[2].mVec128 + m2[2].mVec128);
 885 #else
 886         return b3Matrix3x3(
 887                 m1[0][0] + m2[0][0],
 888                 m1[0][1] + m2[0][1],
 889                 m1[0][2] + m2[0][2],
 890
 891                 m1[1][0] + m2[1][0],
 892                 m1[1][1] + m2[1][1],
 893                 m1[1][2] + m2[1][2],
 894
 895                 m1[2][0] + m2[2][0],
 896                 m1[2][1] + m2[2][1],
 897                 m1[2][2] + m2[2][2]);
 898 #endif
 899 }
 900
 901 B3_FORCE_INLINE b3Matrix3x3
 902 operator-(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
 903 {
 904 #if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
 905         return b3Matrix3x3(
 906                 m1[0].mVec128 - m2[0].mVec128,
 907                 m1[1].mVec128 - m2[1].mVec128,
 908                 m1[2].mVec128 - m2[2].mVec128);
 909 #else
 910         return b3Matrix3x3(
 911                 m1[0][0] - m2[0][0],
 912                 m1[0][1] - m2[0][1],
 913                 m1[0][2] - m2[0][2],
 914
 915                 m1[1][0] - m2[1][0],
 916                 m1[1][1] - m2[1][1],
 917                 m1[1][2] - m2[1][2],
 918
 919                 m1[2][0] - m2[2][0],
 920                 m1[2][1] - m2[2][1],
 921                 m1[2][2] - m2[2][2]);
 922 #endif
 923 }
 924
 925 B3_FORCE_INLINE b3Matrix3x3&
 926 b3Matrix3x3::operator-=(const b3Matrix3x3& m)
 927 {
 928 #if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
 929         m_el[0].mVec128 = m_el[0].mVec128 - m.m_el[0].mVec128;
 930         m_el[1].mVec128 = m_el[1].mVec128 - m.m_el[1].mVec128;
 931         m_el[2].mVec128 = m_el[2].mVec128 - m.m_el[2].mVec128;
 932 #else
 933         setValue(
 934                 m_el[0][0] - m.m_el[0][0],
 935                 m_el[0][1] - m.m_el[0][1],
 936                 m_el[0][2] - m.m_el[0][2],
 937                 m_el[1][0] - m.m_el[1][0],
 938                 m_el[1][1] - m.m_el[1][1],
 939                 m_el[1][2] - m.m_el[1][2],
 940                 m_el[2][0] - m.m_el[2][0],
 941                 m_el[2][1] - m.m_el[2][1],
 942                 m_el[2][2] - m.m_el[2][2]);
 943 #endif
 944         return *this;
 945 }
 946
 947 B3_FORCE_INLINE b3Scalar
 948 b3Matrix3x3::determinant() const
 949 {
 950         return b3Triple((*this)[0], (*this)[1], (*this)[2]);
 951 }
 952
 953 B3_FORCE_INLINE b3Matrix3x3
 954 b3Matrix3x3::absolute() const
 955 {
 956 #if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
 957         return b3Matrix3x3(
 958                 _mm_and_ps(m_el[0].mVec128, b3vAbsfMask),
 959                 _mm_and_ps(m_el[1].mVec128, b3vAbsfMask),
 960                 _mm_and_ps(m_el[2].mVec128, b3vAbsfMask));
 961 #elif defined(B3_USE_NEON)
 962         return b3Matrix3x3(
 963                 (float32x4_t)vandq_s32((int32x4_t)m_el[0].mVec128, b3v3AbsMask),
 964                 (float32x4_t)vandq_s32((int32x4_t)m_el[1].mVec128, b3v3AbsMask),
 965                 (float32x4_t)vandq_s32((int32x4_t)m_el[2].mVec128, b3v3AbsMask));
 966 #else
 967         return b3Matrix3x3(
 968                 b3Fabs(m_el[0].getX()), b3Fabs(m_el[0].getY()), b3Fabs(m_el[0].getZ()),
 969                 b3Fabs(m_el[1].getX()), b3Fabs(m_el[1].getY()), b3Fabs(m_el[1].getZ()),
 970                 b3Fabs(m_el[2].getX()), b3Fabs(m_el[2].getY()), b3Fabs(m_el[2].getZ()));
 971 #endif
 972 }
 973
 974 B3_FORCE_INLINE b3Matrix3x3
 975 b3Matrix3x3::transpose() const
 976 {
 977 #if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
 978         __m128 v0 = m_el[0].mVec128;
 979         __m128 v1 = m_el[1].mVec128;
 980         __m128 v2 = m_el[2].mVec128;  //  x2 y2 z2 w2
 981         __m128 vT;
 982
 983         v2 = _mm_and_ps(v2, b3vFFF0fMask);  //  x2 y2 z2 0
 984
 985         vT = _mm_unpackhi_ps(v0, v1);  //       z0 z1 * *
 986         v0 = _mm_unpacklo_ps(v0, v1);  //       x0 x1 y0 y1
 987
 988         v1 = _mm_shuffle_ps(v0, v2, B3_SHUFFLE(2, 3, 1, 3));                    // y0 y1 y2 0
 989         v0 = _mm_shuffle_ps(v0, v2, B3_SHUFFLE(0, 1, 0, 3));                    // x0 x1 x2 0
 990         v2 = b3CastdTo128f(_mm_move_sd(b3CastfTo128d(v2), b3CastfTo128d(vT)));  // z0 z1 z2 0
 991
 992         return b3Matrix3x3(v0, v1, v2);
 993 #elif defined(B3_USE_NEON)
 994         // note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions.
 995         static const uint32x2_t zMask = (const uint32x2_t){-1, 0};
 996         float32x4x2_t top = vtrnq_f32(m_el[0].mVec128, m_el[1].mVec128);               // {x0 x1 z0 z1}, {y0 y1 w0 w1}
 997         float32x2x2_t bl = vtrn_f32(vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f));  // {x2  0 }, {y2 0}
 998         float32x4_t v0 = vcombine_f32(vget_low_f32(top.val[0]), bl.val[0]);
 999         float32x4_t v1 = vcombine_f32(vget_low_f32(top.val[1]), bl.val[1]);
1000         float32x2_t q = (float32x2_t)vand_u32((uint32x2_t)vget_high_f32(m_el[2].mVec128), zMask);
1001         float32x4_t v2 = vcombine_f32(vget_high_f32(top.val[0]), q);  // z0 z1 z2  0
1002         return b3Matrix3x3(v0, v1, v2);
1003 #else
1004         return b3Matrix3x3(m_el[0].getX(), m_el[1].getX(), m_el[2].getX(),
1005                                            m_el[0].getY(), m_el[1].getY(), m_el[2].getY(),
1006                                            m_el[0].getZ(), m_el[1].getZ(), m_el[2].getZ());
1007 #endif
1008 }
1009
1010 B3_FORCE_INLINE b3Matrix3x3
1011 b3Matrix3x3::adjoint() const
1012 {
1013         return b3Matrix3x3(cofac(1, 1, 2, 2), cofac(0, 2, 2, 1), cofac(0, 1, 1, 2),
1014                                            cofac(1, 2, 2, 0), cofac(0, 0, 2, 2), cofac(0, 2, 1, 0),
1015                                            cofac(1, 0, 2, 1), cofac(0, 1, 2, 0), cofac(0, 0, 1, 1));
1016 }
1017
1018 B3_FORCE_INLINE b3Matrix3x3
1019 b3Matrix3x3::inverse() const
1020 {
1021         b3Vector3 co = b3MakeVector3(cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1));
1022         b3Scalar det = (*this)[0].dot(co);
1023         b3FullAssert(det != b3Scalar(0.0));
1024         b3Scalar s = b3Scalar(1.0) / det;
1025         return b3Matrix3x3(co.getX() * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s,
1026                                            co.getY() * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s,
1027                                            co.getZ() * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s);
1028 }
1029
1030 B3_FORCE_INLINE b3Matrix3x3
1031 b3Matrix3x3::transposeTimes(const b3Matrix3x3& m) const
1032 {
1033 #if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
1034         // zeros w
1035         //    static const __m128i xyzMask = (const __m128i){ -1ULL, 0xffffffffULL };
1036         __m128 row = m_el[0].mVec128;
1037         __m128 m0 = _mm_and_ps(m.getRow(0).mVec128, b3vFFF0fMask);
1038         __m128 m1 = _mm_and_ps(m.getRow(1).mVec128, b3vFFF0fMask);
1039         __m128 m2 = _mm_and_ps(m.getRow(2).mVec128, b3vFFF0fMask);
1040         __m128 r0 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0));
1041         __m128 r1 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0x55));
1042         __m128 r2 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0xaa));
1043         row = m_el[1].mVec128;
1044         r0 = _mm_add_ps(r0, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0)));
1045         r1 = _mm_add_ps(r1, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0x55)));
1046         r2 = _mm_add_ps(r2, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0xaa)));
1047         row = m_el[2].mVec128;
1048         r0 = _mm_add_ps(r0, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0)));
1049         r1 = _mm_add_ps(r1, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0x55)));
1050         r2 = _mm_add_ps(r2, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0xaa)));
1051         return b3Matrix3x3(r0, r1, r2);
1052
1053 #elif defined B3_USE_NEON
1054         // zeros w
1055         static const uint32x4_t xyzMask = (const uint32x4_t){-1, -1, -1, 0};
1056         float32x4_t m0 = (float32x4_t)vandq_u32((uint32x4_t)m.getRow(0).mVec128, xyzMask);
1057         float32x4_t m1 = (float32x4_t)vandq_u32((uint32x4_t)m.getRow(1).mVec128, xyzMask);
1058         float32x4_t m2 = (float32x4_t)vandq_u32((uint32x4_t)m.getRow(2).mVec128, xyzMask);
1059         float32x4_t row = m_el[0].mVec128;
1060         float32x4_t r0 = vmulq_lane_f32(m0, vget_low_f32(row), 0);
1061         float32x4_t r1 = vmulq_lane_f32(m0, vget_low_f32(row), 1);
1062         float32x4_t r2 = vmulq_lane_f32(m0, vget_high_f32(row), 0);
1063         row = m_el[1].mVec128;
1064         r0 = vmlaq_lane_f32(r0, m1, vget_low_f32(row), 0);
1065         r1 = vmlaq_lane_f32(r1, m1, vget_low_f32(row), 1);
1066         r2 = vmlaq_lane_f32(r2, m1, vget_high_f32(row), 0);
1067         row = m_el[2].mVec128;
1068         r0 = vmlaq_lane_f32(r0, m2, vget_low_f32(row), 0);
1069         r1 = vmlaq_lane_f32(r1, m2, vget_low_f32(row), 1);
1070         r2 = vmlaq_lane_f32(r2, m2, vget_high_f32(row), 0);
1071         return b3Matrix3x3(r0, r1, r2);
1072 #else
1073         return b3Matrix3x3(
1074                 m_el[0].getX() * m[0].getX() + m_el[1].getX() * m[1].getX() + m_el[2].getX() * m[2].getX(),
1075                 m_el[0].getX() * m[0].getY() + m_el[1].getX() * m[1].getY() + m_el[2].getX() * m[2].getY(),
1076                 m_el[0].getX() * m[0].getZ() + m_el[1].getX() * m[1].getZ() + m_el[2].getX() * m[2].getZ(),
1077                 m_el[0].getY() * m[0].getX() + m_el[1].getY() * m[1].getX() + m_el[2].getY() * m[2].getX(),
1078                 m_el[0].getY() * m[0].getY() + m_el[1].getY() * m[1].getY() + m_el[2].getY() * m[2].getY(),
1079                 m_el[0].getY() * m[0].getZ() + m_el[1].getY() * m[1].getZ() + m_el[2].getY() * m[2].getZ(),
1080                 m_el[0].getZ() * m[0].getX() + m_el[1].getZ() * m[1].getX() + m_el[2].getZ() * m[2].getX(),
1081                 m_el[0].getZ() * m[0].getY() + m_el[1].getZ() * m[1].getY() + m_el[2].getZ() * m[2].getY(),
1082                 m_el[0].getZ() * m[0].getZ() + m_el[1].getZ() * m[1].getZ() + m_el[2].getZ() * m[2].getZ());
1083 #endif
1084 }
1085
1086 B3_FORCE_INLINE b3Matrix3x3
1087 b3Matrix3x3::timesTranspose(const b3Matrix3x3& m) const
1088 {
1089 #if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
1090         __m128 a0 = m_el[0].mVec128;
1091         __m128 a1 = m_el[1].mVec128;
1092         __m128 a2 = m_el[2].mVec128;
1093
1094         b3Matrix3x3 mT = m.transpose();  // we rely on transpose() zeroing w channel so that we don't have to do it here
1095         __m128 mx = mT[0].mVec128;
1096         __m128 my = mT[1].mVec128;
1097         __m128 mz = mT[2].mVec128;
1098
1099         __m128 r0 = _mm_mul_ps(mx, _mm_shuffle_ps(a0, a0, 0x00));
1100         __m128 r1 = _mm_mul_ps(mx, _mm_shuffle_ps(a1, a1, 0x00));
1101         __m128 r2 = _mm_mul_ps(mx, _mm_shuffle_ps(a2, a2, 0x00));
1102         r0 = _mm_add_ps(r0, _mm_mul_ps(my, _mm_shuffle_ps(a0, a0, 0x55)));
1103         r1 = _mm_add_ps(r1, _mm_mul_ps(my, _mm_shuffle_ps(a1, a1, 0x55)));
1104         r2 = _mm_add_ps(r2, _mm_mul_ps(my, _mm_shuffle_ps(a2, a2, 0x55)));
1105         r0 = _mm_add_ps(r0, _mm_mul_ps(mz, _mm_shuffle_ps(a0, a0, 0xaa)));
1106         r1 = _mm_add_ps(r1, _mm_mul_ps(mz, _mm_shuffle_ps(a1, a1, 0xaa)));
1107         r2 = _mm_add_ps(r2, _mm_mul_ps(mz, _mm_shuffle_ps(a2, a2, 0xaa)));
1108         return b3Matrix3x3(r0, r1, r2);
1109
1110 #elif defined B3_USE_NEON
1111         float32x4_t a0 = m_el[0].mVec128;
1112         float32x4_t a1 = m_el[1].mVec128;
1113         float32x4_t a2 = m_el[2].mVec128;
1114
1115         b3Matrix3x3 mT = m.transpose();  // we rely on transpose() zeroing w channel so that we don't have to do it here
1116         float32x4_t mx = mT[0].mVec128;
1117         float32x4_t my = mT[1].mVec128;
1118         float32x4_t mz = mT[2].mVec128;
1119
1120         float32x4_t r0 = vmulq_lane_f32(mx, vget_low_f32(a0), 0);
1121         float32x4_t r1 = vmulq_lane_f32(mx, vget_low_f32(a1), 0);
1122         float32x4_t r2 = vmulq_lane_f32(mx, vget_low_f32(a2), 0);
1123         r0 = vmlaq_lane_f32(r0, my, vget_low_f32(a0), 1);
1124         r1 = vmlaq_lane_f32(r1, my, vget_low_f32(a1), 1);
1125         r2 = vmlaq_lane_f32(r2, my, vget_low_f32(a2), 1);
1126         r0 = vmlaq_lane_f32(r0, mz, vget_high_f32(a0), 0);
1127         r1 = vmlaq_lane_f32(r1, mz, vget_high_f32(a1), 0);
1128         r2 = vmlaq_lane_f32(r2, mz, vget_high_f32(a2), 0);
1129         return b3Matrix3x3(r0, r1, r2);
1130
1131 #else
1132         return b3Matrix3x3(
1133                 m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
1134                 m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
1135                 m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
1136 #endif
1137 }
1138
1139 B3_FORCE_INLINE b3Vector3
1140 operator*(const b3Matrix3x3& m, const b3Vector3& v)
1141 {
1142 #if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE)) || defined(B3_USE_NEON)
1143         return v.dot3(m[0], m[1], m[2]);
1144 #else
1145         return b3MakeVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
1146 #endif
1147 }
1148
1149 B3_FORCE_INLINE b3Vector3
1150 operator*(const b3Vector3& v, const b3Matrix3x3& m)
1151 {
1152 #if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
1153
1154         const __m128 vv = v.mVec128;
1155
1156         __m128 c0 = b3_splat_ps(vv, 0);
1157         __m128 c1 = b3_splat_ps(vv, 1);
1158         __m128 c2 = b3_splat_ps(vv, 2);
1159
1160         c0 = _mm_mul_ps(c0, _mm_and_ps(m[0].mVec128, b3vFFF0fMask));
1161         c1 = _mm_mul_ps(c1, _mm_and_ps(m[1].mVec128, b3vFFF0fMask));
1162         c0 = _mm_add_ps(c0, c1);
1163         c2 = _mm_mul_ps(c2, _mm_and_ps(m[2].mVec128, b3vFFF0fMask));
1164
1165         return b3MakeVector3(_mm_add_ps(c0, c2));
1166 #elif defined(B3_USE_NEON)
1167         const float32x4_t vv = v.mVec128;
1168         const float32x2_t vlo = vget_low_f32(vv);
1169         const float32x2_t vhi = vget_high_f32(vv);
1170
1171         float32x4_t c0, c1, c2;
1172
1173         c0 = (float32x4_t)vandq_s32((int32x4_t)m[0].mVec128, b3vFFF0Mask);
1174         c1 = (float32x4_t)vandq_s32((int32x4_t)m[1].mVec128, b3vFFF0Mask);
1175         c2 = (float32x4_t)vandq_s32((int32x4_t)m[2].mVec128, b3vFFF0Mask);
1176
1177         c0 = vmulq_lane_f32(c0, vlo, 0);
1178         c1 = vmulq_lane_f32(c1, vlo, 1);
1179         c2 = vmulq_lane_f32(c2, vhi, 0);
1180         c0 = vaddq_f32(c0, c1);
1181         c0 = vaddq_f32(c0, c2);
1182
1183         return b3MakeVector3(c0);
1184 #else
1185         return b3MakeVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
1186 #endif
1187 }
1188
1189 B3_FORCE_INLINE b3Matrix3x3
1190 operator*(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
1191 {
1192 #if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
1193
1194         __m128 m10 = m1[0].mVec128;
1195         __m128 m11 = m1[1].mVec128;
1196         __m128 m12 = m1[2].mVec128;
1197
1198         __m128 m2v = _mm_and_ps(m2[0].mVec128, b3vFFF0fMask);
1199
1200         __m128 c0 = b3_splat_ps(m10, 0);
1201         __m128 c1 = b3_splat_ps(m11, 0);
1202         __m128 c2 = b3_splat_ps(m12, 0);
1203
1204         c0 = _mm_mul_ps(c0, m2v);
1205         c1 = _mm_mul_ps(c1, m2v);
1206         c2 = _mm_mul_ps(c2, m2v);
1207
1208         m2v = _mm_and_ps(m2[1].mVec128, b3vFFF0fMask);
1209
1210         __m128 c0_1 = b3_splat_ps(m10, 1);
1211         __m128 c1_1 = b3_splat_ps(m11, 1);
1212         __m128 c2_1 = b3_splat_ps(m12, 1);
1213
1214         c0_1 = _mm_mul_ps(c0_1, m2v);
1215         c1_1 = _mm_mul_ps(c1_1, m2v);
1216         c2_1 = _mm_mul_ps(c2_1, m2v);
1217
1218         m2v = _mm_and_ps(m2[2].mVec128, b3vFFF0fMask);
1219
1220         c0 = _mm_add_ps(c0, c0_1);
1221         c1 = _mm_add_ps(c1, c1_1);
1222         c2 = _mm_add_ps(c2, c2_1);
1223
1224         m10 = b3_splat_ps(m10, 2);
1225         m11 = b3_splat_ps(m11, 2);
1226         m12 = b3_splat_ps(m12, 2);
1227
1228         m10 = _mm_mul_ps(m10, m2v);
1229         m11 = _mm_mul_ps(m11, m2v);
1230         m12 = _mm_mul_ps(m12, m2v);
1231
1232         c0 = _mm_add_ps(c0, m10);
1233         c1 = _mm_add_ps(c1, m11);
1234         c2 = _mm_add_ps(c2, m12);
1235
1236         return b3Matrix3x3(c0, c1, c2);
1237
1238 #elif defined(B3_USE_NEON)
1239
1240         float32x4_t rv0, rv1, rv2;
1241         float32x4_t v0, v1, v2;
1242         float32x4_t mv0, mv1, mv2;
1243
1244         v0 = m1[0].mVec128;
1245         v1 = m1[1].mVec128;
1246         v2 = m1[2].mVec128;
1247
1248         mv0 = (float32x4_t)vandq_s32((int32x4_t)m2[0].mVec128, b3vFFF0Mask);
1249         mv1 = (float32x4_t)vandq_s32((int32x4_t)m2[1].mVec128, b3vFFF0Mask);
1250         mv2 = (float32x4_t)vandq_s32((int32x4_t)m2[2].mVec128, b3vFFF0Mask);
1251
1252         rv0 = vmulq_lane_f32(mv0, vget_low_f32(v0), 0);
1253         rv1 = vmulq_lane_f32(mv0, vget_low_f32(v1), 0);
1254         rv2 = vmulq_lane_f32(mv0, vget_low_f32(v2), 0);
1255
1256         rv0 = vmlaq_lane_f32(rv0, mv1, vget_low_f32(v0), 1);
1257         rv1 = vmlaq_lane_f32(rv1, mv1, vget_low_f32(v1), 1);
1258         rv2 = vmlaq_lane_f32(rv2, mv1, vget_low_f32(v2), 1);
1259
1260         rv0 = vmlaq_lane_f32(rv0, mv2, vget_high_f32(v0), 0);
1261         rv1 = vmlaq_lane_f32(rv1, mv2, vget_high_f32(v1), 0);
1262         rv2 = vmlaq_lane_f32(rv2, mv2, vget_high_f32(v2), 0);
1263
1264         return b3Matrix3x3(rv0, rv1, rv2);
1265
1266 #else
1267         return b3Matrix3x3(
1268                 m2.tdotx(m1[0]), m2.tdoty(m1[0]), m2.tdotz(m1[0]),
1269                 m2.tdotx(m1[1]), m2.tdoty(m1[1]), m2.tdotz(m1[1]),
1270                 m2.tdotx(m1[2]), m2.tdoty(m1[2]), m2.tdotz(m1[2]));
1271 #endif
1272 }
1273
1274 /*
1275 B3_FORCE_INLINE b3Matrix3x3 b3MultTransposeLeft(const b3Matrix3x3& m1, const b3Matrix3x3& m2) {
1276 return b3Matrix3x3(
1277 m1[0][0] * m2[0][0] + m1[1][0] * m2[1][0] + m1[2][0] * m2[2][0],
1278 m1[0][0] * m2[0][1] + m1[1][0] * m2[1][1] + m1[2][0] * m2[2][1],
1279 m1[0][0] * m2[0][2] + m1[1][0] * m2[1][2] + m1[2][0] * m2[2][2],
1280 m1[0][1] * m2[0][0] + m1[1][1] * m2[1][0] + m1[2][1] * m2[2][0],
1281 m1[0][1] * m2[0][1] + m1[1][1] * m2[1][1] + m1[2][1] * m2[2][1],
1282 m1[0][1] * m2[0][2] + m1[1][1] * m2[1][2] + m1[2][1] * m2[2][2],
1283 m1[0][2] * m2[0][0] + m1[1][2] * m2[1][0] + m1[2][2] * m2[2][0],
1284 m1[0][2] * m2[0][1] + m1[1][2] * m2[1][1] + m1[2][2] * m2[2][1],
1285 m1[0][2] * m2[0][2] + m1[1][2] * m2[1][2] + m1[2][2] * m2[2][2]);
1286 }
1287 */
1288
1289 /**@brief Equality operator between two matrices
1290 * It will test all elements are equal.  */
1291 B3_FORCE_INLINE bool operator==(const b3Matrix3x3& m1, const b3Matrix3x3& m2)
1292 {
1293 #if (defined(B3_USE_SSE_IN_API) && defined(B3_USE_SSE))
1294
1295         __m128 c0, c1, c2;
1296
1297         c0 = _mm_cmpeq_ps(m1[0].mVec128, m2[0].mVec128);
1298         c1 = _mm_cmpeq_ps(m1[1].mVec128, m2[1].mVec128);
1299         c2 = _mm_cmpeq_ps(m1[2].mVec128, m2[2].mVec128);
1300
1301         c0 = _mm_and_ps(c0, c1);
1302         c0 = _mm_and_ps(c0, c2);
1303
1304         return (0x7 == _mm_movemask_ps((__m128)c0));
1305 #else
1306         return (m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
1307                         m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
1308                         m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2]);
1309 #endif
1310 }
1311
1312 ///for serialization
1313 struct b3Matrix3x3FloatData
1314 {
1315         b3Vector3FloatData m_el[3];
1316 };
1317
1318 ///for serialization
1319 struct b3Matrix3x3DoubleData
1320 {
1321         b3Vector3DoubleData m_el[3];
1322 };
1323
1324 B3_FORCE_INLINE void b3Matrix3x3::serialize(struct b3Matrix3x3Data& dataOut) const
1325 {
1326         for (int i = 0; i < 3; i++)
1327                 m_el[i].serialize(dataOut.m_el[i]);
1328 }
1329
1330 B3_FORCE_INLINE void b3Matrix3x3::serializeFloat(struct b3Matrix3x3FloatData& dataOut) const
1331 {
1332         for (int i = 0; i < 3; i++)
1333                 m_el[i].serializeFloat(dataOut.m_el[i]);
1334 }
1335
1336 B3_FORCE_INLINE void b3Matrix3x3::deSerialize(const struct b3Matrix3x3Data& dataIn)
1337 {
1338         for (int i = 0; i < 3; i++)
1339                 m_el[i].deSerialize(dataIn.m_el[i]);
1340 }
1341
1342 B3_FORCE_INLINE void b3Matrix3x3::deSerializeFloat(const struct b3Matrix3x3FloatData& dataIn)
1343 {
1344         for (int i = 0; i < 3; i++)
1345                 m_el[i].deSerializeFloat(dataIn.m_el[i]);
1346 }
1347
1348 B3_FORCE_INLINE void b3Matrix3x3::deSerializeDouble(const struct b3Matrix3x3DoubleData& dataIn)
1349 {
1350         for (int i = 0; i < 3; i++)
1351                 m_el[i].deSerializeDouble(dataIn.m_el[i]);
1352 }
1353
1354 #endif  //B3_MATRIX3x3_H