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, 0, 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   static SGQuat fromRotateTo(const SGVec3<T>& from, const SGVec3<T>& to)
 184   {
 185     T nfrom = norm(from);
 186     T nto = norm(to);
 187     if (nfrom < SGLimits<T>::min() || nto < SGLimits<T>::min())
 188       return SGQuat::unit();
 189
 190     return SGQuat::fromRotateToNorm((1/nfrom)*from, (1/nto)*to);
 191   }
 192
 193   // FIXME more finegrained error behavour.
 194   static SGQuat fromRotateTo(const SGVec3<T>& v1, unsigned i1,
 195                              const SGVec3<T>& v2, unsigned i2)
 196   {
 197     T nrmv1 = norm(v1);
 198     T nrmv2 = norm(v2);
 199     if (nrmv1 < SGLimits<T>::min() || nrmv2 < SGLimits<T>::min())
 200       return SGQuat::unit();
 201
 202     SGVec3<T> nv1 = (1/nrmv1)*v1;
 203     SGVec3<T> nv2 = (1/nrmv2)*v2;
 204     T dv1v2 = dot(nv1, nv2);
 205     if (fabs(fabs(dv1v2)-1) < SGLimits<T>::epsilon())
 206       return SGQuat::unit();
 207
 208     // The target vector for the first rotation
 209     SGVec3<T> nto1 = SGVec3<T>::zeros();
 210     SGVec3<T> nto2 = SGVec3<T>::zeros();
 211     nto1[i1] = 1;
 212     nto2[i2] = 1;
 213
 214     // The first rotation can be done with the usual routine.
 215     SGQuat q = SGQuat::fromRotateToNorm(nv1, nto1);
 216
 217     // The rotation axis for the second rotation is the
 218     // target for the first one, so the rotation axis is nto1
 219     // We need to get the angle.
 220
 221     // Make nv2 exactly orthogonal to nv1.
 222     nv2 = normalize(nv2 - dv1v2*nv1);
 223
 224     SGVec3<T> tnv2 = q.transform(nv2);
 225     T cosang = dot(nto2, tnv2);
 226     T cos05ang = T(0.5+0.5*cosang);
 227     if (cos05ang <= 0)
 228       cosang = T(0);
 229     cos05ang = sqrt(cos05ang);
 230     T sig = dot(nto1, cross(nto2, tnv2));
 231     T sin05ang = T(0.5-0.5*cosang);
 232     if (sin05ang <= 0)
 233       sin05ang = 0;
 234     sin05ang = copysign(sqrt(sin05ang), sig);
 235     q *= SGQuat::fromRealImag(cos05ang, sin05ang*nto1);
 236
 237     return q;
 238   }
 239
 240
 241   // Return a quaternion which rotates the vector given by v
 242   // to the vector -v. Other directions are *not* preserved.
 243   static SGQuat fromChangeSign(const SGVec3<T>& v)
 244   {
 245     // The vector from points to the oposite direction than to.
 246     // Find a vector perpandicular to the vector to.
 247     T absv1 = fabs(v(0));
 248     T absv2 = fabs(v(1));
 249     T absv3 = fabs(v(2));
 250
 251     SGVec3<T> axis;
 252     if (absv2 < absv1 && absv3 < absv1) {
 253       T quot = v(1)/v(0);
 254       axis = (1/sqrt(1+quot*quot))*SGVec3<T>(quot, -1, 0);
 255     } else if (absv1 < absv2 && absv3 < absv2) {
 256       T quot = v(2)/v(1);
 257       axis = (1/sqrt(1+quot*quot))*SGVec3<T>(0, quot, -1);
 258     } else if (absv1 < absv3 && absv2 < absv3) {
 259       T quot = v(0)/v(2);
 260       axis = (1/sqrt(1+quot*quot))*SGVec3<T>(-1, 0, quot);
 261     } else {
 262       // The all zero case.
 263       return SGQuat::unit();
 264     }
 265
 266     return SGQuat::fromRealImag(0, axis);
 267   }
 268
 269   /// Return a quaternion from real and imaginary part
 270   static SGQuat fromRealImag(T r, const SGVec3<T>& i)
 271   {
 272     SGQuat q;
 273     q.w() = r;
 274     q.x() = i.x();
 275     q.y() = i.y();
 276     q.z() = i.z();
 277     return q;
 278   }
 279
 280   /// Return an all zero vector
 281   static SGQuat zeros(void)
 282   { return SGQuat(0, 0, 0, 0); }
 283
 284   /// write the euler angles into the references
 285   void getEulerRad(T& zRad, T& yRad, T& xRad) const
 286   {
 287     T sqrQW = w()*w();
 288     T sqrQX = x()*x();
 289     T sqrQY = y()*y();
 290     T sqrQZ = z()*z();
 291
 292     T num = 2*(y()*z() + w()*x());
 293     T den = sqrQW - sqrQX - sqrQY + sqrQZ;
 294     if (fabs(den) < SGLimits<T>::min() &&
 295         fabs(num) < SGLimits<T>::min())
 296       xRad = 0;
 297     else
 298       xRad = atan2(num, den);
 299
 300     T tmp = 2*(x()*z() - w()*y());
 301     if (tmp < -1)
 302       yRad = 0.5*SGMisc<T>::pi();
 303     else if (1 < tmp)
 304       yRad = -0.5*SGMisc<T>::pi();
 305     else
 306       yRad = -asin(tmp);
 307
 308     num = 2*(x()*y() + w()*z());
 309     den = sqrQW + sqrQX - sqrQY - sqrQZ;
 310     if (fabs(den) < SGLimits<T>::min() &&
 311         fabs(num) < SGLimits<T>::min())
 312       zRad = 0;
 313     else {
 314       T psi = atan2(num, den);
 315       if (psi < 0)
 316         psi += 2*SGMisc<T>::pi();
 317       zRad = psi;
 318     }
 319   }
 320
 321   /// write the euler angles in degrees into the references
 322   void getEulerDeg(T& zDeg, T& yDeg, T& xDeg) const
 323   {
 324     getEulerRad(zDeg, yDeg, xDeg);
 325     zDeg = SGMisc<T>::rad2deg(zDeg);
 326     yDeg = SGMisc<T>::rad2deg(yDeg);
 327     xDeg = SGMisc<T>::rad2deg(xDeg);
 328   }
 329
 330   /// write the angle axis representation into the references
 331   void getAngleAxis(T& angle, SGVec3<T>& axis) const
 332   {
 333     T nrm = norm(*this);
 334     if (nrm < SGLimits<T>::min()) {
 335       angle = 0;
 336       axis = SGVec3<T>(0, 0, 0);
 337     } else {
 338       T rNrm = 1/nrm;
 339       angle = acos(SGMisc<T>::max(-1, SGMisc<T>::min(1, rNrm*w())));
 340       T sAng = sin(angle);
 341       if (fabs(sAng) < SGLimits<T>::min())
 342         axis = SGVec3<T>(1, 0, 0);
 343       else
 344         axis = (rNrm/sAng)*imag(*this);
 345       angle *= 2;
 346     }
 347   }
 348
 349   /// write the angle axis representation into the references
 350   void getAngleAxis(SGVec3<T>& axis) const
 351   {
 352     T angle;
 353     getAngleAxis(angle, axis);
 354     axis *= angle;
 355   }
 356
 357   /// Access by index, the index is unchecked
 358   const T& operator()(unsigned i) const
 359   { return data()[i]; }
 360   /// Access by index, the index is unchecked
 361   T& operator()(unsigned i)
 362   { return data()[i]; }
 363
 364   /// Access raw data by index, the index is unchecked
 365   const T& operator[](unsigned i) const
 366   { return data()[i]; }
 367   /// Access raw data by index, the index is unchecked
 368   T& operator[](unsigned i)
 369   { return data()[i]; }
 370
 371   /// Access the x component
 372   const T& x(void) const
 373   { return data()[0]; }
 374   /// Access the x component
 375   T& x(void)
 376   { return data()[0]; }
 377   /// Access the y component
 378   const T& y(void) const
 379   { return data()[1]; }
 380   /// Access the y component
 381   T& y(void)
 382   { return data()[1]; }
 383   /// Access the z component
 384   const T& z(void) const
 385   { return data()[2]; }
 386   /// Access the z component
 387   T& z(void)
 388   { return data()[2]; }
 389   /// Access the w component
 390   const T& w(void) const
 391   { return data()[3]; }
 392   /// Access the w component
 393   T& w(void)
 394   { return data()[3]; }
 395
 396   /// Get the data pointer
 397   using SGQuatStorage<T>::data;
 398
 399   /// Readonly interface function to ssg's sgQuat/sgdQuat
 400   const T (&sg(void) const)[4]
 401   { return data(); }
 402   /// Interface function to ssg's sgQuat/sgdQuat
 403   T (&sg(void))[4]
 404   { return data(); }
 405
 406   /// Interface function to osg's Quat*
 407   using SGQuatStorage<T>::osg;
 408
 409   /// Inplace addition
 410   SGQuat& operator+=(const SGQuat& v)
 411   { data()[0]+=v(0);data()[1]+=v(1);data()[2]+=v(2);data()[3]+=v(3);return *this; }
 412   /// Inplace subtraction
 413   SGQuat& operator-=(const SGQuat& v)
 414   { data()[0]-=v(0);data()[1]-=v(1);data()[2]-=v(2);data()[3]-=v(3);return *this; }
 415   /// Inplace scalar multiplication
 416   template<typename S>
 417   SGQuat& operator*=(S s)
 418   { data()[0] *= s; data()[1] *= s; data()[2] *= s; data()[3] *= s; return *this; }
 419   /// Inplace scalar multiplication by 1/s
 420   template<typename S>
 421   SGQuat& operator/=(S s)
 422   { return operator*=(1/T(s)); }
 423   /// Inplace quaternion multiplication
 424   SGQuat& operator*=(const SGQuat& v);
 425
 426   /// Transform a vector from the current coordinate frame to a coordinate
 427   /// frame rotated with the quaternion
 428   SGVec3<T> transform(const SGVec3<T>& v) const
 429   {
 430     T r = 2/dot(*this, *this);
 431     SGVec3<T> qimag = imag(*this);
 432     T qr = real(*this);
 433     return (r*qr*qr - 1)*v + (r*dot(qimag, v))*qimag - (r*qr)*cross(qimag, v);
 434   }
 435   /// Transform a vector from the coordinate frame rotated with the quaternion
 436   /// to the current coordinate frame
 437   SGVec3<T> backTransform(const SGVec3<T>& v) const
 438   {
 439     T r = 2/dot(*this, *this);
 440     SGVec3<T> qimag = imag(*this);
 441     T qr = real(*this);
 442     return (r*qr*qr - 1)*v + (r*dot(qimag, v))*qimag + (r*qr)*cross(qimag, v);
 443   }
 444
 445   /// Rotate a given vector with the quaternion
 446   SGVec3<T> rotate(const SGVec3<T>& v) const
 447   { return backTransform(v); }
 448   /// Rotate a given vector with the inverse quaternion
 449   SGVec3<T> rotateBack(const SGVec3<T>& v) const
 450   { return transform(v); }
 451
 452   /// Return the time derivative of the quaternion given the angular velocity
 453   SGQuat
 454   derivative(const SGVec3<T>& angVel) const
 455   {
 456     SGQuat deriv;
 457
 458     deriv.w() = 0.5*(-x()*angVel(0) - y()*angVel(1) - z()*angVel(2));
 459     deriv.x() = 0.5*( w()*angVel(0) - z()*angVel(1) + y()*angVel(2));
 460     deriv.y() = 0.5*( z()*angVel(0) + w()*angVel(1) - x()*angVel(2));
 461     deriv.z() = 0.5*(-y()*angVel(0) + x()*angVel(1) + w()*angVel(2));
 462
 463     return deriv;
 464   }
 465
 466 private:
 467
 468   // Private because it assumes normalized inputs.
 469   static SGQuat
 470   fromRotateToSmaller90Deg(T cosang,
 471                            const SGVec3<T>& from, const SGVec3<T>& to)
 472   {
 473     // In this function we assume that the angle required to rotate from
 474     // the vector from to the vector to is <= 90 deg.
 475     // That is done so because of possible instabilities when we rotate more
 476     // then 90deg.
 477
 478     // Note that the next comment does actually cover a *more* *general* case
 479     // than we need in this function. That shows that this formula is even
 480     // valid for rotations up to 180deg.
 481
 482     // Because of the signs in the axis, it is sufficient to care for angles
 483     // in the interval [-pi,pi]. That means that 0.5*angle is in the interval
 484     // [-pi/2,pi/2]. But in that range the cosine is allways >= 0.
 485     // So we do not need to care for egative roots in the following equation:
 486     T cos05ang = sqrt(0.5+0.5*cosang);
 487
 488
 489     // Now our assumption of angles <= 90 deg comes in play.
 490     // For that reason, we know that cos05ang is not zero.
 491     // It is even more, we can see from the above formula that
 492     // sqrt(0.5) < cos05ang.
 493
 494
 495     // Compute the rotation axis, that is
 496     // sin(angle)*normalized rotation axis
 497     SGVec3<T> axis = cross(to, from);
 498
 499     // We need sin(0.5*angle)*normalized rotation axis.
 500     // So rescale with sin(0.5*x)/sin(x).
 501     // To do that we use the equation:
 502     // sin(x) = 2*sin(0.5*x)*cos(0.5*x)
 503     return SGQuat::fromRealImag( cos05ang, (1/(2*cos05ang))*axis);
 504   }
 505
 506   // Private because it assumes normalized inputs.
 507   static SGQuat
 508   fromRotateToNorm(const SGVec3<T>& from, const SGVec3<T>& to)
 509   {
 510     // To avoid instabilities with roundoff, we distinguish between rotations
 511     // with more then 90deg and rotations with less than 90deg.
 512
 513     // Compute the cosine of the angle.
 514     T cosang = dot(from, to);
 515
 516     // For the small ones do direct computation
 517     if (T(-0.5) < cosang)
 518       return SGQuat::fromRotateToSmaller90Deg(cosang, from, to);
 519
 520     // For larger rotations. first rotate from to -from.
 521     // Past that we will have a smaller angle again.
 522     SGQuat q1 = SGQuat::fromChangeSign(from);
 523     SGQuat q2 = SGQuat::fromRotateToSmaller90Deg(-cosang, -from, to);
 524     return q1*q2;
 525   }
 526 };
 527
 528 /// Unary +, do nothing ...
 529 template<typename T>
 530 inline
 531 const SGQuat<T>&
 532 operator+(const SGQuat<T>& v)
 533 { return v; }
 534
 535 /// Unary -, do nearly nothing
 536 template<typename T>
 537 inline
 538 SGQuat<T>
 539 operator-(const SGQuat<T>& v)
 540 { return SGQuat<T>(-v(0), -v(1), -v(2), -v(3)); }
 541
 542 /// Binary +
 543 template<typename T>
 544 inline
 545 SGQuat<T>
 546 operator+(const SGQuat<T>& v1, const SGQuat<T>& v2)
 547 { return SGQuat<T>(v1(0)+v2(0), v1(1)+v2(1), v1(2)+v2(2), v1(3)+v2(3)); }
 548
 549 /// Binary -
 550 template<typename T>
 551 inline
 552 SGQuat<T>
 553 operator-(const SGQuat<T>& v1, const SGQuat<T>& v2)
 554 { return SGQuat<T>(v1(0)-v2(0), v1(1)-v2(1), v1(2)-v2(2), v1(3)-v2(3)); }
 555
 556 /// Scalar multiplication
 557 template<typename S, typename T>
 558 inline
 559 SGQuat<T>
 560 operator*(S s, const SGQuat<T>& v)
 561 { return SGQuat<T>(s*v(0), s*v(1), s*v(2), s*v(3)); }
 562
 563 /// Scalar multiplication
 564 template<typename S, typename T>
 565 inline
 566 SGQuat<T>
 567 operator*(const SGQuat<T>& v, S s)
 568 { return SGQuat<T>(s*v(0), s*v(1), s*v(2), s*v(3)); }
 569
 570 /// Quaterion multiplication
 571 template<typename T>
 572 inline
 573 SGQuat<T>
 574 operator*(const SGQuat<T>& v1, const SGQuat<T>& v2)
 575 {
 576   SGQuat<T> v;
 577   v.x() = v1.w()*v2.x() + v1.x()*v2.w() + v1.y()*v2.z() - v1.z()*v2.y();
 578   v.y() = v1.w()*v2.y() - v1.x()*v2.z() + v1.y()*v2.w() + v1.z()*v2.x();
 579   v.z() = v1.w()*v2.z() + v1.x()*v2.y() - v1.y()*v2.x() + v1.z()*v2.w();
 580   v.w() = v1.w()*v2.w() - v1.x()*v2.x() - v1.y()*v2.y() - v1.z()*v2.z();
 581   return v;
 582 }
 583
 584 /// Now define the inplace multiplication
 585 template<typename T>
 586 inline
 587 SGQuat<T>&
 588 SGQuat<T>::operator*=(const SGQuat& v)
 589 { (*this) = (*this)*v; return *this; }
 590
 591 /// The conjugate of the quaternion, this is also the
 592 /// inverse for normalized quaternions
 593 template<typename T>
 594 inline
 595 SGQuat<T>
 596 conj(const SGQuat<T>& v)
 597 { return SGQuat<T>(-v(0), -v(1), -v(2), v(3)); }
 598
 599 /// The conjugate of the quaternion, this is also the
 600 /// inverse for normalized quaternions
 601 template<typename T>
 602 inline
 603 SGQuat<T>
 604 inverse(const SGQuat<T>& v)
 605 { return (1/dot(v, v))*SGQuat<T>(-v(0), -v(1), -v(2), v(3)); }
 606
 607 /// The imagniary part of the quaternion
 608 template<typename T>
 609 inline
 610 T
 611 real(const SGQuat<T>& v)
 612 { return v.w(); }
 613
 614 /// The imagniary part of the quaternion
 615 template<typename T>
 616 inline
 617 SGVec3<T>
 618 imag(const SGQuat<T>& v)
 619 { return SGVec3<T>(v.x(), v.y(), v.z()); }
 620
 621 /// Scalar dot product
 622 template<typename T>
 623 inline
 624 T
 625 dot(const SGQuat<T>& v1, const SGQuat<T>& v2)
 626 { return v1(0)*v2(0) + v1(1)*v2(1) + v1(2)*v2(2) + v1(3)*v2(3); }
 627
 628 /// The euclidean norm of the vector, that is what most people call length
 629 template<typename T>
 630 inline
 631 T
 632 norm(const SGQuat<T>& v)
 633 { return sqrt(dot(v, v)); }
 634
 635 /// The euclidean norm of the vector, that is what most people call length
 636 template<typename T>
 637 inline
 638 T
 639 length(const SGQuat<T>& v)
 640 { return sqrt(dot(v, v)); }
 641
 642 /// The 1-norm of the vector, this one is the fastest length function we
 643 /// can implement on modern cpu's
 644 template<typename T>
 645 inline
 646 T
 647 norm1(const SGQuat<T>& v)
 648 { return fabs(v(0)) + fabs(v(1)) + fabs(v(2)) + fabs(v(3)); }
 649
 650 /// The euclidean norm of the vector, that is what most people call length
 651 template<typename T>
 652 inline
 653 SGQuat<T>
 654 normalize(const SGQuat<T>& q)
 655 { return (1/norm(q))*q; }
 656
 657 /// Return true if exactly the same
 658 template<typename T>
 659 inline
 660 bool
 661 operator==(const SGQuat<T>& v1, const SGQuat<T>& v2)
 662 { return v1(0)==v2(0) && v1(1)==v2(1) && v1(2)==v2(2) && v1(3)==v2(3); }
 663
 664 /// Return true if not exactly the same
 665 template<typename T>
 666 inline
 667 bool
 668 operator!=(const SGQuat<T>& v1, const SGQuat<T>& v2)
 669 { return ! (v1 == v2); }
 670
 671 /// Return true if equal to the relative tolerance tol
 672 /// Note that this is not the same than comparing quaternions to represent
 673 /// the same rotation
 674 template<typename T>
 675 inline
 676 bool
 677 equivalent(const SGQuat<T>& v1, const SGQuat<T>& v2, T tol)
 678 { return norm1(v1 - v2) < tol*(norm1(v1) + norm1(v2)); }
 679
 680 /// Return true if about equal to roundoff of the underlying type
 681 /// Note that this is not the same than comparing quaternions to represent
 682 /// the same rotation
 683 template<typename T>
 684 inline
 685 bool
 686 equivalent(const SGQuat<T>& v1, const SGQuat<T>& v2)
 687 { return equivalent(v1, v2, 100*SGLimits<T>::epsilon()); }
 688
 689 #ifndef NDEBUG
 690 template<typename T>
 691 inline
 692 bool
 693 isNaN(const SGQuat<T>& v)
 694 {
 695   return SGMisc<T>::isNaN(v(0)) || SGMisc<T>::isNaN(v(1))
 696     || SGMisc<T>::isNaN(v(2)) || SGMisc<T>::isNaN(v(3));
 697 }
 698 #endif
 699
 700 /// quaternion interpolation for t in [0,1] interpolate between src (=0)
 701 /// and dst (=1)
 702 template<typename T>
 703 inline
 704 SGQuat<T>
 705 interpolate(T t, const SGQuat<T>& src, const SGQuat<T>& dst)
 706 {
 707   T cosPhi = dot(src, dst);
 708   // need to take the shorter way ...
 709   int signCosPhi = SGMisc<T>::sign(cosPhi);
 710   // cosPhi must be corrected for that sign
 711   cosPhi = fabs(cosPhi);
 712
 713   // first opportunity to fail - make sure acos will succeed later -
 714   // result is correct
 715   if (1 <= cosPhi)
 716     return dst;
 717
 718   // now the half angle between the orientations
 719   T o = acos(cosPhi);
 720
 721   // need the scales now, if the angle is very small, do linear interpolation
 722   // to avoid instabilities
 723   T scale0, scale1;
 724   if (fabs(o) < SGLimits<T>::epsilon()) {
 725     scale0 = 1 - t;
 726     scale1 = t;
 727   } else {
 728     // note that we can give a positive lower bound for sin(o) here
 729     T sino = sin(o);
 730     T so = 1/sino;
 731     scale0 = sin((1 - t)*o)*so;
 732     scale1 = sin(t*o)*so;
 733   }
 734
 735   return scale0*src + signCosPhi*scale1*dst;
 736 }
 737
 738 /// Output to an ostream
 739 template<typename char_type, typename traits_type, typename T>
 740 inline
 741 std::basic_ostream<char_type, traits_type>&
 742 operator<<(std::basic_ostream<char_type, traits_type>& s, const SGQuat<T>& v)
 743 { return s << "[ " << v(0) << ", " << v(1) << ", " << v(2) << ", " << v(3) << " ]"; }
 744
 745 inline
 746 SGQuatf
 747 toQuatf(const SGQuatd& v)
 748 { return SGQuatf((float)v(0), (float)v(1), (float)v(2), (float)v(3)); }
 749
 750 inline
 751 SGQuatd
 752 toQuatd(const SGQuatf& v)
 753 { return SGQuatd(v(0), v(1), v(2), v(3)); }
 754
 755 #endif