Quaternion.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 TF_QUATERNION_H_
00018 #define TF_QUATERNION_H_
00019 
00020 
00021 #include "Vector3.h"
00022 #include "QuadWord.h"
00023 
00024 namespace tf
00025 {
00026 
00028 class Quaternion : public QuadWord {
00029 public:
00031         Quaternion() {}
00032 
00033 
00034         //              template <typename tfScalar>
00035         //              explicit Quaternion(const tfScalar *v) : Tuple4<tfScalar>(v) {}
00037         Quaternion(const tfScalar& x, const tfScalar& y, const tfScalar& z, const tfScalar& w) 
00038                 : QuadWord(x, y, z, w) 
00039         {}
00043         Quaternion(const Vector3& axis, const tfScalar& angle) 
00044         { 
00045                 setRotation(axis, angle); 
00046         }
00051   Quaternion(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll) __attribute__((deprecated))
00052         { 
00053 #ifndef TF_EULER_DEFAULT_ZYX
00054                 setEuler(yaw, pitch, roll); 
00055 #else
00056                 setRPY(roll, pitch, yaw);
00057 #endif 
00058         }
00062         void setRotation(const Vector3& axis, const tfScalar& angle)
00063         {
00064                 tfScalar d = axis.length();
00065                 tfAssert(d != tfScalar(0.0));
00066                 tfScalar s = tfSin(angle * tfScalar(0.5)) / d;
00067                 setValue(axis.x() * s, axis.y() * s, axis.z() * s, 
00068                         tfCos(angle * tfScalar(0.5)));
00069         }
00074         void setEuler(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll)
00075         {
00076                 tfScalar halfYaw = tfScalar(yaw) * tfScalar(0.5);  
00077                 tfScalar halfPitch = tfScalar(pitch) * tfScalar(0.5);  
00078                 tfScalar halfRoll = tfScalar(roll) * tfScalar(0.5);  
00079                 tfScalar cosYaw = tfCos(halfYaw);
00080                 tfScalar sinYaw = tfSin(halfYaw);
00081                 tfScalar cosPitch = tfCos(halfPitch);
00082                 tfScalar sinPitch = tfSin(halfPitch);
00083                 tfScalar cosRoll = tfCos(halfRoll);
00084                 tfScalar sinRoll = tfSin(halfRoll);
00085                 setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
00086                         cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
00087                         sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
00088                         cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
00089         }
00094   void setRPY(const tfScalar& roll, const tfScalar& pitch, const tfScalar& yaw)
00095         {
00096                 tfScalar halfYaw = tfScalar(yaw) * tfScalar(0.5);  
00097                 tfScalar halfPitch = tfScalar(pitch) * tfScalar(0.5);  
00098                 tfScalar halfRoll = tfScalar(roll) * tfScalar(0.5);  
00099                 tfScalar cosYaw = tfCos(halfYaw);
00100                 tfScalar sinYaw = tfSin(halfYaw);
00101                 tfScalar cosPitch = tfCos(halfPitch);
00102                 tfScalar sinPitch = tfSin(halfPitch);
00103                 tfScalar cosRoll = tfCos(halfRoll);
00104                 tfScalar sinRoll = tfSin(halfRoll);
00105                 setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
00106                          cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
00107                          cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
00108                          cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
00109         }
00114   void setEulerZYX(const tfScalar& yaw, const tfScalar& pitch, const tfScalar& roll) __attribute__((deprecated))
00115         {
00116           setRPY(roll, pitch, yaw);
00117         }
00120         TFSIMD_FORCE_INLINE     Quaternion& operator+=(const Quaternion& q)
00121         {
00122                 m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3];
00123                 return *this;
00124         }
00125 
00128         Quaternion& operator-=(const Quaternion& q) 
00129         {
00130                 m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3];
00131                 return *this;
00132         }
00133 
00136         Quaternion& operator*=(const tfScalar& s)
00137         {
00138                 m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s;
00139                 return *this;
00140         }
00141 
00145         Quaternion& operator*=(const Quaternion& q)
00146         {
00147                 setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
00148                         m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
00149                         m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
00150                         m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
00151                 return *this;
00152         }
00155         tfScalar dot(const Quaternion& q) const
00156         {
00157                 return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3];
00158         }
00159 
00161         tfScalar length2() const
00162         {
00163                 return dot(*this);
00164         }
00165 
00167         tfScalar length() const
00168         {
00169                 return tfSqrt(length2());
00170         }
00171 
00174         Quaternion& normalize() 
00175         {
00176                 return *this /= length();
00177         }
00178 
00181         TFSIMD_FORCE_INLINE Quaternion
00182         operator*(const tfScalar& s) const
00183         {
00184                 return Quaternion(x() * s, y() * s, z() * s, m_floats[3] * s);
00185         }
00186 
00187 
00190         Quaternion operator/(const tfScalar& s) const
00191         {
00192                 tfAssert(s != tfScalar(0.0));
00193                 return *this * (tfScalar(1.0) / s);
00194         }
00195 
00198         Quaternion& operator/=(const tfScalar& s) 
00199         {
00200                 tfAssert(s != tfScalar(0.0));
00201                 return *this *= tfScalar(1.0) / s;
00202         }
00203 
00205         Quaternion normalized() const 
00206         {
00207                 return *this / length();
00208         } 
00211         tfScalar angle(const Quaternion& q) const 
00212         {
00213                 tfScalar s = tfSqrt(length2() * q.length2());
00214                 tfAssert(s != tfScalar(0.0));
00215                 return tfAcos(dot(q) / s);
00216         }
00219         tfScalar angleShortestPath(const Quaternion& q) const 
00220         {
00221                 tfScalar s = tfSqrt(length2() * q.length2());
00222                 tfAssert(s != tfScalar(0.0));
00223                 if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
00224                         return tfAcos(dot(-q) / s) * tfScalar(2.0);
00225                 else 
00226                         return tfAcos(dot(q) / s) * tfScalar(2.0);
00227         }
00229         tfScalar getAngle() const 
00230         {
00231                 tfScalar s = tfScalar(2.) * tfAcos(m_floats[3]);
00232                 return s;
00233         }
00234 
00236         tfScalar getAngleShortestPath() const 
00237         {
00238                 tfScalar s;
00239                 if (m_floats[3] < 0)
00240                     s = tfScalar(2.) * tfAcos(-m_floats[3]);
00241                 else
00242                     s = tfScalar(2.) * tfAcos(m_floats[3]);
00243                 return s;
00244         }
00245 
00247         Vector3 getAxis() const
00248         {
00249                 tfScalar s_squared = tfScalar(1.) - tfPow(m_floats[3], tfScalar(2.));
00250                 if (s_squared < tfScalar(10.) * TFSIMD_EPSILON) //Check for divide by zero
00251                         return Vector3(1.0, 0.0, 0.0);  // Arbitrary
00252                 tfScalar s = tfSqrt(s_squared);
00253                 return Vector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s);
00254         }
00255 
00257         Quaternion inverse() const
00258         {
00259                 return Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
00260         }
00261 
00264         TFSIMD_FORCE_INLINE Quaternion
00265         operator+(const Quaternion& q2) const
00266         {
00267                 const Quaternion& q1 = *this;
00268                 return Quaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]);
00269         }
00270 
00273         TFSIMD_FORCE_INLINE Quaternion
00274         operator-(const Quaternion& q2) const
00275         {
00276                 const Quaternion& q1 = *this;
00277                 return Quaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]);
00278         }
00279 
00282         TFSIMD_FORCE_INLINE Quaternion operator-() const
00283         {
00284                 const Quaternion& q2 = *this;
00285                 return Quaternion( - q2.x(), - q2.y(),  - q2.z(),  - q2.m_floats[3]);
00286         }
00288         TFSIMD_FORCE_INLINE Quaternion farthest( const Quaternion& qd) const 
00289         {
00290                 Quaternion diff,sum;
00291                 diff = *this - qd;
00292                 sum = *this + qd;
00293                 if( diff.dot(diff) > sum.dot(sum) )
00294                         return qd;
00295                 return (-qd);
00296         }
00297 
00299         TFSIMD_FORCE_INLINE Quaternion nearest( const Quaternion& qd) const 
00300         {
00301                 Quaternion diff,sum;
00302                 diff = *this - qd;
00303                 sum = *this + qd;
00304                 if( diff.dot(diff) < sum.dot(sum) )
00305                         return qd;
00306                 return (-qd);
00307         }
00308 
00309 
00314         Quaternion slerp(const Quaternion& q, const tfScalar& t) const
00315         {
00316           tfScalar theta = angleShortestPath(q) / tfScalar(2.0);
00317                 if (theta != tfScalar(0.0))
00318                 {
00319                         tfScalar d = tfScalar(1.0) / tfSin(theta);
00320                         tfScalar s0 = tfSin((tfScalar(1.0) - t) * theta);
00321                         tfScalar s1 = tfSin(t * theta);   
00322                         if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
00323                           return Quaternion((m_floats[0] * s0 + -q.x() * s1) * d,
00324                                               (m_floats[1] * s0 + -q.y() * s1) * d,
00325                                               (m_floats[2] * s0 + -q.z() * s1) * d,
00326                                               (m_floats[3] * s0 + -q.m_floats[3] * s1) * d);
00327                         else
00328                           return Quaternion((m_floats[0] * s0 + q.x() * s1) * d,
00329                                               (m_floats[1] * s0 + q.y() * s1) * d,
00330                                               (m_floats[2] * s0 + q.z() * s1) * d,
00331                                               (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
00332                         
00333                 }
00334                 else
00335                 {
00336                         return *this;
00337                 }
00338         }
00339 
00340         static const Quaternion&        getIdentity()
00341         {
00342                 static const Quaternion identityQuat(tfScalar(0.),tfScalar(0.),tfScalar(0.),tfScalar(1.));
00343                 return identityQuat;
00344         }
00345 
00346         TFSIMD_FORCE_INLINE const tfScalar& getW() const { return m_floats[3]; }
00347 
00348         
00349 };
00350 
00351 
00353 TFSIMD_FORCE_INLINE Quaternion
00354 operator-(const Quaternion& q)
00355 {
00356         return Quaternion(-q.x(), -q.y(), -q.z(), -q.w());
00357 }
00358 
00359 
00360 
00362 TFSIMD_FORCE_INLINE Quaternion
00363 operator*(const Quaternion& q1, const Quaternion& q2) {
00364         return Quaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
00365                 q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
00366                 q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
00367                 q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); 
00368 }
00369 
00370 TFSIMD_FORCE_INLINE Quaternion
00371 operator*(const Quaternion& q, const Vector3& w)
00372 {
00373         return Quaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
00374                 q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
00375                 q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
00376                 -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); 
00377 }
00378 
00379 TFSIMD_FORCE_INLINE Quaternion
00380 operator*(const Vector3& w, const Quaternion& q)
00381 {
00382         return Quaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
00383                 w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
00384                 w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
00385                 -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); 
00386 }
00387 
00389 TFSIMD_FORCE_INLINE tfScalar 
00390 dot(const Quaternion& q1, const Quaternion& q2) 
00391 { 
00392         return q1.dot(q2); 
00393 }
00394 
00395 
00397 TFSIMD_FORCE_INLINE tfScalar
00398 length(const Quaternion& q) 
00399 { 
00400         return q.length(); 
00401 }
00402 
00404 TFSIMD_FORCE_INLINE tfScalar
00405 angle(const Quaternion& q1, const Quaternion& q2) 
00406 { 
00407         return q1.angle(q2); 
00408 }
00409 
00411 TFSIMD_FORCE_INLINE tfScalar
00412 angleShortestPath(const Quaternion& q1, const Quaternion& q2) 
00413 { 
00414         return q1.angleShortestPath(q2); 
00415 }
00416 
00418 TFSIMD_FORCE_INLINE Quaternion
00419 inverse(const Quaternion& q) 
00420 {
00421         return q.inverse();
00422 }
00423 
00429 TFSIMD_FORCE_INLINE Quaternion
00430 slerp(const Quaternion& q1, const Quaternion& q2, const tfScalar& t) 
00431 {
00432         return q1.slerp(q2, t);
00433 }
00434 
00435 TFSIMD_FORCE_INLINE Vector3 
00436 quatRotate(const Quaternion& rotation, const Vector3& v) 
00437 {
00438         Quaternion q = rotation * v;
00439         q *= rotation.inverse();
00440         return Vector3(q.getX(),q.getY(),q.getZ());
00441 }
00442 
00443 TFSIMD_FORCE_INLINE Quaternion 
00444 shortestArcQuat(const Vector3& v0, const Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
00445 {
00446         Vector3 c = v0.cross(v1);
00447         tfScalar  d = v0.dot(v1);
00448 
00449         if (d < -1.0 + TFSIMD_EPSILON)
00450         {
00451                 Vector3 n,unused;
00452                 tfPlaneSpace1(v0,n,unused);
00453                 return Quaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0
00454         }
00455 
00456         tfScalar  s = tfSqrt((1.0f + d) * 2.0f);
00457         tfScalar rs = 1.0f / s;
00458 
00459         return Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
00460 }
00461 
00462 TFSIMD_FORCE_INLINE Quaternion 
00463 shortestArcQuatNormalize2(Vector3& v0,Vector3& v1)
00464 {
00465         v0.normalize();
00466         v1.normalize();
00467         return shortestArcQuat(v0,v1);
00468 }
00469 
00470 }
00471 #endif
00472 
00473 
00474 
00475 


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Aug 11 2017 02:21:55