← Back to branch summary

~ubuntu-branches/ubuntu/trusty/blender/trusty-proposed

« back to all changes in this revision

Viewing changes to extern/bullet2/src/LinearMath/btMatrix3x3.h

  • Committer: Package Import Robot
  • Author(s): Matteo F. Vescovi
  • Date: 2013-08-14 10:43:49 UTC
  • mfrom: (14.2.19 sid)
  • Revision ID: package-import@ubuntu.com-20130814104349-t1d5mtwkphp12dyj
Tags: 2.68a-3
* Upload to unstable
* debian/: python3.3 Depends simplified
  - debian/control: python3.3 Depends dropped
    for blender-data package
  - 0001-blender_thumbnailer.patch refreshed
* debian/control: libavcodec b-dep versioning dropped

Show diffs side-by-side

added added

removed removed

Lines of Context:
extern/bullet2/src/LinearMath/btMatrix3x3.h
18
18
 
19
19
#include "btVector3.h"
20
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
#endif
 
27
 
 
28
#if defined(BT_USE_SSE) || defined(BT_USE_NEON)
 
29
const btSimdFloat4 ATTRIBUTE_ALIGNED16(v1000) = {1.0f, 0.0f, 0.0f, 0.0f};
 
30
const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0100) = {0.0f, 1.0f, 0.0f, 0.0f};
 
31
const btSimdFloat4 ATTRIBUTE_ALIGNED16(v0010) = {0.0f, 0.0f, 1.0f, 0.0f};
 
32
#endif
21
33
 
22
34
#ifdef BT_USE_DOUBLE_PRECISION
23
35
#define btMatrix3x3Data btMatrix3x3DoubleData 
28
40
 
29
41
/**@brief The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with btQuaternion, btTransform and btVector3.
30
42
* Make sure to only include a pure orthogonal matrix without scaling. */
31
 
class btMatrix3x3 {
 
43
ATTRIBUTE_ALIGNED16(class) btMatrix3x3 {
32
44
 
33
45
        ///Data storage for the matrix, each vector is a row of the matrix
34
46
        btVector3 m_el[3];
57
69
                        yx, yy, yz, 
58
70
                        zx, zy, zz);
59
71
        }
 
72
 
 
73
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
 
74
        SIMD_FORCE_INLINE btMatrix3x3 (const btSimdFloat4 v0, const btSimdFloat4 v1, const btSimdFloat4 v2 ) 
 
75
        {
 
76
        m_el[0].mVec128 = v0;
 
77
        m_el[1].mVec128 = v1;
 
78
        m_el[2].mVec128 = v2;
 
79
        }
 
80
 
 
81
        SIMD_FORCE_INLINE btMatrix3x3 (const btVector3& v0, const btVector3& v1, const btVector3& v2 ) 
 
82
        {
 
83
        m_el[0] = v0;
 
84
        m_el[1] = v1;
 
85
        m_el[2] = v2;
 
86
        }
 
87
 
 
88
        // Copy constructor
 
89
        SIMD_FORCE_INLINE btMatrix3x3(const btMatrix3x3& rhs)
 
90
        {
 
91
                m_el[0].mVec128 = rhs.m_el[0].mVec128;
 
92
                m_el[1].mVec128 = rhs.m_el[1].mVec128;
 
93
                m_el[2].mVec128 = rhs.m_el[2].mVec128;
 
94
        }
 
95
 
 
96
        // Assignment Operator
 
97
        SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& m) 
 
98
        {
 
99
                m_el[0].mVec128 = m.m_el[0].mVec128;
 
100
                m_el[1].mVec128 = m.m_el[1].mVec128;
 
101
                m_el[2].mVec128 = m.m_el[2].mVec128;
 
102
                
 
103
                return *this;
 
104
        }
 
105
 
 
106
#else
 
107
 
60
108
        /** @brief Copy constructor */
61
109
        SIMD_FORCE_INLINE btMatrix3x3 (const btMatrix3x3& other)
62
110
        {
64
112
                m_el[1] = other.m_el[1];
65
113
                m_el[2] = other.m_el[2];
66
114
        }
 
115
    
67
116
        /** @brief Assignment Operator */
68
117
        SIMD_FORCE_INLINE btMatrix3x3& operator=(const btMatrix3x3& other)
69
118
        {
73
122
                return *this;
74
123
        }
75
124
 
 
125
#endif
 
126
 
76
127
        /** @brief Get a column of the matrix as a vector 
77
128
        *  @param i Column number 0 indexed */
78
129
        SIMD_FORCE_INLINE btVector3 getColumn(int i) const
155
206
                btScalar d = q.length2();
156
207
                btFullAssert(d != btScalar(0.0));
157
208
                btScalar s = btScalar(2.0) / d;
 
209
    
 
210
    #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
 
211
        __m128  vs, Q = q.get128();
 
212
                __m128i Qi = btCastfTo128i(Q);
 
213
        __m128  Y, Z;
 
214
        __m128  V1, V2, V3;
 
215
        __m128  V11, V21, V31;
 
216
        __m128  NQ = _mm_xor_ps(Q, btvMzeroMask);
 
217
                __m128i NQi = btCastfTo128i(NQ);
 
218
        
 
219
        V1 = btCastiTo128f(_mm_shuffle_epi32 (Qi, BT_SHUFFLE(1,0,2,3)));        // Y X Z W
 
220
                V2 = _mm_shuffle_ps(NQ, Q, BT_SHUFFLE(0,0,1,3));     // -X -X  Y  W
 
221
        V3 = btCastiTo128f(_mm_shuffle_epi32 (Qi, BT_SHUFFLE(2,1,0,3)));        // Z Y X W
 
222
        V1 = _mm_xor_ps(V1, vMPPP);     //      change the sign of the first element
 
223
                        
 
224
        V11     = btCastiTo128f(_mm_shuffle_epi32 (Qi, BT_SHUFFLE(1,1,0,3)));   // Y Y X W
 
225
                V21 = _mm_unpackhi_ps(Q, Q);                    //  Z  Z  W  W
 
226
                V31 = _mm_shuffle_ps(Q, NQ, BT_SHUFFLE(0,2,0,3));       //  X  Z -X -W
 
227
 
 
228
                V2 = V2 * V1;   //
 
229
                V1 = V1 * V11;  //
 
230
                V3 = V3 * V31;  //
 
231
 
 
232
        V11 = _mm_shuffle_ps(NQ, Q, BT_SHUFFLE(2,3,1,3));       //      -Z -W  Y  W
 
