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

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

core: add the whole BulletPhysics source code to the engine core, because
that’s precisely how they want us to use it.

  • Property svn:keywords set to Id
File size: 14.0 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        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 = 1.f-m_floats[3]*m_floats[3];
216               
217                if (s_squared < btScalar(10.) * SIMD_EPSILON) //Check for divide by zero
218                        return btVector3(1.0, 0.0, 0.0);  // Arbitrary
219                btScalar s = 1.f/btSqrt(s_squared);
220                return btVector3(m_floats[0] * s, m_floats[1] * s, m_floats[2] * s);
221        }
222
223        /**@brief Return the inverse of this quaternion */
224        btQuaternion inverse() const
225        {
226                return btQuaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
227        }
228
229  /**@brief Return the sum of this quaternion and the other
230   * @param q2 The other quaternion */
231        SIMD_FORCE_INLINE btQuaternion
232        operator+(const btQuaternion& q2) const
233        {
234                const btQuaternion& q1 = *this;
235                return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]);
236        }
237
238  /**@brief Return the difference between this quaternion and the other
239   * @param q2 The other quaternion */
240        SIMD_FORCE_INLINE btQuaternion
241        operator-(const btQuaternion& q2) const
242        {
243                const btQuaternion& q1 = *this;
244                return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]);
245        }
246
247  /**@brief Return the negative of this quaternion
248   * This simply negates each element */
249        SIMD_FORCE_INLINE btQuaternion operator-() const
250        {
251                const btQuaternion& q2 = *this;
252                return btQuaternion( - q2.x(), - q2.y(),  - q2.z(),  - q2.m_floats[3]);
253        }
254  /**@todo document this and it's use */
255        SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const
256        {
257                btQuaternion diff,sum;
258                diff = *this - qd;
259                sum = *this + qd;
260                if( diff.dot(diff) > sum.dot(sum) )
261                        return qd;
262                return (-qd);
263        }
264
265        /**@todo document this and it's use */
266        SIMD_FORCE_INLINE btQuaternion nearest( const btQuaternion& qd) const
267        {
268                btQuaternion diff,sum;
269                diff = *this - qd;
270                sum = *this + qd;
271                if( diff.dot(diff) < sum.dot(sum) )
272                        return qd;
273                return (-qd);
274        }
275
276
277  /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
278   * @param q The other quaternion to interpolate with
279   * @param t The ratio between this and q to interpolate.  If t = 0 the result is this, if t=1 the result is q.
280   * Slerp interpolates assuming constant velocity.  */
281        btQuaternion slerp(const btQuaternion& q, const btScalar& t) const
282        {
283          btScalar magnitude = btSqrt(length2() * q.length2());
284          btAssert(magnitude > btScalar(0));
285
286    btScalar product = dot(q) / magnitude;
287    if (btFabs(product) != btScalar(1))
288                {
289      // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
290      const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1);
291
292      const btScalar theta = btAcos(sign * product);
293      const btScalar s1 = btSin(sign * t * theta);   
294      const btScalar d = btScalar(1.0) / btSin(theta);
295      const btScalar s0 = btSin((btScalar(1.0) - t) * theta);
296
297      return btQuaternion(
298          (m_floats[0] * s0 + q.x() * s1) * d,
299          (m_floats[1] * s0 + q.y() * s1) * d,
300          (m_floats[2] * s0 + q.z() * s1) * d,
301          (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
302                }
303                else
304                {
305                        return *this;
306                }
307        }
308
309        static const btQuaternion&      getIdentity()
310        {
311                static const btQuaternion identityQuat(btScalar(0.),btScalar(0.),btScalar(0.),btScalar(1.));
312                return identityQuat;
313        }
314
315        SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; }
316
317       
318};
319
320
321
322
323
324/**@brief Return the product of two quaternions */
325SIMD_FORCE_INLINE btQuaternion
326operator*(const btQuaternion& q1, const btQuaternion& q2) {
327        return btQuaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
328                q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
329                q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
330                q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
331}
332
333SIMD_FORCE_INLINE btQuaternion
334operator*(const btQuaternion& q, const btVector3& w)
335{
336        return btQuaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
337                q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
338                q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
339                -q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
340}
341
342SIMD_FORCE_INLINE btQuaternion
343operator*(const btVector3& w, const btQuaternion& q)
344{
345        return btQuaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
346                w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
347                w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
348                -w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
349}
350
351/**@brief Calculate the dot product between two quaternions */
352SIMD_FORCE_INLINE btScalar
353dot(const btQuaternion& q1, const btQuaternion& q2)
354{
355        return q1.dot(q2);
356}
357
358
359/**@brief Return the length of a quaternion */
360SIMD_FORCE_INLINE btScalar
361length(const btQuaternion& q)
362{
363        return q.length();
364}
365
366/**@brief Return the angle between two quaternions*/
367SIMD_FORCE_INLINE btScalar
368angle(const btQuaternion& q1, const btQuaternion& q2)
369{
370        return q1.angle(q2);
371}
372
373/**@brief Return the inverse of a quaternion*/
374SIMD_FORCE_INLINE btQuaternion
375inverse(const btQuaternion& q)
376{
377        return q.inverse();
378}
379
380/**@brief Return the result of spherical linear interpolation betwen two quaternions
381 * @param q1 The first quaternion
382 * @param q2 The second quaternion
383 * @param t The ration between q1 and q2.  t = 0 return q1, t=1 returns q2
384 * Slerp assumes constant velocity between positions. */
385SIMD_FORCE_INLINE btQuaternion
386slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t)
387{
388        return q1.slerp(q2, t);
389}
390
391SIMD_FORCE_INLINE btVector3
392quatRotate(const btQuaternion& rotation, const btVector3& v)
393{
394        btQuaternion q = rotation * v;
395        q *= rotation.inverse();
396        return btVector3(q.getX(),q.getY(),q.getZ());
397}
398
399SIMD_FORCE_INLINE btQuaternion
400shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
401{
402        btVector3 c = v0.cross(v1);
403        btScalar  d = v0.dot(v1);
404
405        if (d < -1.0 + SIMD_EPSILON)
406        {
407                btVector3 n,unused;
408                btPlaneSpace1(v0,n,unused);
409                return btQuaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0
410        }
411
412        btScalar  s = btSqrt((1.0f + d) * 2.0f);
413        btScalar rs = 1.0f / s;
414
415        return btQuaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
416}
417
418SIMD_FORCE_INLINE btQuaternion
419shortestArcQuatNormalize2(btVector3& v0,btVector3& v1)
420{
421        v0.normalize();
422        v1.normalize();
423        return shortestArcQuat(v0,v1);
424}
425
426#endif //BT_SIMD__QUATERNION_H_
427
428
429
430
Note: See TracBrowser for help on using the repository browser.