simgear/math/SGQuat.hxx

   1 // Copyright (C) 2006-2009  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 #ifndef NO_OPENSCENEGRAPH_INTERFACE
  30 #include <osg/Quat>
  31 #endif
  32
  33 /// Quaternion Class
  34 template<typename T>
  35 class SGQuat {
  36 public:
  37   typedef T value_type;
  38
  39   /// Default constructor. Does not initialize at all.
  40   /// If you need them zero initialized, SGQuat::zeros()
  41   SGQuat(void)
  42   {
  43     /// Initialize with nans in the debug build, that will guarantee to have
  44     /// a fast uninitialized default constructor in the release but shows up
  45     /// uninitialized values in the debug build very fast ...
  46 #ifndef NDEBUG
  47     for (unsigned i = 0; i < 4; ++i)
  48       data()[i] = SGLimits<T>::quiet_NaN();
  49 #endif
  50   }
  51   /// Constructor. Initialize by the given values
  52   SGQuat(T _x, T _y, T _z, T _w)
  53   { x() = _x; y() = _y; z() = _z; w() = _w; }
  54   /// Constructor. Initialize by the content of a plain array,
  55   /// make sure it has at least 4 elements
  56   explicit SGQuat(const T* d)
  57   { data()[0] = d[0]; data()[1] = d[1]; data()[2] = d[2]; data()[3] = d[3]; }
  58
  59   /// Return a unit quaternion
  60   static SGQuat unit(void)
  61   { return fromRealImag(1, SGVec3<T>(0, 0, 0)); }
  62
  63   /// Return a quaternion from euler angles
  64   static SGQuat fromEulerRad(T z, T y, T x)
  65   {
  66     SGQuat q;
  67     T zd2 = T(0.5)*z; T yd2 = T(0.5)*y; T xd2 = T(0.5)*x;
  68     T Szd2 = sin(zd2); T Syd2 = sin(yd2); T Sxd2 = sin(xd2);
  69     T Czd2 = cos(zd2); T Cyd2 = cos(yd2); T Cxd2 = cos(xd2);
  70     T Cxd2Czd2 = Cxd2*Czd2; T Cxd2Szd2 = Cxd2*Szd2;
  71     T Sxd2Szd2 = Sxd2*Szd2; T Sxd2Czd2 = Sxd2*Czd2;
  72     q.w() = Cxd2Czd2*Cyd2 + Sxd2Szd2*Syd2;
  73     q.x() = Sxd2Czd2*Cyd2 - Cxd2Szd2*Syd2;
  74     q.y() = Cxd2Czd2*Syd2 + Sxd2Szd2*Cyd2;
  75     q.z() = Cxd2Szd2*Cyd2 - Sxd2Czd2*Syd2;
  76     return q;
  77   }
  78
  79   /// Return a quaternion from euler angles in degrees
  80   static SGQuat fromEulerDeg(T z, T y, T x)
  81   {
  82     return fromEulerRad(SGMisc<T>::deg2rad(z), SGMisc<T>::deg2rad(y),
  83                         SGMisc<T>::deg2rad(x));
  84   }
  85
  86   /// Return a quaternion from euler angles
  87   static SGQuat fromYawPitchRoll(T y, T p, T r)
  88   { return fromEulerRad(y, p, r); }
  89
  90   /// Return a quaternion from euler angles
  91   static SGQuat fromYawPitchRollDeg(T y, T p, T r)
  92   { return fromEulerDeg(y, p, r); }
  93
  94   /// Return a quaternion from euler angles
  95   static SGQuat fromHeadAttBank(T h, T a, T b)
  96   { return fromEulerRad(h, a, b); }
  97
  98   /// Return a quaternion from euler angles
  99   static SGQuat fromHeadAttBankDeg(T h, T a, T b)
 100   { return fromEulerDeg(h, a, b); }
 101
 102   /// Return a quaternion rotation from the earth centered to the
 103   /// simulation usual horizontal local frame from given
 104   /// longitude and latitude.
 105   /// The horizontal local frame used in simulations is the frame with x-axis
 106   /// pointing north, the y-axis pointing eastwards and the z axis
 107   /// pointing downwards.
 108   static SGQuat fromLonLatRad(T lon, T lat)
 109   {
 110     SGQuat q;
 111     T zd2 = T(0.5)*lon;
 112     T yd2 = T(-0.25)*SGMisc<T>::pi() - T(0.5)*lat;
 113     T Szd2 = sin(zd2);
 114     T Syd2 = sin(yd2);
 115     T Czd2 = cos(zd2);
 116     T Cyd2 = cos(yd2);
 117     q.w() = Czd2*Cyd2;
 118     q.x() = -Szd2*Syd2;
 119     q.y() = Czd2*Syd2;
 120     q.z() = Szd2*Cyd2;
 121     return q;
 122   }
 123   /// Like the above provided for convenience
 124   static SGQuat fromLonLatDeg(T lon, T lat)
 125   { return fromLonLatRad(SGMisc<T>::deg2rad(lon), SGMisc<T>::deg2rad(lat)); }
 126   /// Like the above provided for convenience
 127   static SGQuat fromLonLat(const SGGeod& geod)
 128   { return fromLonLatRad(geod.getLongitudeRad(), geod.getLatitudeRad()); }
 129
 130
 131   /// Create a quaternion from the angle axis representation
 132   static SGQuat fromAngleAxis(T angle, const SGVec3<T>& axis)
 133   {
 134     T angle2 = T(0.5)*angle;
 135     return fromRealImag(cos(angle2), T(sin(angle2))*axis);
 136   }
 137
 138   /// Create a quaternion from the angle axis representation
 139   static SGQuat fromAngleAxisDeg(T angle, const SGVec3<T>& axis)
 140   { return fromAngleAxis(SGMisc<T>::deg2rad(angle), axis); }
 141
 142   /// Create a quaternion from the angle axis representation where the angle
 143   /// is stored in the axis' length
 144   static SGQuat fromAngleAxis(const SGVec3<T>& axis)
 145   {
 146     T nAxis = norm(axis);
 147     if (nAxis <= SGLimits<T>::min())
 148       return SGQuat::unit();
 149     T angle2 = T(0.5)*nAxis;
 150     return fromRealImag(cos(angle2), T(sin(angle2)/nAxis)*axis);
 151   }
 152
 153   /// Return a quaternion that rotates the from vector onto the to vector.
 154   static SGQuat fromRotateTo(const SGVec3<T>& from, const SGVec3<T>& to)
 155   {
 156     T nfrom = norm(from);
 157     T nto = norm(to);
 158     if (nfrom <= SGLimits<T>::min() || nto <= SGLimits<T>::min())
 159       return SGQuat::unit();
 160
 161     return SGQuat::fromRotateToNorm((1/nfrom)*from, (1/nto)*to);
 162   }
 163
 164   /// Return a quaternion that rotates v1 onto the i1-th unit vector
 165   /// and v2 into a plane that is spanned by the i2-th and i1-th unit vector.
 166   static SGQuat fromRotateTo(const SGVec3<T>& v1, unsigned i1,
 167                              const SGVec3<T>& v2, unsigned i2)
 168   {
 169     T nrmv1 = norm(v1);
 170     T nrmv2 = norm(v2);
 171     if (nrmv1 <= SGLimits<T>::min() || nrmv2 <= SGLimits<T>::min())
 172       return SGQuat::unit();
 173
 174     SGVec3<T> nv1 = (1/nrmv1)*v1;
 175     SGVec3<T> nv2 = (1/nrmv2)*v2;
 176     T dv1v2 = dot(nv1, nv2);
 177     if (fabs(fabs(dv1v2)-1) <= SGLimits<T>::epsilon())
 178       return SGQuat::unit();
 179
 180     // The target vector for the first rotation
 181     SGVec3<T> nto1 = SGVec3<T>::zeros();
 182     SGVec3<T> nto2 = SGVec3<T>::zeros();
 183     nto1[i1] = 1;
 184     nto2[i2] = 1;
 185
 186     // The first rotation can be done with the usual routine.
 187     SGQuat q = SGQuat::fromRotateToNorm(nv1, nto1);
 188
 189     // The rotation axis for the second rotation is the
 190     // target for the first one, so the rotation axis is nto1
 191     // We need to get the angle.
 192
 193     // Make nv2 exactly orthogonal to nv1.
 194     nv2 = normalize(nv2 - dv1v2*nv1);
 195
 196     SGVec3<T> tnv2 = q.transform(nv2);
 197     T cosang = dot(nto2, tnv2);
 198     T cos05ang = T(0.5)+T(0.5)*cosang;
 199     if (cos05ang <= 0)
 200       cosang = 0;
 201     cos05ang = sqrt(cos05ang);
 202     T sig = dot(nto1, cross(nto2, tnv2));
 203     T sin05ang = T(0.5)-T(0.5)*cosang;
 204     if (sin05ang <= 0)
 205       sin05ang = 0;
 206     sin05ang = copysign(sqrt(sin05ang), sig);
 207     q *= SGQuat::fromRealImag(cos05ang, sin05ang*nto1);
 208
 209     return q;
 210   }
 211
 212
 213   // Return a quaternion which rotates the vector given by v
 214   // to the vector -v. Other directions are *not* preserved.
 215   static SGQuat fromChangeSign(const SGVec3<T>& v)
 216   {
 217     // The vector from points to the oposite direction than to.
 218     // Find a vector perpendicular to the vector to.
 219     T absv1 = fabs(v(0));
 220     T absv2 = fabs(v(1));
 221     T absv3 = fabs(v(2));
 222
 223     SGVec3<T> axis;
 224     if (absv2 < absv1 && absv3 < absv1) {
 225       T quot = v(1)/v(0);
 226       axis = (1/sqrt(1+quot*quot))*SGVec3<T>(quot, -1, 0);
 227     } else if (absv1 < absv2 && absv3 < absv2) {
 228       T quot = v(2)/v(1);
 229       axis = (1/sqrt(1+quot*quot))*SGVec3<T>(0, quot, -1);
 230     } else if (absv1 < absv3 && absv2 < absv3) {
 231       T quot = v(0)/v(2);
 232       axis = (1/sqrt(1+quot*quot))*SGVec3<T>(-1, 0, quot);
 233     } else {
 234       // The all zero case.
 235       return SGQuat::unit();
 236     }
 237
 238     return SGQuat::fromRealImag(0, axis);
 239   }
 240
 241   /// Return a quaternion from real and imaginary part
 242   static SGQuat fromRealImag(T r, const SGVec3<T>& i)
 243   {
 244     SGQuat q;
 245     q.w() = r;
 246     q.x() = i.x();
 247     q.y() = i.y();
 248     q.z() = i.z();
 249     return q;
 250   }
 251
 252   /// Return an all zero vector
 253   static SGQuat zeros(void)
 254   { return SGQuat(0, 0, 0, 0); }
 255
 256   /// write the euler angles into the references
 257   void getEulerRad(T& zRad, T& yRad, T& xRad) const
 258   {
 259     T sqrQW = w()*w();
 260     T sqrQX = x()*x();
 261     T sqrQY = y()*y();
 262     T sqrQZ = z()*z();
 263
 264     T num = 2*(y()*z() + w()*x());
 265     T den = sqrQW - sqrQX - sqrQY + sqrQZ;
 266     if (fabs(den) <= SGLimits<T>::min() &&
 267         fabs(num) <= SGLimits<T>::min())
 268       xRad = 0;
 269     else
 270       xRad = atan2(num, den);
 271
 272     T tmp = 2*(x()*z() - w()*y());
 273     if (tmp <= -1)
 274       yRad = T(0.5)*SGMisc<T>::pi();
 275     else if (1 <= tmp)
 276       yRad = -T(0.5)*SGMisc<T>::pi();
 277     else
 278       yRad = -asin(tmp);
 279
 280     num = 2*(x()*y() + w()*z());
 281     den = sqrQW + sqrQX - sqrQY - sqrQZ;
 282     if (fabs(den) <= SGLimits<T>::min() &&
 283         fabs(num) <= SGLimits<T>::min())
 284       zRad = 0;
 285     else {
 286       T psi = atan2(num, den);
 287       if (psi < 0)
 288         psi += 2*SGMisc<T>::pi();
 289       zRad = psi;
 290     }
 291   }
 292
 293   /// write the euler angles in degrees into the references
 294   void getEulerDeg(T& zDeg, T& yDeg, T& xDeg) const
 295   {
 296     getEulerRad(zDeg, yDeg, xDeg);
 297     zDeg = SGMisc<T>::rad2deg(zDeg);
 298     yDeg = SGMisc<T>::rad2deg(yDeg);
 299     xDeg = SGMisc<T>::rad2deg(xDeg);
 300   }
 301
 302   /// write the angle axis representation into the references
 303   void getAngleAxis(T& angle, SGVec3<T>& axis) const
 304   {
 305     T nrm = norm(*this);
 306     if (nrm <= SGLimits<T>::min()) {
 307       angle = 0;
 308       axis = SGVec3<T>(0, 0, 0);
 309     } else {
 310       T rNrm = 1/nrm;
 311       angle = acos(SGMisc<T>::max(-1, SGMisc<T>::min(1, rNrm*w())));
 312       T sAng = sin(angle);
 313       if (fabs(sAng) <= SGLimits<T>::min())
 314         axis = SGVec3<T>(1, 0, 0);
 315       else
 316         axis = (rNrm/sAng)*imag(*this);
 317       angle *= 2;
 318     }
 319   }
 320
 321   /// write the angle axis representation into the references
 322   void getAngleAxis(SGVec3<T>& axis) const
 323   {
 324     T angle;
 325     getAngleAxis(angle, axis);
 326     axis *= angle;
 327   }
 328
 329   /// Access by index, the index is unchecked
 330   const T& operator()(unsigned i) const
 331   { return data()[i]; }
 332   /// Access by index, the index is unchecked
 333   T& operator()(unsigned i)
 334   { return data()[i]; }
 335
 336   /// Access raw data by index, the index is unchecked
 337   const T& operator[](unsigned i) const
 338   { return data()[i]; }
 339   /// Access raw data by index, the index is unchecked
 340   T& operator[](unsigned i)
 341   { return data()[i]; }
 342
 343   /// Access the x component
 344   const T& x(void) const
 345   { return data()[0]; }
 346   /// Access the x component
 347   T& x(void)
 348   { return data()[0]; }
 349   /// Access the y component
 350   const T& y(void) const
 351   { return data()[1]; }
 352   /// Access the y component
 353   T& y(void)
 354   { return data()[1]; }
 355   /// Access the z component
 356   const T& z(void) const
 357   { return data()[2]; }
 358   /// Access the z component
 359   T& z(void)
 360   { return data()[2]; }
 361   /// Access the w component
 362   const T& w(void) const
 363   { return data()[3]; }
 364   /// Access the w component
 365   T& w(void)
 366   { return data()[3]; }
 367
 368   /// Get the data pointer
 369   const T (&data(void) const)[4]
 370   { return _data; }
 371   /// Get the data pointer
 372   T (&data(void))[4]
 373   { return _data; }
 374
 375   /// Inplace addition
 376   SGQuat& operator+=(const SGQuat& v)
 377   { data()[0]+=v(0);data()[1]+=v(1);data()[2]+=v(2);data()[3]+=v(3);return *this; }
 378   /// Inplace subtraction
 379   SGQuat& operator-=(const SGQuat& v)
 380   { data()[0]-=v(0);data()[1]-=v(1);data()[2]-=v(2);data()[3]-=v(3);return *this; }
 381   /// Inplace scalar multiplication
 382   template<typename S>
 383   SGQuat& operator*=(S s)
 384   { data()[0] *= s; data()[1] *= s; data()[2] *= s; data()[3] *= s; return *this; }
 385   /// Inplace scalar multiplication by 1/s
 386   template<typename S>
 387   SGQuat& operator/=(S s)
 388   { return operator*=(1/T(s)); }
 389   /// Inplace quaternion multiplication
 390   SGQuat& operator*=(const SGQuat& v);
 391
 392   /// Transform a vector from the current coordinate frame to a coordinate
 393   /// frame rotated with the quaternion
 394   SGVec3<T> transform(const SGVec3<T>& v) const
 395   {
 396     T r = 2/dot(*this, *this);
 397     SGVec3<T> qimag = imag(*this);
 398     T qr = real(*this);
 399     return (r*qr*qr - 1)*v + (r*dot(qimag, v))*qimag - (r*qr)*cross(qimag, v);
 400   }
 401   /// Transform a vector from the coordinate frame rotated with the quaternion
 402   /// to the current coordinate frame
 403   SGVec3<T> backTransform(const SGVec3<T>& v) const
 404   {
 405     T r = 2/dot(*this, *this);
 406     SGVec3<T> qimag = imag(*this);
 407     T qr = real(*this);
 408     return (r*qr*qr - 1)*v + (r*dot(qimag, v))*qimag + (r*qr)*cross(qimag, v);
 409   }
 410
 411   /// Rotate a given vector with the quaternion
 412   SGVec3<T> rotate(const SGVec3<T>& v) const
 413   { return backTransform(v); }
 414   /// Rotate a given vector with the inverse quaternion
 415   SGVec3<T> rotateBack(const SGVec3<T>& v) const
 416   { return transform(v); }
 417
 418   /// Return the time derivative of the quaternion given the angular velocity
 419   SGQuat
 420   derivative(const SGVec3<T>& angVel) const
 421   {
 422     SGQuat deriv;
 423
 424     deriv.w() = T(0.5)*(-x()*angVel(0) - y()*angVel(1) - z()*angVel(2));
 425     deriv.x() = T(0.5)*( w()*angVel(0) - z()*angVel(1) + y()*angVel(2));
 426     deriv.y() = T(0.5)*( z()*angVel(0) + w()*angVel(1) - x()*angVel(2));
 427     deriv.z() = T(0.5)*(-y()*angVel(0) + x()*angVel(1) + w()*angVel(2));
 428
 429     return deriv;
 430   }
 431
 432 private:
 433
 434   // Private because it assumes normalized inputs.
 435   static SGQuat
 436   fromRotateToSmaller90Deg(T cosang,
 437                            const SGVec3<T>& from, const SGVec3<T>& to)
 438   {
 439     // In this function we assume that the angle required to rotate from
 440     // the vector from to the vector to is <= 90 deg.
 441     // That is done so because of possible instabilities when we rotate more
 442     // then 90deg.
 443
 444     // Note that the next comment does actually cover a *more* *general* case
 445     // than we need in this function. That shows that this formula is even
 446     // valid for rotations up to 180deg.
 447
 448     // Because of the signs in the axis, it is sufficient to care for angles
 449     // in the interval [-pi,pi]. That means that 0.5*angle is in the interval
 450     // [-pi/2,pi/2]. But in that range the cosine is allways >= 0.
 451     // So we do not need to care for egative roots in the following equation:
 452     T cos05ang = sqrt(T(0.5)+T(0.5)*cosang);
 453
 454
 455     // Now our assumption of angles <= 90 deg comes in play.
 456     // For that reason, we know that cos05ang is not zero.
 457     // It is even more, we can see from the above formula that
 458     // sqrt(0.5) < cos05ang.
 459
 460
 461     // Compute the rotation axis, that is
 462     // sin(angle)*normalized rotation axis
 463     SGVec3<T> axis = cross(to, from);
 464
 465     // We need sin(0.5*angle)*normalized rotation axis.
 466     // So rescale with sin(0.5*x)/sin(x).
 467     // To do that we use the equation:
 468     // sin(x) = 2*sin(0.5*x)*cos(0.5*x)
 469     return SGQuat::fromRealImag( cos05ang, (1/(2*cos05ang))*axis);
 470   }
 471
 472   // Private because it assumes normalized inputs.
 473   static SGQuat
 474   fromRotateToNorm(const SGVec3<T>& from, const SGVec3<T>& to)
 475   {
 476     // To avoid instabilities with roundoff, we distinguish between rotations
 477     // with more then 90deg and rotations with less than 90deg.
 478
 479     // Compute the cosine of the angle.
 480     T cosang = dot(from, to);
 481
 482     // For the small ones do direct computation
 483     if (T(-0.5) < cosang)
 484       return SGQuat::fromRotateToSmaller90Deg(cosang, from, to);
 485
 486     // For larger rotations. first rotate from to -from.
 487     // Past that we will have a smaller angle again.
 488     SGQuat q1 = SGQuat::fromChangeSign(from);
 489     SGQuat q2 = SGQuat::fromRotateToSmaller90Deg(-cosang, -from, to);
 490     return q1*q2;
 491   }
 492
 493   T _data[4];
 494 };
 495
 496 /// Unary +, do nothing ...
 497 template<typename T>
 498 inline
 499 const SGQuat<T>&
 500 operator+(const SGQuat<T>& v)
 501 { return v; }
 502
 503 /// Unary -, do nearly nothing
 504 template<typename T>
 505 inline
 506 SGQuat<T>
 507 operator-(const SGQuat<T>& v)
 508 { return SGQuat<T>(-v(0), -v(1), -v(2), -v(3)); }
 509
 510 /// Binary +
 511 template<typename T>
 512 inline
 513 SGQuat<T>
 514 operator+(const SGQuat<T>& v1, const SGQuat<T>& v2)
 515 { return SGQuat<T>(v1(0)+v2(0), v1(1)+v2(1), v1(2)+v2(2), v1(3)+v2(3)); }
 516
 517 /// Binary -
 518 template<typename T>
 519 inline
 520 SGQuat<T>
 521 operator-(const SGQuat<T>& v1, const SGQuat<T>& v2)
 522 { return SGQuat<T>(v1(0)-v2(0), v1(1)-v2(1), v1(2)-v2(2), v1(3)-v2(3)); }
 523
 524 /// Scalar multiplication
 525 template<typename S, typename T>
 526 inline
 527 SGQuat<T>
 528 operator*(S s, const SGQuat<T>& v)
 529 { return SGQuat<T>(s*v(0), s*v(1), s*v(2), s*v(3)); }
 530
 531 /// Scalar multiplication
 532 template<typename S, typename T>
 533 inline
 534 SGQuat<T>
 535 operator*(const SGQuat<T>& v, S s)
 536 { return SGQuat<T>(s*v(0), s*v(1), s*v(2), s*v(3)); }
 537
 538 /// Quaterion multiplication
 539 template<typename T>
 540 inline
 541 SGQuat<T>
 542 operator*(const SGQuat<T>& v1, const SGQuat<T>& v2)
 543 {
 544   SGQuat<T> v;
 545   v.x() = v1.w()*v2.x() + v1.x()*v2.w() + v1.y()*v2.z() - v1.z()*v2.y();
 546   v.y() = v1.w()*v2.y() - v1.x()*v2.z() + v1.y()*v2.w() + v1.z()*v2.x();
 547   v.z() = v1.w()*v2.z() + v1.x()*v2.y() - v1.y()*v2.x() + v1.z()*v2.w();
 548   v.w() = v1.w()*v2.w() - v1.x()*v2.x() - v1.y()*v2.y() - v1.z()*v2.z();
 549   return v;
 550 }
 551
 552 /// Now define the inplace multiplication
 553 template<typename T>
 554 inline
 555 SGQuat<T>&
 556 SGQuat<T>::operator*=(const SGQuat& v)
 557 { (*this) = (*this)*v; return *this; }
 558
 559 /// The conjugate of the quaternion, this is also the
 560 /// inverse for normalized quaternions
 561 template<typename T>
 562 inline
 563 SGQuat<T>
 564 conj(const SGQuat<T>& v)
 565 { return SGQuat<T>(-v(0), -v(1), -v(2), v(3)); }
 566
 567 /// The conjugate of the quaternion, this is also the
 568 /// inverse for normalized quaternions
 569 template<typename T>
 570 inline
 571 SGQuat<T>
 572 inverse(const SGQuat<T>& v)
 573 { return (1/dot(v, v))*SGQuat<T>(-v(0), -v(1), -v(2), v(3)); }
 574
 575 /// The imagniary part of the quaternion
 576 template<typename T>
 577 inline
 578 T
 579 real(const SGQuat<T>& v)
 580 { return v.w(); }
 581
 582 /// The imagniary part of the quaternion
 583 template<typename T>
 584 inline
 585 SGVec3<T>
 586 imag(const SGQuat<T>& v)
 587 { return SGVec3<T>(v.x(), v.y(), v.z()); }
 588
 589 /// Scalar dot product
 590 template<typename T>
 591 inline
 592 T
 593 dot(const SGQuat<T>& v1, const SGQuat<T>& v2)
 594 { return v1(0)*v2(0) + v1(1)*v2(1) + v1(2)*v2(2) + v1(3)*v2(3); }
 595
 596 /// The euclidean norm of the vector, that is what most people call length
 597 template<typename T>
 598 inline
 599 T
 600 norm(const SGQuat<T>& v)
 601 { return sqrt(dot(v, v)); }
 602
 603 /// The euclidean norm of the vector, that is what most people call length
 604 template<typename T>
 605 inline
 606 T
 607 length(const SGQuat<T>& v)
 608 { return sqrt(dot(v, v)); }
 609
 610 /// The 1-norm of the vector, this one is the fastest length function we
 611 /// can implement on modern cpu's
 612 template<typename T>
 613 inline
 614 T
 615 norm1(const SGQuat<T>& v)
 616 { return fabs(v(0)) + fabs(v(1)) + fabs(v(2)) + fabs(v(3)); }
 617
 618 /// The euclidean norm of the vector, that is what most people call length
 619 template<typename T>
 620 inline
 621 SGQuat<T>
 622 normalize(const SGQuat<T>& q)
 623 { return (1/norm(q))*q; }
 624
 625 /// Return true if exactly the same
 626 template<typename T>
 627 inline
 628 bool
 629 operator==(const SGQuat<T>& v1, const SGQuat<T>& v2)
 630 { return v1(0)==v2(0) && v1(1)==v2(1) && v1(2)==v2(2) && v1(3)==v2(3); }
 631
 632 /// Return true if not exactly the same
 633 template<typename T>
 634 inline
 635 bool
 636 operator!=(const SGQuat<T>& v1, const SGQuat<T>& v2)
 637 { return ! (v1 == v2); }
 638
 639 /// Return true if equal to the relative tolerance tol
 640 /// Note that this is not the same than comparing quaternions to represent
 641 /// the same rotation
 642 template<typename T>
 643 inline
 644 bool
 645 equivalent(const SGQuat<T>& v1, const SGQuat<T>& v2, T tol)
 646 { return norm1(v1 - v2) < tol*(norm1(v1) + norm1(v2)); }
 647
 648 /// Return true if about equal to roundoff of the underlying type
 649 /// Note that this is not the same than comparing quaternions to represent
 650 /// the same rotation
 651 template<typename T>
 652 inline
 653 bool
 654 equivalent(const SGQuat<T>& v1, const SGQuat<T>& v2)
 655 { return equivalent(v1, v2, 100*SGLimits<T>::epsilon()); }
 656
 657 #ifndef NDEBUG
 658 template<typename T>
 659 inline
 660 bool
 661 isNaN(const SGQuat<T>& v)
 662 {
 663   return SGMisc<T>::isNaN(v(0)) || SGMisc<T>::isNaN(v(1))
 664     || SGMisc<T>::isNaN(v(2)) || SGMisc<T>::isNaN(v(3));
 665 }
 666 #endif
 667
 668 /// quaternion interpolation for t in [0,1] interpolate between src (=0)
 669 /// and dst (=1)
 670 template<typename T>
 671 inline
 672 SGQuat<T>
 673 interpolate(T t, const SGQuat<T>& src, const SGQuat<T>& dst)
 674 {
 675   T cosPhi = dot(src, dst);
 676   // need to take the shorter way ...
 677   int signCosPhi = SGMisc<T>::sign(cosPhi);
 678   // cosPhi must be corrected for that sign
 679   cosPhi = fabs(cosPhi);
 680
 681   // first opportunity to fail - make sure acos will succeed later -
 682   // result is correct
 683   if (1 <= cosPhi)
 684     return dst;
 685
 686   // now the half angle between the orientations
 687   T o = acos(cosPhi);
 688
 689   // need the scales now, if the angle is very small, do linear interpolation
 690   // to avoid instabilities
 691   T scale0, scale1;
 692   if (fabs(o) <= SGLimits<T>::epsilon()) {
 693     scale0 = 1 - t;
 694     scale1 = t;
 695   } else {
 696     // note that we can give a positive lower bound for sin(o) here
 697     T sino = sin(o);
 698     T so = 1/sino;
 699     scale0 = sin((1 - t)*o)*so;
 700     scale1 = sin(t*o)*so;
 701   }
 702
 703   return scale0*src + signCosPhi*scale1*dst;
 704 }
 705
 706 /// Output to an ostream
 707 template<typename char_type, typename traits_type, typename T>
 708 inline
 709 std::basic_ostream<char_type, traits_type>&
 710 operator<<(std::basic_ostream<char_type, traits_type>& s, const SGQuat<T>& v)
 711 { return s << "[ " << v(0) << ", " << v(1) << ", " << v(2) << ", " << v(3) << " ]"; }
 712
 713 inline
 714 SGQuatf
 715 toQuatf(const SGQuatd& v)
 716 { return SGQuatf((float)v(0), (float)v(1), (float)v(2), (float)v(3)); }
 717
 718 inline
 719 SGQuatd
 720 toQuatd(const SGQuatf& v)
 721 { return SGQuatd(v(0), v(1), v(2), v(3)); }
 722
 723 #ifndef NO_OPENSCENEGRAPH_INTERFACE
 724 inline
 725 SGQuatd
 726 toSG(const osg::Quat& q)
 727 { return SGQuatd(q[0], q[1], q[2], q[3]); }
 728
 729 inline
 730 osg::Quat
 731 toOsg(const SGQuatd& q)
 732 { return osg::Quat(q[0], q[1], q[2], q[3]); }
 733 #endif
 734
 735 #endif