233
                V11 = V11 * V21;        //
 
234
        V21 = _mm_xor_ps(V21, vMPPP);   //      change the sign of the first element
 
235
                V31 = _mm_shuffle_ps(Q, NQ, BT_SHUFFLE(3,3,1,3));       //       W  W -Y -W
 
236
        V31 = _mm_xor_ps(V31, vMPPP);   //      change the sign of the first element
 
237
                Y = btCastiTo128f(_mm_shuffle_epi32 (NQi, BT_SHUFFLE(3,2,0,3)));        // -W -Z -X -W
 
238
                Z = btCastiTo128f(_mm_shuffle_epi32 (Qi, BT_SHUFFLE(1,0,1,3))); //  Y  X  Y  W
 
239
 
 
240
                vs = _mm_load_ss(&s);
 
241
                V21 = V21 * Y;
 
242
                V31 = V31 * Z;
 
243
 
 
244
                V1 = V1 + V11;
 
245
        V2 = V2 + V21;
 
246
        V3 = V3 + V31;
 
247
 
 
248
        vs = bt_splat3_ps(vs, 0);
 
249
            //  s ready
 
250
        V1 = V1 * vs;
 
251
        V2 = V2 * vs;
 
252
        V3 = V3 * vs;
 
253
        
 
254
        V1 = V1 + v1000;
 
255
        V2 = V2 + v0100;
 
256
        V3 = V3 + v0010;
 
257
        
 
258
        m_el[0] = V1; 
 
259
        m_el[1] = V2;
 
260
        m_el[2] = V3;
 
261
    #else    
158
262
                btScalar xs = q.x() * s,   ys = q.y() * s,   zs = q.z() * s;
159
263
                btScalar wx = q.w() * xs,  wy = q.w() * ys,  wz = q.w() * zs;
160
264
                btScalar xx = q.x() * xs,  xy = q.x() * ys,  xz = q.x() * zs;
161
265
                btScalar yy = q.y() * ys,  yz = q.y() * zs,  zz = q.z() * zs;
162
 
                setValue(btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
 
266
                setValue(
 
267
            btScalar(1.0) - (yy + zz), xy - wz, xz + wy,
163
268
                        xy + wz, btScalar(1.0) - (xx + zz), yz - wx,
164
269
                        xz - wy, yz + wx, btScalar(1.0) - (xx + yy));
165
 
        }
 
270
        #endif
 
271
    }
166
272
 
167
273
 
168
274
        /** @brief Set the matrix from euler angles using YPR around YXZ respectively
205
311
        /**@brief Set the matrix to the identity */
206
312
        void setIdentity()
207
313
        { 
 
314
#if (defined(BT_USE_SSE_IN_API)&& defined (BT_USE_SSE)) || defined(BT_USE_NEON)
 
315
                        m_el[0] = v1000; 
 
316
                        m_el[1] = v0100;
 
317
                        m_el[2] = v0010;
 
318
#else
208
319
                setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0), 
209
320
                        btScalar(0.0), btScalar(1.0), btScalar(0.0), 
210
321
                        btScalar(0.0), btScalar(0.0), btScalar(1.0)); 
 
322
#endif
211
323
        }
212
324
 
213
325
        static const btMatrix3x3&       getIdentity()
214
326
        {
215
 
                static const btMatrix3x3 identityMatrix(btScalar(1.0), btScalar(0.0), btScalar(0.0), 
 
327
#if (defined(BT_USE_SSE_IN_API)&& defined (BT_USE_SSE)) || defined(BT_USE_NEON)
 
328
        static const btMatrix3x3 
 
329
        identityMatrix(v1000, v0100, v0010);
 
330
#else
 
331
                static const btMatrix3x3 
 
332
        identityMatrix(
 
333
            btScalar(1.0), btScalar(0.0), btScalar(0.0), 
216
334
                        btScalar(0.0), btScalar(1.0), btScalar(0.0), 
217
335
                        btScalar(0.0), btScalar(0.0), btScalar(1.0));
 
336
#endif
218
337
                return identityMatrix;
219
338
        }
220
339
 
222
341
        * @param m The array to be filled */
223
342
        void getOpenGLSubMatrix(btScalar *m) const 
224
343
        {
 
344
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
 
345
        __m128 v0 = m_el[0].mVec128;
 
346
        __m128 v1 = m_el[1].mVec128;
 
347
        __m128 v2 = m_el[2].mVec128;    //  x2 y2 z2 w2
 
348
        __m128 *vm = (__m128 *)m;
 
349
        __m128 vT;
 
350
        
 
351
        v2 = _mm_and_ps(v2, btvFFF0fMask);  //  x2 y2 z2 0
 
352
        
 
353
        vT = _mm_unpackhi_ps(v0, v1);   //      z0 z1 * *
 
354
        v0 = _mm_unpacklo_ps(v0, v1);   //      x0 x1 y0 y1
 
355
 
 
356
        v1 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(2, 3, 1, 3) );   // y0 y1 y2 0
 
357
        v0 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(0, 1, 0, 3) );   // x0 x1 x2 0
 
358
        v2 = btCastdTo128f(_mm_move_sd(btCastfTo128d(v2), btCastfTo128d(vT)));  // z0 z1 z2 0
 
359
 
 
360
        vm[0] = v0;
 
361
        vm[1] = v1;
 
362
        vm[2] = v2;
 
363
#elif defined(BT_USE_NEON)
 
364
        // note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions.
 
365
        static const uint32x2_t zMask = (const uint32x2_t) {-1, 0 };
 
366
        float32x4_t *vm = (float32x4_t *)m;
 
367
        float32x4x2_t top = vtrnq_f32( m_el[0].mVec128, m_el[1].mVec128 );  // {x0 x1 z0 z1}, {y0 y1 w0 w1}
 
368
        float32x2x2_t bl = vtrn_f32( vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f) );       // {x2  0 }, {y2 0}
 
369
        float32x4_t v0 = vcombine_f32( vget_low_f32(top.val[0]), bl.val[0] );
 
370
        float32x4_t v1 = vcombine_f32( vget_low_f32(top.val[1]), bl.val[1] );
 
371
        float32x2_t q = (float32x2_t) vand_u32( (uint32x2_t) vget_high_f32( m_el[2].mVec128), zMask );
 
372
        float32x4_t v2 = vcombine_f32( vget_high_f32(top.val[0]), q );       // z0 z1 z2  0
 
