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 (dot(*this) < 0)
00240                         s = tfScalar(2.) * tfAcos(m_floats[3]);
00241                 else
00242                         s = tfScalar(2.) * tfAcos(-m_floats[3]);
00243 
00244                 return s;
00245         }
00246 
00248         Vector3 getAxis() const
00249         {
00250                 tfScalar s_squared = tfScalar(1.) - tfPow(m_floats[3], tfScalar(2.));
00251                 if (s_squared < tfScalar(10.) * TFSIMD_EPSILON) //Check for divide by zero
00252                         return Vector3(1.0, 0.0, 0.0);  // Arbitrary
00253                 tfScalar s = tfSqrt(s_squared);
00254                 return Vector3(m_floats[0] / s, m_floats[1] / s, m_floats[2] / s);
00255         }
00256 
00258         Quaternion inverse() const
00259         {
00260                 return Quaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
00261         }
00262 
00265         TFSIMD_FORCE_INLINE Quaternion
00266         operator+(const Quaternion& q2) const
00267         {
00268                 const Quaternion& q1 = *this;
00269                 return Quaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]);
00270         }
00271 
00274         TFSIMD_FORCE_INLINE Quaternion
00275         operator-(const Quaternion& q2) const
00276         {
00277                 const Quaternion& q1 = *this;
00278                 return Quaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]);
00279         }
00280 
00283         TFSIMD_FORCE_INLINE Quaternion operator-() const
00284         {
00285                 const Quaternion& q2 = *this;
00286                 return Quaternion( - q2.x(), - q2.y(),  - q2.z(),  - q2.m_floats[3]);
00287         }
00289         TFSIMD_FORCE_INLINE Quaternion farthest( const Quaternion& qd) const 
00290         {
00291                 Quaternion diff,sum;
00292                 diff = *this - qd;
00293                 sum = *this + qd;
00294                 if( diff.dot(diff) > sum.dot(sum) )
00295                         return qd;
00296                 return (-qd);
00297         }
00298 
00300         TFSIMD_FORCE_INLINE Quaternion nearest( const Quaternion& qd) const 
00301         {
00302                 Quaternion diff,sum;
00303                 diff = *this - qd;
00304                 sum = *this + qd;
00305                 if( diff.dot(diff) < sum.dot(sum) )
00306                         return qd;
00307                 return (-qd);
00308         }
00309 
00310 
00315         Quaternion slerp(const Quaternion& q, const tfScalar& t) const
00316         {
00317           tfScalar theta = angleShortestPath(q) / tfScalar(2.0);
00318                 if (theta != tfScalar(0.0))
00319                 {
00320                         tfScalar d = tfScalar(1.0) / tfSin(theta);
00321                         tfScalar s0 = tfSin((tfScalar(1.0) - t) * theta);
00322                         tfScalar s1 = tfSin(t * theta);   
00323                         if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
00324                           return Quaternion((m_floats[0] * s0 + -q.x() * s1) * d,
00325                                               (m_floats[1] * s0 + -q.y() * s1) * d,
00326                                               (m_floats[2] * s0 + -q.z() * s1) * d,
00327                                               (m_floats[3] * s0 + -q.m_floats[3] * s1) * d);
00328                         else
00329                           return Quaternion((m_floats[0] * s0 + q.x() * s1) * d,
00330                                               (m_floats[1] * s0 + q.y() * s1) * d,
00331                                               (m_floats[2] * s0 + q.z() * s1) * d,
00332                                               (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
00333                         
00334                 }
00335                 else
00336                 {
00337                         return *this;
00338                 }
00339         }
00340 
00341         static const Quaternion&        getIdentity()
00342         {
00343                 static const Quaternion identityQuat(tfScalar(0.),tfScalar(0.),tfScalar(0.),tfScalar(1.));
00344                 return identityQuat;
00345         }
00346 
00347         TFSIMD_FORCE_INLINE const tfScalar& getW() const { return m_floats[3]; }
00348 
00349         
00350 };
00351 
00352 
00354 TFSIMD_FORCE_INLINE Quaternion
00355 operator-(const Quaternion& q)
00356 {
00357         return Quaternion(-q.x(), -q.y(), -q.z(), -q.w());
00358 }
00359 
00360 
00361 
00363 TFSIMD_FORCE_INLINE Quaternion
00364 operator*(const Quaternion& q1, const Quaternion& q2) {
00365         return Quaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
00366                 q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
00367                 q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
00368                 q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z()); 
00369 }
00370 
00371 TFSIMD_FORCE_INLINE Quaternion
00372 operator*(const Quaternion& q, const Vector3& w)
00373 {
00374         return Quaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
00375                 q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
00376                 q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
00377                 -q.x() * w.x() - q.y() * w.y() - q.z() * w.z()); 
00378 }
00379 
00380 TFSIMD_FORCE_INLINE Quaternion
00381 operator*(const Vector3& w, const Quaternion& q)
00382 {
00383         return Quaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
00384                 w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
00385                 w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
00386                 -w.x() * q.x() - w.y() * q.y() - w.z() * q.z()); 
00387 }
00388 
00390 TFSIMD_FORCE_INLINE tfScalar 
00391 dot(const Quaternion& q1, const Quaternion& q2) 
00392 { 
00393         return q1.dot(q2); 
00394 }
00395 
00396 
00398 TFSIMD_FORCE_INLINE tfScalar
00399 length(const Quaternion& q) 
00400 { 
00401         return q.length(); 
00402 }
00403 
00405 TFSIMD_FORCE_INLINE tfScalar
00406 angle(const Quaternion& q1, const Quaternion& q2) 
00407 { 
00408         return q1.angle(q2); 
00409 }
00410 
00412 TFSIMD_FORCE_INLINE tfScalar
00413 angleShortestPath(const Quaternion& q1, const Quaternion& q2) 
00414 { 
00415         return q1.angleShortestPath(q2); 
00416 }
00417 
00419 TFSIMD_FORCE_INLINE Quaternion
00420 inverse(const Quaternion& q) 
00421 {
00422         return q.inverse();
00423 }
00424 
00430 TFSIMD_FORCE_INLINE Quaternion
00431 slerp(const Quaternion& q1, const Quaternion& q2, const tfScalar& t) 
00432 {
00433         return q1.slerp(q2, t);
00434 }
00435 
00436 TFSIMD_FORCE_INLINE Vector3 
00437 quatRotate(const Quaternion& rotation, const Vector3& v) 
00438 {
00439         Quaternion q = rotation * v;
00440         q *= rotation.inverse();
00441         return Vector3(q.getX(),q.getY(),q.getZ());
00442 }
00443 
00444 TFSIMD_FORCE_INLINE Quaternion 
00445 shortestArcQuat(const Vector3& v0, const Vector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
00446 {
00447         Vector3 c = v0.cross(v1);
00448         tfScalar  d = v0.dot(v1);
00449 
00450         if (d < -1.0 + TFSIMD_EPSILON)
00451         {
00452                 Vector3 n,unused;
00453                 tfPlaneSpace1(v0,n,unused);
00454                 return Quaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0
00455         }
00456 
00457         tfScalar  s = tfSqrt((1.0f + d) * 2.0f);
00458         tfScalar rs = 1.0f / s;
00459 
00460         return Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
00461 }
00462 
00463 TFSIMD_FORCE_INLINE Quaternion 
00464 shortestArcQuatNormalize2(Vector3& v0,Vector3& v1)
00465 {
00466         v0.normalize();
00467         v1.normalize();
00468         return shortestArcQuat(v0,v1);
00469 }
00470 
00471 }
00472 #endif
00473 
00474 
00475 
00476 


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Thu Aug 27 2015 13:02:09