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