373
 
 
374
        vm[0] = v0;
 
375
        vm[1] = v1;
 
376
        vm[2] = v2;
 
377
#else
225
378
                m[0]  = btScalar(m_el[0].x()); 
226
379
                m[1]  = btScalar(m_el[1].x());
227
380
                m[2]  = btScalar(m_el[2].x());
234
387
                m[9]  = btScalar(m_el[1].z());
235
388
                m[10] = btScalar(m_el[2].z());
236
389
                m[11] = btScalar(0.0); 
 
390
#endif
237
391
        }
238
392
 
239
393
        /**@brief Get the matrix represented as a quaternion 
240
394
        * @param q The quaternion which will be set */
241
395
        void getRotation(btQuaternion& q) const
242
396
        {
 
397
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
 
398
        btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
 
399
        btScalar s, x;
 
400
        
 
401
        union {
 
402
            btSimdFloat4 vec;
 
403
            btScalar f[4];
 
404
        } temp;
 
405
        
 
406
        if (trace > btScalar(0.0)) 
 
407
        {
 
408
            x = trace + btScalar(1.0);
 
409
 
 
410
            temp.f[0]=m_el[2].y() - m_el[1].z();
 
411
            temp.f[1]=m_el[0].z() - m_el[2].x();
 
412
            temp.f[2]=m_el[1].x() - m_el[0].y();
 
413
            temp.f[3]=x;
 
414
            //temp.f[3]= s * btScalar(0.5);
 
415
        } 
 
416
        else 
 
417
        {
 
418
            int i, j, k;
 
419
            if(m_el[0].x() < m_el[1].y()) 
 
420
            { 
 
421
                if( m_el[1].y() < m_el[2].z() )
 
422
                    { i = 2; j = 0; k = 1; }
 
423
                else
 
424
                    { i = 1; j = 2; k = 0; }
 
425
            }
 
426
            else
 
427
            {
 
428
                if( m_el[0].x() < m_el[2].z())
 
429
                    { i = 2; j = 0; k = 1; }
 
430
                else
 
431
                    { i = 0; j = 1; k = 2; }
 
432
            }
 
433
 
 
434
            x = m_el[i][i] - m_el[j][j] - m_el[k][k] + btScalar(1.0);
 
435
 
 
436
            temp.f[3] = (m_el[k][j] - m_el[j][k]);
 
437
            temp.f[j] = (m_el[j][i] + m_el[i][j]);
 
438
            temp.f[k] = (m_el[k][i] + m_el[i][k]);
 
439
            temp.f[i] = x;
 
440
            //temp.f[i] = s * btScalar(0.5);
 
441
        }
 
442
 
 
443
        s = btSqrt(x);
 
444
        q.set128(temp.vec);
 
445
        s = btScalar(0.5) / s;
 
446
 
 
447
        q *= s;
 
448
#else    
243
449
                btScalar trace = m_el[0].x() + m_el[1].y() + m_el[2].z();
 
450
 
244
451
                btScalar temp[4];
245
452
 
246
453
                if (trace > btScalar(0.0)) 
270
477
                        temp[k] = (m_el[k][i] + m_el[i][k]) * s;
271
478
                }
272
479
                q.setValue(temp[0],temp[1],temp[2],temp[3]);
 
480
#endif
273
481
        }
274
482
 
275
483
        /**@brief Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR
376
584
 
377
585
        btMatrix3x3 scaled(const btVector3& s) const
378
586
        {
379
 
                return btMatrix3x3(m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
 
587
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
 
588
                return btMatrix3x3(m_el[0] * s, m_el[1] * s, m_el[2] * s);
 
589
#else           
 
590
                return btMatrix3x3(
 
591
            m_el[0].x() * s.x(), m_el[0].y() * s.y(), m_el[0].z() * s.z(),
380
592
                        m_el[1].x() * s.x(), m_el[1].y() * s.y(), m_el[1].z() * s.z(),
381
593
                        m_el[2].x() * s.x(), m_el[2].y() * s.y(), m_el[2].z() * s.z());
 
594
#endif
382
595
        }
383
596
 
384
597
        /**@brief Return the determinant of the matrix */
