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