maths/Quaternion.h

   1 /***************************************************************************
   2  *   This file is part of Tecorrec.                                        *
   3  *   Copyright 2008 James Hogan <james@albanarts.com>                      *
   4  *                                                                         *
   5  *   Tecorrec is free software: you can redistribute it and/or modify      *
   6  *   it under the terms of the GNU General Public License as published by  *
   7  *   the Free Software Foundation, either version 2 of the License, or     *
   8  *   (at your option) any later version.                                   *
   9  *                                                                         *
  10  *   Tecorrec is distributed in the hope that it will be useful,           *
  11  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  12  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
  13  *   GNU General Public License for more details.                          *
  14  *                                                                         *
  15  *   You should have received a copy of the GNU General Public License     *
  16  *   along with Tecorrec.  If not, write to the Free Software Foundation,  *
  17  *   Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.   *
  18  ***************************************************************************/
  19
  20 /**
  21  * @file Quaterion.h
  22  * @brief Quaternion class for orientations and rotations.
  23  * @author James Hogan (james@albanarts.com)
  24  * @note Copyright (C) 2007
  25  *
  26  * @version 1 : From Computer Graphics and Visualisation Open Assessment.
  27  */
  28
  29 #ifndef _maths_Quaternion_h
  30 #define _maths_Quaternion_h
  31
  32 #include "TemplateMaths.h"
  33 #include "Vector.h"
  34
  35 #include <ostream>
  36
  37 namespace maths {
  38
  39   // These aren't always used.
  40   template <int N, typename T>
  41       class Vector;
  42   template <int N, typename T>
  43       class Matrix;
  44   template <typename T>
  45       class Quaternion;
  46
  47   /// Get a rotation quaterion between two vectors.
  48   template <typename T>
  49       Quaternion<T> rotationArc(const Vector<3,T> & a, const Vector<3,T> & b)
  50   {
  51     Vector<3,T> c;
  52     cross(c, a, b);
  53     T d = a * b;
  54     T s2 = (d+1)*2;
  55     if (s2) {
  56     T s = sqrt<T>(s2);
  57       return Quaternion<T>(c[0] / s,
  58                            c[1] / s,
  59                            c[2] / s,
  60                            s    / 2);
  61     } else {
  62       // 180 degrees, Result would ordinarily be not a number.
  63       return Quaternion<T>((T)1, (T)0, (T)0, (T)0);
  64     }
  65   }
  66
  67   /// Get a rotation quaterion between two vectors.
  68   template <typename T>
  69       Quaternion<T> rotationArc(const Vector<3,T> & a, const Vector<3,T> & b, bool * pDeterminate)
  70   {
  71     Vector<3,T> c;
  72     cross(c, a, b);
  73     T d = a * b;
  74     T s2 = (d+1)*2;
  75     bool determinate = s2;
  76     if (pDeterminate) {
  77       *pDeterminate = determinate;
  78     }
  79     if (determinate) {
  80       T s = sqrt<T>(s2);
  81       return Quaternion<T>(c[0] / s,
  82                            c[1] / s,
  83                            c[2] / s,
  84                            s    / 2);
  85     } else {
  86       // 180 degrees, Result would ordinarily be not a number.
  87       return Quaternion<T>((T)1, (T)0, (T)0, (T)0);
  88     }
  89   }
  90
  91   /// Quaternion orientation.
  92   /**
  93    * @param T typename Type of the components of the quaternion.
  94    */
  95   template <typename T>
  96       class Quaternion
  97   {
  98     public:
  99       T q[4];
 100
 101     public:
 102       /// Default constructor (does not initialise values)
 103       inline Quaternion()
 104       {
 105       }
 106
 107       inline explicit Quaternion(const T * quat)
 108       {
 109         for (int i = 0; i < 4; ++i) {
 110           q[i] = quat[i];
 111         }
 112       }
 113       inline explicit Quaternion(T x, T y, T z, T w)
 114       {
 115         q[0] = x;
 116         q[1] = y;
 117         q[2] = z;
 118         q[3] = w;
 119       }
 120       inline explicit Quaternion(const Vector<3, T> & vec)
 121       {
 122         for (int i = 0; i < 3; ++i) {
 123           q[i] = vec[i];
 124         }
 125         q[3] = (T)0;
 126       }
 127
 128       template <typename U> inline explicit Quaternion(const Quaternion<U> & copy)
 129       {
 130         for (int i = 0; i < 4; ++i) {
 131           q[i] = (T)copy.q[i];
 132         }
 133       }
 134
 135       /// Construct from a 3 by 3 matrix with the same component types.
 136       /**
 137        * Taken from game programming gems 1 i think
 138        */
 139       inline explicit Quaternion(const Matrix<3,T> & m)
 140       {
 141         T tr,s;
 142         tr = m[0][0] + m[1][1] + m[2][2];
 143
 144         if (tr >= 0) {
 145           s = sqrt(tr+(T)1);
 146           q[3] = (T)0.5 * s;
 147           s = (T)0.5 / s;
 148           q[0] = (m[2][1] - m[1][2]) * s;
 149           q[1] = (m[0][2] - m[2][0]) * s;
 150           q[2] = (m[1][0] - m[0][1]) * s;
 151         } else {
 152           int i = 0;
 153           if (m[1][1] > m[0][0])
 154             i = 1;
 155           if (m[2][2] > m[i][i])
 156             i = 2;
 157           switch (i) {
 158             case 0:
 159               s = sqrt((m[0][0] - (m[1][1] + m[2][2])) + (T)1);
 160               q[0] = (T)0.5 * s;
 161               s = (T)0.5 / s;
 162               q[1] = (m[0][1] + m[1][0]) * s;
 163               q[2] = (m[2][0] + m[0][2]) * s;
 164               q[3] = (m[2][1] - m[1][2]) * s;
 165               break;
 166             case 1:
 167               s = sqrt((m[1][1] - (m[2][2] + m[0][0])) + (T)1);
 168               q[1] = (T)0.5 * s;
 169               s = (T)0.5 / s;
 170               q[2] = (m[1][2] + m[2][1]) * s;
 171               q[0] = (m[0][1] + m[1][0]) * s;
 172               q[3] = (m[0][2] - m[2][0]) * s;
 173               break;
 174             case 2:
 175               s = sqrt((m[2][2] - (m[0][0] + m[1][1])) + (T)1);
 176               q[2] = (T)0.5 * s;
 177               s = (T)0.5 / s;
 178               q[0] = (m[2][0] + m[0][2]) * s;
 179               q[1] = (m[1][2] + m[2][1]) * s;
 180               q[3] = (m[1][0] - m[0][1]) * s;
 181               break;
 182           }
 183         }
 184       }
 185
 186       /// Convert to a 3 by 3 rotation matrix.
 187       inline Matrix<3, T> toMatrix3()
 188       {
 189         Matrix<3, T> mat;
 190         T xy = q[0]*q[1];
 191         T xz = q[0]*q[2];
 192         T yy = q[1]*q[1];
 193         T yw = q[1]*q[3];
 194         T zz = q[2]*q[2];
 195         T zw = q[2]*q[3];
 196         setVector3(mat[0].v, 1-2*(yy+zz),   2*(xy+zw),   2*(xz-yw));
 197         T xx = q[0]*q[0];
 198         T xw = q[0]*q[3];
 199         T yz = q[1]*q[2];
 200         setVector3(mat[1].v,  2*(xy-zw), 1-2*(xx+zz),   2*(yz+xw));
 201         setVector3(mat[2].v,  2*(xz+yw),   2*(yz-xw), 1-2*(xx+yy));
 202         return mat;
 203       }
 204
 205       inline T & operator [] (int index)
 206       {
 207         return q[index];
 208       }
 209       inline const T & operator [] (int index) const
 210       {
 211         return q[index];
 212       }
 213
 214       inline operator T * ()
 215       {
 216         return q;
 217       }
 218       inline operator const T * () const
 219       {
 220         return q;
 221       }
 222
 223       // Construct a quaternion from a set of euler angles
 224       /*explicit Quaternion(const mlEulerf & E);
 225       // Construct a quaternion from a rotation matrix
 226             explicit Quaternion(const maths::Matrix<3,float> & M);
 227       // Construct a pure quaternion with V as {x,y,z}
 228             explicit inline Quaternion(const Vector3f & V)
 229             : x(V.q[0]), y(V.q[1]), z(V.q[2]), w((T)0)
 230             {
 231         }*/
 232
 233       // Quick setter, returns self reference
 234       /*inline Quaternion & Set(R fX = (T)0, R fY = (T)0, R fZ = (T)0, R fW = (T)0)
 235             {
 236             x = fX;
 237             y = fY;
 238             z = fZ;
 239             w = fW;
 240             return *this;
 241         }*/
 242
 243
 244       inline T sqr() const
 245       {
 246         T ret = (T)0;
 247         for (int i = 0; i < 4; ++i)
 248           ret += q[i]*q[i];
 249         return ret;
 250       }
 251
 252       inline T mag() const
 253       {
 254         return sqrt<T>(sqr());
 255       }
 256
 257
 258       inline Quaternion & normalize()
 259       {
 260         return operator /= (mag());
 261       }
 262       inline Quaternion normalized()
 263       {
 264         return operator / (this, mag());
 265       }
 266
 267       inline Quaternion & resize(T tLength)
 268       {
 269         return operator *= (tLength / mag());
 270       }
 271       inline Quaternion resized(T tLength)
 272       {
 273         return operator * (this, tLength / mag());
 274       }
 275
 276       inline Quaternion & conjugate()
 277       {
 278         for (int i = 0; i < 3; ++i)
 279           q[i] = -q[i];
 280         return *this;
 281       }
 282       inline Quaternion conjugated()
 283       {
 284         Quaternion ret = *this;
 285         return ret.conjugate();
 286       }
 287
 288       inline Quaternion & identity()
 289       {
 290         for (int i = 0; i < 3; ++i)
 291           q[i] = (T)0;
 292         q[3] = (T)1;
 293         return *this;
 294       }
 295
 296       // Multiplication
 297       inline friend Quaternion operator * (const Quaternion & lhs, const Quaternion & rhs)
 298       {
 299         return Quaternion(lhs.q[3]*rhs.q[0] + lhs.q[0]*rhs.q[3] + lhs.q[1]*rhs.q[2] - lhs.q[2]*rhs.q[1],
 300                           lhs.q[3]*rhs.q[1] - lhs.q[0]*rhs.q[2] + lhs.q[1]*rhs.q[3] + lhs.q[2]*rhs.q[0],
 301                           lhs.q[3]*rhs.q[2] + lhs.q[0]*rhs.q[1] - lhs.q[1]*rhs.q[0] + lhs.q[2]*rhs.q[3],
 302                           lhs.q[3]*rhs.q[3] - lhs.q[0]*rhs.q[0] - lhs.q[1]*rhs.q[1] - lhs.q[2]*rhs.q[2]);
 303       }
 304       inline Quaternion & operator *=(const Quaternion & rhs)
 305       {
 306         q[0] = q[3]*rhs.q[0] + q[0]*rhs.q[3] + q[1]*rhs.q[2] - q[2]*rhs.q[1];
 307         q[1] = q[3]*rhs.q[1] - q[0]*rhs.q[2] + q[1]*rhs.q[3] + q[2]*rhs.q[0];
 308         q[2] = q[3]*rhs.q[2] + q[0]*rhs.q[1] - q[1]*rhs.q[0] + q[2]*rhs.q[3];
 309         q[3] = q[3]*rhs.q[3] - q[0]*rhs.q[0] - q[1]*rhs.q[1] - q[2]*rhs.q[2];
 310         return *this;
 311       }
 312
 313       // Multiplications: Real * Quaternion or Quaternion * Real
 314       inline friend Quaternion operator *(const Quaternion & lhs, T rhs)
 315       {
 316         return Quaternion(lhs.q[0] * rhs,
 317                           lhs.q[1] * rhs,
 318                           lhs.q[2] * rhs,
 319                           lhs.q[3] * rhs);
 320       }
 321       inline friend Quaternion operator *(T lhs, const Quaternion & rhs)
 322       {
 323         return Quaternion(lhs * rhs.q[0],
 324                           lhs * rhs.q[1],
 325                           lhs * rhs.q[2],
 326                           lhs * rhs.q[3]);
 327       }
 328       inline Quaternion operator *=(T rhs)
 329       {
 330         q[0] *= rhs;
 331         q[1] *= rhs;
 332         q[2] *= rhs;
 333         q[3] *= rhs;
 334         return *this;
 335       }
 336
 337       // Division
 338       inline friend Quaternion operator /(const Quaternion & lhs, const Quaternion & rhs)
 339       {
 340         T rhsSqr = rhs.sqr();
 341         return Quaternion((-lhs.q[3]*rhs.q[0] + lhs.q[0]*rhs.q[3] - lhs.q[1]*rhs.q[2] + lhs.q[2]*rhs.q[1]) / rhsSqr,
 342                           (-lhs.q[3]*rhs.q[1] + lhs.q[0]*rhs.q[2] + lhs.q[1]*rhs.q[3] - lhs.q[2]*rhs.q[0]) / rhsSqr,
 343                           (-lhs.q[3]*rhs.q[2] - lhs.q[0]*rhs.q[1] + lhs.q[1]*rhs.q[0] + lhs.q[2]*rhs.q[3]) / rhsSqr,
 344                           ( lhs.q[3]*rhs.q[3] + lhs.q[0]*rhs.q[0] + lhs.q[1]*rhs.q[1] + lhs.q[2]*rhs.q[2]) / rhsSqr);
 345       }
 346       inline Quaternion & operator /=(const Quaternion & den)
 347       {
 348         return operator=(operator/(*this,den));
 349       }
 350
 351       // Divisions: Quaternion / Real
 352       inline friend Quaternion operator /(const Quaternion & lhs, T rhs)
 353       {
 354         return Quaternion(lhs.q[0] / rhs,
 355                           lhs.q[1] / rhs,
 356                           lhs.q[2] / rhs,
 357                           lhs.q[3] / rhs);
 358       }
 359       inline Quaternion & operator /=(T rhs)
 360       {
 361         q[0] /= rhs;
 362         q[1] /= rhs;
 363         q[2] /= rhs;
 364         q[3] /= rhs;
 365         return *this;
 366       }
 367
 368       // Negation
 369       inline Quaternion & negate()
 370       {
 371         q[0] = -q[0];
 372         q[1] = -q[1];
 373         q[2] = -q[2];
 374         q[3] = -q[3];
 375         return *this;
 376       }
 377       inline friend Quaternion operator - (const Quaternion & quat)
 378       {
 379         return Quaternion(-quat.q[0],
 380                            -quat.q[1],
 381                            -quat.q[2],
 382                            -quat.q[3]);
 383       }
 384
 385       // Addition
 386       inline friend Quaternion operator +(const Quaternion & lhs, const Quaternion & rhs)
 387       {
 388         return Quaternion(lhs.q[0] + rhs.q[0],
 389                           lhs.q[1] + rhs.q[1],
 390                           lhs.q[2] + rhs.q[2],
 391                           lhs.q[3] + rhs.q[3]);
 392       }
 393       inline Quaternion & operator +=(const Quaternion & rhs)
 394       {
 395         q[0] = q[0] + rhs.q[0];
 396         q[1] = q[1] + rhs.q[1];
 397         q[2] = q[2] + rhs.q[2];
 398         q[3] = q[3] + rhs.q[3];
 399         return *this;
 400       }
 401
 402       // Subtraction
 403       inline friend Quaternion operator -(const Quaternion & lhs, const Quaternion & rhs)
 404       {
 405         return Quaternion(lhs.q[0] - rhs.q[0],
 406                           lhs.q[1] - rhs.q[1],
 407                           lhs.q[2] - rhs.q[2],
 408                           lhs.q[3] - rhs.q[3]);
 409       }
 410       inline Quaternion & operator -=(const Quaternion & rhs)
 411       {
 412         q[0] = q[0] - rhs.q[0];
 413         q[1] = q[1] - rhs.q[1];
 414         q[2] = q[2] - rhs.q[2];
 415         q[3] = q[3] - rhs.q[3];
 416         return *this;
 417       }
 418
 419       /*
 420           s/c = sin(Th/2)/cos(Th/2) = tan(Th/2)
 421           s = c.tan(Th/2)
 422           s = c.tan(arccos(c))
 423           s = c.sin(arccos(c))/cos(arccos(c))
 424           s = sin(arccos(c))
 425           VAxis = [x/s, y/s, z/s]
 426       */
 427       // Get the axis of rotation
 428       inline Vector<3,T> AxisOfRotation() const
 429       {
 430         T c = (q[3]>1) ? 1 : (q[3]<-1) ? -1 : q[3];
 431         T s = 1/sinf(acos(c));
 432         return Vector<3,T>(q[0]*s,q[1]*s,q[2]*s);
 433       }
 434       // Get the angle of rotation
 435       inline T AngleOfRotation() const
 436       {
 437         T c = (q[3]>1) ? 1 : (q[3]<-1) ? -1 : q[3];
 438         //T c = (q[3]>1) ? 1 : q[3];
 439         return 2 * acos(c);
 440       }
 441
 442       // Get the axis and angle or rotation represented in the form of a non unit quaternion
 443       inline Quaternion ToAxisAngle() const
 444       {
 445         T c = (q[3]>1) ? 1 : (q[3]<-1) ? -1 : q[3];
 446         if (c >= (T)1) {
 447           return Quaternion((T)0, (T)0, (T)0, (T)0);
 448         }
 449         else if (c <= (T)-1) {
 450           return Quaternion((T)1, (T)0, (T)0, (T)M_PI);
 451         }
 452         else {
 453           T theta = acos(c);
 454           T s = (T)1/sin(theta);
 455           return Quaternion(q[0]*s,q[1]*s,q[2]*s,2*theta);
 456         }
 457       }
 458
 459       // Exponentials / Logarithms
 460       // Logarithm of a quaternion, given as:
 461       // log(q) = v*a where q = [cos(a),v*sin(a)]
 462       inline friend Quaternion log(const Quaternion & q)
 463       {
 464         T a = acosf(q.q[3]);
 465         T sina = sinf(a);
 466         Quaternion ret;
 467         ret.q[3] = (T)0;
 468         if (sina > (T)0) {
 469           ret.q[0] = a*q.q[0]/sina;
 470           ret.q[1] = a*q.q[1]/sina;
 471           ret.q[2] = a*q.q[2]/sina;
 472         } else {
 473           ret.q[0] = ret.q[1] = ret.q[2] = (T)0;
 474         }
 475         return ret;
 476       }
 477
 478       // e^quaternion given as:
 479       // exp(v*a) = [cos(a),vsin(a)]
 480       inline friend Quaternion exp(const Quaternion & q)
 481       {
 482         T a = q.mag();
 483         T sinaoa = sinf(a)/a;
 484         T cosa = cosf(a);
 485         Quaternion ret;
 486
 487         ret.q[3] = cosa;
 488         if(a > (T)0) {
 489           ret.q[0] = sinaoa * q.q[0];
 490           ret.q[1] = sinaoa * q.q[1];
 491           ret.q[2] = sinaoa * q.q[2];
 492         } else {
 493           ret.q[0] = ret.q[1] = ret.q[2] = (T)0;
 494         }
 495
 496         return ret;
 497       }
 498
 499       // Linear interpolation between two quaternions
 500       inline friend Quaternion lerp(const Quaternion & q1, const Quaternion & q2, T t)
 501       {
 502         return Quaternion(q1.q[0]+t*(q2.q[0]-q1.q[0]),
 503                           q1.q[1]+t*(q2.q[1]-q1.q[1]),
 504                                      q1.q[2]+t*(q2.q[2]-q1.q[2]),
 505                                          q1.q[3]+t*(q2.q[3]-q1.q[3])).normalize();
 506       }
 507
 508       // Spherical Linear interpolation between two quaternions
 509       inline friend Quaternion slerp(const Quaternion & q1, const Quaternion & q2, T t)
 510       {
 511         Quaternion q3;
 512         T dot = q1.q[0]*q2.q[0] + q1.q[1]*q2.q[1] + q1.q[2]*q2.q[2] + q1.q[3]*q2.q[3];
 513
 514         // dot = cos(theta)
 515         // if (dot < 0), q1 and q2 are more than 90 degrees apart,
 516         // so we can invert one to reduce spinning
 517         if (dot < 0) {
 518           dot = -dot;
 519           q3 = -q2;
 520         } else {
 521           q3 = q2;
 522         }
 523
 524         if (dot < (T)0.95) {
 525           T angle   = acosf(dot);
 526           T sina    = sinf(angle);
 527           T sinat   = sinf(angle*t);
 528           T sinaomt = sinf(angle*((T)1-t));
 529           return (q1*sinaomt+q3*sinat)/sina;
 530         } else {
 531           // if the angle is small, use linear interpolation
 532           return lerp(q1,q3,t);
 533         }
 534       }
 535
 536       // This version of slerp, used by squad, does not check for theta > 90.
 537       inline friend Quaternion slerp_no_invert(const Quaternion & q1, const Quaternion & q2, T t)
 538       {
 539         T dot = q1.q[0]*q2.q[0] + q1.q[1]*q2.q[1] + q1.q[2]*q2.q[2] + q1.q[3]*q2.q[3];
 540
 541         if (dot > (T)-0.95 && dot < (T)0.95) {
 542           T angle   = acos(dot),
 543                            sina    = sin(angle),
 544                                          sinat   = sin(angle*t),
 545                                              sinaomt = sin(angle*((T)1-t));
 546                                              return (q1*sinaomt+q2*sinat)/sina;
 547         } else {
 548           // if the angle is small, use linear interpolation
 549           return lerp(q1,q2,t);
 550         }
 551       }
 552
 553       // Spherical cubic interpolation
 554       inline friend Quaternion squad(const Quaternion & q1, const Quaternion & q2,
 555                                      const Quaternion & a,  const Quaternion & b,  T t)
 556       {
 557         Quaternion c(slerp_no_invert(q1,q2,t)),
 558                      d(slerp_no_invert(a, b, t));
 559                      return slerp_no_invert(c,d,2*t*((T)1-t));
 560       }
 561
 562       // Given 3 quaternions, qn-1,qn and qn+1, calculate a control point to be used in spline interpolation
 563       inline friend Quaternion spline(const Quaternion & qnm1,const Quaternion & qn,const Quaternion & qnp1)
 564       {
 565         Quaternion qni(qn);
 566         qni.conjugate();
 567         return qn*exp((log(qni*qnm1)+log(qni*qnp1))*((T)-0.25));
 568       }
 569   }; // ::maths::Quaternion
 570
 571   /// Write a vector to an output stream.
 572   template <typename T>
 573   inline std::ostream & operator << (std::ostream & out, const Quaternion<T> & v)
 574   {
 575     out << "(" << v[0];
 576     for (int i = 1; i < 4; ++i) {
 577       out << ", " << v[i];
 578     }
 579     return out << ")";
 580   }
 581
 582 } // ::maths
 583
 584 #endif