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