src/LinearMath/btQuaternion.h

   1 /*
   2 Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans  http://continuousphysics.com/Bullet/
   3
   4 This software is provided 'as-is', without any express or implied warranty.
   5 In no event will the authors be held liable for any damages arising from the use of this software.
   6 Permission is granted to anyone to use this software for any purpose,
   7 including commercial applications, and to alter it and redistribute it freely,
   8 subject to the following restrictions:
   9
  10 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.
  11 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  12 3. This notice may not be removed or altered from any source distribution.
  13 */
  14
  15
  16
  17 #ifndef BT_SIMD__QUATERNION_H_
  18 #define BT_SIMD__QUATERNION_H_
  19
  20
  21 #include "btVector3.h"
  22 #include "btQuadWord.h"
  23
  24 /**@brief The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatrix3x3, btVector3 and btTransform. */
  25 class btQuaternion : public btQuadWord {
  26 public:
  27   /**@brief No initialization constructor */
  28         btQuaternion() {}
  29
  30         //              template <typename btScalar>
  31         //              explicit Quaternion(const btScalar *v) : Tuple4<btScalar>(v) {}
  32   /**@brief Constructor from scalars */
  33         btQuaternion(const btScalar& x, const btScalar& y, const btScalar& z, const btScalar& w)
  34                 : btQuadWord(x, y, z, w)
  35         {}
  36   /**@brief Axis angle Constructor
  37    * @param axis The axis which the rotation is around
  38    * @param angle The magnitude of the rotation around the angle (Radians) */
  39         btQuaternion(const btVector3& axis, const btScalar& angle)
  40         {
  41                 setRotation(axis, angle);
  42         }
  43   /**@brief Constructor from Euler angles
  44    * @param yaw Angle around Y unless BT_EULER_DEFAULT_ZYX defined then Z
  45    * @param pitch Angle around X unless BT_EULER_DEFAULT_ZYX defined then Y
  46    * @param roll Angle around Z unless BT_EULER_DEFAULT_ZYX defined then X */
  47         btQuaternion(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
  48         {
  49 #ifndef BT_EULER_DEFAULT_ZYX
  50                 setEuler(yaw, pitch, roll);
  51 #else
  52                 setEulerZYX(yaw, pitch, roll);
  53 #endif
  54         }
  55   /**@brief Set the rotation using axis angle notation
  56    * @param axis The axis around which to rotate
  57    * @param angle The magnitude of the rotation in Radians */
  58         void setRotation(const btVector3& axis, const btScalar& angle)
  59         {
  60                 btScalar d = axis.length();
  61                 btAssert(d != btScalar(0.0));
  62                 btScalar s = btSin(angle * btScalar(0.5)) / d;
  63                 setValue(axis.x() * s, axis.y() * s, axis.z() * s,
  64                         btCos(angle * btScalar(0.5)));
  65         }
  66   /**@brief Set the quaternion using Euler angles
  67    * @param yaw Angle around Y
  68    * @param pitch Angle around X
  69    * @param roll Angle around Z */
  70         void setEuler(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
  71         {
  72                 btScalar halfYaw = btScalar(yaw) * btScalar(0.5);
  73                 btScalar halfPitch = btScalar(pitch) * btScalar(0.5);
  74                 btScalar halfRoll = btScalar(roll) * btScalar(0.5);
  75                 btScalar cosYaw = btCos(halfYaw);
  76                 btScalar sinYaw = btSin(halfYaw);
  77                 btScalar cosPitch = btCos(halfPitch);
  78                 btScalar sinPitch = btSin(halfPitch);
  79                 btScalar cosRoll = btCos(halfRoll);
  80                 btScalar sinRoll = btSin(halfRoll);
  81                 setValue(cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw,
  82                         cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw,
  83                         sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw,
  84                         cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw);
  85         }
  86   /**@brief Set the quaternion using euler angles
  87    * @param yaw Angle around Z
  88    * @param pitch Angle around Y
  89    * @param roll Angle around X */
  90         void setEulerZYX(const btScalar& yaw, const btScalar& pitch, const btScalar& roll)
  91         {
  92                 btScalar halfYaw = btScalar(yaw) * btScalar(0.5);
  93                 btScalar halfPitch = btScalar(pitch) * btScalar(0.5);
  94                 btScalar halfRoll = btScalar(roll) * btScalar(0.5);
  95                 btScalar cosYaw = btCos(halfYaw);
  96                 btScalar sinYaw = btSin(halfYaw);
  97                 btScalar cosPitch = btCos(halfPitch);
  98                 btScalar sinPitch = btSin(halfPitch);
  99                 btScalar cosRoll = btCos(halfRoll);
 100                 btScalar sinRoll = btSin(halfRoll);
 101                 setValue(sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, //x
 102                          cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y
 103                          cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
 104                          cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx
 105         }
 106   /**@brief Add two quaternions
 107    * @param q The quaternion to add to this one */
 108         SIMD_FORCE_INLINE       btQuaternion& operator+=(const btQuaternion& q)
 109         {
 110                 m_floats[0] += q.x(); m_floats[1] += q.y(); m_floats[2] += q.z(); m_floats[3] += q.m_floats[3];
 111                 return *this;
 112         }
 113
 114   /**@brief Subtract out a quaternion
 115    * @param q The quaternion to subtract from this one */
 116         btQuaternion& operator-=(const btQuaternion& q)
 117         {
 118                 m_floats[0] -= q.x(); m_floats[1] -= q.y(); m_floats[2] -= q.z(); m_floats[3] -= q.m_floats[3];
 119                 return *this;
 120         }
 121
 122   /**@brief Scale this quaternion
 123    * @param s The scalar to scale by */
 124         btQuaternion& operator*=(const btScalar& s)
 125         {
 126                 m_floats[0] *= s; m_floats[1] *= s; m_floats[2] *= s; m_floats[3] *= s;
 127                 return *this;
 128         }
 129
 130   /**@brief Multiply this quaternion by q on the right
 131    * @param q The other quaternion
 132    * Equivilant to this = this * q */
 133         btQuaternion& operator*=(const btQuaternion& q)
 134         {
 135                 setValue(m_floats[3] * q.x() + m_floats[0] * q.m_floats[3] + m_floats[1] * q.z() - m_floats[2] * q.y(),
 136                         m_floats[3] * q.y() + m_floats[1] * q.m_floats[3] + m_floats[2] * q.x() - m_floats[0] * q.z(),
 137                         m_floats[3] * q.z() + m_floats[2] * q.m_floats[3] + m_floats[0] * q.y() - m_floats[1] * q.x(),
 138                         m_floats[3] * q.m_floats[3] - m_floats[0] * q.x() - m_floats[1] * q.y() - m_floats[2] * q.z());
 139                 return *this;
 140         }
 141   /**@brief Return the dot product between this quaternion and another
 142    * @param q The other quaternion */
 143         btScalar dot(const btQuaternion& q) const
 144         {
 145                 return m_floats[0] * q.x() + m_floats[1] * q.y() + m_floats[2] * q.z() + m_floats[3] * q.m_floats[3];
 146         }
 147
 148   /**@brief Return the length squared of the quaternion */
 149         btScalar length2() const
 150         {
 151                 return dot(*this);
 152         }
 153
 154   /**@brief Return the length of the quaternion */
 155         btScalar length() const
 156         {
 157                 return btSqrt(length2());
 158         }
 159
 160   /**@brief Normalize the quaternion
 161    * Such that x^2 + y^2 + z^2 +w^2 = 1 */
 162         btQuaternion& normalize()
 163         {
 164                 return *this /= length();
 165         }
 166
 167   /**@brief Return a scaled version of this quaternion
 168    * @param s The scale factor */
 169         SIMD_FORCE_INLINE btQuaternion
 170         operator*(const btScalar& s) const
 171         {
 172                 return btQuaternion(x() * s, y() * s, z() * s, m_floats[3] * s);
 173         }
 174
 175
 176   /**@brief Return an inversely scaled versionof this quaternion
 177    * @param s The inverse scale factor */
 178         btQuaternion operator/(const btScalar& s) const
 179         {
 180                 btAssert(s != btScalar(0.0));
 181                 return *this * (btScalar(1.0) / s);
 182         }
 183
 184   /**@brief Inversely scale this quaternion
 185    * @param s The scale factor */
 186         btQuaternion& operator/=(const btScalar& s)
 187         {
 188                 btAssert(s != btScalar(0.0));
 189                 return *this *= btScalar(1.0) / s;
 190         }
 191
 192   /**@brief Return a normalized version of this quaternion */
 193         btQuaternion normalized() const
 194         {
 195                 return *this / length();
 196         }
 197   /**@brief Return the angle between this quaternion and the other
 198    * @param q The other quaternion */
 199         btScalar angle(const btQuaternion& q) const
 200         {
 201                 btScalar s = btSqrt(length2() * q.length2());
 202                 btAssert(s != btScalar(0.0));
 203                 return btAcos(dot(q) / s);
 204         }
 205   /**@brief Return the angle of rotation represented by this quaternion */
 206         btScalar getAngle() const
 207         {
 208                 btScalar s = btScalar(2.) * btAcos(m_floats[3]);
 209                 return s;
 210         }
 211
 212         /**@brief Return the axis of the rotation represented by this quaternion */
 213         btVector3 getAxis() const
 214         {
 215                 btScalar s_squared = 1.f-m_floats[3]*m_floats[3];
 216
 217                 if (s_squared < btScalar(10.) * SIMD_EPSILON) //Check for divide by zero
 218                         return btVector3(1.0, 0.0, 0.0);  // Arbitrary
 219                 btScalar s = 1.f/btSqrt(s_squared);
 220                 return btVector3(m_floats[0] * s, m_floats[1] * s, m_floats[2] * s);
 221         }
 222
 223         /**@brief Return the inverse of this quaternion */
 224         btQuaternion inverse() const
 225         {
 226                 return btQuaternion(-m_floats[0], -m_floats[1], -m_floats[2], m_floats[3]);
 227         }
 228
 229   /**@brief Return the sum of this quaternion and the other
 230    * @param q2 The other quaternion */
 231         SIMD_FORCE_INLINE btQuaternion
 232         operator+(const btQuaternion& q2) const
 233         {
 234                 const btQuaternion& q1 = *this;
 235                 return btQuaternion(q1.x() + q2.x(), q1.y() + q2.y(), q1.z() + q2.z(), q1.m_floats[3] + q2.m_floats[3]);
 236         }
 237
 238   /**@brief Return the difference between this quaternion and the other
 239    * @param q2 The other quaternion */
 240         SIMD_FORCE_INLINE btQuaternion
 241         operator-(const btQuaternion& q2) const
 242         {
 243                 const btQuaternion& q1 = *this;
 244                 return btQuaternion(q1.x() - q2.x(), q1.y() - q2.y(), q1.z() - q2.z(), q1.m_floats[3] - q2.m_floats[3]);
 245         }
 246
 247   /**@brief Return the negative of this quaternion
 248    * This simply negates each element */
 249         SIMD_FORCE_INLINE btQuaternion operator-() const
 250         {
 251                 const btQuaternion& q2 = *this;
 252                 return btQuaternion( - q2.x(), - q2.y(),  - q2.z(),  - q2.m_floats[3]);
 253         }
 254   /**@todo document this and it's use */
 255         SIMD_FORCE_INLINE btQuaternion farthest( const btQuaternion& qd) const
 256         {
 257                 btQuaternion diff,sum;
 258                 diff = *this - qd;
 259                 sum = *this + qd;
 260                 if( diff.dot(diff) > sum.dot(sum) )
 261                         return qd;
 262                 return (-qd);
 263         }
 264
 265         /**@todo document this and it's use */
 266         SIMD_FORCE_INLINE btQuaternion nearest( const btQuaternion& qd) const
 267         {
 268                 btQuaternion diff,sum;
 269                 diff = *this - qd;
 270                 sum = *this + qd;
 271                 if( diff.dot(diff) < sum.dot(sum) )
 272                         return qd;
 273                 return (-qd);
 274         }
 275
 276
 277   /**@brief Return the quaternion which is the result of Spherical Linear Interpolation between this and the other quaternion
 278    * @param q The other quaternion to interpolate with
 279    * @param t The ratio between this and q to interpolate.  If t = 0 the result is this, if t=1 the result is q.
 280    * Slerp interpolates assuming constant velocity.  */
 281         btQuaternion slerp(const btQuaternion& q, const btScalar& t) const
 282         {
 283           btScalar magnitude = btSqrt(length2() * q.length2());
 284           btAssert(magnitude > btScalar(0));
 285
 286     btScalar product = dot(q) / magnitude;
 287     if (btFabs(product) != btScalar(1))
 288                 {
 289       // Take care of long angle case see http://en.wikipedia.org/wiki/Slerp
 290       const btScalar sign = (product < 0) ? btScalar(-1) : btScalar(1);
 291
 292       const btScalar theta = btAcos(sign * product);
 293       const btScalar s1 = btSin(sign * t * theta);
 294       const btScalar d = btScalar(1.0) / btSin(theta);
 295       const btScalar s0 = btSin((btScalar(1.0) - t) * theta);
 296
 297       return btQuaternion(
 298           (m_floats[0] * s0 + q.x() * s1) * d,
 299           (m_floats[1] * s0 + q.y() * s1) * d,
 300           (m_floats[2] * s0 + q.z() * s1) * d,
 301           (m_floats[3] * s0 + q.m_floats[3] * s1) * d);
 302                 }
 303                 else
 304                 {
 305                         return *this;
 306                 }
 307         }
 308
 309         static const btQuaternion&      getIdentity()
 310         {
 311                 static const btQuaternion identityQuat(btScalar(0.),btScalar(0.),btScalar(0.),btScalar(1.));
 312                 return identityQuat;
 313         }
 314
 315         SIMD_FORCE_INLINE const btScalar& getW() const { return m_floats[3]; }
 316
 317
 318 };
 319
 320
 321
 322
 323
 324 /**@brief Return the product of two quaternions */
 325 SIMD_FORCE_INLINE btQuaternion
 326 operator*(const btQuaternion& q1, const btQuaternion& q2) {
 327         return btQuaternion(q1.w() * q2.x() + q1.x() * q2.w() + q1.y() * q2.z() - q1.z() * q2.y(),
 328                 q1.w() * q2.y() + q1.y() * q2.w() + q1.z() * q2.x() - q1.x() * q2.z(),
 329                 q1.w() * q2.z() + q1.z() * q2.w() + q1.x() * q2.y() - q1.y() * q2.x(),
 330                 q1.w() * q2.w() - q1.x() * q2.x() - q1.y() * q2.y() - q1.z() * q2.z());
 331 }
 332
 333 SIMD_FORCE_INLINE btQuaternion
 334 operator*(const btQuaternion& q, const btVector3& w)
 335 {
 336         return btQuaternion( q.w() * w.x() + q.y() * w.z() - q.z() * w.y(),
 337                 q.w() * w.y() + q.z() * w.x() - q.x() * w.z(),
 338                 q.w() * w.z() + q.x() * w.y() - q.y() * w.x(),
 339                 -q.x() * w.x() - q.y() * w.y() - q.z() * w.z());
 340 }
 341
 342 SIMD_FORCE_INLINE btQuaternion
 343 operator*(const btVector3& w, const btQuaternion& q)
 344 {
 345         return btQuaternion( w.x() * q.w() + w.y() * q.z() - w.z() * q.y(),
 346                 w.y() * q.w() + w.z() * q.x() - w.x() * q.z(),
 347                 w.z() * q.w() + w.x() * q.y() - w.y() * q.x(),
 348                 -w.x() * q.x() - w.y() * q.y() - w.z() * q.z());
 349 }
 350
 351 /**@brief Calculate the dot product between two quaternions */
 352 SIMD_FORCE_INLINE btScalar
 353 dot(const btQuaternion& q1, const btQuaternion& q2)
 354 {
 355         return q1.dot(q2);
 356 }
 357
 358
 359 /**@brief Return the length of a quaternion */
 360 SIMD_FORCE_INLINE btScalar
 361 length(const btQuaternion& q)
 362 {
 363         return q.length();
 364 }
 365
 366 /**@brief Return the angle between two quaternions*/
 367 SIMD_FORCE_INLINE btScalar
 368 angle(const btQuaternion& q1, const btQuaternion& q2)
 369 {
 370         return q1.angle(q2);
 371 }
 372
 373 /**@brief Return the inverse of a quaternion*/
 374 SIMD_FORCE_INLINE btQuaternion
 375 inverse(const btQuaternion& q)
 376 {
 377         return q.inverse();
 378 }
 379
 380 /**@brief Return the result of spherical linear interpolation betwen two quaternions
 381  * @param q1 The first quaternion
 382  * @param q2 The second quaternion
 383  * @param t The ration between q1 and q2.  t = 0 return q1, t=1 returns q2
 384  * Slerp assumes constant velocity between positions. */
 385 SIMD_FORCE_INLINE btQuaternion
 386 slerp(const btQuaternion& q1, const btQuaternion& q2, const btScalar& t)
 387 {
 388         return q1.slerp(q2, t);
 389 }
 390
 391 SIMD_FORCE_INLINE btVector3
 392 quatRotate(const btQuaternion& rotation, const btVector3& v)
 393 {
 394         btQuaternion q = rotation * v;
 395         q *= rotation.inverse();
 396         return btVector3(q.getX(),q.getY(),q.getZ());
 397 }
 398
 399 SIMD_FORCE_INLINE btQuaternion
 400 shortestArcQuat(const btVector3& v0, const btVector3& v1) // Game Programming Gems 2.10. make sure v0,v1 are normalized
 401 {
 402         btVector3 c = v0.cross(v1);
 403         btScalar  d = v0.dot(v1);
 404
 405         if (d < -1.0 + SIMD_EPSILON)
 406         {
 407                 btVector3 n,unused;
 408                 btPlaneSpace1(v0,n,unused);
 409                 return btQuaternion(n.x(),n.y(),n.z(),0.0f); // just pick any vector that is orthogonal to v0
 410         }
 411
 412         btScalar  s = btSqrt((1.0f + d) * 2.0f);
 413         btScalar rs = 1.0f / s;
 414
 415         return btQuaternion(c.getX()*rs,c.getY()*rs,c.getZ()*rs,s * 0.5f);
 416 }
 417
 418 SIMD_FORCE_INLINE btQuaternion
 419 shortestArcQuatNormalize2(btVector3& v0,btVector3& v1)
 420 {
 421         v0.normalize();
 422         v1.normalize();
 423         return shortestArcQuat(v0,v1);
 424 }
 425
 426 #endif //BT_SIMD__QUATERNION_H_
 427
 428
 429
 430