← Back to branch summary

~ubuntu-branches/ubuntu/vivid/emscripten/vivid

« back to all changes in this revision

Viewing changes to tests/bullet/src/LinearMath/btQuaternion.h

  • Committer: Package Import Robot
  • Author(s): Sylvestre Ledru
  • Date: 2013-05-02 13:11:51 UTC
  • Revision ID: package-import@ubuntu.com-20130502131151-q8dvteqr1ef2x7xz
Tags: upstream-1.4.1~20130504~adb56cb
Import upstream version 1.4.1~20130504~adb56cb

Show diffs side-by-side

added added

removed removed

Lines of Context:
tests/bullet/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
/**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */
 
25
class btQuaternion : public btQuadWord {
 
26
public:
 
27
  /**@brief No initialization constructor */
 
28
        btQuaternion() {}
 
29
 
 
30
        //              template <typename btScalar>
 
31
        //              explicit Quaternion(const btScalar *v) : Tuple4<btScalar>(v) {}
 
32
  /**@brief Constructor from scalars */
 
33
        btQuaternion(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w) 
 
34
                : btQuadWord(x, y, z, w) 
 
35
        {}
 
36
  /**@brief Axis angle Constructor
 
37
   * @param axis The axis which the rotation is around
 
38
   * @param angle The magnitude of the rotation around the angle (Radians) */
 
39
        btQuaternion(const btVector3& axis, const btScalar& angle) 
 
40
        { 
 
41
                setRotation(axis, angle); 
 
42
        }
 
43
  /**@brief Constructor from Euler angles
 
44
   * @param yaw Angle around Y unless BT_EULER_DEFAULT_ZYX defined then Z
 
45
   * @param pitch Angle around X unless BT_EULER_DEFAULT_ZYX defined then Y
 
46
   * @param roll Angle around Z unless BT_EULER_DEFAULT_ZYX defined then X */
 
47
        btQuaternion(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
 
48
        { 
 
49
#ifndef BT_EULER_DEFAULT_ZYX
 
50
                setEuler(yaw, pitch, roll); 
 
51
#else
 
52
                setEulerZYX(yaw, pitch, roll); 
 
53
#endif 
 
54
        }
 
55
  /**@brief Set the rotation using axis angle notation 
 
56
   * @param axis The axis around which to rotate
 
57
   * @param angle The magnitude of the rotation in Radians */
 
58
        void setRotation(const btVector3& axis, const btScalar& angle)
 
59
        {
 
60
                btScalar d = axis.length();
 
61
                btAssert(d != btScalar(0.0));
 
62
                btScalar s = btSin(angle * btScalar(0.5)) / d;
 
63
                setValue(axis.x() * s, axis.y() * s, axis.z() * s, 
 
64
                        btCos(angle * btScalar(0.5)));
 
65
        }
 
66
  /**@brief Set the quaternion using Euler angles
 
67
   * @param yaw Angle around Y
 
68
   * @param pitch Angle around X
 
69
   * @param roll Angle around Z */
 
70
        void setEuler(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
 
71
        {
 
72
                btScalar halfYaw = btScalar(yaw) * btScalar(0.5);  
 
73
                btScalar halfPitch = btScalar(pitch) * btScalar(0.5);  
 
74
                btScalar halfRoll = btScalar(roll) * btScalar(0.5);  
 
75
                btScalar cosYaw = btCos(halfYaw);
 
76
                btScalar sinYaw = btSin(halfYaw);
 
77
                btScalar cosPitch = btCos(halfPitch);
 
78
                btScalar sinPitch = btSin(halfPitch);
 
79
                btScalar cosRoll = btCos(halfRoll);
 
80
                btScalar sinRoll = btSin(halfRoll);
 
81
                setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
 
82
                        cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
 
83
                        sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
 
84
                        cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
 
85
        }
 
86
  /**@brief Set the quaternion using euler angles 
 
87
   * @param yaw Angle around Z
 
88
   * @param pitch Angle around Y
 
89
   * @param roll Angle around X */
 
90
        void setEulerZYX(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
 
91
        {
 
92
                btScalar halfYaw = btScalar(yaw) * btScalar(0.5);  
 
93
                btScalar halfPitch = btScalar(pitch) * btScalar(0.5);  
 
94
                btScalar halfRoll = btScalar(roll) * btScalar(0.5);  
 
95
                btScalar cosYaw = btCos(halfYaw);
 
96
                btScalar sinYaw = btSin(halfYaw);
 
97
                btScalar cosPitch = btCos(halfPitch);
 
98
                btScalar sinPitch = btSin(halfPitch);
 
99
                btScalar cosRoll = btCos(halfRoll);
 
100
                btScalar sinRoll = btSin(halfRoll);
 
101
                setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
 
102
                         cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
 
103
                         cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
 
104
                         cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
 
105
        }
 
106
  /**@brief Add two quaternions
 
107
   * @param q The quaternion to add to this one */
 
108
        SIMD_FORCE_INLINE       btQuaternion& operator+=(const btQuaternion& q)
 
109
        {
 
110
                m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3];
 
111
                return *this;
 
112
        }
 
113
 
 
114
  /**@brief Subtract out a quaternion
 
115
   * @param q The quaternion to subtract from this one */
 
116
        btQuaternion& operator-=(const btQuaternion& q) 
 
117
        {
 
118
                m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3];
 
119
                return *this;
 
120
        }
 
121
 
 
122
  /**@brief Scale this quaternion
 
123
   * @param s The scalar to scale by */
 
124
        btQuaternion& operator*=(const btScalar& s)
 
125
        {
 
126
                m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s;
 
127
                return *this;
 
128
        }
 
129
 
 
130
  /**@brief Multiply this quaternion by q on the right
 
131
   * @param q The other quaternion 
 
132
   * Equivilant to this = this * q */
 
133
        btQuaternion& operator*=(const btQuaternion& q)
 
134
        {
 
135
                setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
 
136
                        m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
 
137
                        m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
 
138
                        m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
 
139
                return *this;
 
140
        }
 
141
  /**@brief Return the dot product between this quaternion and another
 
142
   * @param q The other quaternion */
 
143
        btScalar dot(const btQuaternion& q) const
 
144
        {
 
145
                return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3];
 
146
        }
 
147
 
 
148
  /**@brief Return the length squared of the quaternion */
 
149
        btScalar length2() const
 
150
        {
 
151
                return dot(*this);
 
152
        }
 
153
 
 
154
  /**@brief Return the length of the quaternion */
 
155
        btScalar length() const
 
156
        {
 
157
                return btSqrt(length2());
 
158
        }
 
159
 
 
160
  /**@brief Normalize the quaternion 
 
161
   * Such that x^2 + y^2 + z^2 +w^2 = 1 */
 
162
        btQuaternion& normalize() 
 
163
        {
 
164
                return *this /= length();
 
165
        }
 
166
 
 
167
  /**@brief Return a scaled version of this quaternion
 
168
   * @param s The scale factor */
 
169
        SIMD_FORCE_INLINE btQuaternion
 
170
        operator*(const btScalar& s) const
 
171
        {
 
172
                return btQuaternion(x() * s, y() * s, z() * s, m_floats[3] * s);
 
173
        }
 
174
 
 
175
 
 
176
  /**@brief Return an inversely scaled versionof this quaternion
 
177
   * @param s The inverse scale factor */
 
178
        btQuaternion operator/(const btScalar& s) const
 
179
        {
 
180
                btAssert(s != btScalar(0.0));
 
181
                return *this * (btScalar(1.0) / s);
 
182
        }
 
183
 
 
184
  /**@brief Inversely scale this quaternion
 
185
   * @param s The scale factor */
 
186
        btQuaternion& operator/=(const btScalar& s) 
 
187
        {
 
188
                btAssert(s != btScalar(0.0));
 
189
                return *this *= btScalar(1.0) / s;
 
190
        }
 
191
 
 
192
  /**@brief Return a normalized version of this quaternion */
 
193
        btQuaternion normalized() const 
 
194
        {
 
195
                return *this / length();
 
196
        } 
 
197
  /**@brief Return the angle between this quaternion and the other 
 
198
   * @param q The other quaternion */
 
199
        btScalar angle(const btQuaternion& q) const 
 
200
        {
 
201
                btScalar s = btSqrt(length2() * q.length2());
 
202
                btAssert(s != btScalar(0.0));
 
203
                return btAcos(dot(q) / s);
 
204
        }
 
205
  /**@brief Return the angle of rotation represented by this quaternion */
 
206
        btScalar getAngle() const 
 
207
        {
 
208
                btScalar s = btScalar(2.) * btAcos(m_floats[3]);
 
209
                return s;
 
210
        }
 
211
 
 
212
        /**@brief Return the axis of the rotation represented by this quaternion */
 
213
        btVector3 getAxis() const
 
214
        {
 
215
                btScalar s_squared = btScalar(1.) - btPow(m_floats[3], btScalar(2.));
 
216
                if (s_squared < btScalar(10.) * SIMD_EPSILON) //Check for divide by zero
 
217
                        return btVector3(1.0, 0.0, 0.0);  // Arbitrary
 
218
                btScalar s = btSqrt(s_squared);
 
219
                return btVector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s);
 
220
        }
 
