source: trunk/src/bullet/LinearMath/btQuaternion.h @ 1640

Last change on this file since 1640 was 1640, checked in by sam, 9 years ago

core: fix shitloads of compiler warnings in the Bullet source code.

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