527
740
SIMD_FORCE_INLINE btMatrix3x3& 
528
741
btMatrix3x3::operator*=(const btMatrix3x3& m)
529
742
{
530
 
        setValue(m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
 
743
#if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
 
744
    __m128 rv00, rv01, rv02;
 
745
    __m128 rv10, rv11, rv12;
 
746
    __m128 rv20, rv21, rv22;
 
747
    __m128 mv0, mv1, mv2;
 
748
 
 
749
    rv02 = m_el[0].mVec128;
 
750
    rv12 = m_el[1].mVec128;
 
751
    rv22 = m_el[2].mVec128;
 
752
 
 
753
    mv0 = _mm_and_ps(m[0].mVec128, btvFFF0fMask); 
 
754
    mv1 = _mm_and_ps(m[1].mVec128, btvFFF0fMask); 
 
755
    mv2 = _mm_and_ps(m[2].mVec128, btvFFF0fMask); 
 
756
    
 
757
    // rv0
 
758
    rv00 = bt_splat_ps(rv02, 0);
 
759
    rv01 = bt_splat_ps(rv02, 1);
 
760
    rv02 = bt_splat_ps(rv02, 2);
 
761
    
 
762
    rv00 = _mm_mul_ps(rv00, mv0);
 
763
    rv01 = _mm_mul_ps(rv01, mv1);
 
764
    rv02 = _mm_mul_ps(rv02, mv2);
 
765
    
 
766
    // rv1
 
767
    rv10 = bt_splat_ps(rv12, 0);
 
768
    rv11 = bt_splat_ps(rv12, 1);
 
769
    rv12 = bt_splat_ps(rv12, 2);
 
770
    
 
771
    rv10 = _mm_mul_ps(rv10, mv0);
 
772
    rv11 = _mm_mul_ps(rv11, mv1);
 
773
    rv12 = _mm_mul_ps(rv12, mv2);
 
774
    
 
775
    // rv2
 
776
    rv20 = bt_splat_ps(rv22, 0);
 
777
    rv21 = bt_splat_ps(rv22, 1);
 
778
    rv22 = bt_splat_ps(rv22, 2);
 
779
    
 
780
    rv20 = _mm_mul_ps(rv20, mv0);
 
781
    rv21 = _mm_mul_ps(rv21, mv1);
 
782
    rv22 = _mm_mul_ps(rv22, mv2);
 
783
 
 
784
    rv00 = _mm_add_ps(rv00, rv01);
 
785
    rv10 = _mm_add_ps(rv10, rv11);
 
786
    rv20 = _mm_add_ps(rv20, rv21);
 
787
 
 
788
    m_el[0].mVec128 = _mm_add_ps(rv00, rv02);
 
789
    m_el[1].mVec128 = _mm_add_ps(rv10, rv12);
 
790
    m_el[2].mVec128 = _mm_add_ps(rv20, rv22);
 
791
 
 
792
#elif defined(BT_USE_NEON)
 
793
 
 
794
    float32x4_t rv0, rv1, rv2;
 
795
    float32x4_t v0, v1, v2;
 
796
    float32x4_t mv0, mv1, mv2;
 
797
 
 
798
    v0 = m_el[0].mVec128;
 
799
    v1 = m_el[1].mVec128;
 
800
    v2 = m_el[2].mVec128;
 
801
 
 
802
    mv0 = (float32x4_t) vandq_s32((int32x4_t)m[0].mVec128, btvFFF0Mask); 
 
803
    mv1 = (float32x4_t) vandq_s32((int32x4_t)m[1].mVec128, btvFFF0Mask); 
 
804
    mv2 = (float32x4_t) vandq_s32((int32x4_t)m[2].mVec128, btvFFF0Mask); 
 
805
    
 
806
    rv0 = vmulq_lane_f32(mv0, vget_low_f32(v0), 0);
 
807
    rv1 = vmulq_lane_f32(mv0, vget_low_f32(v1), 0);
 
808
    rv2 = vmulq_lane_f32(mv0, vget_low_f32(v2), 0);
 
809
    
 
810
    rv0 = vmlaq_lane_f32(rv0, mv1, vget_low_f32(v0), 1);
 
811
    rv1 = vmlaq_lane_f32(rv1, mv1, vget_low_f32(v1), 1);
 
812
    rv2 = vmlaq_lane_f32(rv2, mv1, vget_low_f32(v2), 1);
 
813
    
 
814
    rv0 = vmlaq_lane_f32(rv0, mv2, vget_high_f32(v0), 0);
 
815
    rv1 = vmlaq_lane_f32(rv1, mv2, vget_high_f32(v1), 0);
 
816
    rv2 = vmlaq_lane_f32(rv2, mv2, vget_high_f32(v2), 0);
 
817
 
 
818
    m_el[0].mVec128 = rv0;
 
819
    m_el[1].mVec128 = rv1;
 
820
    m_el[2].mVec128 = rv2;
 
821
#else    
 
822
        setValue(
 
823
        m.tdotx(m_el[0]), m.tdoty(m_el[0]), m.tdotz(m_el[0]),
531
824
                m.tdotx(m_el[1]), m.tdoty(m_el[1]), m.tdotz(m_el[1]),
532
825
                m.tdotx(m_el[2]), m.tdoty(m_el[2]), m.tdotz(m_el[2]));
 
826
#endif
533
827
        return *this;
534
828
}
535
829
 
536
830
SIMD_FORCE_INLINE btMatrix3x3& 
537
831
btMatrix3x3::operator+=(const btMatrix3x3& m)
538
832
{
 
833
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
 
834
    m_el[0].mVec128 = m_el[0].mVec128 + m.m_el[0].mVec128;
 
835
    m_el[1].mVec128 = m_el[1].mVec128 + m.m_el[1].mVec128;
 
836
    m_el[2].mVec128 = m_el[2].mVec128 + m.m_el[2].mVec128;
 
837
#else
539
838
        setValue(
540
839
                m_el[0][0]+m.m_el[0][0], 
541
840
                m_el[0][1]+m.m_el[0][1],
546
845
                m_el[2][0]+m.m_el[2][0], 
547
846
                m_el[2][1]+m.m_el[2][1],
548
847
                m_el[2][2]+m.m_el[2][2]);
 
848
#endif
549
849
        return *this;
550
850
}
551
851
 
552
852
SIMD_FORCE_INLINE btMatrix3x3
553
853
operator*(const btMatrix3x3& m, const btScalar & k)
554
854
{
 
855
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
 
856
    __m128 vk = bt_splat_ps(_mm_load_ss((float *)&k), 0x80);
 
857
    return btMatrix3x3(
 
858
                _mm_mul_ps(m[0].mVec128, vk), 
 
859
                _mm_mul_ps(m[1].mVec128, vk), 
 
860
                _mm_mul_ps(m[2].mVec128, vk)); 
 
861
#elif defined(BT_USE_NEON)
 
862
    return btMatrix3x3(
 
863
                vmulq_n_f32(m[0].mVec128, k),
 
864
                vmulq_n_f32(m[1].mVec128, k),
 
865
                vmulq_n_f32(m[2].mVec128, k)); 
 
866
#else
555
867
        return btMatrix3x3(
556
868
                m[0].x()*k,m[0].y()*k,m[0].z()*k,
557
869
                m[1].x()*k,m[1].y()*k,m[1].z()*k,
558
870
                m[2].x()*k,m[2].y()*k,m[2].z()*k);
 
871
#endif
559
872
}
560
873
 
561
 
 SIMD_FORCE_INLINE btMatrix3x3 
 
874
SIMD_FORCE_INLINE btMatrix3x3 
562
875
operator+(const btMatrix3x3& m1, const btMatrix3x3& m2)
563
876
{
564
 
        return btMatrix3x3(
565
 
        m1[0][0]+m2[0][0], 
566
 
        m1[0][1]+m2[0][1],
567
 
        m1[0][2]+m2[0][2],
568
 
        m1[1][0]+m2[1][0], 
569
 
        m1[1][1]+m2[1][1],
570
 
        m1[1][2]+m2[1][2],
571
 
        m1[2][0]+m2[2][0], 
572
 
        m1[2][1]+m2[2][1],
573
 
        m1[2][2]+m2[2][2]);
 
877
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
 
878
        return btMatrix3x3(
 
879
        m1[0].mVec128 + m2[0].mVec128,
 
880
        m1[1].mVec128 + m2[1].mVec128,
 
881
        m1[2].mVec128 + m2[2].mVec128);
 
882
#else
 
883
        return btMatrix3x3(
 
884
        m1[0][0]+m2[0][0], 
 
885
        m1[0][1]+m2[0][1],
 
886
        m1[0][2]+m2[0][2],
 
887
        
 
888
        m1[1][0]+m2[1][0], 
 
889
        m1[1][1]+m2[1][1],
 
890
        m1[1][2]+m2[1][2],
 
891
        
 
892
        m1[2][0]+m2[2][0], 
 
893
        m1[2][1]+m2[2][1],
 
894
        m1[2][2]+m2[2][2]);
 
895
#endif    
574
896
}
575
897
 
576
898
SIMD_FORCE_INLINE btMatrix3x3 
577
899
operator-(const btMatrix3x3& m1, const btMatrix3x3& m2)
578
900
{
579
 
        return btMatrix3x3(
580
 
        m1[0][0]-m2[0][0], 
581
 
        m1[0][1]-m2[0][1],
582
 
        m1[0][2]-m2[0][2],
583
 
        m1[1][0]-m2[1][0], 
584
 
        m1[1][1]-m2[1][1],
585
 
        m1[1][2]-m2[1][2],
586
 
        m1[2][0]-m2[2][0], 
587
 
        m1[2][1]-m2[2][1],
588
 
        m1[2][2]-m2[2][2]);
 
901
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
 
902
        return btMatrix3x3(
 
903
        m1[0].mVec128 - m2[0].mVec128,
 
904
        m1[1].mVec128 - m2[1].mVec128,
 
905
        m1[2].mVec128 - m2[2].mVec128);
 
906
#else
 
907
        return btMatrix3x3(
 
908
        m1[0][0]-m2[0][0], 
 
909
        m1[0][1]-m2[0][1],
 
910
        m1[0][2]-m2[0][2],
 
911
        
 
912
        m1[1][0]-m2[1][0], 
 
913
        m1[1][1]-m2[1][1],
 
914
        m1[1][2]-m2[1][2],
 
915
        
 
916
        m1[2][0]-m2[2][0], 
 
917
        m1[2][1]-m2[2][1],
 
918
        m1[2][2]-m2[2][2]);
 
919
#endif
589
920
}
590
921
 
591
922
 
592
923
SIMD_FORCE_INLINE btMatrix3x3& 
593
924
btMatrix3x3::operator-=(const btMatrix3x3& m)
594
925
{
 
926
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
 
927
    m_el[0].mVec128 = m_el[0].mVec128 - m.m_el[0].mVec128;
 
928
    m_el[1].mVec128 = m_el[1].mVec128 - m.m_el[1].mVec128;
 
929
    m_el[2].mVec128 = m_el[2].mVec128 - m.m_el[2].mVec128;
 
930
#else
595
931
        setValue(
596
932
        m_el[0][0]-m.m_el[0][0], 
597
933
        m_el[0][1]-m.m_el[0][1],
602
938
        m_el[2][0]-m.m_el[2][0], 
603
939
        m_el[2][1]-m.m_el[2][1],
604
940
        m_el[2][2]-m.m_el[2][2]);
 
941
#endif
605
942
        return *this;
606
943
}
607
944
 
616
953
SIMD_FORCE_INLINE btMatrix3x3 
617
954
btMatrix3x3::absolute() const
618
955
{
 
956
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
 
957
    return btMatrix3x3(
 
958
            _mm_and_ps(m_el[0].mVec128, btvAbsfMask),
 
959
            _mm_and_ps(m_el[1].mVec128, btvAbsfMask),
 
960
            _mm_and_ps(m_el[2].mVec128, btvAbsfMask));
 
961
#elif defined(BT_USE_NEON)
 
962
    return btMatrix3x3(
 
963
            (float32x4_t)vandq_s32((int32x4_t)m_el[0].mVec128, btv3AbsMask),
 
964
            (float32x4_t)vandq_s32((int32x4_t)m_el[1].mVec128, btv3AbsMask),
 
965
            (float32x4_t)vandq_s32((int32x4_t)m_el[2].mVec128, btv3AbsMask));
 
966
#else   
619
967
        return btMatrix3x3(
620
 
                btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
621
 
                btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
622
 
                btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
 
968
            btFabs(m_el[0].x()), btFabs(m_el[0].y()), btFabs(m_el[0].z()),
 
969
            btFabs(m_el[1].x()), btFabs(m_el[1].y()), btFabs(m_el[1].z()),
 
970
            btFabs(m_el[2].x()), btFabs(m_el[2].y()), btFabs(m_el[2].z()));
 
971
#endif
623
972
}
624
973
 
625
974
SIMD_FORCE_INLINE btMatrix3x3 
626
975
btMatrix3x3::transpose() const 
627
976
{
628
 
        return btMatrix3x3(m_el[0].x(), m_el[1].x(), m_el[2].x(),
629
 
                m_el[0].y(), m_el[1].y(), m_el[2].y(),
630
 
                m_el[0].z(), m_el[1].z(), m_el[2].z());
 
977
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
 
978
    __m128 v0 = m_el[0].mVec128;
 
979
    __m128 v1 = m_el[1].mVec128;
 
980
    __m128 v2 = m_el[2].mVec128;    //  x2 y2 z2 w2
 
981
    __m128 vT;
 
982
    
 
983
    v2 = _mm_and_ps(v2, btvFFF0fMask);  //  x2 y2 z2 0
 
984
    
 
985
    vT = _mm_unpackhi_ps(v0, v1);       //      z0 z1 * *
 
986
    v0 = _mm_unpacklo_ps(v0, v1);       //      x0 x1 y0 y1
 
987
 
 
988
    v1 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(2, 3, 1, 3) );       // y0 y1 y2 0
 
989
    v0 = _mm_shuffle_ps(v0, v2, BT_SHUFFLE(0, 1, 0, 3) );       // x0 x1 x2 0
 
990
    v2 = btCastdTo128f(_mm_move_sd(btCastfTo128d(v2), btCastfTo128d(vT)));      // z0 z1 z2 0
 
991
 
 
992
 
 
993
    return btMatrix3x3( v0, v1, v2 );
 
994
#elif defined(BT_USE_NEON)
 
995
    // note: zeros the w channel. We can preserve it at the cost of two more vtrn instructions.
 
996
    static const uint32x2_t zMask = (const uint32x2_t) {-1, 0 };
 
997
    float32x4x2_t top = vtrnq_f32( m_el[0].mVec128, m_el[1].mVec128 );  // {x0 x1 z0 z1}, {y0 y1 w0 w1}
 
998
    float32x2x2_t bl = vtrn_f32( vget_low_f32(m_el[2].mVec128), vdup_n_f32(0.0f) );       // {x2  0 }, {y2 0}
 
999
    float32x4_t v0 = vcombine_f32( vget_low_f32(top.val[0]), bl.val[0] );
 
1000
    float32x4_t v1 = vcombine_f32( vget_low_f32(top.val[1]), bl.val[1] );
 
1001
    float32x2_t q = (float32x2_t) vand_u32( (uint32x2_t) vget_high_f32( m_el[2].mVec128), zMask );
 
1002
    float32x4_t v2 = vcombine_f32( vget_high_f32(top.val[0]), q );       // z0 z1 z2  0
 
1003
    return btMatrix3x3( v0, v1, v2 ); 
 
1004
#else
 
1005
        return btMatrix3x3( m_el[0].x(), m_el[1].x(), m_el[2].x(),
 
1006
                        m_el[0].y(), m_el[1].y(), m_el[2].y(),
 
1007
                        m_el[0].z(), m_el[1].z(), m_el[2].z());
 
1008
#endif
631
1009
}
632
1010
 
