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   /// Return the angular velocity w that makes q0 translate to q1 using
 457   /// an explicit euler step with stepsize h.
 458   /// That is, look for an w where
 459   /// q1 = normalize(q0 + h*q0.derivative(w))
 460   static SGVec3<T>
 461   forwardDifferenceVelocity(const SGQuat& q0, const SGQuat& q1, const T& h)
 462   {
 463     // Let D_q0*w = q0.derivative(w), D_q0 the above 4x3 matrix.
 464     // Then D_q0^t*D_q0 = 0.25*Id and D_q0*q0 = 0.
 465     // Let lambda be a nonzero normailzation factor, then
 466     //  q1 = normalize(q0 + h*q0.derivative(w))
 467     // can be rewritten
 468     //  lambda*q1 = q0 + h*D_q0*w.
 469     // Multiply left by the transpose D_q0^t and reorder gives
 470     //  4*lambda/h*D_q0^t*q1 = w.
 471     // Now compute lambda by substitution of w into the original
 472     // equation
 473     //  lambda*q1 = q0 + 4*lambda*D_q0*D_q0^t*q1,
 474     // multiply by q1^t from the left
 475     //  lambda*<q1,q1> = <q0,q1> + 4*lambda*<D_q0^t*q1,D_q0^t*q1>
 476     // and solving for lambda gives
 477     //  lambda = <q0,q1>/(1 - 4*<D_q0^t*q1,D_q0^t*q1>).
 478
 479     // The transpose of the derivative matrix
 480     // the 0.5 factor is handled below
 481     // also note that the initializer uses x, y, z, w instead of w, x, y, z
 482     SGQuat d0(q0.w(), q0.z(), -q0.y(), -q0.x());
 483     SGQuat d1(-q0.z(), q0.w(), q0.x(), -q0.y());
 484     SGQuat d2(q0.y(), -q0.x(), q0.w(), -q0.z());
 485     // 2*D_q0^t*q1
 486     SGVec3<T> Dq(dot(d0, q1), dot(d1, q1), dot(d2, q1));
 487     // Like above, but take into account that Dq = 2*D_q0^t*q1
 488     T lambda = dot(q0, q1)/(T(1) - dot(Dq, Dq));
 489     return (2*lambda/h)*Dq;
 490   }
 491
 492 private:
 493
 494   // Private because it assumes normalized inputs.
 495   static SGQuat
 496   fromRotateToSmaller90Deg(T cosang,
 497                            const SGVec3<T>& from, const SGVec3<T>& to)
 498   {
 499     // In this function we assume that the angle required to rotate from
 500     // the vector from to the vector to is <= 90 deg.
 501     // That is done so because of possible instabilities when we rotate more
 502     // then 90deg.
 503
 504     // Note that the next comment does actually cover a *more* *general* case
 505     // than we need in this function. That shows that this formula is even
 506     // valid for rotations up to 180deg.
 507
 508     // Because of the signs in the axis, it is sufficient to care for angles
 509     // in the interval [-pi,pi]. That means that 0.5*angle is in the interval
 510     // [-pi/2,pi/2]. But in that range the cosine is allways >= 0.
 511     // So we do not need to care for egative roots in the following equation:
 512     T cos05ang = sqrt(T(0.5)+T(0.5)*cosang);
 513
 514
 515     // Now our assumption of angles <= 90 deg comes in play.
 516     // For that reason, we know that cos05ang is not zero.
 517     // It is even more, we can see from the above formula that
 518     // sqrt(0.5) < cos05ang.
 519
 520
 521     // Compute the rotation axis, that is
 522     // sin(angle)*normalized rotation axis
 523     SGVec3<T> axis = cross(to, from);
 524
 525     // We need sin(0.5*angle)*normalized rotation axis.
 526     // So rescale with sin(0.5*x)/sin(x).
 527     // To do that we use the equation:
 528     // sin(x) = 2*sin(0.5*x)*cos(0.5*x)
 529     return SGQuat::fromRealImag( cos05ang, (1/(2*cos05ang))*axis);
 530   }
 531
 532   // Private because it assumes normalized inputs.
 533   static SGQuat
 534   fromRotateToNorm(const SGVec3<T>& from, const SGVec3<T>& to)
 535   {
 536     // To avoid instabilities with roundoff, we distinguish between rotations
 537     // with more then 90deg and rotations with less than 90deg.
 538
 539     // Compute the cosine of the angle.
 540     T cosang = dot(from, to);
 541
 542     // For the small ones do direct computation
 543     if (T(-0.5) < cosang)
 544       return SGQuat::fromRotateToSmaller90Deg(cosang, from, to);
 545
 546     // For larger rotations. first rotate from to -from.
 547     // Past that we will have a smaller angle again.
 548     SGQuat q1 = SGQuat::fromChangeSign(from);
 549     SGQuat q2 = SGQuat::fromRotateToSmaller90Deg(-cosang, -from, to);
 550     return q1*q2;
 551   }
 552
 553   T _data[4];
 554 };
 555
 556 /// Unary +, do nothing ...
 557 template<typename T>
 558 inline
 559 const SGQuat<T>&
 560 operator+(const SGQuat<T>& v)
 561 { return v; }
 562
 563 /// Unary -, do nearly nothing
 564 template<typename T>
 565 inline
 566 SGQuat<T>
 567 operator-(const SGQuat<T>& v)
 568 { return SGQuat<T>(-v(0), -v(1), -v(2), -v(3)); }
 569
 570 /// Binary +
 571 template<typename T>
 572 inline
 573 SGQuat<T>
 574 operator+(const SGQuat<T>& v1, const SGQuat<T>& v2)
 575 { return SGQuat<T>(v1(0)+v2(0), v1(1)+v2(1), v1(2)+v2(2), v1(3)+v2(3)); }
 576
 577 /// Binary -
 578 template<typename T>
 579 inline
 580 SGQuat<T>
 581 operator-(const SGQuat<T>& v1, const SGQuat<T>& v2)
 582 { return SGQuat<T>(v1(0)-v2(0), v1(1)-v2(1), v1(2)-v2(2), v1(3)-v2(3)); }
 583
 584 /// Scalar multiplication
 585 template<typename S, typename T>
 586 inline
 587 SGQuat<T>
 588 operator*(S s, const SGQuat<T>& v)
 589 { return SGQuat<T>(s*v(0), s*v(1), s*v(2), s*v(3)); }
 590
 591 /// Scalar multiplication
 592 template<typename S, typename T>
 593 inline
 594 SGQuat<T>
 595 operator*(const SGQuat<T>& v, S s)
 596 { return SGQuat<T>(s*v(0), s*v(1), s*v(2), s*v(3)); }
 597
 598 /// Quaterion multiplication
 599 template<typename T>
 600 inline
 601 SGQuat<T>
 602 operator*(const SGQuat<T>& v1, const SGQuat<T>& v2)
 603 {
 604   SGQuat<T> v;
 605   v.x() = v1.w()*v2.x() + v1.x()*v2.w() + v1.y()*v2.z() - v1.z()*v2.y();
 606   v.y() = v1.w()*v2.y() - v1.x()*v2.z() + v1.y()*v2.w() + v1.z()*v2.x();
 607   v.z() = v1.w()*v2.z() + v1.x()*v2.y() - v1.y()*v2.x() + v1.z()*v2.w();
 608   v.w() = v1.w()*v2.w() - v1.x()*v2.x() - v1.y()*v2.y() - v1.z()*v2.z();
 609   return v;
 610 }
 611
 612 /// Now define the inplace multiplication
 613 template<typename T>
 614 inline
 615 SGQuat<T>&
 616 SGQuat<T>::operator*=(const SGQuat& v)
 617 { (*this) = (*this)*v; return *this; }
 618
 619 /// The conjugate of the quaternion, this is also the
 620 /// inverse for normalized quaternions
 621 template<typename T>
 622 inline
 623 SGQuat<T>
 624 conj(const SGQuat<T>& v)
 625 { return SGQuat<T>(-v(0), -v(1), -v(2), v(3)); }
 626
 627 /// The conjugate of the quaternion, this is also the
 628 /// inverse for normalized quaternions
 629 template<typename T>
 630 inline
 631 SGQuat<T>
 632 inverse(const SGQuat<T>& v)
 633 { return (1/dot(v, v))*SGQuat<T>(-v(0), -v(1), -v(2), v(3)); }
 634
 635 /// The imagniary part of the quaternion
 636 template<typename T>
 637 inline
 638 T
 639 real(const SGQuat<T>& v)
 640 { return v.w(); }
 641
 642 /// The imagniary part of the quaternion
 643 template<typename T>
 644 inline
 645 SGVec3<T>
 646 imag(const SGQuat<T>& v)
 647 { return SGVec3<T>(v.x(), v.y(), v.z()); }
 648
 649 /// Scalar dot product
 650 template<typename T>
 651 inline
 652 T
 653 dot(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 /// The euclidean norm of the vector, that is what most people call length
 657 template<typename T>
 658 inline
 659 T
 660 norm(const SGQuat<T>& v)
 661 { return sqrt(dot(v, v)); }
 662
 663 /// The euclidean norm of the vector, that is what most people call length
 664 template<typename T>
 665 inline
 666 T
 667 length(const SGQuat<T>& v)
 668 { return sqrt(dot(v, v)); }
 669
 670 /// The 1-norm of the vector, this one is the fastest length function we
 671 /// can implement on modern cpu's
 672 template<typename T>
 673 inline
 674 T
 675 norm1(const SGQuat<T>& v)
 676 { return fabs(v(0)) + fabs(v(1)) + fabs(v(2)) + fabs(v(3)); }
 677
 678 /// The euclidean norm of the vector, that is what most people call length
 679 template<typename T>
 680 inline
 681 SGQuat<T>
 682 normalize(const SGQuat<T>& q)
 683 { return (1/norm(q))*q; }
 684
 685 /// Return true if exactly the same
 686 template<typename T>
 687 inline
 688 bool
 689 operator==(const SGQuat<T>& v1, const SGQuat<T>& v2)
 690 { return v1(0)==v2(0) && v1(1)==v2(1) && v1(2)==v2(2) && v1(3)==v2(3); }
 691
 692 /// Return true if not exactly the same
 693 template<typename T>
 694 inline
 695 bool
 696 operator!=(const SGQuat<T>& v1, const SGQuat<T>& v2)
 697 { return ! (v1 == v2); }
 698
 699 /// Return true if equal to the relative tolerance tol
 700 /// Note that this is not the same than comparing quaternions to represent
 701 /// the same rotation
 702 template<typename T>
 703 inline
 704 bool
 705 equivalent(const SGQuat<T>& v1, const SGQuat<T>& v2, T tol)
 706 { return norm1(v1 - v2) < tol*(norm1(v1) + norm1(v2)); }
 707
 708 /// Return true if about equal to roundoff of the underlying type
 709 /// Note that this is not the same than comparing quaternions to represent
 710 /// the same rotation
 711 template<typename T>
 712 inline
 713 bool
 714 equivalent(const SGQuat<T>& v1, const SGQuat<T>& v2)
 715 { return equivalent(v1, v2, 100*SGLimits<T>::epsilon()); }
 716
 717 #ifndef NDEBUG
 718 template<typename T>
 719 inline
 720 bool
 721 isNaN(const SGQuat<T>& v)
 722 {
 723   return SGMisc<T>::isNaN(v(0)) || SGMisc<T>::isNaN(v(1))
 724     || SGMisc<T>::isNaN(v(2)) || SGMisc<T>::isNaN(v(3));
 725 }
 726 #endif
 727
 728 /// quaternion interpolation for t in [0,1] interpolate between src (=0)
 729 /// and dst (=1)
 730 template<typename T>
 731 inline
 732 SGQuat<T>
 733 interpolate(T t, const SGQuat<T>& src, const SGQuat<T>& dst)
 734 {
 735   T cosPhi = dot(src, dst);
 736   // need to take the shorter way ...
 737   int signCosPhi = SGMisc<T>::sign(cosPhi);
 738   // cosPhi must be corrected for that sign
 739   cosPhi = fabs(cosPhi);
 740
 741   // first opportunity to fail - make sure acos will succeed later -
 742   // result is correct
 743   if (1 <= cosPhi)
 744     return dst;
 745
 746   // now the half angle between the orientations
 747   T o = acos(cosPhi);
 748
 749   // need the scales now, if the angle is very small, do linear interpolation
 750   // to avoid instabilities
 751   T scale0, scale1;
 752   if (fabs(o) <= SGLimits<T>::epsilon()) {
 753     scale0 = 1 - t;
 754     scale1 = t;
 755   } else {
 756     // note that we can give a positive lower bound for sin(o) here
 757     T sino = sin(o);
 758     T so = 1/sino;
 759     scale0 = sin((1 - t)*o)*so;
 760     scale1 = sin(t*o)*so;
 761   }
 762
 763   return scale0*src + signCosPhi*scale1*dst;
 764 }
 765
 766 /// Output to an ostream
 767 template<typename char_type, typename traits_type, typename T>
 768 inline
 769 std::basic_ostream<char_type, traits_type>&
 770 operator<<(std::basic_ostream<char_type, traits_type>& s, const SGQuat<T>& v)
 771 { return s << "[ " << v(0) << ", " << v(1) << ", " << v(2) << ", " << v(3) << " ]"; }
 772
 773 inline
 774 SGQuatf
 775 toQuatf(const SGQuatd& v)
 776 { return SGQuatf((float)v(0), (float)v(1), (float)v(2), (float)v(3)); }
 777
 778 inline
 779 SGQuatd
 780 toQuatd(const SGQuatf& v)
 781 { return SGQuatd(v(0), v(1), v(2), v(3)); }
 782
 783 #ifndef NO_OPENSCENEGRAPH_INTERFACE
 784 inline
 785 SGQuatd
 786 toSG(const osg::Quat& q)
 787 { return SGQuatd(q[0], q[1], q[2], q[3]); }
 788
 789 inline
 790 osg::Quat
 791 toOsg(const SGQuatd& q)
 792 { return osg::Quat(q[0], q[1], q[2], q[3]); }
 793 #endif
 794
 795 #endif