Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/LinearMath/btQuaternion.h @ 2258

Last change on this file since 2258 was 2192, checked in by rgrieder, 16 years ago

Reverted all changes of attempt to update physics branch.

  • Property svn:eol-style set to native
File size: 8.2 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#include "btVector3.h"
21
22///The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform.
23class btQuaternion : public btQuadWord {
24public:
25        btQuaternion() {}
26
27        //              template <typename btScalar>
28        //              explicit Quaternion(const btScalar *v) : Tuple4<btScalar>(v) {}
29
30        btQuaternion(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w) 
31                : btQuadWord(x, y, z, w) 
32        {}
33
34        btQuaternion(const btVector3& axis, const btScalar& angle) 
35        { 
36                setRotation(axis, angle); 
37        }
38
39        btQuaternion(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
40        { 
41                setEuler(yaw, pitch, roll); 
42        }
43
44        void setRotation(const btVector3& axis, const btScalar& angle)
45        {
46                btScalar d = axis.length();
47                assert(d != btScalar(0.0));
48                btScalar s = btSin(angle * btScalar(0.5)) / d;
49                setValue(axis.x() * s, axis.y() * s, axis.z() * s, 
50                        btCos(angle * btScalar(0.5)));
51        }
52
53        void setEuler(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
54        {
55                btScalar halfYaw = btScalar(yaw) * btScalar(0.5); 
56                btScalar halfPitch = btScalar(pitch) * btScalar(0.5); 
57                btScalar halfRoll = btScalar(roll) * btScalar(0.5); 
58                btScalar cosYaw = btCos(halfYaw);
59                btScalar sinYaw = btSin(halfYaw);
60                btScalar cosPitch = btCos(halfPitch);
61                btScalar sinPitch = btSin(halfPitch);
62                btScalar cosRoll = btCos(halfRoll);
63                btScalar sinRoll = btSin(halfRoll);
64                setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
65                        cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
66                        sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
67                        cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
68        }
69
70        btQuaternion& operator+=(const btQuaternion& q)
71        {
72                m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q.m_unusedW;
73                return *this;
74        }
75
76        btQuaternion& operator-=(const btQuaternion& q) 
77        {
78                m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q.m_unusedW;
79                return *this;
80        }
81
82        btQuaternion& operator*=(const btScalar& s)
83        {
84                m_x *= s; m_y *= s; m_z *= s; m_unusedW *= s;
85                return *this;
86        }
87
88
89        btQuaternion& operator*=(const btQuaternion& q)
90        {
91                setValue(m_unusedW * q.x() + m_x * q.m_unusedW + m_y * q.z() - m_z * q.y(),
92                        m_unusedW * q.y() + m_y * q.m_unusedW + m_z * q.x() - m_x * q.z(),
93                        m_unusedW * q.z() + m_z * q.m_unusedW + m_x * q.y() - m_y * q.x(),
94                        m_unusedW * q.m_unusedW - m_x * q.x() - m_y * q.y() - m_z * q.z());
95                return *this;
96        }
97
98        btScalar dot(const btQuaternion& q) const
99        {
100                return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q.m_unusedW;
101        }
102
103        btScalar length2() const
104        {
105                return dot(*this);
106        }
107
108        btScalar length() const
109        {
110                return btSqrt(length2());
111        }
112
113        btQuaternion& normalize() 
114        {
115                return *this /= length();
116        }
117
118        SIMD_FORCE_INLINE btQuaternion
119        operator*(const btScalar& s) const
120        {
121                return btQuaternion(x() * s, y() * s, z() * s, m_unusedW * s);
122        }
123
124
125
126        btQuaternion operator/(const btScalar& s) const
127        {
128                assert(s != btScalar(0.0));
129                return *this * (btScalar(1.0) / s);
130        }
131
132
133        btQuaternion& operator/=(const btScalar& s) 
134        {
135                assert(s != btScalar(0.0));
136                return *this *= btScalar(1.0) / s;
137        }
138
139
140        btQuaternion normalized() const 
141        {
142                return *this / length();
143        } 
144
145        btScalar angle(const btQuaternion& q) const 
146        {
147                btScalar s = btSqrt(length2() * q.length2());
148                assert(s != btScalar(0.0));
149                return btAcos(dot(q) / s);
150        }
151
152        btScalar getAngle() const 
153        {
154                btScalar s = btScalar(2.) * btAcos(m_unusedW);
155                return s;
156        }
157
158
159
160        btQuaternion inverse() const
161        {
162                return btQuaternion(-m_x, -m_y, -m_z, m_unusedW);
163        }
164
165        SIMD_FORCE_INLINE btQuaternion
166        operator+(const btQuaternion& q2) const
167        {
168                const btQuaternion& q1 = *this;
169                return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_unusedW + q2.m_unusedW);
170        }
171
172        SIMD_FORCE_INLINE btQuaternion
173        operator-(const btQuaternion& q2) const
174        {
175                const btQuaternion& q1 = *this;
176                return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_unusedW - q2.m_unusedW);
177        }
178
179        SIMD_FORCE_INLINE btQuaternion operator-() const
180        {
181                const btQuaternion& q2 = *this;
182                return btQuaternion( - q2.x(), - q2.y(),  - q2.z(),  - q2.m_unusedW);
183        }
184
185        SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const 
186        {
187                btQuaternion diff,sum;
188                diff = *this - qd;
189                sum = *this + qd;
190                if( diff.dot(diff) > sum.dot(sum) )
191                        return qd;
192                return (-qd);
193        }
194
195        btQuaternion slerp(const btQuaternion& q, const btScalar& t) const
196        {
197                btScalar theta = angle(q);
198                if (theta != btScalar(0.0))
199                {
200                        btScalar d = btScalar(1.0) / btSin(theta);
201                        btScalar s0 = btSin((btScalar(1.0) - t) * theta);
202                        btScalar s1 = btSin(t * theta);   
203                        return btQuaternion((m_x * s0 + q.x() * s1) * d,
204                                (m_y * s0 + q.y() * s1) * d,
205                                (m_z * s0 + q.z() * s1) * d,
206                                (m_unusedW * s0 + q.m_unusedW * s1) * d);
207                }
208                else
209                {
210                        return *this;
211                }
212        }
213
214        SIMD_FORCE_INLINE const btScalar& getW() const { return m_unusedW; }
215
216       
217};
218
219
220
221SIMD_FORCE_INLINE btQuaternion
222operator-(const btQuaternion& q)
223{
224        return btQuaternion(-q.x(), -q.y(), -q.z(), -q.w());
225}
226
227
228
229
230SIMD_FORCE_INLINE btQuaternion
231operator*(const btQuaternion& q1, const btQuaternion& q2) {
232        return btQuaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
233                q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
234                q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
235                q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); 
236}
237
238SIMD_FORCE_INLINE btQuaternion
239operator*(const btQuaternion& q, const btVector3& w)
240{
241        return btQuaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
242                q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
243                q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
244                -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); 
245}
246
247SIMD_FORCE_INLINE btQuaternion
248operator*(const btVector3& w, const btQuaternion& q)
249{
250        return btQuaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
251                w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
252                w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
253                -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); 
254}
255
256SIMD_FORCE_INLINE btScalar
257dot(const btQuaternion& q1, const btQuaternion& q2) 
258{ 
259        return q1.dot(q2); 
260}
261
262
263SIMD_FORCE_INLINE btScalar
264length(const btQuaternion& q) 
265{ 
266        return q.length(); 
267}
268
269SIMD_FORCE_INLINE btScalar
270angle(const btQuaternion& q1, const btQuaternion& q2) 
271{ 
272        return q1.angle(q2); 
273}
274
275
276SIMD_FORCE_INLINE btQuaternion
277inverse(const btQuaternion& q) 
278{
279        return q.inverse();
280}
281
282SIMD_FORCE_INLINE btQuaternion
283slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t) 
284{
285        return q1.slerp(q2, t);
286}
287
288SIMD_FORCE_INLINE btVector3
289quatRotate(const btQuaternion& rotation, const btVector3& v) 
290{
291        btQuaternion q = rotation * v;
292        q *= rotation.inverse();
293        return btVector3(q.getX(),q.getY(),q.getZ());
294}
295
296SIMD_FORCE_INLINE btQuaternion
297shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
298{
299        btVector3 c = v0.cross(v1);
300        btScalar  d = v0.dot(v1);
301
302        if (d < -1.0 + SIMD_EPSILON)
303                return btQuaternion(0.0f,1.0f,0.0f,0.0f); // just pick any vector
304
305        btScalar  s = btSqrt((1.0f + d) * 2.0f);
306        btScalar rs = 1.0f / s;
307
308        return btQuaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
309}
310
311SIMD_FORCE_INLINE btQuaternion
312shortestArcQuatNormalize2(btVector3& v0,btVector3& v1)
313{
314        v0.normalize();
315        v1.normalize();
316        return shortestArcQuat(v0,v1);
317}
318
319#endif
320
321
322
323
Note: See TracBrowser for help on using the repository browser.