633
1011
SIMD_FORCE_INLINE btMatrix3x3 
653
1031
SIMD_FORCE_INLINE btMatrix3x3 
654
1032
btMatrix3x3::transposeTimes(const btMatrix3x3& m) const
655
1033
{
656
 
        return btMatrix3x3(
 
1034
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
 
1035
    // zeros w
 
1036
//    static const __m128i xyzMask = (const __m128i){ -1ULL, 0xffffffffULL };
 
1037
    __m128 row = m_el[0].mVec128;
 
1038
    __m128 m0 = _mm_and_ps( m.getRow(0).mVec128, btvFFF0fMask );
 
1039
    __m128 m1 = _mm_and_ps( m.getRow(1).mVec128, btvFFF0fMask);
 
1040
    __m128 m2 = _mm_and_ps( m.getRow(2).mVec128, btvFFF0fMask );
 
1041
    __m128 r0 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0));
 
1042
    __m128 r1 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0x55));
 
1043
    __m128 r2 = _mm_mul_ps(m0, _mm_shuffle_ps(row, row, 0xaa));
 
1044
    row = m_el[1].mVec128;
 
1045
    r0 = _mm_add_ps( r0, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0)));
 
1046
    r1 = _mm_add_ps( r1, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0x55)));
 
1047
    r2 = _mm_add_ps( r2, _mm_mul_ps(m1, _mm_shuffle_ps(row, row, 0xaa)));
 
