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