sglQuaternion.hpp Source File

00001 /*****************************************************************************
00002  * SGL: A Scene Graph Library
00003  *
00004  * Copyright (C) 1997-2001  Scott McMillan   All Rights Reserved.
00005  *
00006  * This library is free software; you can redistribute it and/or
00007  * modify it under the terms of the GNU Library General Public
00008  * License as published by the Free Software Foundation; either
00009  * version 2 of the License, or (at your option) any later version.
00010  *
00011  * This library is distributed in the hope that it will be useful,
00012  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00014  * Library General Public License for more details.
00015  *
00016  * You should have received a copy of the GNU Library General Public
00017  * License along with this library; if not, write to the Free
00018  * Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
00019  *****************************************************************************
00020  *     File: sglQuaternion.hpp
00021  *   Author: Will Devore
00022  *  Created: 18 July 2000
00023  *  Summary: 4D templated quaternion class
00024  *****************************************************************************/
00025 
00026 #ifndef __SGL_QUATERNION_HPP
00027 #define __SGL_QUATERNION_HPP
00028 
00029 #include <sgl.h>
00030 #include <sglVector.hpp>
00031 #include <sglMatrix.hpp>
00032 
00033 // ---------------------------------------------------------------------------
00034 template <class T>
00035 class sglQuaternion
00036 {
00037 public:
00038    // contructors
00039    sglQuaternion() {};
00040 
00041    sglQuaternion(T i, T j, T k, T w)
00042       {
00043          m_quat[0] = i; m_quat[1] = j;
00044          m_quat[2] = k; m_quat[3] = w;
00045       }
00046 
00047    template <class S>
00048    sglQuaternion(const sglQuaternion<S>& v)
00049       {
00050          m_quat[0] = v[0];
00051          m_quat[1] = v[1];
00052          m_quat[2] = v[2];
00053          m_quat[3] = v[3];
00054       }
00055 
00056    template <class S>
00057    sglQuaternion(const sglVec4<S>& v)
00058       {
00059          m_quat[0] = v[0];
00060          m_quat[1] = v[1];
00061          m_quat[2] = v[2];
00062          m_quat[3] = v[3];
00063       }
00064 
00065    // axis/angle constructor
00066    template <class S>
00067    sglQuaternion(const sglVec3<S>& v, S a)
00068       {
00069          S len = v.length();
00070          if (len > 0.0)
00071          {
00072             double s = sin( (double)a/2.0 );
00073             m_quat[0] = (T)s * v[0]/len;
00074             m_quat[1] = (T)s * v[1]/len;
00075             m_quat[2] = (T)s * v[2]/len;
00076             m_quat[3] = (T)cos( (double)a/2.0 );
00077          }
00078          else
00079          {
00080             m_quat[0] = m_quat[1] = m_quat[2] = 0.0;
00081             m_quat[3] = 1.0;
00082          }
00083       }
00084 
00085    // destructor
00086    ~sglQuaternion() {};  // not virtual, do not subclass
00087 
00088    // indexing operators
00089    inline       T& operator[](unsigned int i)       { return m_quat[i]; }
00090    inline const T& operator[](unsigned int i) const { return m_quat[i]; }
00091 
00092    // set from any other type
00093    template <class S>
00094    inline void set(S i, S j, S k, S w)
00095       {
00096          m_quat[0] = (T)i; m_quat[1] = (T)j;
00097          m_quat[2] = (T)k; m_quat[3] = (T)w;
00098       }
00099 
00100    template <class S>
00101    inline void set(const sglVec3<S> &v, S a) // axis angle
00102       {
00103          S len = v.length();
00104          if (len > 0.0)
00105          {
00106             double s = sin( (double)a/2.0 );
00107             m_quat[0] = (T)s * v[0]/len;
00108             m_quat[1] = (T)s * v[1]/len;
00109             m_quat[2] = (T)s * v[2]/len;
00110             m_quat[3] = (T)cos( (double)a/2.0 );
00111          }
00112          else
00113          {
00114             m_quat[0] = m_quat[1] = m_quat[2] = 0.0;
00115             m_quat[3] = 1.0;
00116          }
00117       }
00118 
00119    // assignment operators
00120 #ifndef WIN32
00121    inline sglQuaternion& operator=(const sglQuaternion& q)
00122       {
00123          m_quat[0] = q[0];
00124          m_quat[1] = q[1];
00125          m_quat[2] = q[2];
00126          m_quat[3] = q[3];
00127          return *this;
00128       }
00129 #endif
00130 
00131    template <class S>
00132    inline sglQuaternion &operator=(const sglQuaternion<S>& q)
00133       {
00134          m_quat[0] = (T)q[0];
00135          m_quat[1] = (T)q[1];
00136          m_quat[2] = (T)q[2];
00137          m_quat[3] = (T)q[3];
00138          return *this;
00139       }
00140 
00141    template <class S>
00142    inline sglQuaternion &operator=(const sglVec4<S>& v)
00143       {
00144          m_quat[0] = v[0];
00145          m_quat[1] = v[1];
00146          m_quat[2] = v[2];
00147          m_quat[3] = v[3];
00148 
00149          return *this;
00150       }
00151 
00152    //------------------------------------------------------------------------
00153    // boolean operators
00154    template <class S>
00155    inline bool operator==(const sglQuaternion<S>& q) const
00156       {
00157          return (m_quat[0] == (T)q[0] && m_quat[1] == (T)q[1] &&
00158                  m_quat[2] == (T)q[2] && m_quat[3] == (T)q[3]);
00159       }
00160 
00161    template <class S>
00162    inline bool operator!=(const sglQuaternion<S>& q) const
00163       {
00164          return (m_quat[0] != (T)q[0] || m_quat[1] != (T)q[1] ||
00165                  m_quat[2] != (T)q[2] || m_quat[3] != (T)q[3]);
00166       }
00167 
00168    // math operators
00169    template <class S>
00170    inline sglQuaternion& operator+=(const sglQuaternion<S>& q)
00171       {
00172          m_quat[0] += (T)q[0]; m_quat[1] += (T)q[1];
00173          m_quat[2] += (T)q[2]; m_quat[3] += (T)q[3];
00174          return *this;
00175       }
00176 
00177    template <class S>
00178    inline sglQuaternion& operator-=(const sglQuaternion<S>& q)
00179       {
00180          m_quat[0] -= (T)q[0]; m_quat[1] -= (T)q[1];
00181          m_quat[2] -= (T)q[2]; m_quat[3] -= (T)q[3];
00182          return *this;
00183       }
00184 
00185    inline sglQuaternion& operator*=(const T s)
00186       {
00187          m_quat[0]*=s; m_quat[1]*=s; m_quat[2]*=s; m_quat[3]*=s;
00188          return *this;
00189       }
00190 
00191    template <class S>
00192    inline sglQuaternion& operator*=(const sglQuaternion<S> &rhs)
00193       {
00194          T q0 = (m_quat[3]*(T)rhs[0] +
00195                  m_quat[0]*(T)rhs[3] +
00196                  m_quat[1]*(T)rhs[2] -
00197                  m_quat[2]*(T)rhs[1]);
00198          T q1 = (m_quat[3]*(T)rhs[1] +
00199                  m_quat[1]*(T)rhs[3] +
00200                  m_quat[2]*(T)rhs[0] -
00201                  m_quat[0]*(T)rhs[2]);
00202          T q2 = (m_quat[3]*(T)rhs[2] +
00203                  m_quat[2]*(T)rhs[3] +
00204                  m_quat[0]*(T)rhs[1] -
00205                  m_quat[1]*(T)rhs[0]);
00206          T q3 = (m_quat[3]*(T)rhs[3] -
00207                  m_quat[0]*(T)rhs[0] -
00208                  m_quat[1]*(T)rhs[1] -
00209                  m_quat[2]*(T)rhs[2]);
00210          set(q0, q1, q2, q3);
00211 
00212          return *this;
00213       }
00214 
00215    template <class S>
00216    inline const sglQuaternion<T> operator+(const sglQuaternion<S> &rhs) const
00217       {
00218          return sglQuaternion(m_quat[0]+(T)rhs[0],
00219                               m_quat[1]+(T)rhs[1],
00220                               m_quat[2]+(T)rhs[2],
00221                               m_quat[3]+(T)rhs[3]);
00222       }
00223 
00224    template <class S>
00225    inline const sglQuaternion<T> operator-(const sglQuaternion<S> &rhs) const
00226       {
00227          return sglQuaternion(m_quat[0]-(T)rhs[0],
00228                               m_quat[1]-(T)rhs[1],
00229                               m_quat[2]-(T)rhs[2],
00230                               m_quat[3]-(T)rhs[3]);
00231       }
00232 
00233    inline const sglQuaternion<T> operator*(T scalar) const
00234       {
00235          return sglQuaternion(m_quat[0]*scalar,
00236                               m_quat[1]*scalar,
00237                               m_quat[2]*scalar,
00238                               m_quat[3]*scalar);
00239       }
00240 
00241    // multiply two quaternions
00242    template <class S>
00243    inline const sglQuaternion<T> operator*(const sglQuaternion<S> &rhs) const
00244       {
00245          return sglQuaternion(m_quat[3]*(T)rhs[0] +
00246                               m_quat[0]*(T)rhs[3] +
00247                               m_quat[1]*(T)rhs[2] -
00248                               m_quat[2]*(T)rhs[1],
00249 
00250                               m_quat[3]*(T)rhs[1] +
00251                               m_quat[1]*(T)rhs[3] +
00252                               m_quat[2]*(T)rhs[0] -
00253                               m_quat[0]*(T)rhs[2],
00254 
00255                               m_quat[3]*(T)rhs[2] +
00256                               m_quat[2]*(T)rhs[3] +
00257                               m_quat[0]*(T)rhs[1] -
00258                               m_quat[1]*(T)rhs[0],
00259 
00260                               m_quat[3]*(T)rhs[3] -
00261                               m_quat[0]*(T)rhs[0] -
00262                               m_quat[1]*(T)rhs[1] -
00263                               m_quat[2]*(T)rhs[2]);
00264       }
00265 
00266    // explicit math operations (for backward compatibility)
00267    template <class S1, class S2>
00268    inline void add(const sglQuaternion<S1> &q1, const sglQuaternion<S2> &q2)
00269       {
00270          m_quat[0] = (T)q1[0] + (T)q2[0];
00271          m_quat[1] = (T)q1[1] + (T)q2[1];
00272          m_quat[2] = (T)q1[2] + (T)q2[2];
00273          m_quat[3] = (T)q1[3] + (T)q2[3];
00274       }
00275 
00276    template <class S1, class S2>
00277    inline void sub(const sglQuaternion<S1>& q1,
00278                    const sglQuaternion<S2>& q2)
00279       {
00280          m_quat[0] = (T)q1[0] - (T)q2[0];
00281          m_quat[1] = (T)q1[1] - (T)q2[1];
00282          m_quat[2] = (T)q1[2] - (T)q2[2];
00283          m_quat[3] = (T)q1[3] - (T)q2[3];
00284       }
00285 
00286    template <class S>
00287    inline void scale(const sglQuaternion<S> &q, T s)
00288       {
00289          m_quat[0] = (T)q[0]*s;
00290          m_quat[1] = (T)q[1]*s;
00291          m_quat[2] = (T)q[2]*s;
00292          m_quat[3] = (T)q[3]*s;
00293       }
00294 
00295    // multiply two quaternions
00296    template <class S>
00297    inline void mul(const sglQuaternion<S> &rhs)
00298       {
00299          T q0 = (m_quat[3]*(T)rhs[0] +
00300                  m_quat[0]*(T)rhs[3] +
00301                  m_quat[1]*(T)rhs[2] -
00302                  m_quat[2]*(T)rhs[1]);
00303          T q1 = (m_quat[3]*(T)rhs[1] +
00304                  m_quat[1]*(T)rhs[3] +
00305                  m_quat[2]*(T)rhs[0] -
00306                  m_quat[0]*(T)rhs[2]);
00307          T q2 = (m_quat[3]*(T)rhs[2] +
00308                  m_quat[2]*(T)rhs[3] +
00309                  m_quat[0]*(T)rhs[1] -
00310                  m_quat[1]*(T)rhs[0]);
00311          T q3 = (m_quat[3]*(T)rhs[3] -
00312                  m_quat[0]*(T)rhs[0] -
00313                  m_quat[1]*(T)rhs[1] -
00314                  m_quat[2]*(T)rhs[2]);
00315          set(q0, q1, q2, q3);
00316       }
00317 
00318    // other ops
00319    template <class S>
00320    inline bool almostEqual(const sglQuaternion<S>& q, double eps) const
00321       {
00322          return (SGL_ALMOST_EQUAL(m_quat[0], q[0], eps) &&
00323                  SGL_ALMOST_EQUAL(m_quat[1], q[1], eps) &&
00324                  SGL_ALMOST_EQUAL(m_quat[2], q[2], eps) &&
00325                  SGL_ALMOST_EQUAL(m_quat[3], q[3], eps));
00326       }
00327 
00328    // reducing methods which result is a T
00329    template <class S>
00330    inline T dot(const sglQuaternion<S>& q) const
00331       {
00332          return (m_quat[0]*(T)q[0] + m_quat[1]*(T)q[1] +
00333                  m_quat[2]*(T)q[2] + m_quat[3]*(T)q[3]);
00334       }
00335 
00336    template <class S, class R>
00337    inline static R dot(const sglQuaternion<R>& q1, const sglQuaternion<S>& q2)
00338       {
00339          return (q1[0]*(R)q2[0] + q1[1]*(R)q2[1] +
00340                  q1[2]*(R)q2[2] + q1[3]*(R)q2[3]);
00341       }
00342 
00343    inline T norm() const
00344       {
00345          return (T) sqrt(dot(*this));
00346       }
00347 
00348    inline T normalize()
00349       {
00350          T len = norm();
00351          if ( len != 0.0 )
00352             *this *= (1.0/len);
00353          return len;
00354       }
00355 
00356    template <class S>
00357    inline T normalize(const sglQuaternion<S> &q)
00358       {
00359          T len = (T)q.norm();
00360          if (len > 0.0)
00361             scale(q, 1.0/len);
00362          return len;
00363       }
00364 
00365    inline void invert()
00366       {
00367          T len = norm();
00368          if (len > 0.0)
00369          {
00370             set(-m_quat[0]/len,
00371                 -m_quat[1]/len,
00372                 -m_quat[2]/len,
00373                  m_quat[3]/len);
00374          }
00375          else
00376             buildIdentity();
00377       }
00378 
00379    inline const sglQuaternion<T> conjugate( void ) const
00380       {
00381          T len = norm();
00382          if (len > 0.0)
00383          {
00384             return sglQuaternion(-m_quat[0]/len,
00385                                  -m_quat[1]/len,
00386                                  -m_quat[2]/len,
00387                                   m_quat[3]/len);
00388          }
00389          else
00390             return sglQuaternion(0.0, 0.0, 0.0, 1.0);
00391       }
00392 
00393    //------------------------------------------------------------------------
00394    // build functions
00395    inline void buildIdentity()
00396       {
00397          m_quat[0] = m_quat[1] = m_quat[2] = 0.0;
00398          m_quat[1] = 1.0;
00399       }
00400 
00401    template <class S>
00402    inline void buildFromAxisAngle(const sglVec4<S> &v, S angle)
00403       {
00404          S len = v.norm();
00405          if (len > 0.0)
00406          {
00407             double s = sin( (T)angle/2.0 );
00408             m_quat[0] = s * (T)v[0]/len;
00409             m_quat[1] = s * (T)v[1]/len;
00410             m_quat[2] = s * (T)v[2]/len;
00411             m_quat[3] = cos( (T)angle/2.0 );
00412          }
00413          else
00414          {
00415             m_quat[0] = m_quat[1] = m_quat[2] = 0.0;
00416             m_quat[3] = 1.0;
00417          }
00418       }
00419 
00420    template <class S>
00421    inline void buildFromTransformation(const sglMat4<S> &mat)
00422       {
00423          double trace = mat[0][0] + mat[1][1] + mat[2][2];
00424          if (trace > 0.)
00425          {
00426             double s = sqrt(trace + 1.);
00427             m_quat[3] = (T)(s*0.5);
00428             s = 0.5/s;
00429             m_quat[0] = (T)((mat[1][2] - mat[2][1])*s);
00430             m_quat[1] = (T)((mat[2][0] - mat[0][2])*s);
00431             m_quat[2] = (T)((mat[0][1] - mat[1][0])*s);
00432          }
00433          else // negative or zero trace
00434          {
00435             unsigned int ix = 0;
00436             if (mat[1][1] > mat[0][0]) ix = 1;
00437             if (mat[2][2] > mat[1][1]) ix = 2;
00438             unsigned int iy = (ix+1)%3;
00439             unsigned int iz = (iy+1)%3;
00440 
00441             double s = sqrt((mat[ix][ix] - (mat[iy][iy] + mat[iz][iz])) + 1.);
00442             m_quat[ix] = (T)(s*0.5);
00443 
00444             if (s != 0.) s = 0.5/s;
00445             m_quat[3]  = (T)((mat[iy][iz] - mat[iz][iy])*s);
00446             m_quat[iy] = (T)((mat[ix][iy] + mat[iy][ix])*s);
00447             m_quat[iz] = (T)((mat[ix][iz] + mat[iz][ix])*s);
00448          }
00449       }
00450 
00451    //------------------------------------------------------------------------
00452    // Extractors
00453    //------------------------------------------------------------------------
00454    // An extractor for Axis Angle assuming a normalized quaternion
00455    template <class S>
00456    inline void getAxisAngle(sglVec3<S> &axis, S &angle) const
00457       {
00458          if (SGL_ALMOST_EQUAL(norm(), 1.0, 1.0e-4))
00459          {
00460             double st2 = sqrt(m_quat[0]*m_quat[0] +
00461                               m_quat[1]*m_quat[1] +
00462                               m_quat[2]*m_quat[2]);
00463             angle = 2.0*atan2(st2, m_quat[3]); // sin(angle/2), cos(angle/2);
00464 
00465             if (st2 > 1.0e-5)
00466             {
00467                axis.set((S)m_quat[0]/st2,
00468                         (S)m_quat[1]/st2,
00469                         (S)m_quat[2]/st2);
00470             }
00471             else
00472             {
00473                axis.set(1,0,0);  // some reasonable default axis
00474             }
00475          }
00476          else
00477          {
00478             axis.set(0,0,0);
00479             angle = 0;
00480          }
00481       }
00482 
00483    inline const sglMat4<T> getTransformation( void ) const
00484       {
00485          T Nq = dot( *this );
00486          T s = (Nq > 0.0) ? (2.0 / Nq) : 0.0;
00487          T xs = m_quat[0] * s;
00488          T ys = m_quat[1] * s;
00489          T zs = m_quat[2] * s;
00490          T wx = m_quat[3] * xs;
00491          T wy = m_quat[3] * ys;
00492          T wz = m_quat[3] * zs;
00493          T xx = m_quat[0] * xs;
00494          T xy = m_quat[0] * ys;
00495          T xz = m_quat[0] * zs;
00496          T yy = m_quat[1] * ys;
00497          T yz = m_quat[1] * zs;
00498          T zz = m_quat[2] * zs;
00499 
00500          return sglMat4<T>(1.0-(yy + zz), xy + wz,       xz - wy,       0.0,
00501                            xy - wz,       1.0-(xx + zz), yz + wx,       0.0,
00502                            xz + wy,       yz - wx,       1.0-(xx + yy), 0.0,
00503                            0.0,           0.0,           0.0,           1.0);
00504       }
00505 
00506    template <class S>
00507    inline sglVec3<S> rotate(const sglVec3<S> &vec) const
00508       {
00509          if (SGL_ALMOST_EQUAL(norm(), 1.0, 1.0e-4))
00510          {
00511             S ss = (S)m_quat[3]*m_quat[3];
00512             S vdotn = (vec[0]*(S)m_quat[0] +
00513                        vec[1]*(S)m_quat[1] +
00514                        vec[2]*(S)m_quat[2]);
00515             sglVec3<S> ncrossv((S)m_quat[1]*vec[2] - (S)m_quat[2]*vec[1],
00516                                (S)m_quat[2]*vec[0] - (S)m_quat[0]*vec[2],
00517                                (S)m_quat[0]*vec[1] - (S)m_quat[1]*vec[0]);
00518             sglVec3<S> svnv(m_quat[3]*vec[0] + ncrossv[0],
00519                             m_quat[3]*vec[1] + ncrossv[1],
00520                             m_quat[3]*vec[2] + ncrossv[2]);
00521 
00522             return sglVec3<S>(
00523                ss*vec[0] + m_quat[3]*ncrossv[0] + vdotn*m_quat[0] +
00524                            m_quat[1]*svnv[2] - m_quat[2]*svnv[1],
00525                ss*vec[1] + m_quat[3]*ncrossv[1] + vdotn*m_quat[1] +
00526                            m_quat[2]*svnv[0] - m_quat[0]*svnv[2],
00527                ss*vec[2] + m_quat[3]*ncrossv[2] + vdotn*m_quat[2] +
00528                            m_quat[0]*svnv[1] - m_quat[1]*svnv[0]);
00529          }
00530          else
00531          {
00532             // I don't think this result makes much sense.
00533             sglQuaternion<S> result =
00534                *this * sglQuaternion<S>(vec, (S)0.0) * (*this).conjugate();
00535             return sglVec3<S>((S)result[0], (S)result[1], (S)result[2]);
00536          }
00537       }
00538 
00539    // stream operators
00540 #if defined(TEMPLATE_OSTREAM_HACK)
00541    template <class S>
00542    friend ostream& operator<<(ostream& ostrm, const sglQuaternion<S>& q);
00543 
00544    template <class S>
00545    friend istream& operator>>(istream& istrm, sglQuaternion<S>& q);
00546 #else
00547    friend ostream& operator<<(ostream& ostrm, const sglQuaternion<T>& q)
00548       {
00549          return (ostrm << "(" << q[0] << ", " << q[1] << ", "
00550                  << q[2] << " : " << q[3] << ")");
00551       }
00552 
00553    friend istream& operator>>(istream& istrm, sglQuaternion<T>& q)
00554       {
00555          return (istrm >> q[0] >> q[1] >> q[2] >> q[3]);
00556       }
00557 #endif
00558 private:
00559    T m_quat[4];
00560 };
00561 
00562 //----------------------------------------------------------------------------
00563 #if defined(TEMPLATE_OSTREAM_HACK)
00564 template <class T>
00565 inline ostream& operator<<(ostream& ostrm, const sglQuaternion<T>& q)
00566 {
00567    return (ostrm << "(" << q[0] << ", " << q[1] << ", "
00568            << q[2] << " : " << q[3] << ")");
00569 }
00570 
00571 template <class T>
00572 inline istream& operator>>(istream& istrm, sglQuaternion<T>& vec)
00573 {
00574    return (istrm >> q[0] >> q[1] >> q[2] >> q[3]);
00575 }
00576 #endif
00577 //----------------------------------------------------------------------------
00578 typedef sglQuaternion<double> sglQuaterniond;
00579 typedef sglQuaternion<float>  sglQuaternionf;
00580 
00581 #endif