1048
    row = m_el[2].mVec128;
 
1049
    r0 = _mm_add_ps( r0, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0)));
 
1050
    r1 = _mm_add_ps( r1, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0x55)));
 
1051
    r2 = _mm_add_ps( r2, _mm_mul_ps(m2, _mm_shuffle_ps(row, row, 0xaa)));
 
1052
    return btMatrix3x3( r0, r1, r2 );
 
1053
 
 
1054
#elif defined BT_USE_NEON
 
1055
    // zeros w
 
1056
    static const uint32x4_t xyzMask = (const uint32x4_t){ -1, -1, -1, 0 };
 
1057
    float32x4_t m0 = (float32x4_t) vandq_u32( (uint32x4_t) m.getRow(0).mVec128, xyzMask );
 
1058
    float32x4_t m1 = (float32x4_t) vandq_u32( (uint32x4_t) m.getRow(1).mVec128, xyzMask );
 
1059
    float32x4_t m2 = (float32x4_t) vandq_u32( (uint32x4_t) m.getRow(2).mVec128, xyzMask );
 
1060
    float32x4_t row = m_el[0].mVec128;
 
1061
    float32x4_t r0 = vmulq_lane_f32( m0, vget_low_f32(row), 0);
 
1062
    float32x4_t r1 = vmulq_lane_f32( m0, vget_low_f32(row), 1);
 
1063
    float32x4_t r2 = vmulq_lane_f32( m0, vget_high_f32(row), 0);
 
1064
    row = m_el[1].mVec128;
 
1065
    r0 = vmlaq_lane_f32( r0, m1, vget_low_f32(row), 0);
 
1066
    r1 = vmlaq_lane_f32( r1, m1, vget_low_f32(row), 1);
 
1067
    r2 = vmlaq_lane_f32( r2, m1, vget_high_f32(row), 0);
 
1068
    row = m_el[2].mVec128;
 
1069
    r0 = vmlaq_lane_f32( r0, m2, vget_low_f32(row), 0);
 
1070
    r1 = vmlaq_lane_f32( r1, m2, vget_low_f32(row), 1);
 
1071
    r2 = vmlaq_lane_f32( r2, m2, vget_high_f32(row), 0);
 
1072
    return btMatrix3x3( r0, r1, r2 );
 
1073
#else
 
1074
    return btMatrix3x3(
657
1075
                m_el[0].x() * m[0].x() + m_el[1].x() * m[1].x() + m_el[2].x() * m[2].x(),
658
1076
                m_el[0].x() * m[0].y() + m_el[1].x() * m[1].y() + m_el[2].x() * m[2].y(),
659
1077
                m_el[0].x() * m[0].z() + m_el[1].x() * m[1].z() + m_el[2].x() * m[2].z(),
663
1081
                m_el[0].z() * m[0].x() + m_el[1].z() * m[1].x() + m_el[2].z() * m[2].x(),
664
1082
                m_el[0].z() * m[0].y() + m_el[1].z() * m[1].y() + m_el[2].z() * m[2].y(),
665
1083
                m_el[0].z() * m[0].z() + m_el[1].z() * m[1].z() + m_el[2].z() * m[2].z());
 
1084
#endif
666
1085
}
667
1086
 
