Back to index

supertuxkart  0.5+dfsg1
btQuaternion.h
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
00003 
00004 This software is provided 'as-is', without any express or implied warranty.
00005 In no event will the authors be held liable for any damages arising from the use of this software.
00006 Permission is granted to anyone to use this software for any purpose, 
00007 including commercial applications, and to alter it and redistribute it freely, 
00008 subject to the following restrictions:
00009 
00010 1. 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.
00011 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00012 3. This notice may not be removed or altered from any source distribution.
00013 */
00014 
00015 
00016 
00017 #ifndef SIMD__QUATERNION_H_
00018 #define SIMD__QUATERNION_H_
00019 
00020 #include "btVector3.h"
00021 
00022 class btQuaternion : public btQuadWord {
00023 public:
00024        btQuaternion() {}
00025 
00026        //            template <typename btScalar>
00027        //            explicit Quaternion(const btScalar *v) : Tuple4<btScalar>(v) {}
00028 
00029        btQuaternion(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w) 
00030               : btQuadWord(x, y, z, w) 
00031        {}
00032 
00033        btQuaternion(const btVector3& axis, const btScalar& angle) 
00034        { 
00035               setRotation(axis, angle); 
00036        }
00037 
00038        btQuaternion(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
00039        { 
00040               setEuler(yaw, pitch, roll); 
00041        }
00042 
00043        void setRotation(const btVector3& axis, const btScalar& angle)
00044        {
00045               btScalar d = axis.length();
00046               assert(d != btScalar(0.0));
00047               btScalar s = btSin(angle * btScalar(0.5)) / d;
00048               setValue(axis.x() * s, axis.y() * s, axis.z() * s, 
00049                      btCos(angle * btScalar(0.5)));
00050        }
00051 
00052        void setEuler(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
00053        {
00054               btScalar halfYaw = btScalar(yaw) * btScalar(0.5);  
00055               btScalar halfPitch = btScalar(pitch) * btScalar(0.5);  
00056               btScalar halfRoll = btScalar(roll) * btScalar(0.5);  
00057               btScalar cosYaw = btCos(halfYaw);
00058               btScalar sinYaw = btSin(halfYaw);
00059               btScalar cosPitch = btCos(halfPitch);
00060               btScalar sinPitch = btSin(halfPitch);
00061               btScalar cosRoll = btCos(halfRoll);
00062               btScalar sinRoll = btSin(halfRoll);
00063               setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
00064                      cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
00065                      sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
00066                      cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
00067        }
00068 
00069        btQuaternion& operator+=(const btQuaternion& q)
00070        {
00071               m_x += q.x(); m_y += q.y(); m_z += q.z(); m_unusedW += q.m_unusedW;
00072               return *this;
00073        }
00074 
00075        btQuaternion& operator-=(const btQuaternion& q) 
00076        {
00077               m_x -= q.x(); m_y -= q.y(); m_z -= q.z(); m_unusedW -= q.m_unusedW;
00078               return *this;
00079        }
00080 
00081        btQuaternion& operator*=(const btScalar& s)
00082        {
00083               m_x *= s; m_y *= s; m_z *= s; m_unusedW *= s;
00084               return *this;
00085        }
00086 
00087 
00088        btQuaternion& operator*=(const btQuaternion& q)
00089        {
00090               setValue(m_unusedW * q.x() + m_x * q.m_unusedW + m_y * q.z() - m_z * q.y(),
00091                      m_unusedW * q.y() + m_y * q.m_unusedW + m_z * q.x() - m_x * q.z(),
00092                      m_unusedW * q.z() + m_z * q.m_unusedW + m_x * q.y() - m_y * q.x(),
00093                      m_unusedW * q.m_unusedW - m_x * q.x() - m_y * q.y() - m_z * q.z());
00094               return *this;
00095        }
00096 
00097        btScalar dot(const btQuaternion& q) const
00098        {
00099               return m_x * q.x() + m_y * q.y() + m_z * q.z() + m_unusedW * q.m_unusedW;
00100        }
00101 
00102        btScalar length2() const
00103        {
00104               return dot(*this);
00105        }
00106 
00107        btScalar length() const
00108        {
00109               return btSqrt(length2());
00110        }
00111 
00112        btQuaternion& normalize() 
00113        {
00114               return *this /= length();
00115        }
00116 
00117        SIMD_FORCE_INLINE btQuaternion
00118        operator*(const btScalar& s) const
00119        {
00120               return btQuaternion(x() * s, y() * s, z() * s, m_unusedW * s);
00121        }
00122 
00123 
00124 
00125        btQuaternion operator/(const btScalar& s) const
00126        {
00127               assert(s != btScalar(0.0));
00128               return *this * (btScalar(1.0) / s);
00129        }
00130 
00131 
00132        btQuaternion& operator/=(const btScalar& s) 
00133        {
00134               assert(s != btScalar(0.0));
00135               return *this *= btScalar(1.0) / s;
00136        }
00137 
00138 
00139        btQuaternion normalized() const 
00140        {
00141               return *this / length();
00142        } 
00143 
00144        btScalar angle(const btQuaternion& q) const 
00145        {
00146               btScalar s = btSqrt(length2() * q.length2());
00147               assert(s != btScalar(0.0));
00148               return btAcos(dot(q) / s);
00149        }
00150 
00151        btScalar getAngle() const 
00152        {
00153               btScalar s = btScalar(2.) * btAcos(m_unusedW);
00154               return s;
00155        }
00156 
00157 
00158 
00159        btQuaternion inverse() const
00160        {
00161               return btQuaternion(-m_x, -m_y, -m_z, m_unusedW);
00162        }
00163 
00164        SIMD_FORCE_INLINE btQuaternion
00165        operator+(const btQuaternion& q2) const
00166        {
00167               const btQuaternion& q1 = *this;
00168               return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_unusedW + q2.m_unusedW);
00169        }
00170 
00171        SIMD_FORCE_INLINE btQuaternion
00172        operator-(const btQuaternion& q2) const
00173        {
00174               const btQuaternion& q1 = *this;
00175               return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_unusedW - q2.m_unusedW);
00176        }
00177 
00178        SIMD_FORCE_INLINE btQuaternion operator-() const
00179        {
00180               const btQuaternion& q2 = *this;
00181               return btQuaternion( - q2.x(), - q2.y(),  - q2.z(),  - q2.m_unusedW);
00182        }
00183 
00184        SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const 
00185        {
00186               btQuaternion diff,sum;
00187               diff = *this - qd;
00188               sum = *this + qd;
00189               if( diff.dot(diff) > sum.dot(sum) )
00190                      return qd;
00191               return (-qd);
00192        }
00193 
00194        btQuaternion slerp(const btQuaternion& q, const btScalar& t) const
00195        {
00196               btScalar theta = angle(q);
00197               if (theta != btScalar(0.0))
00198               {
00199                      btScalar d = btScalar(1.0) / btSin(theta);
00200                      btScalar s0 = btSin((btScalar(1.0) - t) * theta);
00201                      btScalar s1 = btSin(t * theta);   
00202                      return btQuaternion((m_x * s0 + q.x() * s1) * d,
00203                             (m_y * s0 + q.y() * s1) * d,
00204                             (m_z * s0 + q.z() * s1) * d,
00205                             (m_unusedW * s0 + q.m_unusedW * s1) * d);
00206               }
00207               else
00208               {
00209                      return *this;
00210               }
00211        }
00212 
00213        SIMD_FORCE_INLINE const btScalar& getW() const { return m_unusedW; }
00214 
00215        
00216 };
00217 
00218 
00219 
00220 SIMD_FORCE_INLINE btQuaternion
00221 operator-(const btQuaternion& q)
00222 {
00223        return btQuaternion(-q.x(), -q.y(), -q.z(), -q.w());
00224 }
00225 
00226 
00227 
00228 
00229 SIMD_FORCE_INLINE btQuaternion
00230 operator*(const btQuaternion& q1, const btQuaternion& q2) {
00231        return btQuaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
00232               q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
00233               q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
00234               q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); 
00235 }
00236 
00237 SIMD_FORCE_INLINE btQuaternion
00238 operator*(const btQuaternion& q, const btVector3& w)
00239 {
00240        return btQuaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
00241               q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
00242               q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
00243               -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); 
00244 }
00245 
00246 SIMD_FORCE_INLINE btQuaternion
00247 operator*(const btVector3& w, const btQuaternion& q)
00248 {
00249        return btQuaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
00250               w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
00251               w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
00252               -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); 
00253 }
00254 
00255 SIMD_FORCE_INLINE btScalar 
00256 dot(const btQuaternion& q1, const btQuaternion& q2) 
00257 { 
00258        return q1.dot(q2); 
00259 }
00260 
00261 
00262 SIMD_FORCE_INLINE btScalar
00263 length(const btQuaternion& q) 
00264 { 
00265        return q.length(); 
00266 }
00267 
00268 SIMD_FORCE_INLINE btScalar
00269 angle(const btQuaternion& q1, const btQuaternion& q2) 
00270 { 
00271        return q1.angle(q2); 
00272 }
00273 
00274 
00275 SIMD_FORCE_INLINE btQuaternion
00276 inverse(const btQuaternion& q) 
00277 {
00278        return q.inverse();
00279 }
00280 
00281 SIMD_FORCE_INLINE btQuaternion
00282 slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t) 
00283 {
00284        return q1.slerp(q2, t);
00285 }
00286 
00287 SIMD_FORCE_INLINE btVector3 
00288 quatRotate(const btQuaternion& rotation, const btVector3& v) 
00289 {
00290        btQuaternion q = rotation * v;
00291        q *= rotation.inverse();
00292        return btVector3(q.getX(),q.getY(),q.getZ());
00293 }
00294 
00295 SIMD_FORCE_INLINE btQuaternion 
00296 shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
00297 {
00298        btVector3 c = v0.cross(v1);
00299        btScalar  d = v0.dot(v1);
00300 
00301        if (d < -1.0 + SIMD_EPSILON)
00302               return btQuaternion(0.0f,1.0f,0.0f,0.0f); // just pick any vector
00303 
00304        btScalar  s = btSqrt((1.0f + d) * 2.0f);
00305        btScalar rs = 1.0f / s;
00306 
00307        return btQuaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
00308 }
00309 
00310 SIMD_FORCE_INLINE btQuaternion 
00311 shortestArcQuatNormalize2(btVector3& v0,btVector3& v1)
00312 {
00313        v0.normalize();
00314        v1.normalize();
00315        return shortestArcQuat(v0,v1);
00316 }
00317 
00318 #endif
00319 
00320 
00321 
00322