Quaternion.cpp Source File

00001 
00002 /*
00003     TEDDY - General graphics application library
00004     Copyright (C) 1999-2002  Timo Suoranta
00005     tksuoran@cc.helsinki.fi
00006 
00007         Adapted from
00008 
00009         The Universe Development Kit
00010         Copyright (C) 2000  Sean O'Neil
00011         s_p_oneil@hotmail.com
00012 
00013     This library is free software; you can redistribute it and/or
00014     modify it under the terms of the GNU Lesser General Public
00015     License as published by the Free Software Foundation; either
00016     version 2.1 of the License, or (at your option) any later version.
00017 
00018     This library is distributed in the hope that it will be useful,
00019     but WITHOUT ANY WARRANTY; without even the implied warranty of
00020     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00021     Lesser General Public License for more details.
00022 
00023     You should have received a copy of the GNU Lesser General Public
00024     License along with this library; if not, write to the Free Software
00025     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00026 
00027     $Id: Quaternion.cpp,v 1.6 2002/01/22 19:30:05 tksuoran Exp $
00028 */
00029 
00030 
00031 #include "Teddy/Maths/Quaternion.h"
00032 #include "Teddy/Maths/Matrix.h"
00033 #include "Teddy/SysSupport/Messages.h"
00034 using namespace Teddy::SysSupport;
00035 
00036 
00037 namespace Teddy {
00038 namespace Maths {
00039 
00040 
00041 #define DELTA 1e-6f
00042 
00043 #define QUATERNION_EPSILON 1e-6f
00044 
00045 
00046 
00047 
00048 Quaternion::Quaternion(){
00049 }
00050 
00051 Quaternion::Quaternion( const float a, const float b, const float c, const float d ){
00052     v[0] = a;
00053     v[1] = b;
00054     v[2] = c;
00055     v[3] = d;
00056 }
00057 Quaternion::Quaternion( const double a, const double b, const double c, const double d ){
00058     v[0] = a;
00059     v[1] = b;
00060     v[2] = c;
00061     v[3] = d;
00062 }
00063 Quaternion::Quaternion( const Vector     &v, const double f){setAxisAngle(v, f); }
00064 Quaternion::Quaternion( const Vector     &v ){ *this = v; }
00065 Quaternion::Quaternion( const Quaternion &q ){ *this = q; }
00066 Quaternion::Quaternion( const Matrix     &m ){ *this = m; }
00067 Quaternion::Quaternion( const double     *p ){ *this = p; }
00068 
00069 // Casting and unary operators
00070             Quaternion::operator       double*()                    { return v; }
00071 double     &Quaternion::operator           [] ( const int n )       { return v[n]; }
00072             Quaternion::operator const double*()              const { return v; }
00073 double      Quaternion::operator           [] ( const int n ) const { return v[n]; }
00074 Quaternion  Quaternion::operator            - ()              const { return Quaternion(-v[0], -v[1], -v[2], -v[3]); }
00075 
00076 // Equal and comparison operators
00077 void Quaternion::operator=( const Vector     &vec ){ v[0] = vec.v[0]; v[1] = vec.v[1]; v[2] = vec.v[2]; v[3] =    0; }
00078 void Quaternion::operator=( const Quaternion &q   ){ v[0] = q  .v[0]; v[1] = q  .v[1]; v[2] = q  .v[2]; v[3] = q.v[3]; }
00079 void Quaternion::operator=( const double     *p   ){ v[0] = p[0]; v[1] = p[1]; v[2] = p[2]; v[3] = p[3]; }
00080 
00081 // Arithmetic operators (quaternion and scalar)
00082 Quaternion Quaternion::operator+( const double f ) const { return Quaternion(v[0]+f, v[1]+f, v[2]+f, v[3]+f); }
00083 Quaternion Quaternion::operator-( const double f ) const { return Quaternion(v[0]-f, v[1]-f, v[2]-f, v[3]-f); }
00084 Quaternion Quaternion::operator*( const double f ) const { return Quaternion(v[0]*f, v[1]*f, v[2]*f, v[3]*f); }
00085 Quaternion Quaternion::operator/( const double f ) const { return Quaternion(v[0]/f, v[1]/f, v[2]/f, v[3]/f); }
00086 const Quaternion &Quaternion::operator+=( const double f ){ v[0]+=f; v[1]+=f; v[2]+=f; v[3]+=f; return *this; }
00087 const Quaternion &Quaternion::operator-=( const double f ){ v[0]-=f; v[1]-=f; v[2]-=f; v[3]-=f; return *this; }
00088 const Quaternion &Quaternion::operator*=( const double f ){ v[0]*=f; v[1]*=f; v[2]*=f; v[3]*=f; return *this; }
00089 const Quaternion &Quaternion::operator/=( const double f ){ v[0]/=f; v[1]/=f; v[2]/=f; v[3]/=f; return *this; }
00090 
00091 // Arithmetic operators (quaternion and quaternion)
00092 Quaternion Quaternion::operator+( const Quaternion &q ) const { return Quaternion(v[0]+q.v[0], v[1]+q.v[1], v[2]+q.v[2], v[3]+q.v[3]); }
00093 Quaternion Quaternion::operator-( const Quaternion &q ) const { return Quaternion(v[0]-q.v[0], v[1]-q.v[1], v[2]-q.v[2], v[3]-q.v[3]); }
00094                                                                                                                            
00095 const Quaternion &Quaternion::operator+=( const Quaternion &q ){ v[0]+=q.v[0]; v[1]+=q.v[1]; v[2]+=q.v[2]; v[3]+=q.v[3]; return *this; }
00096 const Quaternion &Quaternion::operator-=( const Quaternion &q ){ v[0]-=q.v[0]; v[1]-=q.v[1]; v[2]-=q.v[2]; v[3]-=q.v[3]; return *this; }
00097 const Quaternion &Quaternion::operator*=( const Quaternion &q ){ *this = *this * q; return *this; }
00098 
00099 // Magnitude/normalize methods
00100 double Quaternion::magnitudeSquared() const { return v[0]*v[0] + v[1]*v[1] + v[2]*v[2] + v[3]*v[3]; }
00101 double Quaternion::magnitude       () const { return sqrt( magnitudeSquared() ); }
00102 void   Quaternion::normalize       ()       { *this /= magnitude(); }
00103 
00104 // Advanced quaternion methods
00105 Quaternion Quaternion::conjugate   () const { return Quaternion(-v[0], -v[1], -v[2], v[3]); }
00106 Quaternion Quaternion::inverse     () const { return conjugate() / magnitudeSquared(); }
00107 Quaternion Quaternion::unitInverse () const { return conjugate(); }
00108 
00109 void Quaternion::rotate( const Quaternion &q ){ *this = q * *this; }
00110 
00111 void Quaternion::rotate( const Vector &vAxis, const double fAngle ){
00112     Quaternion q;
00113     q.setAxisAngle( vAxis, fAngle );
00114     rotate( q );
00115 }
00116 
00117 /*Vector getViewAxis() const {
00118     // 6 muls, 7 adds
00119     double x2 = v[0] + v[0];
00120     double y2 = v[1] + v[1];
00121     double z2 = v[2] + v[2];
00122     double xx = v[0] * x2; double xz = v[0] * z2;
00123     double yy = v[1] * y2; double yz = v[1] * z2;
00124     double wx = v[3] * x2; double wy = v[3] * y2;
00125     return -Vector(xz+wy, yz-wx, 1-(xx+yy));
00126 }*/
00127 
00128 Vector Quaternion::getViewAxis() const {
00129     // 6 muls, 7 adds
00130     double x2 = v[0] + v[0];
00131     double y2 = v[1] + v[1];
00132     double z2 = v[2] + v[2];
00133     double xx = v[0] * x2; double xz = v[0] * z2;
00134     double yy = v[1] * y2; double yz = v[1] * z2;
00135     double wx = v[3] * x2; double wy = v[3] * y2;
00136     return -Vector(xz+wy, yz-wx, 1-(xx+yy));
00137 }
00138 
00139 Vector Quaternion::getUpAxis() const {
00140     // 6 muls, 7 adds
00141     double x2 = v[0] + v[0];
00142     double y2 = v[1] + v[1];
00143     double z2 = v[2] + v[2];
00144     double xx = v[0] * x2; double xy = v[0] * y2;
00145     double yz = v[1] * z2; double zz = v[2] * z2;
00146     double wx = v[3] * x2; double wz = v[3] * z2;
00147     return Vector(xy-wz, 1-(xx+zz), yz+wx);
00148 }
00149 
00150 Vector Quaternion::getRightAxis() const {
00151     // 6 muls, 7 adds
00152     double x2 = v[0] + v[0];
00153     double y2 = v[1] + v[1];
00154     double z2 = v[2] + v[2];
00155     double xy = v[0] * y2; double xz = v[0] * z2;
00156     double yy = v[1] * y2; double zz = v[2] * z2;
00157     double wy = v[3] * y2; double wz = v[3] * z2;
00158     return Vector(1-(yy+zz), xy+wz, xz-wy);
00159 }
00160 
00161 
00163 
00164 
00165 Vector Quaternion::rotateVector( Vector &vec ) const {
00166     return Vector( *this * Quaternion(vec) * unitInverse() );
00167 }
00168 
00169 
00170 void Quaternion::setAxisAngle( const Vector &vAxis, const double radians ){
00171     // 4 muls, 2 trig function calls
00172     double d = radians * 0.5;
00173 
00174     *this = vAxis * sin( d );
00175     v[3]  = cos( d );
00176 }
00177 
00178 
00179 void Quaternion::getAxisAngle( Vector &vAxis, double &radians ) const {
00180     // 4 muls, 1 div, 2 trig function calls
00181     radians = acos( v[3] );
00182     vAxis   = *this / sin( radians );
00183     radians *= 2;
00184 }
00185 
00186 
00187 #if 0
00188 void Quaternion::operator=( const Matrix &m ){
00189     // Check the sum of the diagonal
00190     double tr = m(0, 0) + m(1, 1) + m(2, 2);
00191     emsg( M_DEBUG, "tr = % 10.7f", tr );
00192     if( tr > 0.0f ){
00193         // The sum is positive
00194         // 4 muls, 1 div, 6 adds, 1 trig function call
00195         double s = sqrt(tr + 1);
00196         v[3] = s * 0.5;
00197         s    = 0.5 / s;
00198         v[0] = (m(1, 2) - m(2, 1)) * s;
00199         v[1] = (m(2, 0) - m(0, 2)) * s;
00200         v[2] = (m(0, 1) - m(1, 0)) * s;
00201     }else{
00202         // The sum is negative
00203         // 4 muls, 1 div, 8 adds, 1 trig function call
00204         const int nIndex[3] = {1, 2, 0};
00205         int i;
00206         int j;
00207         int k;
00208         i = 0;
00209         if( m(1, 1) > m(i, i) ) i = 1;
00210         if( m(2, 2) > m(i, i) ) i = 2;
00211         j = nIndex[i];
00212         k = nIndex[j];
00213 
00214         double s = sqrtf((m(i, i) - (m(j, j) + m(k, k))) + 1);
00215         (*this)[i] = s * 0.5;
00216         if( s != 0.0 ) s = 0.5 / s;
00217         (*this)[j] = (m(i, j) + m(j, i)) * s;
00218         (*this)[k] = (m(i, k) + m(k, i)) * s;
00219         (*this)[3] = (m(j, k) - m(k, j)) * s;
00220     }
00221 }
00222 #else
00223 void Quaternion::operator=( const Matrix &m ){
00224     // Check the sum of the diagonal
00225     double tr = m.m[0][0] + m.m[1][1] + m.m[2][2];
00226     if( tr > 0.0 ){
00227         // The sum is positive
00228         // 4 muls, 1 div, 6 adds, 1 trig function call
00229         double s = sqrt(tr + 1.0);
00230         //emsg( M_DEBUG, "s = % 10.7f", s );
00231         v[3] = s * 0.5;
00232         s    = 0.5 / s;
00233         v[0] = (m.m[1][2] - m.m[2][1]) * s;
00234         v[1] = (m.m[2][0] - m.m[0][2]) * s;
00235         v[2] = (m.m[0][1] - m.m[1][0]) * s;
00236         //emsg( M_DEBUG, "s = % 10.7f", s );
00237         //emsg( M_DEBUG, "% 10.7f % 10.7f % 10.7f % 10.7f ", v[0], v[1], v[2], v[3] );
00238     }else{
00239         // The sum is negative
00240         // 4 muls, 1 div, 8 adds, 1 trig function call
00241         const int nIndex[3] = {1, 2, 0};
00242         int i;
00243         int j;
00244         int k;
00245         i = 0;
00246         if( m.m[1][1] > m.m[i][i] ) i = 1;
00247         if( m.m[2][2] > m.m[i][i] ) i = 2;
00248         j = nIndex[i];
00249         k = nIndex[j];
00250 
00251         //emsg( M_DEBUG, "i = %d", i );
00252         double s = sqrt((m.m[i][i] - (m.m[j][j] + m.m[k][k])) + 1);
00253         //emsg( M_DEBUG, "s = % 10.7f", s );
00254         (*this)[i] = s * 0.5;
00255         if( s != 0.0 ) s = 0.5 / s;
00256         //emsg( M_DEBUG, "s = % 10.7f", s );
00257         (*this)[j] = (m.m[i][j] + m.m[j][i]) * s;
00258         (*this)[k] = (m.m[i][k] + m.m[k][i]) * s;
00259         (*this)[3] = (m.m[j][k] - m.m[k][j]) * s;
00260     }
00261 }
00262 #endif
00263 
00264 Quaternion Quaternion::operator*( const Quaternion &q ) const {
00265     // 12 muls, 30 adds
00266     double E = (v[0] + v[2])*(q.v[0] + q.v[1]);
00267     double F = (v[2] - v[0])*(q.v[0] - q.v[1]);
00268     double G = (v[3] + v[1])*(q.v[3] - q.v[2]);
00269     double H = (v[3] - v[1])*(q.v[3] + q.v[2]);
00270     double A = F - E;
00271     double B = F + E;
00272     return Quaternion(
00273         (v[3] + v[0])*(q.v[3] + q.v[0]) + (A - G - H) * 0.5,
00274         (v[3] - v[0])*(q.v[1] + q.v[2]) + (B + G - H) * 0.5,
00275         (v[1] + v[2])*(q.v[3] - q.v[0]) + (B - G + H) * 0.5,
00276         (v[2] - v[1])*(q.v[1] - q.v[2]) + (A + G + H) * 0.5
00277     );
00278 }
00279 
00280 
00282 Quaternion slerp( const Quaternion &q1, const Quaternion &q2, const double t ){
00283     // Calculate the cosine of the angle between the two
00284     double scale0;
00285     double scale1;
00286     double cosinus = q1.v[0] * q2.v[0] + q1.v[1] * q2.v[1] + q1.v[2] * q2.v[2] + q1.v[3] * q2.v[3];
00287 
00288     // If the angle is significant, use the spherical interpolation
00289     if( (1 - fabs(cosinus)) > DELTA ){
00290         double temp  = acos( fabs(cosinus) );
00291         double sinus = sin ( temp );
00292         scale0 = sin( ( - t) * temp) / sinus;
00293         scale1 = sin(     t  * temp) / sinus;
00294     }else{  //  Else use the cheaper linear interpolation
00295         scale0 = 1 - t;
00296         scale1 = t;
00297     }
00298 
00299     if( cosinus < 0 ) scale1 = -scale1;
00300 
00301     // Return the interpolated result
00302     return (q1 * scale0) + (q2 * scale1);
00303 }
00304 
00305 
00306 };  //  namespace Maths
00307 };  //  namespace Teddy
00308 
00309