668
1087
SIMD_FORCE_INLINE btMatrix3x3 
669
1088
btMatrix3x3::timesTranspose(const btMatrix3x3& m) const
670
1089
{
 
1090
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
 
1091
    __m128 a0 = m_el[0].mVec128;
 
1092
    __m128 a1 = m_el[1].mVec128;
 
1093
    __m128 a2 = m_el[2].mVec128;
 
1094
    
 
1095
    btMatrix3x3 mT = m.transpose(); // we rely on transpose() zeroing w channel so that we don't have to do it here
 
1096
    __m128 mx = mT[0].mVec128;
 
1097
    __m128 my = mT[1].mVec128;
 
1098
    __m128 mz = mT[2].mVec128;
 
1099
    
 
1100
    __m128 r0 = _mm_mul_ps(mx, _mm_shuffle_ps(a0, a0, 0x00));
 
1101
    __m128 r1 = _mm_mul_ps(mx, _mm_shuffle_ps(a1, a1, 0x00));
 
1102
    __m128 r2 = _mm_mul_ps(mx, _mm_shuffle_ps(a2, a2, 0x00));
 
1103
    r0 = _mm_add_ps(r0, _mm_mul_ps(my, _mm_shuffle_ps(a0, a0, 0x55)));
 
1104
    r1 = _mm_add_ps(r1, _mm_mul_ps(my, _mm_shuffle_ps(a1, a1, 0x55)));
 
1105
    r2 = _mm_add_ps(r2, _mm_mul_ps(my, _mm_shuffle_ps(a2, a2, 0x55)));
 
1106
    r0 = _mm_add_ps(r0, _mm_mul_ps(mz, _mm_shuffle_ps(a0, a0, 0xaa)));
 
1107
    r1 = _mm_add_ps(r1, _mm_mul_ps(mz, _mm_shuffle_ps(a1, a1, 0xaa)));
 
1108
    r2 = _mm_add_ps(r2, _mm_mul_ps(mz, _mm_shuffle_ps(a2, a2, 0xaa)));
 
1109
    return btMatrix3x3( r0, r1, r2);
 
1110
            
 
1111
#elif defined BT_USE_NEON
 
1112
    float32x4_t a0 = m_el[0].mVec128;
 
1113
    float32x4_t a1 = m_el[1].mVec128;
 
1114
    float32x4_t a2 = m_el[2].mVec128;
 
1115
    
 
1116
    btMatrix3x3 mT = m.transpose(); // we rely on transpose() zeroing w channel so that we don't have to do it here
 
1117
    float32x4_t mx = mT[0].mVec128;
 
1118
    float32x4_t my = mT[1].mVec128;
 
1119
    float32x4_t mz = mT[2].mVec128;
 
1120
    
 
1121
    float32x4_t r0 = vmulq_lane_f32( mx, vget_low_f32(a0), 0);
 
1122
    float32x4_t r1 = vmulq_lane_f32( mx, vget_low_f32(a1), 0);
 
1123
    float32x4_t r2 = vmulq_lane_f32( mx, vget_low_f32(a2), 0);
 
1124
    r0 = vmlaq_lane_f32( r0, my, vget_low_f32(a0), 1);
 
1125
    r1 = vmlaq_lane_f32( r1, my, vget_low_f32(a1), 1);
 
1126
    r2 = vmlaq_lane_f32( r2, my, vget_low_f32(a2), 1);
 
1127
    r0 = vmlaq_lane_f32( r0, mz, vget_high_f32(a0), 0);
 
1128
    r1 = vmlaq_lane_f32( r1, mz, vget_high_f32(a1), 0);
 
1129
    r2 = vmlaq_lane_f32( r2, mz, vget_high_f32(a2), 0);
 
1130
    return btMatrix3x3( r0, r1, r2 );
 
1131
    
 
1132
#else
671
1133
        return btMatrix3x3(
672
1134
                m_el[0].dot(m[0]), m_el[0].dot(m[1]), m_el[0].dot(m[2]),
673
1135
                m_el[1].dot(m[0]), m_el[1].dot(m[1]), m_el[1].dot(m[2]),
674
1136
                m_el[2].dot(m[0]), m_el[2].dot(m[1]), m_el[2].dot(m[2]));
675
 
 
 
1137
#endif
676
1138
}
677
1139
 
678
1140
SIMD_FORCE_INLINE btVector3 
679
1141
operator*(const btMatrix3x3& m, const btVector3& v) 
680
1142
{
 
1143
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))|| defined (BT_USE_NEON)
 
1144
    return v.dot3(m[0], m[1], m[2]);
 
1145
#else
681
1146
        return btVector3(m[0].dot(v), m[1].dot(v), m[2].dot(v));
 
1147
#endif
682
1148
}
683
1149
 
684
1150
 
685
1151
SIMD_FORCE_INLINE btVector3
686
1152
operator*(const btVector3& v, const btMatrix3x3& m)
687
1153
{
 
1154
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
 
1155
 
 
1156
    const __m128 vv = v.mVec128;
 
1157
 
 
1158
    __m128 c0 = bt_splat_ps( vv, 0);
 
1159
    __m128 c1 = bt_splat_ps( vv, 1);
 
1160
    __m128 c2 = bt_splat_ps( vv, 2);
 
1161
 
 
1162
    c0 = _mm_mul_ps(c0, _mm_and_ps(m[0].mVec128, btvFFF0fMask) );
 
1163
    c1 = _mm_mul_ps(c1, _mm_and_ps(m[1].mVec128, btvFFF0fMask) );
 
1164
    c0 = _mm_add_ps(c0, c1);
 
1165
    c2 = _mm_mul_ps(c2, _mm_and_ps(m[2].mVec128, btvFFF0fMask) );
 
1166
    
 
1167
    return btVector3(_mm_add_ps(c0, c2));
 
1168
#elif defined(BT_USE_NEON)
 
1169
    const float32x4_t vv = v.mVec128;
 
1170
    const float32x2_t vlo = vget_low_f32(vv);
 
1171
    const float32x2_t vhi = vget_high_f32(vv);
 
1172
 
 
1173
    float32x4_t c0, c1, c2;
 
1174
 
 
1175
    c0 = (float32x4_t) vandq_s32((int32x4_t)m[0].mVec128, btvFFF0Mask);
 
1176
    c1 = (float32x4_t) vandq_s32((int32x4_t)m[1].mVec128, btvFFF0Mask);
 
1177
    c2 = (float32x4_t) vandq_s32((int32x4_t)m[2].mVec128, btvFFF0Mask);
 
1178
 
 
1179
    c0 = vmulq_lane_f32(c0, vlo, 0);
 
1180
    c1 = vmulq_lane_f32(c1, vlo, 1);
 
1181
    c2 = vmulq_lane_f32(c2, vhi, 0);
 
1182
    c0 = vaddq_f32(c0, c1);
 
1183
    c0 = vaddq_f32(c0, c2);
 
1184
    
 
1185
    return btVector3(c0);
 
1186
#else
688
1187
        return btVector3(m.tdotx(v), m.tdoty(v), m.tdotz(v));
 
1188
#endif
689
1189
}
690
1190
 