221
 
 
222
        /**@brief Return the inverse of this quaternion */
 
223
        btQuaternion inverse() const
 
224
        {
 
225
                return btQuaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
 
226
        }
 
227
 
 
228
  /**@brief Return the sum of this quaternion and the other 
 
229
   * @param q2 The other quaternion */
 
230
        SIMD_FORCE_INLINE btQuaternion
 
231
        operator+(const btQuaternion& q2) const
 
232
        {
 
233
                const btQuaternion& q1 = *this;
 
234
                return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]);
 
235
        }
 
236
 
 
237
  /**@brief Return the difference between this quaternion and the other 
 
238
   * @param q2 The other quaternion */
 
239
        SIMD_FORCE_INLINE btQuaternion
 
240
        operator-(const btQuaternion& q2) const
 
241
        {
 
242
                const btQuaternion& q1 = *this;
 
243
                return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]);
 
244
        }
 
245
 
 
246
  /**@brief Return the negative of this quaternion 
 
247
   * This simply negates each element */
 
248
        SIMD_FORCE_INLINE btQuaternion operator-() const
 
249
        {
 
250
                const btQuaternion& q2 = *this;
 
251
                return btQuaternion( - q2.x(), - q2.y(),  - q2.z(),  - q2.m_floats[3]);
 
252
        }
 
