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 TF2_QUATERNION_H_
00018 #define TF2_QUATERNION_H_
00019 
00020 
00021 #include "Vector3.h"
00022 #include "QuadWord.h"
00023 
00024 namespace tf2
00025 {
00026 
00028 class Quaternion : public QuadWord {
00029 public:
00031         Quaternion() {}
00032 
00033         //              template <typename tf2Scalar>
00034         //              explicit Quaternion(const tf2Scalar *v) : Tuple4<tf2Scalar>(v) {}
00036         Quaternion(const tf2Scalar& x, const tf2Scalar& y, const tf2Scalar& z, const tf2Scalar& w) 
00037                 : QuadWord(x, y, z, w) 
00038         {}
00042         Quaternion(const Vector3& axis, const tf2Scalar& angle) 
00043         { 
00044                 setRotation(axis, angle); 
00045         }
00050   Quaternion(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll) __attribute__((deprecated))
00051         { 
00052 #ifndef TF2_EULER_DEFAULT_ZYX
00053                 setEuler(yaw, pitch, roll); 
00054 #else
00055                 setRPY(roll, pitch, yaw);
00056 #endif 
00057         }
00061         void setRotation(const Vector3& axis, const tf2Scalar& angle)
00062         {
00063                 tf2Scalar d = axis.length();
00064                 tf2Assert(d != tf2Scalar(0.0));
00065                 tf2Scalar s = tf2Sin(angle * tf2Scalar(0.5)) / d;
00066                 setValue(axis.x() * s, axis.y() * s, axis.z() * s, 
00067                         tf2Cos(angle * tf2Scalar(0.5)));
00068         }
00073         void setEuler(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll)
00074         {
00075                 tf2Scalar halfYaw = tf2Scalar(yaw) * tf2Scalar(0.5);  
00076                 tf2Scalar halfPitch = tf2Scalar(pitch) * tf2Scalar(0.5);  
00077                 tf2Scalar halfRoll = tf2Scalar(roll) * tf2Scalar(0.5);  
00078                 tf2Scalar cosYaw = tf2Cos(halfYaw);
00079                 tf2Scalar sinYaw = tf2Sin(halfYaw);
00080                 tf2Scalar cosPitch = tf2Cos(halfPitch);
00081                 tf2Scalar sinPitch = tf2Sin(halfPitch);
00082                 tf2Scalar cosRoll = tf2Cos(halfRoll);
00083                 tf2Scalar sinRoll = tf2Sin(halfRoll);
00084                 setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
00085                         cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
00086                         sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
00087                         cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
00088         }
00093   void setRPY(const tf2Scalar& roll, const tf2Scalar& pitch, const tf2Scalar& yaw)
00094         {
00095                 tf2Scalar halfYaw = tf2Scalar(yaw) * tf2Scalar(0.5);  
00096                 tf2Scalar halfPitch = tf2Scalar(pitch) * tf2Scalar(0.5);  
00097                 tf2Scalar halfRoll = tf2Scalar(roll) * tf2Scalar(0.5);  
00098                 tf2Scalar cosYaw = tf2Cos(halfYaw);
00099                 tf2Scalar sinYaw = tf2Sin(halfYaw);
00100                 tf2Scalar cosPitch = tf2Cos(halfPitch);
00101                 tf2Scalar sinPitch = tf2Sin(halfPitch);
00102                 tf2Scalar cosRoll = tf2Cos(halfRoll);
00103                 tf2Scalar sinRoll = tf2Sin(halfRoll);
00104                 setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
00105                          cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
00106                          cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
00107                          cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
00108         }
00113   void setEulerZYX(const tf2Scalar& yaw, const tf2Scalar& pitch, const tf2Scalar& roll) __attribute__((deprecated))
00114         {
00115           setRPY(roll, pitch, yaw);
00116         }
00119         TF2SIMD_FORCE_INLINE    Quaternion& operator+=(const Quaternion& q)
00120         {
00121                 m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3];
00122                 return *this;
00123         }
00124 
00127         Quaternion& operator-=(const Quaternion& q) 
00128         {
00129                 m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3];
00130                 return *this;
00131         }
00132 
00135         Quaternion& operator*=(const tf2Scalar& s)
00136         {
00137                 m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s;
00138                 return *this;
00139         }
00140 
00144         Quaternion& operator*=(const Quaternion& q)
00145         {
00146                 setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
00147                         m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
00148                         m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
00149                         m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
00150                 return *this;
00151         }
00154         tf2Scalar dot(const Quaternion& q) const
00155         {
00156                 return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3];
00157         }
00158 
00160         tf2Scalar length2() const
00161         {
00162                 return dot(*this);
00163         }
00164 
00166         tf2Scalar length() const
00167         {
00168                 return tf2Sqrt(length2());
00169         }
00170 
00173         Quaternion& normalize() 
00174         {
00175                 return *this /= length();
00176         }
00177 
00180         TF2SIMD_FORCE_INLINE Quaternion
00181         operator*(const tf2Scalar& s) const
00182         {
00183                 return Quaternion(x() * s, y() * s, z() * s, m_floats[3] * s);
00184         }
00185 
00186 
00189         Quaternion operator/(const tf2Scalar& s) const
00190         {
00191                 tf2Assert(s != tf2Scalar(0.0));
00192                 return *this * (tf2Scalar(1.0) / s);
00193         }
00194 
00197         Quaternion& operator/=(const tf2Scalar& s) 
00198         {
00199                 tf2Assert(s != tf2Scalar(0.0));
00200                 return *this *= tf2Scalar(1.0) / s;
00201         }
00202 
00204         Quaternion normalized() const 
00205         {
00206                 return *this / length();
00207         } 
00210         tf2Scalar angle(const Quaternion& q) const 
00211         {
00212                 tf2Scalar s = tf2Sqrt(length2() * q.length2());
00213                 tf2Assert(s != tf2Scalar(0.0));
00214                 return tf2Acos(dot(q) / s);
00215         }
00218         tf2Scalar angleShortestPath(const Quaternion& q) const 
00219         {
00220                 tf2Scalar s = tf2Sqrt(length2() * q.length2());
00221                 tf2Assert(s != tf2Scalar(0.0));
00222                 if (dot(q) < 0) // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
00223                         return tf2Acos(dot(-q) / s) * tf2Scalar(2.0);
00224                 else 
00225                         return tf2Acos(dot(q) / s) * tf2Scalar(2.0);
00226         }
00228         tf2Scalar getAngle() const 
00229         {
00230                 tf2Scalar s = tf2Scalar(2.) * tf2Acos(m_floats[3]);
00231                 return s;
00232         }
00233 
00235         tf2Scalar getAngleShortestPath() const 
00236         {
00237         tf2Scalar s;
00238                 if (m_floats[3] >= 0)
00239                         s = tf2Scalar(2.) * tf2Acos(m_floats[3]);
00240                 else
00241                         s = tf2Scalar(2.) * tf2Acos(-m_floats[3]);
00242 
00243                 return s;
00244         }
00245 
00247         Vector3 getAxis() const
00248         {
00249                 tf2Scalar s_squared = tf2Scalar(1.) - tf2Pow(m_floats[3], tf2Scalar(2.));
00250                 if (s_squared < tf2Scalar(10.) * TF2SIMD_EPSILON) //Check for divide by zero
00251                         return Vector3(1.0, 0.0, 0.0);  // Arbitrary
00252                 tf2Scalar s = tf2Sqrt(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         TF2SIMD_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         TF2SIMD_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         TF2SIMD_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         TF2SIMD_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         TF2SIMD_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 tf2Scalar& t) const
00315         {
00316           tf2Scalar theta = angleShortestPath(q) / tf2Scalar(2.0);
00317                 if (theta != tf2Scalar(0.0))
00318                 {
00319                         tf2Scalar d = tf2Scalar(1.0) / tf2Sin(theta);
00320                         tf2Scalar s0 = tf2Sin((tf2Scalar(1.0) - t) * theta);
00321                         tf2Scalar s1 = tf2Sin(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(tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(0.),tf2Scalar(1.));
00343                 return identityQuat;
00344         }
00345 
00346         TF2SIMD_FORCE_INLINE const tf2Scalar& getW() const { return m_floats[3]; }
00347 
00348         
00349 };
00350 
00351 
00353 TF2SIMD_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 TF2SIMD_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 TF2SIMD_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 TF2SIMD_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 TF2SIMD_FORCE_INLINE tf2Scalar 
00390 dot(const Quaternion& q1, const Quaternion& q2) 
00391 { 
00392         return q1.dot(q2); 
00393 }
00394 
00395 
00397 TF2SIMD_FORCE_INLINE tf2Scalar
00398 length(const Quaternion& q) 
00399 { 
00400         return q.length(); 
00401 }
00402 
00404 TF2SIMD_FORCE_INLINE tf2Scalar
00405 angle(const Quaternion& q1, const Quaternion& q2) 
00406 { 
00407         return q1.angle(q2); 
00408 }
00409 
00411 TF2SIMD_FORCE_INLINE tf2Scalar
00412 angleShortestPath(const Quaternion& q1, const Quaternion& q2) 
00413 { 
00414         return q1.angleShortestPath(q2); 
00415 }
00416 
00418 TF2SIMD_FORCE_INLINE Quaternion
00419 inverse(const Quaternion& q) 
00420 {
00421         return q.inverse();
00422 }
00423 
00429 TF2SIMD_FORCE_INLINE Quaternion
00430 slerp(const Quaternion& q1, const Quaternion& q2, const tf2Scalar& t) 
00431 {
00432         return q1.slerp(q2, t);
00433 }
00434 
00435 TF2SIMD_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 TF2SIMD_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         tf2Scalar  d = v0.dot(v1);
00448 
00449         if (d < -1.0 + TF2SIMD_EPSILON)
00450         {
00451                 Vector3 n,unused;
00452                 tf2PlaneSpace1(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         tf2Scalar  s = tf2Sqrt((1.0f + d) * 2.0f);
00457         tf2Scalar rs = 1.0f / s;
00458 
00459         return Quaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
00460 }
00461 
00462 TF2SIMD_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 


tf2
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Thu Jun 6 2019 20:22:56