simgear/math/SGQuat.hxx

   1 // Copyright (C) 2006  Mathias Froehlich - Mathias.Froehlich@web.de
   2 //
   3 // This library is free software; you can redistribute it and/or
   4 // modify it under the terms of the GNU Library General Public
   5 // License as published by the Free Software Foundation; either
   6 // version 2 of the License, or (at your option) any later version.
   7 //
   8 // This library is distributed in the hope that it will be useful,
   9 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  10 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  11 // Library General Public License for more details.
  12 //
  13 // You should have received a copy of the GNU General Public License
  14 // along with this program; if not, write to the Free Software
  15 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  16 //
  17
  18 #ifndef SGQuat_H
  19 #define SGQuat_H
  20
  21 #ifdef min
  22 #undef min
  23 #endif
  24
  25 #ifdef max
  26 #undef max
  27 #endif
  28
  29 #include <osg/Quat>
  30
  31 template<typename T>
  32 struct SGQuatStorage {
  33   /// Readonly raw storage interface
  34   const T (&data(void) const)[4]
  35   { return _data; }
  36   /// Readonly raw storage interface
  37   T (&data(void))[4]
  38   { return _data; }
  39
  40   void osg() const
  41   { }
  42
  43 private:
  44   T _data[4];
  45 };
  46
  47 template<>
  48 struct SGQuatStorage<double> : public osg::Quat {
  49   /// Access raw data by index, the index is unchecked
  50   const double (&data(void) const)[4]
  51   { return osg::Quat::_v; }
  52   /// Access raw data by index, the index is unchecked
  53   double (&data(void))[4]
  54   { return osg::Quat::_v; }
  55
  56   const osg::Quat& osg() const
  57   { return *this; }
  58   osg::Quat& osg()
  59   { return *this; }
  60 };
  61
  62 /// 3D Vector Class
  63 template<typename T>
  64 class SGQuat : protected SGQuatStorage<T> {
  65 public:
  66   typedef T value_type;
  67
  68   /// Default constructor. Does not initialize at all.
  69   /// If you need them zero initialized, SGQuat::zeros()
  70   SGQuat(void)
  71   {
  72     /// Initialize with nans in the debug build, that will guarantee to have
  73     /// a fast uninitialized default constructor in the release but shows up
  74     /// uninitialized values in the debug build very fast ...
  75 #ifndef NDEBUG
  76     for (unsigned i = 0; i < 4; ++i)
  77       data()[i] = SGLimits<T>::quiet_NaN();
  78 #endif
  79   }
  80   /// Constructor. Initialize by the given values
  81   SGQuat(T _x, T _y, T _z, T _w)
  82   { x() = _x; y() = _y; z() = _z; w() = _w; }
  83   /// Constructor. Initialize by the content of a plain array,
  84   /// make sure it has at least 4 elements
  85   explicit SGQuat(const T* d)
  86   { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; data()[3] = d[3]; }
  87   explicit SGQuat(const osg::Quat& d)
  88   { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; data()[3] = d[3]; }
  89
  90   /// Return a unit quaternion
  91   static SGQuat unit(void)
  92   { return fromRealImag(1, SGVec3<T>(0)); }
  93
  94   /// Return a quaternion from euler angles
  95   static SGQuat fromEulerRad(T z, T y, T x)
  96   {
  97     SGQuat q;
  98     T zd2 = T(0.5)*z; T yd2 = T(0.5)*y; T xd2 = T(0.5)*x;
  99     T Szd2 = sin(zd2); T Syd2 = sin(yd2); T Sxd2 = sin(xd2);
 100     T Czd2 = cos(zd2); T Cyd2 = cos(yd2); T Cxd2 = cos(xd2);
 101     T Cxd2Czd2 = Cxd2*Czd2; T Cxd2Szd2 = Cxd2*Szd2;
 102     T Sxd2Szd2 = Sxd2*Szd2; T Sxd2Czd2 = Sxd2*Czd2;
 103     q.w() = Cxd2Czd2*Cyd2 + Sxd2Szd2*Syd2;
 104     q.x() = Sxd2Czd2*Cyd2 - Cxd2Szd2*Syd2;
 105     q.y() = Cxd2Czd2*Syd2 + Sxd2Szd2*Cyd2;
 106     q.z() = Cxd2Szd2*Cyd2 - Sxd2Czd2*Syd2;
 107     return q;
 108   }
 109
 110   /// Return a quaternion from euler angles in degrees
 111   static SGQuat fromEulerDeg(T z, T y, T x)
 112   {
 113     return fromEulerRad(SGMisc<T>::deg2rad(z), SGMisc<T>::deg2rad(y),
 114                         SGMisc<T>::deg2rad(x));
 115   }
 116
 117   /// Return a quaternion from euler angles
 118   static SGQuat fromYawPitchRoll(T y, T p, T r)
 119   { return fromEulerRad(y, p, r); }
 120
 121   /// Return a quaternion from euler angles
 122   static SGQuat fromYawPitchRollDeg(T y, T p, T r)
 123   { return fromEulerDeg(y, p, r); }
 124
 125   /// Return a quaternion from euler angles
 126   static SGQuat fromHeadAttBank(T h, T a, T b)
 127   { return fromEulerRad(h, a, b); }
 128
 129   /// Return a quaternion from euler angles
 130   static SGQuat fromHeadAttBankDeg(T h, T a, T b)
 131   { return fromEulerDeg(h, a, b); }
 132
 133   /// Return a quaternion rotation the the horizontal local frame from given
 134   /// longitude and latitude
 135   static SGQuat fromLonLatRad(T lon, T lat)
 136   {
 137     SGQuat q;
 138     T zd2 = T(0.5)*lon;
 139     T yd2 = T(-0.25)*SGMisc<value_type>::pi() - T(0.5)*lat;
 140     T Szd2 = sin(zd2);
 141     T Syd2 = sin(yd2);
 142     T Czd2 = cos(zd2);
 143     T Cyd2 = cos(yd2);
 144     q.w() = Czd2*Cyd2;
 145     q.x() = -Szd2*Syd2;
 146     q.y() = Czd2*Syd2;
 147     q.z() = Szd2*Cyd2;
 148     return q;
 149   }
 150
 151   /// Return a quaternion rotation the the horizontal local frame from given
 152   /// longitude and latitude
 153   static SGQuat fromLonLatDeg(T lon, T lat)
 154   { return fromLonLatRad(SGMisc<T>::deg2rad(lon), SGMisc<T>::deg2rad(lat)); }
 155
 156   /// Return a quaternion rotation the the horizontal local frame from given
 157   /// longitude and latitude
 158   static SGQuat fromLonLat(const SGGeod& geod)
 159   { return fromLonLatRad(geod.getLongitudeRad(), geod.getLatitudeRad()); }
 160
 161   /// Create a quaternion from the angle axis representation
 162   static SGQuat fromAngleAxis(T angle, const SGVec3<T>& axis)
 163   {
 164     T angle2 = 0.5*angle;
 165     return fromRealImag(cos(angle2), T(sin(angle2))*axis);
 166   }
 167
 168   /// Create a quaternion from the angle axis representation
 169   static SGQuat fromAngleAxisDeg(T angle, const SGVec3<T>& axis)
 170   { return fromAngleAxis(SGMisc<T>::deg2rad(angle), axis); }
 171
 172   /// Create a quaternion from the angle axis representation where the angle
 173   /// is stored in the axis' length
 174   static SGQuat fromAngleAxis(const SGVec3<T>& axis)
 175   {
 176     T nAxis = norm(axis);
 177     if (nAxis <= SGLimits<T>::min())
 178       return SGQuat(1, 0, 0, 0);
 179     T angle2 = 0.5*nAxis;
 180     return fromRealImag(cos(angle2), T(sin(angle2)/nAxis)*axis);
 181   }
 182
 183   /// Return a quaternion from real and imaginary part
 184   static SGQuat fromRealImag(T r, const SGVec3<T>& i)
 185   {
 186     SGQuat q;
 187     q.w() = r;
 188     q.x() = i(0);
 189     q.y() = i(1);
 190     q.z() = i(2);
 191     return q;
 192   }
 193
 194   /// Return an all zero vector
 195   static SGQuat zeros(void)
 196   { return SGQuat(0, 0, 0, 0); }
 197
 198   /// write the euler angles into the references
 199   void getEulerRad(T& zRad, T& yRad, T& xRad) const
 200   {
 201     value_type sqrQW = w()*w();
 202     value_type sqrQX = x()*x();
 203     value_type sqrQY = y()*y();
 204     value_type sqrQZ = z()*z();
 205
 206     value_type num = 2*(y()*z() + w()*x());
 207     value_type den = sqrQW - sqrQX - sqrQY + sqrQZ;
 208     if (fabs(den) < SGLimits<value_type>::min() &&
 209         fabs(num) < SGLimits<value_type>::min())
 210       xRad = 0;
 211     else
 212       xRad = atan2(num, den);
 213
 214     value_type tmp = 2*(x()*z() - w()*y());
 215     if (tmp < -1)
 216       yRad = 0.5*SGMisc<value_type>::pi();
 217     else if (1 < tmp)
 218       yRad = -0.5*SGMisc<value_type>::pi();
 219     else
 220       yRad = -asin(tmp);
 221
 222     num = 2*(x()*y() + w()*z());
 223     den = sqrQW + sqrQX - sqrQY - sqrQZ;
 224     if (fabs(den) < SGLimits<value_type>::min() &&
 225         fabs(num) < SGLimits<value_type>::min())
 226       zRad = 0;
 227     else {
 228       value_type psi = atan2(num, den);
 229       if (psi < 0)
 230         psi += 2*SGMisc<value_type>::pi();
 231       zRad = psi;
 232     }
 233   }
 234
 235   /// write the euler angles in degrees into the references
 236   void getEulerDeg(T& zDeg, T& yDeg, T& xDeg) const
 237   {
 238     getEulerRad(zDeg, yDeg, xDeg);
 239     zDeg = SGMisc<T>::rad2deg(zDeg);
 240     yDeg = SGMisc<T>::rad2deg(yDeg);
 241     xDeg = SGMisc<T>::rad2deg(xDeg);
 242   }
 243
 244   /// write the angle axis representation into the references
 245   void getAngleAxis(T& angle, SGVec3<T>& axis) const
 246   {
 247     T nrm = norm(*this);
 248     if (nrm < SGLimits<T>::min()) {
 249       angle = 0;
 250       axis = SGVec3<T>(0, 0, 0);
 251     } else {
 252       T rNrm = 1/nrm;
 253       angle = acos(SGMisc<T>::max(-1, SGMisc<T>::min(1, rNrm*w())));
 254       T sAng = sin(angle);
 255       if (fabs(sAng) < SGLimits<T>::min())
 256         axis = SGVec3<T>(1, 0, 0);
 257       else
 258         axis = (rNrm/sAng)*imag(*this);
 259       angle *= 2;
 260     }
 261   }
 262
 263   /// write the angle axis representation into the references
 264   void getAngleAxis(SGVec3<T>& axis) const
 265   {
 266     T angle;
 267     getAngleAxis(angle, axis);
 268     axis *= angle;
 269   }
 270
 271   /// Access by index, the index is unchecked
 272   const T& operator()(unsigned i) const
 273   { return data()[i]; }
 274   /// Access by index, the index is unchecked
 275   T& operator()(unsigned i)
 276   { return data()[i]; }
 277
 278   /// Access raw data by index, the index is unchecked
 279   const T& operator[](unsigned i) const
 280   { return data()[i]; }
 281   /// Access raw data by index, the index is unchecked
 282   T& operator[](unsigned i)
 283   { return data()[i]; }
 284
 285   /// Access the x component
 286   const T& x(void) const
 287   { return data()[0]; }
 288   /// Access the x component
 289   T& x(void)
 290   { return data()[0]; }
 291   /// Access the y component
 292   const T& y(void) const
 293   { return data()[1]; }
 294   /// Access the y component
 295   T& y(void)
 296   { return data()[1]; }
 297   /// Access the z component
 298   const T& z(void) const
 299   { return data()[2]; }
 300   /// Access the z component
 301   T& z(void)
 302   { return data()[2]; }
 303   /// Access the w component
 304   const T& w(void) const
 305   { return data()[3]; }
 306   /// Access the w component
 307   T& w(void)
 308   { return data()[3]; }
 309
 310   /// Get the data pointer
 311   using SGQuatStorage<T>::data;
 312
 313   /// Readonly interface function to ssg's sgQuat/sgdQuat
 314   const T (&sg(void) const)[4]
 315   { return data(); }
 316   /// Interface function to ssg's sgQuat/sgdQuat
 317   T (&sg(void))[4]
 318   { return data(); }
 319
 320   /// Interface function to osg's Quat*
 321   using SGQuatStorage<T>::osg;
 322
 323   /// Inplace addition
 324   SGQuat& operator+=(const SGQuat& v)
 325   { data()[0]+=v(0);data()[1]+=v(1);data()[2]+=v(2);data()[3]+=v(3);return *this; }
 326   /// Inplace subtraction
 327   SGQuat& operator-=(const SGQuat& v)
 328   { data()[0]-=v(0);data()[1]-=v(1);data()[2]-=v(2);data()[3]-=v(3);return *this; }
 329   /// Inplace scalar multiplication
 330   template<typename S>
 331   SGQuat& operator*=(S s)
 332   { data()[0] *= s; data()[1] *= s; data()[2] *= s; data()[3] *= s; return *this; }
 333   /// Inplace scalar multiplication by 1/s
 334   template<typename S>
 335   SGQuat& operator/=(S s)
 336   { return operator*=(1/T(s)); }
 337   /// Inplace quaternion multiplication
 338   SGQuat& operator*=(const SGQuat& v);
 339
 340   /// Transform a vector from the current coordinate frame to a coordinate
 341   /// frame rotated with the quaternion
 342   SGVec3<T> transform(const SGVec3<T>& v) const
 343   {
 344     value_type r = 2/dot(*this, *this);
 345     SGVec3<T> qimag = imag(*this);
 346     value_type qr = real(*this);
 347     return (r*qr*qr - 1)*v + (r*dot(qimag, v))*qimag - (r*qr)*cross(qimag, v);
 348   }
 349   /// Transform a vector from the coordinate frame rotated with the quaternion
 350   /// to the current coordinate frame
 351   SGVec3<T> backTransform(const SGVec3<T>& v) const
 352   {
 353     value_type r = 2/dot(*this, *this);
 354     SGVec3<T> qimag = imag(*this);
 355     value_type qr = real(*this);
 356     return (r*qr*qr - 1)*v + (r*dot(qimag, v))*qimag + (r*qr)*cross(qimag, v);
 357   }
 358
 359   /// Rotate a given vector with the quaternion
 360   SGVec3<T> rotate(const SGVec3<T>& v) const
 361   { return backTransform(v); }
 362   /// Rotate a given vector with the inverse quaternion
 363   SGVec3<T> rotateBack(const SGVec3<T>& v) const
 364   { return transform(v); }
 365
 366   /// Return the time derivative of the quaternion given the angular velocity
 367   SGQuat
 368   derivative(const SGVec3<T>& angVel)
 369   {
 370     SGQuat deriv;
 371
 372     deriv.w() = 0.5*(-x()*angVel(0) - y()*angVel(1) - z()*angVel(2));
 373     deriv.x() = 0.5*( w()*angVel(0) - z()*angVel(1) + y()*angVel(2));
 374     deriv.y() = 0.5*( z()*angVel(0) + w()*angVel(1) - x()*angVel(2));
 375     deriv.z() = 0.5*(-y()*angVel(0) + x()*angVel(1) + w()*angVel(2));
 376
 377     return deriv;
 378   }
 379 };
 380
 381 /// Unary +, do nothing ...
 382 template<typename T>
 383 inline
 384 const SGQuat<T>&
 385 operator+(const SGQuat<T>& v)
 386 { return v; }
 387
 388 /// Unary -, do nearly nothing
 389 template<typename T>
 390 inline
 391 SGQuat<T>
 392 operator-(const SGQuat<T>& v)
 393 { return SGQuat<T>(-v(0), -v(1), -v(2), -v(3)); }
 394
 395 /// Binary +
 396 template<typename T>
 397 inline
 398 SGQuat<T>
 399 operator+(const SGQuat<T>& v1, const SGQuat<T>& v2)
 400 { return SGQuat<T>(v1(0)+v2(0), v1(1)+v2(1), v1(2)+v2(2), v1(3)+v2(3)); }
 401
 402 /// Binary -
 403 template<typename T>
 404 inline
 405 SGQuat<T>
 406 operator-(const SGQuat<T>& v1, const SGQuat<T>& v2)
 407 { return SGQuat<T>(v1(0)-v2(0), v1(1)-v2(1), v1(2)-v2(2), v1(3)-v2(3)); }
 408
 409 /// Scalar multiplication
 410 template<typename S, typename T>
 411 inline
 412 SGQuat<T>
 413 operator*(S s, const SGQuat<T>& v)
 414 { return SGQuat<T>(s*v(0), s*v(1), s*v(2), s*v(3)); }
 415
 416 /// Scalar multiplication
 417 template<typename S, typename T>
 418 inline
 419 SGQuat<T>
 420 operator*(const SGQuat<T>& v, S s)
 421 { return SGQuat<T>(s*v(0), s*v(1), s*v(2), s*v(3)); }
 422
 423 /// Quaterion multiplication
 424 template<typename T>
 425 inline
 426 SGQuat<T>
 427 operator*(const SGQuat<T>& v1, const SGQuat<T>& v2)
 428 {
 429   SGQuat<T> v;
 430   v.x() = v1.w()*v2.x() + v1.x()*v2.w() + v1.y()*v2.z() - v1.z()*v2.y();
 431   v.y() = v1.w()*v2.y() - v1.x()*v2.z() + v1.y()*v2.w() + v1.z()*v2.x();
 432   v.z() = v1.w()*v2.z() + v1.x()*v2.y() - v1.y()*v2.x() + v1.z()*v2.w();
 433   v.w() = v1.w()*v2.w() - v1.x()*v2.x() - v1.y()*v2.y() - v1.z()*v2.z();
 434   return v;
 435 }
 436
 437 /// Now define the inplace multiplication
 438 template<typename T>
 439 inline
 440 SGQuat<T>&
 441 SGQuat<T>::operator*=(const SGQuat& v)
 442 { (*this) = (*this)*v; return *this; }
 443
 444 /// The conjugate of the quaternion, this is also the
 445 /// inverse for normalized quaternions
 446 template<typename T>
 447 inline
 448 SGQuat<T>
 449 conj(const SGQuat<T>& v)
 450 { return SGQuat<T>(-v(0), -v(1), -v(2), v(3)); }
 451
 452 /// The conjugate of the quaternion, this is also the
 453 /// inverse for normalized quaternions
 454 template<typename T>
 455 inline
 456 SGQuat<T>
 457 inverse(const SGQuat<T>& v)
 458 { return (1/dot(v, v))*SGQuat<T>(-v(0), -v(1), -v(2), v(3)); }
 459
 460 /// The imagniary part of the quaternion
 461 template<typename T>
 462 inline
 463 T
 464 real(const SGQuat<T>& v)
 465 { return v.w(); }
 466
 467 /// The imagniary part of the quaternion
 468 template<typename T>
 469 inline
 470 SGVec3<T>
 471 imag(const SGQuat<T>& v)
 472 { return SGVec3<T>(v.x(), v.y(), v.z()); }
 473
 474 /// Scalar dot product
 475 template<typename T>
 476 inline
 477 T
 478 dot(const SGQuat<T>& v1, const SGQuat<T>& v2)
 479 { return v1(0)*v2(0) + v1(1)*v2(1) + v1(2)*v2(2) + v1(3)*v2(3); }
 480
 481 /// The euclidean norm of the vector, that is what most people call length
 482 template<typename T>
 483 inline
 484 T
 485 norm(const SGQuat<T>& v)
 486 { return sqrt(dot(v, v)); }
 487
 488 /// The euclidean norm of the vector, that is what most people call length
 489 template<typename T>
 490 inline
 491 T
 492 length(const SGQuat<T>& v)
 493 { return sqrt(dot(v, v)); }
 494
 495 /// The 1-norm of the vector, this one is the fastest length function we
 496 /// can implement on modern cpu's
 497 template<typename T>
 498 inline
 499 T
 500 norm1(const SGQuat<T>& v)
 501 { return fabs(v(0)) + fabs(v(1)) + fabs(v(2)) + fabs(v(3)); }
 502
 503 /// The euclidean norm of the vector, that is what most people call length
 504 template<typename T>
 505 inline
 506 SGQuat<T>
 507 normalize(const SGQuat<T>& q)
 508 { return (1/norm(q))*q; }
 509
 510 /// Return true if exactly the same
 511 template<typename T>
 512 inline
 513 bool
 514 operator==(const SGQuat<T>& v1, const SGQuat<T>& v2)
 515 { return v1(0)==v2(0) && v1(1)==v2(1) && v1(2)==v2(2) && v1(3)==v2(3); }
 516
 517 /// Return true if not exactly the same
 518 template<typename T>
 519 inline
 520 bool
 521 operator!=(const SGQuat<T>& v1, const SGQuat<T>& v2)
 522 { return ! (v1 == v2); }
 523
 524 /// Return true if equal to the relative tolerance tol
 525 /// Note that this is not the same than comparing quaternions to represent
 526 /// the same rotation
 527 template<typename T>
 528 inline
 529 bool
 530 equivalent(const SGQuat<T>& v1, const SGQuat<T>& v2, T tol)
 531 { return norm1(v1 - v2) < tol*(norm1(v1) + norm1(v2)); }
 532
 533 /// Return true if about equal to roundoff of the underlying type
 534 /// Note that this is not the same than comparing quaternions to represent
 535 /// the same rotation
 536 template<typename T>
 537 inline
 538 bool
 539 equivalent(const SGQuat<T>& v1, const SGQuat<T>& v2)
 540 { return equivalent(v1, v2, 100*SGLimits<T>::epsilon()); }
 541
 542 #ifndef NDEBUG
 543 template<typename T>
 544 inline
 545 bool
 546 isNaN(const SGQuat<T>& v)
 547 {
 548   return SGMisc<T>::isNaN(v(0)) || SGMisc<T>::isNaN(v(1))
 549     || SGMisc<T>::isNaN(v(2)) || SGMisc<T>::isNaN(v(3));
 550 }
 551 #endif
 552
 553 /// quaternion interpolation for t in [0,1] interpolate between src (=0)
 554 /// and dst (=1)
 555 template<typename T>
 556 inline
 557 SGQuat<T>
 558 interpolate(T t, const SGQuat<T>& src, const SGQuat<T>& dst)
 559 {
 560   T cosPhi = dot(src, dst);
 561   // need to take the shorter way ...
 562   int signCosPhi = SGMisc<T>::sign(cosPhi);
 563   // cosPhi must be corrected for that sign
 564   cosPhi = fabs(cosPhi);
 565
 566   // first opportunity to fail - make sure acos will succeed later -
 567   // result is correct
 568   if (1 <= cosPhi)
 569     return dst;
 570
 571   // now the half angle between the orientations
 572   T o = acos(cosPhi);
 573
 574   // need the scales now, if the angle is very small, do linear interpolation
 575   // to avoid instabilities
 576   T scale0, scale1;
 577   if (fabs(o) < SGLimits<T>::epsilon()) {
 578     scale0 = 1 - t;
 579     scale1 = t;
 580   } else {
 581     // note that we can give a positive lower bound for sin(o) here
 582     T sino = sin(o);
 583     T so = 1/sino;
 584     scale0 = sin((1 - t)*o)*so;
 585     scale1 = sin(t*o)*so;
 586   }
 587
 588   return scale0*src + signCosPhi*scale1*dst;
 589 }
 590
 591 /// Output to an ostream
 592 template<typename char_type, typename traits_type, typename T>
 593 inline
 594 std::basic_ostream<char_type, traits_type>&
 595 operator<<(std::basic_ostream<char_type, traits_type>& s, const SGQuat<T>& v)
 596 { return s << "[ " << v(0) << ", " << v(1) << ", " << v(2) << ", " << v(3) << " ]"; }
 597
 598 inline
 599 SGQuatf
 600 toQuatf(const SGQuatd& v)
 601 { return SGQuatf((float)v(0), (float)v(1), (float)v(2), (float)v(3)); }
 602
 603 inline
 604 SGQuatd
 605 toQuatd(const SGQuatf& v)
 606 { return SGQuatd(v(0), v(1), v(2), v(3)); }
 607
 608 #endif