Upgrade Bullet to version 2.83.
[blender.git] / extern / bullet2 / src / LinearMath / btQuaternion.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_SIMD__QUATERNION_H_
18 #define BT_SIMD__QUATERNION_H_
19
20
21 #include "btVector3.h"
22 #include "btQuadWord.h"
23
24
25 #ifdef BT_USE_DOUBLE_PRECISION
26 #define btQuaternionData btQuaternionDoubleData
27 #define btQuaternionDataName "btQuaternionDoubleData"
28 #else
29 #define btQuaternionData btQuaternionFloatData
30 #define btQuaternionDataName "btQuaternionFloatData"
31 #endif //BT_USE_DOUBLE_PRECISION
32
33
34
35 #ifdef BT_USE_SSE
36
37 //const __m128 ATTRIBUTE_ALIGNED16(vOnes) = {1.0f, 1.0f, 1.0f, 1.0f};
38 #define vOnes (_mm_set_ps(1.0f, 1.0f, 1.0f, 1.0f))
39
40 #endif
41
42 #if defined(BT_USE_SSE) 
43
44 #define vQInv (_mm_set_ps(+0.0f, -0.0f, -0.0f, -0.0f))
45 #define vPPPM (_mm_set_ps(-0.0f, +0.0f, +0.0f, +0.0f))
46
47 #elif defined(BT_USE_NEON)
48
49 const btSimdFloat4 ATTRIBUTE_ALIGNED16(vQInv) = {-0.0f, -0.0f, -0.0f, +0.0f};
50 const btSimdFloat4 ATTRIBUTE_ALIGNED16(vPPPM) = {+0.0f, +0.0f, +0.0f, -0.0f};
51
52 #endif
53
54 /**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */
55 class btQuaternion : public btQuadWord {
56 public:
57   /**@brief No initialization constructor */
58         btQuaternion() {}
59
60 #if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE))|| defined(BT_USE_NEON) 
61         // Set Vector 
62         SIMD_FORCE_INLINE btQuaternion(const btSimdFloat4 vec)
63         {
64                 mVec128 = vec;
65         }
66
67         // Copy constructor
68         SIMD_FORCE_INLINE btQuaternion(const btQuaternion& rhs)
69         {
70                 mVec128 = rhs.mVec128;
71         }
72
73         // Assignment Operator
74         SIMD_FORCE_INLINE btQuaternion& 
75         operator=(const btQuaternion& v) 
76         {
77                 mVec128 = v.mVec128;
78                 
79                 return *this;
80         }
81         
82 #endif
83
84         //              template <typename btScalar>
85         //              explicit Quaternion(const btScalar *v) : Tuple4<btScalar>(v) {}
86   /**@brief Constructor from scalars */
87         btQuaternion(const btScalar& _x, const btScalar& _y, const btScalar& _z, const btScalar& _w) 
88                 : btQuadWord(_x, _y, _z, _w) 
89         {}
90   /**@brief Axis angle Constructor
91    * @param axis The axis which the rotation is around
92    * @param angle The magnitude of the rotation around the angle (Radians) */
93         btQuaternion(const btVector3& _axis, const btScalar& _angle) 
94         { 
95                 setRotation(_axis, _angle); 
96         }
97   /**@brief Constructor from Euler angles
98    * @param yaw Angle around Y unless BT_EULER_DEFAULT_ZYX defined then Z
99    * @param pitch Angle around X unless BT_EULER_DEFAULT_ZYX defined then Y
100    * @param roll Angle around Z unless BT_EULER_DEFAULT_ZYX defined then X */
101         btQuaternion(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
102         { 
103 #ifndef BT_EULER_DEFAULT_ZYX
104                 setEuler(yaw, pitch, roll); 
105 #else
106                 setEulerZYX(yaw, pitch, roll); 
107 #endif 
108         }
109   /**@brief Set the rotation using axis angle notation 
110    * @param axis The axis around which to rotate
111    * @param angle The magnitude of the rotation in Radians */
112         void setRotation(const btVector3& axis, const btScalar& _angle)
113         {
114                 btScalar d = axis.length();
115                 btAssert(d != btScalar(0.0));
116                 btScalar s = btSin(_angle * btScalar(0.5)) / d;
117                 setValue(axis.x() * s, axis.y() * s, axis.z() * s, 
118                         btCos(_angle * btScalar(0.5)));
119         }
120   /**@brief Set the quaternion using Euler angles
121    * @param yaw Angle around Y
122    * @param pitch Angle around X
123    * @param roll Angle around Z */
124         void setEuler(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
125         {
126                 btScalar halfYaw = btScalar(yaw) * btScalar(0.5);  
127                 btScalar halfPitch = btScalar(pitch) * btScalar(0.5);  
128                 btScalar halfRoll = btScalar(roll) * btScalar(0.5);  
129                 btScalar cosYaw = btCos(halfYaw);
130                 btScalar sinYaw = btSin(halfYaw);
131                 btScalar cosPitch = btCos(halfPitch);
132                 btScalar sinPitch = btSin(halfPitch);
133                 btScalar cosRoll = btCos(halfRoll);
134                 btScalar sinRoll = btSin(halfRoll);
135                 setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
136                         cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
137                         sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
138                         cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
139         }
140   /**@brief Set the quaternion using euler angles 
141    * @param yaw Angle around Z
142    * @param pitch Angle around Y
143    * @param roll Angle around X */
144         void setEulerZYX(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
145         {
146                 btScalar halfYaw = btScalar(yaw) * btScalar(0.5);  
147                 btScalar halfPitch = btScalar(pitch) * btScalar(0.5);  
148                 btScalar halfRoll = btScalar(roll) * btScalar(0.5);  
149                 btScalar cosYaw = btCos(halfYaw);
150                 btScalar sinYaw = btSin(halfYaw);
151                 btScalar cosPitch = btCos(halfPitch);
152                 btScalar sinPitch = btSin(halfPitch);
153                 btScalar cosRoll = btCos(halfRoll);
154                 btScalar sinRoll = btSin(halfRoll);
155                 setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
156                          cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
157                          cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
158                          cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
159         }
160   /**@brief Add two quaternions
161    * @param q The quaternion to add to this one */
162         SIMD_FORCE_INLINE       btQuaternion& operator+=(const btQuaternion& q)
163         {
164 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
165                 mVec128 = _mm_add_ps(mVec128, q.mVec128);
166 #elif defined(BT_USE_NEON)
167                 mVec128 = vaddq_f32(mVec128, q.mVec128);
168 #else   
169                 m_floats[0] += q.x(); 
170         m_floats[1] += q.y(); 
171         m_floats[2] += q.z(); 
172         m_floats[3] += q.m_floats[3];
173 #endif
174                 return *this;
175         }
176
177   /**@brief Subtract out a quaternion
178    * @param q The quaternion to subtract from this one */
179         btQuaternion& operator-=(const btQuaternion& q) 
180         {
181 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
182                 mVec128 = _mm_sub_ps(mVec128, q.mVec128);
183 #elif defined(BT_USE_NEON)
184                 mVec128 = vsubq_f32(mVec128, q.mVec128);
185 #else   
186                 m_floats[0] -= q.x(); 
187         m_floats[1] -= q.y(); 
188         m_floats[2] -= q.z(); 
189         m_floats[3] -= q.m_floats[3];
190 #endif
191         return *this;
192         }
193
194   /**@brief Scale this quaternion
195    * @param s The scalar to scale by */
196         btQuaternion& operator*=(const btScalar& s)
197         {
198 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
199                 __m128  vs = _mm_load_ss(&s);   //      (S 0 0 0)
200                 vs = bt_pshufd_ps(vs, 0);       //      (S S S S)
201                 mVec128 = _mm_mul_ps(mVec128, vs);
202 #elif defined(BT_USE_NEON)
203                 mVec128 = vmulq_n_f32(mVec128, s);
204 #else
205                 m_floats[0] *= s; 
206         m_floats[1] *= s; 
207         m_floats[2] *= s; 
208         m_floats[3] *= s;
209 #endif
210                 return *this;
211         }
212
213   /**@brief Multiply this quaternion by q on the right
214    * @param q The other quaternion 
215    * Equivilant to this = this * q */
216         btQuaternion& operator*=(const btQuaternion& q)
217         {
218 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
219                 __m128 vQ2 = q.get128();
220                 
221                 __m128 A1 = bt_pshufd_ps(mVec128, BT_SHUFFLE(0,1,2,0));
222                 __m128 B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3,3,3,0));
223                 
224                 A1 = A1 * B1;
225                 
226                 __m128 A2 = bt_pshufd_ps(mVec128, BT_SHUFFLE(1,2,0,1));
227                 __m128 B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1));
228                 
229                 A2 = A2 * B2;
230                 
231                 B1 = bt_pshufd_ps(mVec128, BT_SHUFFLE(2,0,1,2));
232                 B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2));
233                 
234                 B1 = B1 * B2;   //      A3 *= B3
235                 
236                 mVec128 = bt_splat_ps(mVec128, 3);      //      A0
237                 mVec128 = mVec128 * vQ2;        //      A0 * B0
238                 
239                 A1 = A1 + A2;   //      AB12
240                 mVec128 = mVec128 - B1; //      AB03 = AB0 - AB3 
241                 A1 = _mm_xor_ps(A1, vPPPM);     //      change sign of the last element
242                 mVec128 = mVec128+ A1;  //      AB03 + AB12
243
244 #elif defined(BT_USE_NEON)     
245
246         float32x4_t vQ1 = mVec128;
247         float32x4_t vQ2 = q.get128();
248         float32x4_t A0, A1, B1, A2, B2, A3, B3;
249         float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
250         
251         {
252         float32x2x2_t tmp;
253         tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) );       // {z x}, {w y}
254         vQ1zx = tmp.val[0];
255
256         tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) );       // {z x}, {w y}
257         vQ2zx = tmp.val[0];
258         }
259         vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1); 
260
261         vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
262
263         vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
264         vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
265
266         A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx);                    // X Y  z x 
267         B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W  W X 
268
269         A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
270         B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
271
272         A3 = vcombine_f32(vQ1zx, vQ1yz);        // Z X Y Z
273         B3 = vcombine_f32(vQ2yz, vQ2xz);        // Y Z x z
274
275         A1 = vmulq_f32(A1, B1);
276         A2 = vmulq_f32(A2, B2);
277         A3 = vmulq_f32(A3, B3); //      A3 *= B3
278         A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1); //     A0 * B0
279
280         A1 = vaddq_f32(A1, A2); //      AB12 = AB1 + AB2
281         A0 = vsubq_f32(A0, A3); //      AB03 = AB0 - AB3 
282         
283         //      change the sign of the last element
284         A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);  
285         A0 = vaddq_f32(A0, A1); //      AB03 + AB12
286         
287         mVec128 = A0;
288 #else
289                 setValue(
290             m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
291                         m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
292                         m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
293                         m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
294 #endif
295                 return *this;
296         }
297   /**@brief Return the dot product between this quaternion and another
298    * @param q The other quaternion */
299         btScalar dot(const btQuaternion& q) const
300         {
301 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
302                 __m128  vd;
303                 
304                 vd = _mm_mul_ps(mVec128, q.mVec128);
305                 
306         __m128 t = _mm_movehl_ps(vd, vd);
307                 vd = _mm_add_ps(vd, t);
308                 t = _mm_shuffle_ps(vd, vd, 0x55);
309                 vd = _mm_add_ss(vd, t);
310                 
311         return _mm_cvtss_f32(vd);
312 #elif defined(BT_USE_NEON)
313                 float32x4_t vd = vmulq_f32(mVec128, q.mVec128);
314                 float32x2_t x = vpadd_f32(vget_low_f32(vd), vget_high_f32(vd));  
315                 x = vpadd_f32(x, x);
316                 return vget_lane_f32(x, 0);
317 #else    
318                 return  m_floats[0] * q.x() + 
319                 m_floats[1] * q.y() + 
320                 m_floats[2] * q.z() + 
321                 m_floats[3] * q.m_floats[3];
322 #endif
323         }
324
325   /**@brief Return the length squared of the quaternion */
326         btScalar length2() const
327         {
328                 return dot(*this);
329         }
330
331   /**@brief Return the length of the quaternion */
332         btScalar length() const
333         {
334                 return btSqrt(length2());
335         }
336
337   /**@brief Normalize the quaternion 
338    * Such that x^2 + y^2 + z^2 +w^2 = 1 */
339         btQuaternion& normalize() 
340         {
341 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
342                 __m128  vd;
343                 
344                 vd = _mm_mul_ps(mVec128, mVec128);
345                 
346         __m128 t = _mm_movehl_ps(vd, vd);
347                 vd = _mm_add_ps(vd, t);
348                 t = _mm_shuffle_ps(vd, vd, 0x55);
349                 vd = _mm_add_ss(vd, t);
350
351                 vd = _mm_sqrt_ss(vd);
352                 vd = _mm_div_ss(vOnes, vd);
353         vd = bt_pshufd_ps(vd, 0); // splat
354                 mVec128 = _mm_mul_ps(mVec128, vd);
355     
356                 return *this;
357 #else    
358                 return *this /= length();
359 #endif
360         }
361
362   /**@brief Return a scaled version of this quaternion
363    * @param s The scale factor */
364         SIMD_FORCE_INLINE btQuaternion
365         operator*(const btScalar& s) const
366         {
367 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
368                 __m128  vs = _mm_load_ss(&s);   //      (S 0 0 0)
369                 vs = bt_pshufd_ps(vs, 0x00);    //      (S S S S)
370                 
371                 return btQuaternion(_mm_mul_ps(mVec128, vs));
372 #elif defined(BT_USE_NEON)
373                 return btQuaternion(vmulq_n_f32(mVec128, s));
374 #else
375                 return btQuaternion(x() * s, y() * s, z() * s, m_floats[3] * s);
376 #endif
377         }
378
379   /**@brief Return an inversely scaled versionof this quaternion
380    * @param s The inverse scale factor */
381         btQuaternion operator/(const btScalar& s) const
382         {
383                 btAssert(s != btScalar(0.0));
384                 return *this * (btScalar(1.0) / s);
385         }
386
387   /**@brief Inversely scale this quaternion
388    * @param s The scale factor */
389         btQuaternion& operator/=(const btScalar& s) 
390         {
391                 btAssert(s != btScalar(0.0));
392                 return *this *= btScalar(1.0) / s;
393         }
394
395   /**@brief Return a normalized version of this quaternion */
396         btQuaternion normalized() const 
397         {
398                 return *this / length();
399         } 
400         /**@brief Return the ***half*** angle between this quaternion and the other
401    * @param q The other quaternion */
402         btScalar angle(const btQuaternion& q) const 
403         {
404                 btScalar s = btSqrt(length2() * q.length2());
405                 btAssert(s != btScalar(0.0));
406                 return btAcos(dot(q) / s);
407         }
408         
409         /**@brief Return the angle between this quaternion and the other along the shortest path
410         * @param q The other quaternion */
411         btScalar angleShortestPath(const btQuaternion& q) const 
412         {
413                 btScalar s = btSqrt(length2() * q.length2());
414                 btAssert(s != btScalar(0.0));
415                 if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
416                         return btAcos(dot(-q) / s) * btScalar(2.0);
417                 else 
418                         return btAcos(dot(q) / s) * btScalar(2.0);
419         }
420
421   /**@brief Return the angle of rotation represented by this quaternion */
422         btScalar getAngle() const 
423         {
424                 btScalar s = btScalar(2.) * btAcos(m_floats[3]);
425                 return s;
426         }
427
428         /**@brief Return the angle of rotation represented by this quaternion along the shortest path*/
429         btScalar getAngleShortestPath() const 
430         {
431                 btScalar s;
432                 if (dot(*this) < 0)
433                         s = btScalar(2.) * btAcos(m_floats[3]);
434                 else
435                         s = btScalar(2.) * btAcos(-m_floats[3]);
436
437                 return s;
438         }
439
440
441         /**@brief Return the axis of the rotation represented by this quaternion */
442         btVector3 getAxis() const
443         {
444                 btScalar s_squared = 1.f-m_floats[3]*m_floats[3];
445                 
446                 if (s_squared < btScalar(10.) * SIMD_EPSILON) //Check for divide by zero
447                         return btVector3(1.0, 0.0, 0.0);  // Arbitrary
448                 btScalar s = 1.f/btSqrt(s_squared);
449                 return btVector3(m_floats[0] * s, m_floats[1] * s, m_floats[2] * s);
450         }
451
452         /**@brief Return the inverse of this quaternion */
453         btQuaternion inverse() const
454         {
455 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
456                 return btQuaternion(_mm_xor_ps(mVec128, vQInv));
457 #elif defined(BT_USE_NEON)
458         return btQuaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)vQInv));
459 #else   
460                 return btQuaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
461 #endif
462         }
463
464   /**@brief Return the sum of this quaternion and the other 
465    * @param q2 The other quaternion */
466         SIMD_FORCE_INLINE btQuaternion
467         operator+(const btQuaternion& q2) const
468         {
469 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
470                 return btQuaternion(_mm_add_ps(mVec128, q2.mVec128));
471 #elif defined(BT_USE_NEON)
472         return btQuaternion(vaddq_f32(mVec128, q2.mVec128));
473 #else   
474                 const btQuaternion& q1 = *this;
475                 return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]);
476 #endif
477         }
478
479   /**@brief Return the difference between this quaternion and the other 
480    * @param q2 The other quaternion */
481         SIMD_FORCE_INLINE btQuaternion
482         operator-(const btQuaternion& q2) const
483         {
484 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
485                 return btQuaternion(_mm_sub_ps(mVec128, q2.mVec128));
486 #elif defined(BT_USE_NEON)
487         return btQuaternion(vsubq_f32(mVec128, q2.mVec128));
488 #else   
489                 const btQuaternion& q1 = *this;
490                 return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]);
491 #endif
492         }
493
494   /**@brief Return the negative of this quaternion 
495    * This simply negates each element */
496         SIMD_FORCE_INLINE btQuaternion operator-() const
497         {
498 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
499                 return btQuaternion(_mm_xor_ps(mVec128, btvMzeroMask));
500 #elif defined(BT_USE_NEON)
501                 return btQuaternion((btSimdFloat4)veorq_s32((int32x4_t)mVec128, (int32x4_t)btvMzeroMask) );
502 #else   
503                 const btQuaternion& q2 = *this;
504                 return btQuaternion( - q2.x(), - q2.y(),  - q2.z(),  - q2.m_floats[3]);
505 #endif
506         }
507   /**@todo document this and it's use */
508         SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const 
509         {
510                 btQuaternion diff,sum;
511                 diff = *this - qd;
512                 sum = *this + qd;
513                 if( diff.dot(diff) > sum.dot(sum) )
514                         return qd;
515                 return (-qd);
516         }
517
518         /**@todo document this and it's use */
519         SIMD_FORCE_INLINE btQuaternion nearest( const btQuaternion& qd) const 
520         {
521                 btQuaternion diff,sum;
522                 diff = *this - qd;
523                 sum = *this + qd;
524                 if( diff.dot(diff) < sum.dot(sum) )
525                         return qd;
526                 return (-qd);
527         }
528
529
530   /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
531    * @param q The other quaternion to interpolate with 
532    * @param t The ratio between this and q to interpolate.  If t = 0 the result is this, if t=1 the result is q.
533    * Slerp interpolates assuming constant velocity.  */
534         btQuaternion slerp(const btQuaternion& q, const btScalar& t) const
535         {
536           btScalar magnitude = btSqrt(length2() * q.length2()); 
537           btAssert(magnitude > btScalar(0));
538
539     btScalar product = dot(q) / magnitude;
540     if (btFabs(product) < btScalar(1))
541                 {
542       // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
543       const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1);
544
545       const btScalar theta = btAcos(sign * product);
546       const btScalar s1 = btSin(sign * t * theta);   
547       const btScalar d = btScalar(1.0) / btSin(theta);
548       const btScalar s0 = btSin((btScalar(1.0) - t) * theta);
549
550       return btQuaternion(
551           (m_floats[0] * s0 + q.x() * s1) * d,
552           (m_floats[1] * s0 + q.y() * s1) * d,
553           (m_floats[2] * s0 + q.z() * s1) * d,
554           (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
555                 }
556                 else
557                 {
558                         return *this;
559                 }
560         }
561
562         static const btQuaternion&      getIdentity()
563         {
564                 static const btQuaternion identityQuat(btScalar(0.),btScalar(0.),btScalar(0.),btScalar(1.));
565                 return identityQuat;
566         }
567
568         SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; }
569
570         SIMD_FORCE_INLINE       void    serialize(struct        btQuaternionData& dataOut) const;
571
572         SIMD_FORCE_INLINE       void    deSerialize(const struct        btQuaternionData& dataIn);
573
574         SIMD_FORCE_INLINE       void    serializeFloat(struct   btQuaternionFloatData& dataOut) const;
575
576         SIMD_FORCE_INLINE       void    deSerializeFloat(const struct   btQuaternionFloatData& dataIn);
577
578         SIMD_FORCE_INLINE       void    serializeDouble(struct  btQuaternionDoubleData& dataOut) const;
579
580         SIMD_FORCE_INLINE       void    deSerializeDouble(const struct  btQuaternionDoubleData& dataIn);
581
582 };
583
584
585
586
587
588 /**@brief Return the product of two quaternions */
589 SIMD_FORCE_INLINE btQuaternion
590 operator*(const btQuaternion& q1, const btQuaternion& q2) 
591 {
592 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
593         __m128 vQ1 = q1.get128();
594         __m128 vQ2 = q2.get128();
595         __m128 A0, A1, B1, A2, B2;
596     
597         A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(0,1,2,0)); // X Y  z x     //      vtrn
598         B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3,3,3,0)); // W W  W X     // vdup vext
599
600         A1 = A1 * B1;
601         
602         A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1,2,0,1)); // Y Z  X Y     // vext 
603         B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1)); // z x  Y Y     // vtrn vdup
604
605         A2 = A2 * B2;
606
607         B1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2,0,1,2)); // z x Y Z      // vtrn vext
608         B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2)); // Y Z x z      // vext vtrn
609         
610         B1 = B1 * B2;   //      A3 *= B3
611
612         A0 = bt_splat_ps(vQ1, 3);       //      A0
613         A0 = A0 * vQ2;  //      A0 * B0
614
615         A1 = A1 + A2;   //      AB12
616         A0 =  A0 - B1;  //      AB03 = AB0 - AB3 
617         
618     A1 = _mm_xor_ps(A1, vPPPM); //      change sign of the last element
619         A0 = A0 + A1;   //      AB03 + AB12
620         
621         return btQuaternion(A0);
622
623 #elif defined(BT_USE_NEON)     
624
625         float32x4_t vQ1 = q1.get128();
626         float32x4_t vQ2 = q2.get128();
627         float32x4_t A0, A1, B1, A2, B2, A3, B3;
628     float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
629     
630     {
631     float32x2x2_t tmp;
632     tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) );       // {z x}, {w y}
633     vQ1zx = tmp.val[0];
634
635     tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) );       // {z x}, {w y}
636     vQ2zx = tmp.val[0];
637     }
638     vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1); 
639
640     vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
641
642     vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
643     vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
644
645     A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx);                    // X Y  z x 
646     B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W  W X 
647
648         A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
649     B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
650
651     A3 = vcombine_f32(vQ1zx, vQ1yz);        // Z X Y Z
652     B3 = vcombine_f32(vQ2yz, vQ2xz);        // Y Z x z
653
654         A1 = vmulq_f32(A1, B1);
655         A2 = vmulq_f32(A2, B2);
656         A3 = vmulq_f32(A3, B3); //      A3 *= B3
657         A0 = vmulq_lane_f32(vQ2, vget_high_f32(vQ1), 1); //     A0 * B0
658
659         A1 = vaddq_f32(A1, A2); //      AB12 = AB1 + AB2
660         A0 = vsubq_f32(A0, A3); //      AB03 = AB0 - AB3 
661         
662     //  change the sign of the last element
663     A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);      
664         A0 = vaddq_f32(A0, A1); //      AB03 + AB12
665         
666         return btQuaternion(A0);
667
668 #else
669         return btQuaternion(
670         q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
671                 q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
672                 q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
673                 q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); 
674 #endif
675 }
676
677 SIMD_FORCE_INLINE btQuaternion
678 operator*(const btQuaternion& q, const btVector3& w)
679 {
680 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
681         __m128 vQ1 = q.get128();
682         __m128 vQ2 = w.get128();
683         __m128 A1, B1, A2, B2, A3, B3;
684         
685         A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(3,3,3,0));
686         B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(0,1,2,0));
687
688         A1 = A1 * B1;
689         
690         A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1,2,0,1));
691         B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1));
692
693         A2 = A2 * B2;
694
695         A3 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2,0,1,2));
696         B3 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2));
697         
698         A3 = A3 * B3;   //      A3 *= B3
699
700         A1 = A1 + A2;   //      AB12
701         A1 = _mm_xor_ps(A1, vPPPM);     //      change sign of the last element
702     A1 = A1 - A3;       //      AB123 = AB12 - AB3 
703         
704         return btQuaternion(A1);
705     
706 #elif defined(BT_USE_NEON)     
707
708         float32x4_t vQ1 = q.get128();
709         float32x4_t vQ2 = w.get128();
710         float32x4_t A1, B1, A2, B2, A3, B3;
711     float32x2_t vQ1wx, vQ2zx, vQ1yz, vQ2yz, vQ1zx, vQ2xz;
712     
713     vQ1wx = vext_f32(vget_high_f32(vQ1), vget_low_f32(vQ1), 1); 
714     {
715     float32x2x2_t tmp;
716
717     tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) );       // {z x}, {w y}
718     vQ2zx = tmp.val[0];
719
720     tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) );       // {z x}, {w y}
721     vQ1zx = tmp.val[0];
722     }
723
724     vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
725
726     vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
727     vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
728
729     A1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ1), 1), vQ1wx); // W W  W X 
730     B1 = vcombine_f32(vget_low_f32(vQ2), vQ2zx);                    // X Y  z x 
731
732         A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
733     B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
734
735     A3 = vcombine_f32(vQ1zx, vQ1yz);        // Z X Y Z
736     B3 = vcombine_f32(vQ2yz, vQ2xz);        // Y Z x z
737
738         A1 = vmulq_f32(A1, B1);
739         A2 = vmulq_f32(A2, B2);
740         A3 = vmulq_f32(A3, B3); //      A3 *= B3
741
742         A1 = vaddq_f32(A1, A2); //      AB12 = AB1 + AB2
743         
744     //  change the sign of the last element
745     A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);      
746         
747     A1 = vsubq_f32(A1, A3);     //      AB123 = AB12 - AB3
748         
749         return btQuaternion(A1);
750     
751 #else
752         return btQuaternion( 
753          q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
754                  q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
755                  q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
756                 -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); 
757 #endif
758 }
759
760 SIMD_FORCE_INLINE btQuaternion
761 operator*(const btVector3& w, const btQuaternion& q)
762 {
763 #if defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
764         __m128 vQ1 = w.get128();
765         __m128 vQ2 = q.get128();
766         __m128 A1, B1, A2, B2, A3, B3;
767         
768         A1 = bt_pshufd_ps(vQ1, BT_SHUFFLE(0,1,2,0));  // X Y  z x
769         B1 = bt_pshufd_ps(vQ2, BT_SHUFFLE(3,3,3,0));  // W W  W X 
770
771         A1 = A1 * B1;
772         
773         A2 = bt_pshufd_ps(vQ1, BT_SHUFFLE(1,2,0,1));
774         B2 = bt_pshufd_ps(vQ2, BT_SHUFFLE(2,0,1,1));
775
776         A2 = A2 *B2;
777
778         A3 = bt_pshufd_ps(vQ1, BT_SHUFFLE(2,0,1,2));
779         B3 = bt_pshufd_ps(vQ2, BT_SHUFFLE(1,2,0,2));
780         
781         A3 = A3 * B3;   //      A3 *= B3
782
783         A1 = A1 + A2;   //      AB12
784         A1 = _mm_xor_ps(A1, vPPPM);     //      change sign of the last element
785         A1 = A1 - A3;   //      AB123 = AB12 - AB3 
786         
787         return btQuaternion(A1);
788
789 #elif defined(BT_USE_NEON)     
790
791         float32x4_t vQ1 = w.get128();
792         float32x4_t vQ2 = q.get128();
793         float32x4_t  A1, B1, A2, B2, A3, B3;
794     float32x2_t vQ1zx, vQ2wx, vQ1yz, vQ2zx, vQ2yz, vQ2xz;
795     
796     {
797     float32x2x2_t tmp;
798    
799     tmp = vtrn_f32( vget_high_f32(vQ1), vget_low_f32(vQ1) );       // {z x}, {w y}
800     vQ1zx = tmp.val[0];
801
802     tmp = vtrn_f32( vget_high_f32(vQ2), vget_low_f32(vQ2) );       // {z x}, {w y}
803     vQ2zx = tmp.val[0];
804     }
805     vQ2wx = vext_f32(vget_high_f32(vQ2), vget_low_f32(vQ2), 1); 
806
807     vQ1yz = vext_f32(vget_low_f32(vQ1), vget_high_f32(vQ1), 1);
808
809     vQ2yz = vext_f32(vget_low_f32(vQ2), vget_high_f32(vQ2), 1);
810     vQ2xz = vext_f32(vQ2zx, vQ2zx, 1);
811
812     A1 = vcombine_f32(vget_low_f32(vQ1), vQ1zx);                    // X Y  z x 
813     B1 = vcombine_f32(vdup_lane_f32(vget_high_f32(vQ2), 1), vQ2wx); // W W  W X 
814
815         A2 = vcombine_f32(vQ1yz, vget_low_f32(vQ1));
816     B2 = vcombine_f32(vQ2zx, vdup_lane_f32(vget_low_f32(vQ2), 1));
817
818     A3 = vcombine_f32(vQ1zx, vQ1yz);        // Z X Y Z
819     B3 = vcombine_f32(vQ2yz, vQ2xz);        // Y Z x z
820
821         A1 = vmulq_f32(A1, B1);
822         A2 = vmulq_f32(A2, B2);
823         A3 = vmulq_f32(A3, B3); //      A3 *= B3
824
825         A1 = vaddq_f32(A1, A2); //      AB12 = AB1 + AB2
826         
827     //  change the sign of the last element
828     A1 = (btSimdFloat4)veorq_s32((int32x4_t)A1, (int32x4_t)vPPPM);      
829         
830     A1 = vsubq_f32(A1, A3);     //      AB123 = AB12 - AB3
831         
832         return btQuaternion(A1);
833     
834 #else
835         return btQuaternion( 
836         +w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
837                 +w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
838                 +w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
839                 -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); 
840 #endif
841 }
842
843 /**@brief Calculate the dot product between two quaternions */
844 SIMD_FORCE_INLINE btScalar 
845 dot(const btQuaternion& q1, const btQuaternion& q2) 
846
847         return q1.dot(q2); 
848 }
849
850
851 /**@brief Return the length of a quaternion */
852 SIMD_FORCE_INLINE btScalar
853 length(const btQuaternion& q) 
854
855         return q.length(); 
856 }
857
858 /**@brief Return the angle between two quaternions*/
859 SIMD_FORCE_INLINE btScalar
860 btAngle(const btQuaternion& q1, const btQuaternion& q2) 
861
862         return q1.angle(q2); 
863 }
864
865 /**@brief Return the inverse of a quaternion*/
866 SIMD_FORCE_INLINE btQuaternion
867 inverse(const btQuaternion& q) 
868 {
869         return q.inverse();
870 }
871
872 /**@brief Return the result of spherical linear interpolation betwen two quaternions 
873  * @param q1 The first quaternion
874  * @param q2 The second quaternion 
875  * @param t The ration between q1 and q2.  t = 0 return q1, t=1 returns q2 
876  * Slerp assumes constant velocity between positions. */
877 SIMD_FORCE_INLINE btQuaternion
878 slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t) 
879 {
880         return q1.slerp(q2, t);
881 }
882
883 SIMD_FORCE_INLINE btVector3 
884 quatRotate(const btQuaternion& rotation, const btVector3& v) 
885 {
886         btQuaternion q = rotation * v;
887         q *= rotation.inverse();
888 #if defined BT_USE_SIMD_VECTOR3 && defined (BT_USE_SSE_IN_API) && defined (BT_USE_SSE)
889         return btVector3(_mm_and_ps(q.get128(), btvFFF0fMask));
890 #elif defined(BT_USE_NEON)
891     return btVector3((float32x4_t)vandq_s32((int32x4_t)q.get128(), btvFFF0Mask));
892 #else   
893         return btVector3(q.getX(),q.getY(),q.getZ());
894 #endif
895 }
896
897 SIMD_FORCE_INLINE btQuaternion 
898 shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
899 {
900         btVector3 c = v0.cross(v1);
901         btScalar  d = v0.dot(v1);
902
903         if (d < -1.0 + SIMD_EPSILON)
904         {
905                 btVector3 n,unused;
906                 btPlaneSpace1(v0,n,unused);
907                 return btQuaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0
908         }
909
910         btScalar  s = btSqrt((1.0f + d) * 2.0f);
911         btScalar rs = 1.0f / s;
912
913         return btQuaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
914 }
915
916 SIMD_FORCE_INLINE btQuaternion 
917 shortestArcQuatNormalize2(btVector3& v0,btVector3& v1)
918 {
919         v0.normalize();
920         v1.normalize();
921         return shortestArcQuat(v0,v1);
922 }
923
924
925
926
927 struct  btQuaternionFloatData
928 {
929         float   m_floats[4];
930 };
931
932 struct  btQuaternionDoubleData
933 {
934         double  m_floats[4];
935
936 };
937
938 SIMD_FORCE_INLINE       void    btQuaternion::serializeFloat(struct     btQuaternionFloatData& dataOut) const
939 {
940         ///could also do a memcpy, check if it is worth it
941         for (int i=0;i<4;i++)
942                 dataOut.m_floats[i] = float(m_floats[i]);
943 }
944
945 SIMD_FORCE_INLINE void  btQuaternion::deSerializeFloat(const struct     btQuaternionFloatData& dataIn)
946 {
947         for (int i=0;i<4;i++)
948                 m_floats[i] = btScalar(dataIn.m_floats[i]);
949 }
950
951
952 SIMD_FORCE_INLINE       void    btQuaternion::serializeDouble(struct    btQuaternionDoubleData& dataOut) const
953 {
954         ///could also do a memcpy, check if it is worth it
955         for (int i=0;i<4;i++)
956                 dataOut.m_floats[i] = double(m_floats[i]);
957 }
958
959 SIMD_FORCE_INLINE void  btQuaternion::deSerializeDouble(const struct    btQuaternionDoubleData& dataIn)
960 {
961         for (int i=0;i<4;i++)
962                 m_floats[i] = btScalar(dataIn.m_floats[i]);
963 }
964
965
966 SIMD_FORCE_INLINE       void    btQuaternion::serialize(struct  btQuaternionData& dataOut) const
967 {
968         ///could also do a memcpy, check if it is worth it
969         for (int i=0;i<4;i++)
970                 dataOut.m_floats[i] = m_floats[i];
971 }
972
973 SIMD_FORCE_INLINE void  btQuaternion::deSerialize(const struct  btQuaternionData& dataIn)
974 {
975         for (int i=0;i<4;i++)
976                 m_floats[i] = dataIn.m_floats[i];
977 }
978
979
980 #endif //BT_SIMD__QUATERNION_H_
981
982
983