Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/steering/src/external/bullet/LinearMath/btQuaternion.h @ 8744

Last change on this file since 8744 was 5781, checked in by rgrieder, 15 years ago

Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).

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