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