253
  /**@todo document this and it's use */
 
254
        SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const 
 
255
        {
 
256
                btQuaternion diff,sum;
 
257
                diff = *this - qd;
 
258
                sum = *this + qd;
 
259
                if( diff.dot(diff) > sum.dot(sum) )
 
260
                        return qd;
 
261
                return (-qd);
 
262
        }
 
263
 
 
264
        /**@todo document this and it's use */
 
265
        SIMD_FORCE_INLINE btQuaternion nearest( const btQuaternion& qd) const 
 
266
        {
 
267
                btQuaternion diff,sum;
 
268
                diff = *this - qd;
 
269
                sum = *this + qd;
 
270
                if( diff.dot(diff) < sum.dot(sum) )
 
271
                        return qd;
 
272
                return (-qd);
 
273
        }
 
274
 
 
275
 
 
276
  /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
 
277
   * @param q The other quaternion to interpolate with 
 
278
   * @param t The ratio between this and q to interpolate.  If t = 0 the result is this, if t=1 the result is q.
 
279
   * Slerp interpolates assuming constant velocity.  */
 
280
        btQuaternion slerp(const btQuaternion& q, const btScalar& t) const
 
281
        {
 
282
                btScalar theta = angle(q);
 
283
                if (theta != btScalar(0.0))
 
284
                {
 
285
                        btScalar d = btScalar(1.0) / btSin(theta);
 
286
                        btScalar s0 = btSin((btScalar(1.0) - t) * theta);
 
287
                        btScalar s1 = btSin(t * theta);   
 
288
                        if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
 
289
                          return btQuaternion((m_floats[0] * s0 + -q.x() * s1) * d,
 
290
                                              (m_floats[1] * s0 + -q.y() * s1) * d,
 
291
                                              (m_floats[2] * s0 + -q.z() * s1) * d,
 
292
                                              (m_floats[3] * s0 + -q.m_floats[3] * s1) * d);
 
293
                        else
 
294
                          return btQuaternion((m_floats[0] * s0 + q.x() * s1) * d,
 
295
                                              (m_floats[1] * s0 + q.y() * s1) * d,
 
296
                                              (m_floats[2] * s0 + q.z() * s1) * d,
 
297
                                              (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
 
298
                        
 
299
                }
 
300
                else
 
301
                {
 
302
                        return *this;
 
303
                }
 
304
        }
 
305
 
 
306
        static const btQuaternion&      getIdentity()
 
307
        {
 
308
                static const btQuaternion identityQuat(btScalar(0.),btScalar(0.),btScalar(0.),btScalar(1.));
 
309
                return identityQuat;
 
310
        }
 
311
 
 
312
        SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; }
 
313
 
 
314
        
 
315
};
 
