Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/netp2/src/bullet/LinearMath/btQuaternion.h @ 2837

Last change on this file since 2837 was 2662, checked in by rgrieder, 16 years ago

Merged presentation branch back to trunk.

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