bullet: Update to current svn, r2636
[blender.git] / extern / bullet2 / src / LinearMath / btVector3.h
1 /*
2 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
3
4 This software is provided 'as-is', without any express or implied warranty.
5 In no event will the authors be held liable for any damages arising from the use of this software.
6 Permission is granted to anyone to use this software for any purpose, 
7 including commercial applications, and to alter it and redistribute it freely, 
8 subject to the following restrictions:
9
10 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
12 3. This notice may not be removed or altered from any source distribution.
13 */
14
15
16
17 #ifndef BT_VECTOR3_H
18 #define BT_VECTOR3_H
19
20 //#include <stdint.h>
21 #include "btScalar.h"
22 #include "btMinMax.h"
23 #include "btAlignedAllocator.h"
24
25 #ifdef BT_USE_DOUBLE_PRECISION
26 #define btVector3Data btVector3DoubleData
27 #define btVector3DataName "btVector3DoubleData"
28 #else
29 #define btVector3Data btVector3FloatData
30 #define btVector3DataName "btVector3FloatData"
31 #endif //BT_USE_DOUBLE_PRECISION
32
33 #if defined BT_USE_SSE
34
35 //typedef  uint32_t __m128i __attribute__ ((vector_size(16)));
36
37 #ifdef _MSC_VER
38 #pragma warning(disable: 4556) // value of intrinsic immediate argument '4294967239' is out of range '0 - 255'
39 #endif
40
41
42 #define BT_SHUFFLE(x,y,z,w) ((w)<<6 | (z)<<4 | (y)<<2 | (x))
43 //#define bt_pshufd_ps( _a, _mask ) (__m128) _mm_shuffle_epi32((__m128i)(_a), (_mask) )
44 #define bt_pshufd_ps( _a, _mask ) _mm_shuffle_ps((_a), (_a), (_mask) )
45 #define bt_splat3_ps( _a, _i ) bt_pshufd_ps((_a), BT_SHUFFLE(_i,_i,_i, 3) )
46 #define bt_splat_ps( _a, _i )  bt_pshufd_ps((_a), BT_SHUFFLE(_i,_i,_i,_i) )
47
48 #define btv3AbsiMask (_mm_set_epi32(0x00000000, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF))
49 #define btvAbsMask (_mm_set_epi32( 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF))
50 #define btvFFF0Mask (_mm_set_epi32(0x00000000, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF))
51 #define btv3AbsfMask btCastiTo128f(btv3AbsiMask)
52 #define btvFFF0fMask btCastiTo128f(btvFFF0Mask)
53 #define btvxyzMaskf btvFFF0fMask
54 #define btvAbsfMask btCastiTo128f(btvAbsMask)
55
56
57
58 const __m128 ATTRIBUTE_ALIGNED16(btvMzeroMask) = {-0.0f, -0.0f, -0.0f, -0.0f};
59 const __m128 ATTRIBUTE_ALIGNED16(v1110) = {1.0f, 1.0f, 1.0f, 0.0f};
60 const __m128 ATTRIBUTE_ALIGNED16(vHalf) = {0.5f, 0.5f, 0.5f, 0.5f};
61 const __m128 ATTRIBUTE_ALIGNED16(v1_5)  = {1.5f, 1.5f, 1.5f, 1.5f};
62
63 #endif
64
65 #ifdef BT_USE_NEON
66
67 const float32x4_t ATTRIBUTE_ALIGNED16(btvMzeroMask) = (float32x4_t){-0.0f, -0.0f, -0.0f, -0.0f};
68 const int32x4_t ATTRIBUTE_ALIGNED16(btvFFF0Mask) = (int32x4_t){0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0x0};
69 const int32x4_t ATTRIBUTE_ALIGNED16(btvAbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF};
70 const int32x4_t ATTRIBUTE_ALIGNED16(btv3AbsMask) = (int32x4_t){0x7FFFFFFF, 0x7FFFFFFF, 0x7FFFFFFF, 0x0};
71
72 #endif
73
74 /**@brief btVector3 can be used to represent 3D points and vectors.
75  * It has an un-used w component to suit 16-byte alignment when btVector3 is stored in containers. This extra component can be used by derived classes (Quaternion?) or by user
76  * Ideally, this class should be replaced by a platform optimized SIMD version that keeps the data in registers
77  */
78 ATTRIBUTE_ALIGNED16(class) btVector3
79 {
80 public:
81
82         BT_DECLARE_ALIGNED_ALLOCATOR();
83
84 #if defined (__SPU__) && defined (__CELLOS_LV2__)
85                 btScalar        m_floats[4];
86 public:
87         SIMD_FORCE_INLINE const vec_float4&     get128() const
88         {
89                 return *((const vec_float4*)&m_floats[0]);
90         }
91 public:
92 #else //__CELLOS_LV2__ __SPU__
93     #if defined (BT_USE_SSE) || defined(BT_USE_NEON) // _WIN32 || ARM
94         union {
95             btSimdFloat4      mVec128;
96             btScalar    m_floats[4];
97         };
98         SIMD_FORCE_INLINE       btSimdFloat4    get128() const
99         {
100             return mVec128;
101         }
102         SIMD_FORCE_INLINE       void    set128(btSimdFloat4 v128)
103         {
104             mVec128 = v128;
105         }
106     #else
107         btScalar        m_floats[4];
108     #endif
109 #endif //__CELLOS_LV2__ __SPU__
110
111         public:
112
113   /**@brief No initialization constructor */
114         SIMD_FORCE_INLINE btVector3() 
115         {
116
117         }
118
119  
120         
121   /**@brief Constructor from scalars 
122    * @param x X value
123    * @param y Y value 
124    * @param z Z value 
125    */
126         SIMD_FORCE_INLINE btVector3(const btScalar& _x, const btScalar& _y, const btScalar& _z)
127         {
128                 m_floats[0] = _x;
129                 m_floats[1] = _y;
130                 m_floats[2] = _z;
131                 m_floats[3] = btScalar(0.f);
132         }
133
134 #if (defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) )|| defined (BT_USE_NEON)
135         // Set Vector 
136         SIMD_FORCE_INLINE btVector3( btSimdFloat4 v)
137         {
138                 mVec128 = v;
139         }
140
141         // Copy constructor
142         SIMD_FORCE_INLINE btVector3(const btVector3& rhs)
143         {
144                 mVec128 = rhs.mVec128;
145         }
146
147         // Assignment Operator
148         SIMD_FORCE_INLINE btVector3& 
149         operator=(const btVector3& v) 
150         {
151                 mVec128 = v.mVec128;
152                 
153                 return *this;
154         }
155 #endif // #if defined (BT_USE_SSE_IN_API) || defined (BT_USE_NEON) 
156     
157 /**@brief Add a vector to this one 
158  * @param The vector to add to this one */
159         SIMD_FORCE_INLINE btVector3& operator+=(const btVector3& v)
160         {
161 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
162                 mVec128 = _mm_add_ps(mVec128, v.mVec128);
163 #elif defined(BT_USE_NEON)
164                 mVec128 = vaddq_f32(mVec128, v.mVec128);
165 #else
166                 m_floats[0] += v.m_floats[0]; 
167                 m_floats[1] += v.m_floats[1];
168                 m_floats[2] += v.m_floats[2];
169 #endif
170                 return *this;
171         }
172
173
174   /**@brief Subtract a vector from this one
175    * @param The vector to subtract */
176         SIMD_FORCE_INLINE btVector3& operator-=(const btVector3& v) 
177         {
178 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
179                 mVec128 = _mm_sub_ps(mVec128, v.mVec128);
180 #elif defined(BT_USE_NEON)
181                 mVec128 = vsubq_f32(mVec128, v.mVec128);
182 #else
183                 m_floats[0] -= v.m_floats[0]; 
184                 m_floats[1] -= v.m_floats[1];
185                 m_floats[2] -= v.m_floats[2];
186 #endif
187                 return *this;
188         }
189         
190   /**@brief Scale the vector
191    * @param s Scale factor */
192         SIMD_FORCE_INLINE btVector3& operator*=(const btScalar& s)
193         {
194 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
195                 __m128  vs = _mm_load_ss(&s);   //      (S 0 0 0)
196                 vs = bt_pshufd_ps(vs, 0x80);    //      (S S S 0.0)
197                 mVec128 = _mm_mul_ps(mVec128, vs);
198 #elif defined(BT_USE_NEON)
199                 mVec128 = vmulq_n_f32(mVec128, s);
200 #else
201                 m_floats[0] *= s; 
202                 m_floats[1] *= s;
203                 m_floats[2] *= s;
204 #endif
205                 return *this;
206         }
207
208   /**@brief Inversely scale the vector 
209    * @param s Scale factor to divide by */
210         SIMD_FORCE_INLINE btVector3& operator/=(const btScalar& s) 
211         {
212                 btFullAssert(s != btScalar(0.0));
213
214 #if 0 //defined(BT_USE_SSE_IN_API)
215 // this code is not faster !
216                 __m128 vs = _mm_load_ss(&s);
217                 vs = _mm_div_ss(v1110, vs);
218                 vs = bt_pshufd_ps(vs, 0x00);    //      (S S S S)
219
220                 mVec128 = _mm_mul_ps(mVec128, vs);
221                 
222                 return *this;
223 #else
224                 return *this *= btScalar(1.0) / s;
225 #endif
226         }
227
228   /**@brief Return the dot product
229    * @param v The other vector in the dot product */
230         SIMD_FORCE_INLINE btScalar dot(const btVector3& v) const
231         {
232 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
233                 __m128 vd = _mm_mul_ps(mVec128, v.mVec128);
234                 __m128 z = _mm_movehl_ps(vd, vd);
235                 __m128 y = _mm_shuffle_ps(vd, vd, 0x55);
236                 vd = _mm_add_ss(vd, y);
237                 vd = _mm_add_ss(vd, z);
238                 return _mm_cvtss_f32(vd);
239 #elif defined(BT_USE_NEON)
240                 float32x4_t vd = vmulq_f32(mVec128, v.mVec128);
241                 float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_low_f32(vd));  
242                 x = vadd_f32(x, vget_high_f32(vd));
243                 return vget_lane_f32(x, 0);
244 #else   
245                 return  m_floats[0] * v.m_floats[0] + 
246                                 m_floats[1] * v.m_floats[1] + 
247                                 m_floats[2] * v.m_floats[2];
248 #endif
249         }
250
251   /**@brief Return the length of the vector squared */
252         SIMD_FORCE_INLINE btScalar length2() const
253         {
254                 return dot(*this);
255         }
256
257   /**@brief Return the length of the vector */
258         SIMD_FORCE_INLINE btScalar length() const
259         {
260                 return btSqrt(length2());
261         }
262
263   /**@brief Return the distance squared between the ends of this and another vector
264    * This is symantically treating the vector like a point */
265         SIMD_FORCE_INLINE btScalar distance2(const btVector3& v) const;
266
267   /**@brief Return the distance between the ends of this and another vector
268    * This is symantically treating the vector like a point */
269         SIMD_FORCE_INLINE btScalar distance(const btVector3& v) const;
270
271         SIMD_FORCE_INLINE btVector3& safeNormalize() 
272         {
273                 btVector3 absVec = this->absolute();
274                 int maxIndex = absVec.maxAxis();
275                 if (absVec[maxIndex]>0)
276                 {
277                         *this /= absVec[maxIndex];
278                         return *this /= length();
279                 }
280                 setValue(1,0,0);
281                 return *this;
282         }
283
284   /**@brief Normalize this vector 
285    * x^2 + y^2 + z^2 = 1 */
286         SIMD_FORCE_INLINE btVector3& normalize() 
287         {
288 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)          
289         // dot product first
290                 __m128 vd = _mm_mul_ps(mVec128, mVec128);
291                 __m128 z = _mm_movehl_ps(vd, vd);
292                 __m128 y = _mm_shuffle_ps(vd, vd, 0x55);
293                 vd = _mm_add_ss(vd, y);
294                 vd = _mm_add_ss(vd, z);
295                 
296         #if 0
297         vd = _mm_sqrt_ss(vd);
298                 vd = _mm_div_ss(v1110, vd);
299                 vd = bt_splat_ps(vd, 0x80);
300                 mVec128 = _mm_mul_ps(mVec128, vd);
301         #else
302         
303         // NR step 1/sqrt(x) - vd is x, y is output 
304         y = _mm_rsqrt_ss(vd); // estimate 
305         
306         //  one step NR 
307         z = v1_5;
308         vd = _mm_mul_ss(vd, vHalf); // vd * 0.5 
309         //x2 = vd;
310         vd = _mm_mul_ss(vd, y); // vd * 0.5 * y0
311         vd = _mm_mul_ss(vd, y); // vd * 0.5 * y0 * y0
312         z = _mm_sub_ss(z, vd);  // 1.5 - vd * 0.5 * y0 * y0 
313
314         y = _mm_mul_ss(y, z);   // y0 * (1.5 - vd * 0.5 * y0 * y0)
315
316                 y = bt_splat_ps(y, 0x80);
317                 mVec128 = _mm_mul_ps(mVec128, y);
318
319         #endif
320
321                 
322                 return *this;
323 #else   
324                 return *this /= length();
325 #endif
326         }
327
328   /**@brief Return a normalized version of this vector */
329         SIMD_FORCE_INLINE btVector3 normalized() const;
330
331   /**@brief Return a rotated version of this vector
332    * @param wAxis The axis to rotate about 
333    * @param angle The angle to rotate by */
334         SIMD_FORCE_INLINE btVector3 rotate( const btVector3& wAxis, const btScalar angle ) const;
335
336   /**@brief Return the angle between this and another vector
337    * @param v The other vector */
338         SIMD_FORCE_INLINE btScalar angle(const btVector3& v) const 
339         {
340                 btScalar s = btSqrt(length2() * v.length2());
341                 btFullAssert(s != btScalar(0.0));
342                 return btAcos(dot(v) / s);
343         }
344         
345   /**@brief Return a vector will the absolute values of each element */
346         SIMD_FORCE_INLINE btVector3 absolute() const 
347         {
348
349 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE) 
350                 return btVector3(_mm_and_ps(mVec128, btv3AbsfMask));
351 #elif defined(BT_USE_NEON)
352                 return btVector3(vabsq_f32(mVec128));
353 #else   
354                 return btVector3(
355                         btFabs(m_floats[0]), 
356                         btFabs(m_floats[1]), 
357                         btFabs(m_floats[2]));
358 #endif
359         }
360         
361   /**@brief Return the cross product between this and another vector 
362    * @param v The other vector */
363         SIMD_FORCE_INLINE btVector3 cross(const btVector3& v) const
364         {
365 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
366                 __m128  T, V;
367                 
368                 T = bt_pshufd_ps(mVec128, BT_SHUFFLE(1, 2, 0, 3));      //      (Y Z X 0)
369                 V = bt_pshufd_ps(v.mVec128, BT_SHUFFLE(1, 2, 0, 3));    //      (Y Z X 0)
370                 
371                 V = _mm_mul_ps(V, mVec128);
372                 T = _mm_mul_ps(T, v.mVec128);
373                 V = _mm_sub_ps(V, T);
374                 
375                 V = bt_pshufd_ps(V, BT_SHUFFLE(1, 2, 0, 3));
376                 return btVector3(V);
377 #elif defined(BT_USE_NEON)
378                 float32x4_t T, V;
379                 // form (Y, Z, X, _) of mVec128 and v.mVec128
380                 float32x2_t Tlow = vget_low_f32(mVec128);
381                 float32x2_t Vlow = vget_low_f32(v.mVec128);
382                 T = vcombine_f32(vext_f32(Tlow, vget_high_f32(mVec128), 1), Tlow);
383                 V = vcombine_f32(vext_f32(Vlow, vget_high_f32(v.mVec128), 1), Vlow);
384                 
385                 V = vmulq_f32(V, mVec128);
386                 T = vmulq_f32(T, v.mVec128);
387                 V = vsubq_f32(V, T);
388                 Vlow = vget_low_f32(V);
389                 // form (Y, Z, X, _);
390                 V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow);
391                 V = (float32x4_t)vandq_s32((int32x4_t)V, btvFFF0Mask);
392                 
393                 return btVector3(V);
394 #else
395                 return btVector3(
396                         m_floats[1] * v.m_floats[2] - m_floats[2] * v.m_floats[1],
397                         m_floats[2] * v.m_floats[0] - m_floats[0] * v.m_floats[2],
398                         m_floats[0] * v.m_floats[1] - m_floats[1] * v.m_floats[0]);
399 #endif
400         }
401
402         SIMD_FORCE_INLINE btScalar triple(const btVector3& v1, const btVector3& v2) const
403         {
404 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
405                 // cross:
406                 __m128 T = _mm_shuffle_ps(v1.mVec128, v1.mVec128, BT_SHUFFLE(1, 2, 0, 3));      //      (Y Z X 0)
407                 __m128 V = _mm_shuffle_ps(v2.mVec128, v2.mVec128, BT_SHUFFLE(1, 2, 0, 3));      //      (Y Z X 0)
408                 
409                 V = _mm_mul_ps(V, v1.mVec128);
410                 T = _mm_mul_ps(T, v2.mVec128);
411                 V = _mm_sub_ps(V, T);
412                 
413                 V = _mm_shuffle_ps(V, V, BT_SHUFFLE(1, 2, 0, 3));
414
415                 // dot: 
416                 V = _mm_mul_ps(V, mVec128);
417                 __m128 z = _mm_movehl_ps(V, V);
418                 __m128 y = _mm_shuffle_ps(V, V, 0x55);
419                 V = _mm_add_ss(V, y);
420                 V = _mm_add_ss(V, z);
421                 return _mm_cvtss_f32(V);
422
423 #elif defined(BT_USE_NEON)
424                 // cross:
425                 float32x4_t T, V;
426                 // form (Y, Z, X, _) of mVec128 and v.mVec128
427                 float32x2_t Tlow = vget_low_f32(v1.mVec128);
428                 float32x2_t Vlow = vget_low_f32(v2.mVec128);
429                 T = vcombine_f32(vext_f32(Tlow, vget_high_f32(v1.mVec128), 1), Tlow);
430                 V = vcombine_f32(vext_f32(Vlow, vget_high_f32(v2.mVec128), 1), Vlow);
431                 
432                 V = vmulq_f32(V, v1.mVec128);
433                 T = vmulq_f32(T, v2.mVec128);
434                 V = vsubq_f32(V, T);
435                 Vlow = vget_low_f32(V);
436                 // form (Y, Z, X, _);
437                 V = vcombine_f32(vext_f32(Vlow, vget_high_f32(V), 1), Vlow);
438
439                 // dot: 
440                 V = vmulq_f32(mVec128, V);
441                 float32x2_t x = vpadd_f32(vget_low_f32(V), vget_low_f32(V));  
442                 x = vadd_f32(x, vget_high_f32(V));
443                 return vget_lane_f32(x, 0);
444 #else
445                 return 
446                         m_floats[0] * (v1.m_floats[1] * v2.m_floats[2] - v1.m_floats[2] * v2.m_floats[1]) + 
447                         m_floats[1] * (v1.m_floats[2] * v2.m_floats[0] - v1.m_floats[0] * v2.m_floats[2]) + 
448                         m_floats[2] * (v1.m_floats[0] * v2.m_floats[1] - v1.m_floats[1] * v2.m_floats[0]);
449 #endif
450         }
451
452   /**@brief Return the axis with the smallest value 
453    * Note return values are 0,1,2 for x, y, or z */
454         SIMD_FORCE_INLINE int minAxis() const
455         {
456                 return m_floats[0] < m_floats[1] ? (m_floats[0] <m_floats[2] ? 0 : 2) : (m_floats[1] <m_floats[2] ? 1 : 2);
457         }
458
459   /**@brief Return the axis with the largest value 
460    * Note return values are 0,1,2 for x, y, or z */
461         SIMD_FORCE_INLINE int maxAxis() const 
462         {
463                 return m_floats[0] < m_floats[1] ? (m_floats[1] <m_floats[2] ? 2 : 1) : (m_floats[0] <m_floats[2] ? 2 : 0);
464         }
465
466         SIMD_FORCE_INLINE int furthestAxis() const
467         {
468                 return absolute().minAxis();
469         }
470
471         SIMD_FORCE_INLINE int closestAxis() const 
472         {
473                 return absolute().maxAxis();
474         }
475
476         
477         SIMD_FORCE_INLINE void setInterpolate3(const btVector3& v0, const btVector3& v1, btScalar rt)
478         {
479 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
480                 __m128  vrt = _mm_load_ss(&rt); //      (rt 0 0 0)
481                 btScalar s = btScalar(1.0) - rt;
482                 __m128  vs = _mm_load_ss(&s);   //      (S 0 0 0)
483                 vs = bt_pshufd_ps(vs, 0x80);    //      (S S S 0.0)
484                 __m128 r0 = _mm_mul_ps(v0.mVec128, vs);
485                 vrt = bt_pshufd_ps(vrt, 0x80);  //      (rt rt rt 0.0)
486                 __m128 r1 = _mm_mul_ps(v1.mVec128, vrt);
487                 __m128 tmp3 = _mm_add_ps(r0,r1);
488                 mVec128 = tmp3;
489 #elif defined(BT_USE_NEON)
490                 mVec128 = vsubq_f32(v1.mVec128, v0.mVec128);
491                 mVec128 = vmulq_n_f32(mVec128, rt);
492                 mVec128 = vaddq_f32(mVec128, v0.mVec128);
493 #else   
494                 btScalar s = btScalar(1.0) - rt;
495                 m_floats[0] = s * v0.m_floats[0] + rt * v1.m_floats[0];
496                 m_floats[1] = s * v0.m_floats[1] + rt * v1.m_floats[1];
497                 m_floats[2] = s * v0.m_floats[2] + rt * v1.m_floats[2];
498                 //don't do the unused w component
499                 //              m_co[3] = s * v0[3] + rt * v1[3];
500 #endif
501         }
502
503   /**@brief Return the linear interpolation between this and another vector 
504    * @param v The other vector 
505    * @param t The ration of this to v (t = 0 => return this, t=1 => return other) */
506         SIMD_FORCE_INLINE btVector3 lerp(const btVector3& v, const btScalar& t) const 
507         {
508 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
509                 __m128  vt = _mm_load_ss(&t);   //      (t 0 0 0)
510                 vt = bt_pshufd_ps(vt, 0x80);    //      (rt rt rt 0.0)
511                 __m128 vl = _mm_sub_ps(v.mVec128, mVec128);
512                 vl = _mm_mul_ps(vl, vt);
513                 vl = _mm_add_ps(vl, mVec128);
514                 
515                 return btVector3(vl);
516 #elif defined(BT_USE_NEON)
517                 float32x4_t vl = vsubq_f32(v.mVec128, mVec128);
518                 vl = vmulq_n_f32(vl, t);
519                 vl = vaddq_f32(vl, mVec128);
520                 
521                 return btVector3(vl);
522 #else   
523                 return 
524                         btVector3(      m_floats[0] + (v.m_floats[0] - m_floats[0]) * t,
525                                                 m_floats[1] + (v.m_floats[1] - m_floats[1]) * t,
526                                                 m_floats[2] + (v.m_floats[2] - m_floats[2]) * t);
527 #endif
528         }
529
530   /**@brief Elementwise multiply this vector by the other 
531    * @param v The other vector */
532         SIMD_FORCE_INLINE btVector3& operator*=(const btVector3& v)
533         {
534 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
535                 mVec128 = _mm_mul_ps(mVec128, v.mVec128);
536 #elif defined(BT_USE_NEON)
537                 mVec128 = vmulq_f32(mVec128, v.mVec128);
538 #else   
539                 m_floats[0] *= v.m_floats[0]; 
540                 m_floats[1] *= v.m_floats[1];
541                 m_floats[2] *= v.m_floats[2];
542 #endif
543                 return *this;
544         }
545
546          /**@brief Return the x value */
547                 SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
548   /**@brief Return the y value */
549                 SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
550   /**@brief Return the z value */
551                 SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
552   /**@brief Set the x value */
553                 SIMD_FORCE_INLINE void  setX(btScalar _x) { m_floats[0] = _x;};
554   /**@brief Set the y value */
555                 SIMD_FORCE_INLINE void  setY(btScalar _y) { m_floats[1] = _y;};
556   /**@brief Set the z value */
557                 SIMD_FORCE_INLINE void  setZ(btScalar _z) { m_floats[2] = _z;};
558   /**@brief Set the w value */
559                 SIMD_FORCE_INLINE void  setW(btScalar _w) { m_floats[3] = _w;};
560   /**@brief Return the x value */
561                 SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
562   /**@brief Return the y value */
563                 SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
564   /**@brief Return the z value */
565                 SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
566   /**@brief Return the w value */
567                 SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
568
569         //SIMD_FORCE_INLINE btScalar&       operator[](int i)       { return (&m_floats[0])[i]; }      
570         //SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
571         ///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
572         SIMD_FORCE_INLINE       operator       btScalar *()       { return &m_floats[0]; }
573         SIMD_FORCE_INLINE       operator const btScalar *() const { return &m_floats[0]; }
574
575         SIMD_FORCE_INLINE       bool    operator==(const btVector3& other) const
576         {
577 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
578         return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
579 #else 
580                 return ((m_floats[3]==other.m_floats[3]) && 
581                 (m_floats[2]==other.m_floats[2]) && 
582                 (m_floats[1]==other.m_floats[1]) && 
583                 (m_floats[0]==other.m_floats[0]));
584 #endif
585         }
586
587         SIMD_FORCE_INLINE       bool    operator!=(const btVector3& other) const
588         {
589                 return !(*this == other);
590         }
591
592   /**@brief Set each element to the max of the current values and the values of another btVector3
593    * @param other The other btVector3 to compare with 
594    */
595         SIMD_FORCE_INLINE void  setMax(const btVector3& other)
596         {
597 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
598                 mVec128 = _mm_max_ps(mVec128, other.mVec128);
599 #elif defined(BT_USE_NEON)
600                 mVec128 = vmaxq_f32(mVec128, other.mVec128);
601 #else
602                 btSetMax(m_floats[0], other.m_floats[0]);
603                 btSetMax(m_floats[1], other.m_floats[1]);
604                 btSetMax(m_floats[2], other.m_floats[2]);
605                 btSetMax(m_floats[3], other.w());
606 #endif
607         }
608
609   /**@brief Set each element to the min of the current values and the values of another btVector3
610    * @param other The other btVector3 to compare with 
611    */
612         SIMD_FORCE_INLINE void  setMin(const btVector3& other)
613         {
614 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
615                 mVec128 = _mm_min_ps(mVec128, other.mVec128);
616 #elif defined(BT_USE_NEON)
617                 mVec128 = vminq_f32(mVec128, other.mVec128);
618 #else
619                 btSetMin(m_floats[0], other.m_floats[0]);
620                 btSetMin(m_floats[1], other.m_floats[1]);
621                 btSetMin(m_floats[2], other.m_floats[2]);
622                 btSetMin(m_floats[3], other.w());
623 #endif
624         }
625
626         SIMD_FORCE_INLINE void  setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z)
627         {
628                 m_floats[0]=_x;
629                 m_floats[1]=_y;
630                 m_floats[2]=_z;
631                 m_floats[3] = btScalar(0.f);
632         }
633
634         void    getSkewSymmetricMatrix(btVector3* v0,btVector3* v1,btVector3* v2) const
635         {
636 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
637  
638                 __m128 V  = _mm_and_ps(mVec128, btvFFF0fMask);
639                 __m128 V0 = _mm_xor_ps(btvMzeroMask, V);
640                 __m128 V2 = _mm_movelh_ps(V0, V);
641                 
642                 __m128 V1 = _mm_shuffle_ps(V, V0, 0xCE);
643                 
644         V0 = _mm_shuffle_ps(V0, V, 0xDB);
645                 V2 = _mm_shuffle_ps(V2, V, 0xF9);
646                 
647                 v0->mVec128 = V0;
648                 v1->mVec128 = V1;
649                 v2->mVec128 = V2;
650 #else
651                 v0->setValue(0.         ,-z()           ,y());
652                 v1->setValue(z()        ,0.                     ,-x());
653                 v2->setValue(-y()       ,x()    ,0.);
654 #endif
655         }
656
657         void setZero()
658         {
659 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
660                 mVec128 = (__m128)_mm_xor_ps(mVec128, mVec128);
661 #elif defined(BT_USE_NEON)
662                 int32x4_t vi = vdupq_n_s32(0); 
663                 mVec128 = vreinterpretq_f32_s32(vi);
664 #else   
665                 setValue(btScalar(0.),btScalar(0.),btScalar(0.));
666 #endif
667         }
668
669         SIMD_FORCE_INLINE bool isZero() const 
670         {
671                 return m_floats[0] == btScalar(0) && m_floats[1] == btScalar(0) && m_floats[2] == btScalar(0);
672         }
673
674         SIMD_FORCE_INLINE bool fuzzyZero() const 
675         {
676                 return length2() < SIMD_EPSILON;
677         }
678
679         SIMD_FORCE_INLINE       void    serialize(struct        btVector3Data& dataOut) const;
680
681         SIMD_FORCE_INLINE       void    deSerialize(const struct        btVector3Data& dataIn);
682
683         SIMD_FORCE_INLINE       void    serializeFloat(struct   btVector3FloatData& dataOut) const;
684
685         SIMD_FORCE_INLINE       void    deSerializeFloat(const struct   btVector3FloatData& dataIn);
686
687         SIMD_FORCE_INLINE       void    serializeDouble(struct  btVector3DoubleData& dataOut) const;
688
689         SIMD_FORCE_INLINE       void    deSerializeDouble(const struct  btVector3DoubleData& dataIn);
690     
691         /**@brief returns index of maximum dot product between this and vectors in array[]
692          * @param array The other vectors 
693          * @param array_count The number of other vectors 
694          * @param dotOut The maximum dot product */
695         SIMD_FORCE_INLINE   long    maxDot( const btVector3 *array, long array_count, btScalar &dotOut ) const; 
696
697         /**@brief returns index of minimum dot product between this and vectors in array[]
698          * @param array The other vectors 
699          * @param array_count The number of other vectors 
700          * @param dotOut The minimum dot product */    
701         SIMD_FORCE_INLINE   long    minDot( const btVector3 *array, long array_count, btScalar &dotOut ) const; 
702
703     /* create a vector as  btVector3( this->dot( btVector3 v0 ), this->dot( btVector3 v1), this->dot( btVector3 v2 ))  */
704     SIMD_FORCE_INLINE btVector3  dot3( const btVector3 &v0, const btVector3 &v1, const btVector3 &v2 ) const
705     {
706 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
707
708         __m128 a0 = _mm_mul_ps( v0.mVec128, this->mVec128 );
709         __m128 a1 = _mm_mul_ps( v1.mVec128, this->mVec128 );
710         __m128 a2 = _mm_mul_ps( v2.mVec128, this->mVec128 );
711         __m128 b0 = _mm_unpacklo_ps( a0, a1 );
712         __m128 b1 = _mm_unpackhi_ps( a0, a1 );
713         __m128 b2 = _mm_unpacklo_ps( a2, _mm_setzero_ps() );
714         __m128 r = _mm_movelh_ps( b0, b2 );
715         r = _mm_add_ps( r, _mm_movehl_ps( b2, b0 ));
716         a2 = _mm_and_ps( a2, btvxyzMaskf);
717         r = _mm_add_ps( r, btCastdTo128f (_mm_move_sd( btCastfTo128d(a2), btCastfTo128d(b1) )));
718         return btVector3(r);
719         
720 #elif defined(BT_USE_NEON)
721         static const uint32x4_t xyzMask = (const uint32x4_t){ -1, -1, -1, 0 };
722         float32x4_t a0 = vmulq_f32( v0.mVec128, this->mVec128);
723         float32x4_t a1 = vmulq_f32( v1.mVec128, this->mVec128);
724         float32x4_t a2 = vmulq_f32( v2.mVec128, this->mVec128);
725         float32x2x2_t zLo = vtrn_f32( vget_high_f32(a0), vget_high_f32(a1));
726         a2 = (float32x4_t) vandq_u32((uint32x4_t) a2, xyzMask );
727         float32x2_t b0 = vadd_f32( vpadd_f32( vget_low_f32(a0), vget_low_f32(a1)), zLo.val[0] );
728         float32x2_t b1 = vpadd_f32( vpadd_f32( vget_low_f32(a2), vget_high_f32(a2)), vdup_n_f32(0.0f));
729         return btVector3( vcombine_f32(b0, b1) );
730 #else   
731                 return btVector3( dot(v0), dot(v1), dot(v2));
732 #endif
733     }
734 };
735
736 /**@brief Return the sum of two vectors (Point symantics)*/
737 SIMD_FORCE_INLINE btVector3 
738 operator+(const btVector3& v1, const btVector3& v2) 
739 {
740 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
741         return btVector3(_mm_add_ps(v1.mVec128, v2.mVec128));
742 #elif defined(BT_USE_NEON)
743         return btVector3(vaddq_f32(v1.mVec128, v2.mVec128));
744 #else
745         return btVector3(
746                         v1.m_floats[0] + v2.m_floats[0], 
747                         v1.m_floats[1] + v2.m_floats[1], 
748                         v1.m_floats[2] + v2.m_floats[2]);
749 #endif
750 }
751
752 /**@brief Return the elementwise product of two vectors */
753 SIMD_FORCE_INLINE btVector3 
754 operator*(const btVector3& v1, const btVector3& v2) 
755 {
756 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
757         return btVector3(_mm_mul_ps(v1.mVec128, v2.mVec128));
758 #elif defined(BT_USE_NEON)
759         return btVector3(vmulq_f32(v1.mVec128, v2.mVec128));
760 #else
761         return btVector3(
762                         v1.m_floats[0] * v2.m_floats[0], 
763                         v1.m_floats[1] * v2.m_floats[1], 
764                         v1.m_floats[2] * v2.m_floats[2]);
765 #endif
766 }
767
768 /**@brief Return the difference between two vectors */
769 SIMD_FORCE_INLINE btVector3 
770 operator-(const btVector3& v1, const btVector3& v2)
771 {
772 #if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API)  && defined(BT_USE_SSE))
773
774         //      without _mm_and_ps this code causes slowdown in Concave moving
775         __m128 r = _mm_sub_ps(v1.mVec128, v2.mVec128);
776         return btVector3(_mm_and_ps(r, btvFFF0fMask));
777 #elif defined(BT_USE_NEON)
778         float32x4_t r = vsubq_f32(v1.mVec128, v2.mVec128);
779         return btVector3((float32x4_t)vandq_s32((int32x4_t)r, btvFFF0Mask));
780 #else
781         return btVector3(
782                         v1.m_floats[0] - v2.m_floats[0], 
783                         v1.m_floats[1] - v2.m_floats[1], 
784                         v1.m_floats[2] - v2.m_floats[2]);
785 #endif
786 }
787
788 /**@brief Return the negative of the vector */
789 SIMD_FORCE_INLINE btVector3 
790 operator-(const btVector3& v)
791 {
792 #if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE))
793         __m128 r = _mm_xor_ps(v.mVec128, btvMzeroMask);
794         return btVector3(_mm_and_ps(r, btvFFF0fMask)); 
795 #elif defined(BT_USE_NEON)
796         return btVector3((btSimdFloat4)veorq_s32((int32x4_t)v.mVec128, (int32x4_t)btvMzeroMask));
797 #else   
798         return btVector3(-v.m_floats[0], -v.m_floats[1], -v.m_floats[2]);
799 #endif
800 }
801
802 /**@brief Return the vector scaled by s */
803 SIMD_FORCE_INLINE btVector3 
804 operator*(const btVector3& v, const btScalar& s)
805 {
806 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
807         __m128  vs = _mm_load_ss(&s);   //      (S 0 0 0)
808         vs = bt_pshufd_ps(vs, 0x80);    //      (S S S 0.0)
809         return btVector3(_mm_mul_ps(v.mVec128, vs));
810 #elif defined(BT_USE_NEON)
811         float32x4_t r = vmulq_n_f32(v.mVec128, s);
812         return btVector3((float32x4_t)vandq_s32((int32x4_t)r, btvFFF0Mask));
813 #else
814         return btVector3(v.m_floats[0] * s, v.m_floats[1] * s, v.m_floats[2] * s);
815 #endif
816 }
817
818 /**@brief Return the vector scaled by s */
819 SIMD_FORCE_INLINE btVector3 
820 operator*(const btScalar& s, const btVector3& v)
821
822         return v * s; 
823 }
824
825 /**@brief Return the vector inversely scaled by s */
826 SIMD_FORCE_INLINE btVector3
827 operator/(const btVector3& v, const btScalar& s)
828 {
829         btFullAssert(s != btScalar(0.0));
830 #if 0 //defined(BT_USE_SSE_IN_API)
831 // this code is not faster !
832         __m128 vs = _mm_load_ss(&s);
833     vs = _mm_div_ss(v1110, vs);
834         vs = bt_pshufd_ps(vs, 0x00);    //      (S S S S)
835
836         return btVector3(_mm_mul_ps(v.mVec128, vs));
837 #else
838         return v * (btScalar(1.0) / s);
839 #endif
840 }
841
842 /**@brief Return the vector inversely scaled by s */
843 SIMD_FORCE_INLINE btVector3
844 operator/(const btVector3& v1, const btVector3& v2)
845 {
846 #if defined BT_USE_SIMD_VECTOR3 && (defined(BT_USE_SSE_IN_API)&& defined (BT_USE_SSE))
847         __m128 vec = _mm_div_ps(v1.mVec128, v2.mVec128);
848         vec = _mm_and_ps(vec, btvFFF0fMask);
849         return btVector3(vec); 
850 #elif defined(BT_USE_NEON)
851         float32x4_t x, y, v, m;
852
853         x = v1.mVec128;
854         y = v2.mVec128;
855         
856         v = vrecpeq_f32(y);                     // v ~ 1/y
857         m = vrecpsq_f32(y, v);          // m = (2-v*y)
858         v = vmulq_f32(v, m);            // vv = v*m ~~ 1/y
859         m = vrecpsq_f32(y, v);          // mm = (2-vv*y)
860         v = vmulq_f32(v, x);            // x*vv
861         v = vmulq_f32(v, m);            // (x*vv)*(2-vv*y) = x*(vv(2-vv*y)) ~~~ x/y
862
863         return btVector3(v);
864 #else
865         return btVector3(
866                         v1.m_floats[0] / v2.m_floats[0], 
867                         v1.m_floats[1] / v2.m_floats[1],
868                         v1.m_floats[2] / v2.m_floats[2]);
869 #endif
870 }
871
872 /**@brief Return the dot product between two vectors */
873 SIMD_FORCE_INLINE btScalar 
874 btDot(const btVector3& v1, const btVector3& v2) 
875
876         return v1.dot(v2); 
877 }
878
879
880 /**@brief Return the distance squared between two vectors */
881 SIMD_FORCE_INLINE btScalar
882 btDistance2(const btVector3& v1, const btVector3& v2) 
883
884         return v1.distance2(v2); 
885 }
886
887
888 /**@brief Return the distance between two vectors */
889 SIMD_FORCE_INLINE btScalar
890 btDistance(const btVector3& v1, const btVector3& v2) 
891
892         return v1.distance(v2); 
893 }
894
895 /**@brief Return the angle between two vectors */
896 SIMD_FORCE_INLINE btScalar
897 btAngle(const btVector3& v1, const btVector3& v2) 
898
899         return v1.angle(v2); 
900 }
901
902 /**@brief Return the cross product of two vectors */
903 SIMD_FORCE_INLINE btVector3 
904 btCross(const btVector3& v1, const btVector3& v2) 
905
906         return v1.cross(v2); 
907 }
908
909 SIMD_FORCE_INLINE btScalar
910 btTriple(const btVector3& v1, const btVector3& v2, const btVector3& v3)
911 {
912         return v1.triple(v2, v3);
913 }
914
915 /**@brief Return the linear interpolation between two vectors
916  * @param v1 One vector 
917  * @param v2 The other vector 
918  * @param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */
919 SIMD_FORCE_INLINE btVector3 
920 lerp(const btVector3& v1, const btVector3& v2, const btScalar& t)
921 {
922         return v1.lerp(v2, t);
923 }
924
925
926
927 SIMD_FORCE_INLINE btScalar btVector3::distance2(const btVector3& v) const
928 {
929         return (v - *this).length2();
930 }
931
932 SIMD_FORCE_INLINE btScalar btVector3::distance(const btVector3& v) const
933 {
934         return (v - *this).length();
935 }
936
937 SIMD_FORCE_INLINE btVector3 btVector3::normalized() const
938 {
939 #if defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
940         btVector3 norm = *this;
941
942         return norm.normalize();
943 #else
944         return *this / length();
945 #endif
946
947
948 SIMD_FORCE_INLINE btVector3 btVector3::rotate( const btVector3& wAxis, const btScalar _angle ) const
949 {
950         // wAxis must be a unit lenght vector
951
952 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
953
954     __m128 O = _mm_mul_ps(wAxis.mVec128, mVec128);
955         btScalar ssin = btSin( _angle );
956     __m128 C = wAxis.cross( mVec128 ).mVec128;
957         O = _mm_and_ps(O, btvFFF0fMask);
958     btScalar scos = btCos( _angle );
959         
960         __m128 vsin = _mm_load_ss(&ssin);       //      (S 0 0 0)
961     __m128 vcos = _mm_load_ss(&scos);   //      (S 0 0 0)
962         
963         __m128 Y = bt_pshufd_ps(O, 0xC9);       //      (Y Z X 0)
964         __m128 Z = bt_pshufd_ps(O, 0xD2);       //      (Z X Y 0)
965         O = _mm_add_ps(O, Y);
966         vsin = bt_pshufd_ps(vsin, 0x80);        //      (S S S 0)
967         O = _mm_add_ps(O, Z);
968     vcos = bt_pshufd_ps(vcos, 0x80);    //      (S S S 0)
969         
970     vsin = vsin * C; 
971         O = O * wAxis.mVec128; 
972         __m128 X = mVec128 - O; 
973         
974     O = O + vsin;
975         vcos = vcos * X;
976         O = O + vcos;   
977         
978         return btVector3(O);
979 #else
980         btVector3 o = wAxis * wAxis.dot( *this );
981         btVector3 _x = *this - o;
982         btVector3 _y;
983
984         _y = wAxis.cross( *this );
985
986         return ( o + _x * btCos( _angle ) + _y * btSin( _angle ) );
987 #endif
988 }
989
990 SIMD_FORCE_INLINE   long    btVector3::maxDot( const btVector3 *array, long array_count, btScalar &dotOut ) const
991 {
992 #if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
993     #if defined _WIN32 || defined (BT_USE_SSE)
994         const long scalar_cutoff = 10;
995         long _maxdot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut );
996     #elif defined BT_USE_NEON
997         const long scalar_cutoff = 4;
998         extern long (*_maxdot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut );
999     #endif
1000     if( array_count < scalar_cutoff )   
1001 #endif
1002     {
1003         btScalar maxDot = -SIMD_INFINITY;
1004         int i = 0;
1005         int ptIndex = -1;
1006         for( i = 0; i < array_count; i++ )
1007         {
1008             btScalar dot = array[i].dot(*this);
1009             
1010             if( dot > maxDot )
1011             {
1012                 maxDot = dot;
1013                 ptIndex = i;
1014             }
1015         }
1016         
1017         dotOut = maxDot;
1018         return ptIndex;
1019     }
1020 #if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
1021     return _maxdot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut );
1022 #endif
1023 }
1024
1025 SIMD_FORCE_INLINE   long    btVector3::minDot( const btVector3 *array, long array_count, btScalar &dotOut ) const
1026 {
1027 #if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
1028     #if defined BT_USE_SSE
1029         const long scalar_cutoff = 10;
1030         long _mindot_large( const float *array, const float *vec, unsigned long array_count, float *dotOut );
1031     #elif defined BT_USE_NEON
1032         const long scalar_cutoff = 4;
1033         extern long (*_mindot_large)( const float *array, const float *vec, unsigned long array_count, float *dotOut );
1034     #else
1035         #error unhandled arch!
1036     #endif
1037     
1038     if( array_count < scalar_cutoff )
1039 #endif
1040     {
1041         btScalar  minDot = SIMD_INFINITY;
1042         int i = 0;
1043         int ptIndex = -1;
1044         
1045         for( i = 0; i < array_count; i++ )
1046         {
1047             btScalar dot = array[i].dot(*this);
1048             
1049             if( dot < minDot )
1050             {
1051                 minDot = dot;
1052                 ptIndex = i;
1053             }
1054         }
1055         
1056         dotOut = minDot;
1057         
1058         return ptIndex;
1059     }
1060 #if (defined BT_USE_SSE && defined BT_USE_SIMD_VECTOR3 && defined BT_USE_SSE_IN_API) || defined (BT_USE_NEON)
1061     return _mindot_large( (float*) array, (float*) &m_floats[0], array_count, &dotOut );
1062 #endif//BT_USE_SIMD_VECTOR3
1063 }
1064
1065
1066 class btVector4 : public btVector3
1067 {
1068 public:
1069
1070         SIMD_FORCE_INLINE btVector4() {}
1071
1072
1073         SIMD_FORCE_INLINE btVector4(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w) 
1074                 : btVector3(_x,_y,_z)
1075         {
1076                 m_floats[3] = _w;
1077         }
1078
1079 #if (defined (BT_USE_SSE_IN_API)&& defined (BT_USE_SSE)) || defined (BT_USE_NEON) 
1080         SIMD_FORCE_INLINE btVector4(const btSimdFloat4 vec)
1081         {
1082                 mVec128 = vec;
1083         }
1084
1085         SIMD_FORCE_INLINE btVector4(const btVector3& rhs)
1086         {
1087                 mVec128 = rhs.mVec128;
1088         }
1089
1090         SIMD_FORCE_INLINE btVector4& 
1091         operator=(const btVector4& v) 
1092         {
1093                 mVec128 = v.mVec128;
1094                 return *this;
1095         }
1096 #endif // #if defined (BT_USE_SSE_IN_API) || defined (BT_USE_NEON) 
1097
1098         SIMD_FORCE_INLINE btVector4 absolute4() const 
1099         {
1100 #if defined BT_USE_SIMD_VECTOR3 && defined(BT_USE_SSE_IN_API) && defined (BT_USE_SSE) 
1101                 return btVector4(_mm_and_ps(mVec128, btvAbsfMask));
1102 #elif defined(BT_USE_NEON)
1103                 return btVector4(vabsq_f32(mVec128));
1104 #else   
1105                 return btVector4(
1106                         btFabs(m_floats[0]), 
1107                         btFabs(m_floats[1]), 
1108                         btFabs(m_floats[2]),
1109                         btFabs(m_floats[3]));
1110 #endif
1111         }
1112
1113
1114         btScalar        getW() const { return m_floats[3];}
1115
1116
1117                 SIMD_FORCE_INLINE int maxAxis4() const
1118         {
1119                 int maxIndex = -1;
1120                 btScalar maxVal = btScalar(-BT_LARGE_FLOAT);
1121                 if (m_floats[0] > maxVal)
1122                 {
1123                         maxIndex = 0;
1124                         maxVal = m_floats[0];
1125                 }
1126                 if (m_floats[1] > maxVal)
1127                 {
1128                         maxIndex = 1;
1129                         maxVal = m_floats[1];
1130                 }
1131                 if (m_floats[2] > maxVal)
1132                 {
1133                         maxIndex = 2;
1134                         maxVal =m_floats[2];
1135                 }
1136                 if (m_floats[3] > maxVal)
1137                 {
1138                         maxIndex = 3;
1139                         maxVal = m_floats[3];
1140                 }
1141
1142                 return maxIndex;
1143         }
1144
1145
1146         SIMD_FORCE_INLINE int minAxis4() const
1147         {
1148                 int minIndex = -1;
1149                 btScalar minVal = btScalar(BT_LARGE_FLOAT);
1150                 if (m_floats[0] < minVal)
1151                 {
1152                         minIndex = 0;
1153                         minVal = m_floats[0];
1154                 }
1155                 if (m_floats[1] < minVal)
1156                 {
1157                         minIndex = 1;
1158                         minVal = m_floats[1];
1159                 }
1160                 if (m_floats[2] < minVal)
1161                 {
1162                         minIndex = 2;
1163                         minVal =m_floats[2];
1164                 }
1165                 if (m_floats[3] < minVal)
1166                 {
1167                         minIndex = 3;
1168                         minVal = m_floats[3];
1169                 }
1170                 
1171                 return minIndex;
1172         }
1173
1174
1175         SIMD_FORCE_INLINE int closestAxis4() const 
1176         {
1177                 return absolute4().maxAxis4();
1178         }
1179
1180         
1181  
1182
1183   /**@brief Set x,y,z and zero w 
1184    * @param x Value of x
1185    * @param y Value of y
1186    * @param z Value of z
1187    */
1188                 
1189
1190 /*              void getValue(btScalar *m) const 
1191                 {
1192                         m[0] = m_floats[0];
1193                         m[1] = m_floats[1];
1194                         m[2] =m_floats[2];
1195                 }
1196 */
1197 /**@brief Set the values 
1198    * @param x Value of x
1199    * @param y Value of y
1200    * @param z Value of z
1201    * @param w Value of w
1202    */
1203                 SIMD_FORCE_INLINE void  setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w)
1204                 {
1205                         m_floats[0]=_x;
1206                         m_floats[1]=_y;
1207                         m_floats[2]=_z;
1208                         m_floats[3]=_w;
1209                 }
1210
1211
1212 };
1213
1214
1215 ///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
1216 SIMD_FORCE_INLINE void  btSwapScalarEndian(const btScalar& sourceVal, btScalar& destVal)
1217 {
1218         #ifdef BT_USE_DOUBLE_PRECISION
1219         unsigned char* dest = (unsigned char*) &destVal;
1220         unsigned char* src  = (unsigned char*) &sourceVal;
1221         dest[0] = src[7];
1222     dest[1] = src[6];
1223     dest[2] = src[5];
1224     dest[3] = src[4];
1225     dest[4] = src[3];
1226     dest[5] = src[2];
1227     dest[6] = src[1];
1228     dest[7] = src[0];
1229 #else
1230         unsigned char* dest = (unsigned char*) &destVal;
1231         unsigned char* src  = (unsigned char*) &sourceVal;
1232         dest[0] = src[3];
1233     dest[1] = src[2];
1234     dest[2] = src[1];
1235     dest[3] = src[0];
1236 #endif //BT_USE_DOUBLE_PRECISION
1237 }
1238 ///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
1239 SIMD_FORCE_INLINE void  btSwapVector3Endian(const btVector3& sourceVec, btVector3& destVec)
1240 {
1241         for (int i=0;i<4;i++)
1242         {
1243                 btSwapScalarEndian(sourceVec[i],destVec[i]);
1244         }
1245
1246 }
1247
1248 ///btUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
1249 SIMD_FORCE_INLINE void  btUnSwapVector3Endian(btVector3& vector)
1250 {
1251
1252         btVector3       swappedVec;
1253         for (int i=0;i<4;i++)
1254         {
1255                 btSwapScalarEndian(vector[i],swappedVec[i]);
1256         }
1257         vector = swappedVec;
1258 }
1259
1260 template <class T>
1261 SIMD_FORCE_INLINE void btPlaneSpace1 (const T& n, T& p, T& q)
1262 {
1263   if (btFabs(n[2]) > SIMDSQRT12) {
1264     // choose p in y-z plane
1265     btScalar a = n[1]*n[1] + n[2]*n[2];
1266     btScalar k = btRecipSqrt (a);
1267     p[0] = 0;
1268         p[1] = -n[2]*k;
1269         p[2] = n[1]*k;
1270     // set q = n x p
1271     q[0] = a*k;
1272         q[1] = -n[0]*p[2];
1273         q[2] = n[0]*p[1];
1274   }
1275   else {
1276     // choose p in x-y plane
1277     btScalar a = n[0]*n[0] + n[1]*n[1];
1278     btScalar k = btRecipSqrt (a);
1279     p[0] = -n[1]*k;
1280         p[1] = n[0]*k;
1281         p[2] = 0;
1282     // set q = n x p
1283     q[0] = -n[2]*p[1];
1284         q[1] = n[2]*p[0];
1285         q[2] = a*k;
1286   }
1287 }
1288
1289
1290 struct  btVector3FloatData
1291 {
1292         float   m_floats[4];
1293 };
1294
1295 struct  btVector3DoubleData
1296 {
1297         double  m_floats[4];
1298
1299 };
1300
1301 SIMD_FORCE_INLINE       void    btVector3::serializeFloat(struct        btVector3FloatData& dataOut) const
1302 {
1303         ///could also do a memcpy, check if it is worth it
1304         for (int i=0;i<4;i++)
1305                 dataOut.m_floats[i] = float(m_floats[i]);
1306 }
1307
1308 SIMD_FORCE_INLINE void  btVector3::deSerializeFloat(const struct        btVector3FloatData& dataIn)
1309 {
1310         for (int i=0;i<4;i++)
1311                 m_floats[i] = btScalar(dataIn.m_floats[i]);
1312 }
1313
1314
1315 SIMD_FORCE_INLINE       void    btVector3::serializeDouble(struct       btVector3DoubleData& dataOut) const
1316 {
1317         ///could also do a memcpy, check if it is worth it
1318         for (int i=0;i<4;i++)
1319                 dataOut.m_floats[i] = double(m_floats[i]);
1320 }
1321
1322 SIMD_FORCE_INLINE void  btVector3::deSerializeDouble(const struct       btVector3DoubleData& dataIn)
1323 {
1324         for (int i=0;i<4;i++)
1325                 m_floats[i] = btScalar(dataIn.m_floats[i]);
1326 }
1327
1328
1329 SIMD_FORCE_INLINE       void    btVector3::serialize(struct     btVector3Data& dataOut) const
1330 {
1331         ///could also do a memcpy, check if it is worth it
1332         for (int i=0;i<4;i++)
1333                 dataOut.m_floats[i] = m_floats[i];
1334 }
1335
1336 SIMD_FORCE_INLINE void  btVector3::deSerialize(const struct     btVector3Data& dataIn)
1337 {
1338         for (int i=0;i<4;i++)
1339                 m_floats[i] = dataIn.m_floats[i];
1340 }
1341
1342 #endif //BT_VECTOR3_H