316
 
 
317
 
 
318
/**@brief Return the negative of a quaternion */
 
319
SIMD_FORCE_INLINE btQuaternion
 
320
operator-(const btQuaternion& q)
 
321
{
 
322
        return btQuaternion(-q.x(), -q.y(), -q.z(), -q.w());
 
323
}
 
324
 
 
325
 
 
326
 
 
327
/**@brief Return the product of two quaternions */
 
328
SIMD_FORCE_INLINE btQuaternion
 
329
operator*(const btQuaternion& q1, const btQuaternion& q2) {
 
330
        return btQuaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
 
331
                q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
 
332
                q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
 
333
                q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); 
 
334
}
 
335
 
 
336
SIMD_FORCE_INLINE btQuaternion
 
337
operator*(const btQuaternion& q, const btVector3& w)
 
338
{
 
339
        return btQuaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
 
340
                q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
 
341
                q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
 
342
                -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); 
 
343
}
 
344
 
 
345
SIMD_FORCE_INLINE btQuaternion
 
346
operator*(const btVector3& w, const btQuaternion& q)
 
347
{
 
348
        return btQuaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
 
349
                w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
 
350
                w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
 
351
                -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); 
 
352
}
 
353
 
 
354
/**@brief Calculate the dot product between two quaternions */
 
355
SIMD_FORCE_INLINE btScalar 
 
356
dot(const btQuaternion& q1, const btQuaternion& q2) 
 
357
{ 
 
358
        return q1.dot(q2); 
 
359
}
 
360
 
 
361
 
 
362
/**@brief Return the length of a quaternion */
 
363
SIMD_FORCE_INLINE btScalar
 
364
length(const btQuaternion& q) 
 
365
{ 
 
366
        return q.length(); 
 
367
}
 
368
 
 
369
/**@brief Return the angle between two quaternions*/
 
370
SIMD_FORCE_INLINE btScalar
 
371
angle(const btQuaternion& q1, const btQuaternion& q2) 
 
372
{ 
 
373
        return q1.angle(q2); 
 
374
}
 
375
 
 
376
/**@brief Return the inverse of a quaternion*/
 
377
SIMD_FORCE_INLINE btQuaternion
 
378
inverse(const btQuaternion& q) 
 
379
{
 
380
        return q.inverse();
 
381
}
 
382
 
 
383
/**@brief Return the result of spherical linear interpolation betwen two quaternions 
 
384
 * @param q1 The first quaternion
 
385
 * @param q2 The second quaternion 
 
386
 * @param t The ration between q1 and q2.  t = 0 return q1, t=1 returns q2 
 
387
 * Slerp assumes constant velocity between positions. */
 
388
SIMD_FORCE_INLINE btQuaternion
 
389
slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t) 
 
390
{
 
391
        return q1.slerp(q2, t);
 
392
}
 
393
 
 
394
SIMD_FORCE_INLINE btVector3 
 
395
quatRotate(const btQuaternion& rotation, const btVector3& v) 
 
396
{
 
397
        btQuaternion q = rotation * v;
 
398
        q *= rotation.inverse();
 
399
        return btVector3(q.getX(),q.getY(),q.getZ());
 
400
}
 
401
 
 
402
SIMD_FORCE_INLINE btQuaternion 
 
403
shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
 
404
{
 
405
        btVector3 c = v0.cross(v1);
 
406
        btScalar  d = v0.dot(v1);
 
407
 
 
408
        if (d < -1.0 + SIMD_EPSILON)
 
409
        {
 
410
                btVector3 n,unused;
 
411
                btPlaneSpace1(v0,n,unused);
 
412
                return btQuaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0
 
413
        }
 
414
 
 
415
        btScalar  s = btSqrt((1.0f + d) * 2.0f);
 
416
        btScalar rs = 1.0f / s;
 
417
 
 
418
        return btQuaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
 
419
}
 
420
 
 
421
SIMD_FORCE_INLINE btQuaternion 
 
422
shortestArcQuatNormalize2(btVector3& v0,btVector3& v1)
 
423
{
 
424
        v0.normalize();
 
425
        v1.normalize();
 
426
        return shortestArcQuat(v0,v1);
 
427
}
 
428
 
 
429
#endif //BT_SIMD__QUATERNION_H_
 
430
 
 
431
 
 
432
 
 
433