691
1191
SIMD_FORCE_INLINE btMatrix3x3 
692
1192
operator*(const btMatrix3x3& m1, const btMatrix3x3& m2)
693
1193
{
 
1194
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
 
1195
 
 
1196
    __m128 m10 = m1[0].mVec128;  
 
1197
    __m128 m11 = m1[1].mVec128;
 
1198
    __m128 m12 = m1[2].mVec128;
 
1199
    
 
1200
    __m128 m2v = _mm_and_ps(m2[0].mVec128, btvFFF0fMask);
 
1201
    
 
1202
    __m128 c0 = bt_splat_ps( m10, 0);
 
1203
    __m128 c1 = bt_splat_ps( m11, 0);
 
1204
    __m128 c2 = bt_splat_ps( m12, 0);
 
1205
    
 
1206
    c0 = _mm_mul_ps(c0, m2v);
 
1207
    c1 = _mm_mul_ps(c1, m2v);
 
1208
    c2 = _mm_mul_ps(c2, m2v);
 
1209
    
 
1210
    m2v = _mm_and_ps(m2[1].mVec128, btvFFF0fMask);
 
1211
    
 
1212
    __m128 c0_1 = bt_splat_ps( m10, 1);
 
1213
    __m128 c1_1 = bt_splat_ps( m11, 1);
 
1214
    __m128 c2_1 = bt_splat_ps( m12, 1);
 
1215
    
 
1216
    c0_1 = _mm_mul_ps(c0_1, m2v);
 
1217
    c1_1 = _mm_mul_ps(c1_1, m2v);
 
1218
    c2_1 = _mm_mul_ps(c2_1, m2v);
 
1219
    
 
1220
    m2v = _mm_and_ps(m2[2].mVec128, btvFFF0fMask);
 
1221
    
 
1222
    c0 = _mm_add_ps(c0, c0_1);
 
1223
    c1 = _mm_add_ps(c1, c1_1);
 
1224
    c2 = _mm_add_ps(c2, c2_1);
 
1225
    
 
1226
    m10 = bt_splat_ps( m10, 2);
 
1227
    m11 = bt_splat_ps( m11, 2);
 
1228
    m12 = bt_splat_ps( m12, 2);
 
1229
    
 
1230
    m10 = _mm_mul_ps(m10, m2v);
 
1231
    m11 = _mm_mul_ps(m11, m2v);
 
1232
    m12 = _mm_mul_ps(m12, m2v);
 
1233
    
 
1234
    c0 = _mm_add_ps(c0, m10);
 
1235
    c1 = _mm_add_ps(c1, m11);
 
1236
    c2 = _mm_add_ps(c2, m12);
 
1237
    
 
1238
    return btMatrix3x3(c0, c1, c2);
 
1239
 
 
1240
#elif defined(BT_USE_NEON)
 
1241
 
 
1242
    float32x4_t rv0, rv1, rv2;
 
1243
    float32x4_t v0, v1, v2;
 
1244
    float32x4_t mv0, mv1, mv2;
 
1245
 
 
1246
    v0 = m1[0].mVec128;
 
1247
    v1 = m1[1].mVec128;
 
1248
    v2 = m1[2].mVec128;
 
1249
 
 
1250
    mv0 = (float32x4_t) vandq_s32((int32x4_t)m2[0].mVec128, btvFFF0Mask); 
 
1251
    mv1 = (float32x4_t) vandq_s32((int32x4_t)m2[1].mVec128, btvFFF0Mask); 
 
1252
    mv2 = (float32x4_t) vandq_s32((int32x4_t)m2[2].mVec128, btvFFF0Mask); 
 
1253
    
 
1254
    rv0 = vmulq_lane_f32(mv0, vget_low_f32(v0), 0);
 
1255
    rv1 = vmulq_lane_f32(mv0, vget_low_f32(v1), 0);
 
1256
    rv2 = vmulq_lane_f32(mv0, vget_low_f32(v2), 0);
 
1257
    
 
1258
    rv0 = vmlaq_lane_f32(rv0, mv1, vget_low_f32(v0), 1);
 
1259
    rv1 = vmlaq_lane_f32(rv1, mv1, vget_low_f32(v1), 1);
 
1260
    rv2 = vmlaq_lane_f32(rv2, mv1, vget_low_f32(v2), 1);
 
1261
    
 
1262
    rv0 = vmlaq_lane_f32(rv0, mv2, vget_high_f32(v0), 0);
 
1263
    rv1 = vmlaq_lane_f32(rv1, mv2, vget_high_f32(v1), 0);
 
1264
    rv2 = vmlaq_lane_f32(rv2, mv2, vget_high_f32(v2), 0);
 
1265
 
 
1266
        return btMatrix3x3(rv0, rv1, rv2);
 
1267
        
 
1268
#else   
694
1269
        return btMatrix3x3(
695
1270
                m2.tdotx( m1[0]), m2.tdoty( m1[0]), m2.tdotz( m1[0]),
696
1271
                m2.tdotx( m1[1]), m2.tdoty( m1[1]), m2.tdotz( m1[1]),
697
1272
                m2.tdotx( m1[2]), m2.tdoty( m1[2]), m2.tdotz( m1[2]));
 
1273
#endif
698
1274
}
699
1275
 
700
1276
/*
716
1292
* It will test all elements are equal.  */
717
1293
SIMD_FORCE_INLINE bool operator==(const btMatrix3x3& m1, const btMatrix3x3& m2)
718
1294
{
719
 
        return ( m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
 
1295
#if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
 
1296
 
 
1297
    __m128 c0, c1, c2;
 
1298
 
 
1299
    c0 = _mm_cmpeq_ps(m1[0].mVec128, m2[0].mVec128);
 
1300
    c1 = _mm_cmpeq_ps(m1[1].mVec128, m2[1].mVec128);
 
1301
    c2 = _mm_cmpeq_ps(m1[2].mVec128, m2[2].mVec128);
 
1302
    
 
1303
    c0 = _mm_and_ps(c0, c1);
 
1304
    c0 = _mm_and_ps(c0, c2);
 
1305
 
 
1306
    return (0x7 == _mm_movemask_ps((__m128)c0));
 
1307
#else 
 
1308
        return 
 
1309
    (   m1[0][0] == m2[0][0] && m1[1][0] == m2[1][0] && m1[2][0] == m2[2][0] &&
720
1310
                m1[0][1] == m2[0][1] && m1[1][1] == m2[1][1] && m1[2][1] == m2[2][1] &&
721
1311
                m1[0][2] == m2[0][2] && m1[1][2] == m2[1][2] && m1[2][2] == m2[2][2] );
 
1312
#endif
722
1313
}
723
1314
 
724
1315
///for serialization