Back to index

extremetuxracer  0.5beta
quat.cpp
Go to the documentation of this file.
00001 /* 
00002  * PPGLTK
00003  *
00004  * Copyright (C) 2004-2005 Volker Stroebel <volker@planetpenguin.de>
00005  * 
00006  * Copyright (C) 1999-2001 Jasmin F. Patry
00007  * 
00008  * This program is free software; you can redistribute it and/or
00009  * modify it under the terms of the GNU General Public License
00010  * as published by the Free Software Foundation; either version 2
00011  * of the License, or (at your option) any later version.
00012  * 
00013  * This program is distributed in the hope that it will be useful,
00014  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016  * GNU General Public License for more details.
00017  * 
00018  * You should have received a copy of the GNU General Public License
00019  * along with this program; if not, write to the Free Software
00020  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
00021  */
00022 
00023 #include "quat.h"
00024 #include "defs.h"
00025 
00026 #include <math.h>
00027 
00028 namespace pp {
00029 
00030        
00031 Quat::Quat(const double x, const double y, const double z, const double w)
00032  : x(x), y(y), z(z), w(w)
00033 {
00034 }      
00035        
00036 Quat::Quat(const Vec3d& s, const Vec3d& t)
00037 {
00038     Vec3d u = s^t;
00039        if (u.normalize() < EPS ){
00040               x=0.0;
00041               y=0.0;
00042               z=0.0;
00043               w=1.0;
00044     }else{
00045               double cos2phi = s*t;
00046               double sinphi = sqrt( ( 1 - cos2phi ) / 2.0 );
00047               double cosphi = sqrt( ( 1 + cos2phi ) / 2.0 );
00048 
00049               x = sinphi * u.x;
00050               y = sinphi * u.y;
00051               z = sinphi * u.z;
00052               w = cosphi;
00053     }
00054 }
00055 
00056 // source:
00057 // http://www.gamasutra.com/features/19980703/quaternions_01.htm
00058 //
00059 Quat::Quat(const Matrix matrix)
00060 {
00061     static int nxt[3] = {1, 2, 0};
00062     double tr = matrix.data[0][0] + matrix.data[1][1] + matrix.data[2][2];
00063 
00064     // check the diagonal
00065        if (tr > 0.0) {
00066               double s = sqrt (tr + 1.0);
00067               w = 0.5 * s;
00068               s = 0.5 / s;
00069               x = (matrix.data[1][2] - matrix.data[2][1]) * s;
00070               y = (matrix.data[2][0] - matrix.data[0][2]) * s;
00071               z = (matrix.data[0][1] - matrix.data[1][0]) * s;
00072     } else {                
00073               // diagonal is negative
00074               int i = 0;
00075               if (matrix.data[1][1] > matrix.data[0][0]) i = 1;
00076               if (matrix.data[2][2] > matrix.data[i][i]) i = 2;
00077               int j = nxt[i];
00078               int k = nxt[j];
00079 
00080               double s = sqrt (matrix.data[i][i] - matrix.data[j][j] - matrix.data[k][k] + 1.0);
00081         
00082               double q[4];         
00083               q[i] = s * 0.5;
00084                              
00085               if (s != 0.0) s = 0.5 / s;
00086 
00087               q[3] = (matrix.data[j][k] - matrix.data[k][j]) * s;
00088               q[j] = (matrix.data[i][j] + matrix.data[j][i]) * s;
00089               q[k] = (matrix.data[i][k] + matrix.data[k][i]) * s;
00090 
00091               x = q[0];
00092               y = q[1];
00093               z = q[2];
00094               w = q[3];
00095        }
00096 }
00097 
00098 
00099 
00100 void
00101 Quat::set(const double x, const double y, const double z, const double w){
00102        this->x=x;
00103        this->y=y;
00104        this->z=z;
00105        this->w=w;
00106 }
00107 
00108 
00109 Quat
00110 Quat::conjugate(void) const
00111 {
00112        return Quat(-x, -y, -z, w); 
00113 }
00114 
00115 Vec3d
00116 Quat::rotate( const Vec3d& v ) const
00117 {
00118     Quat p(v.x,v.y,v.z,1.0);
00119     Quat res_q = (*this)*(p*conjugate());
00120     
00121        return Vec3d(res_q.x,res_q.y,res_q.z);
00122 }
00123 
00124 Quat
00125 Quat::operator*(const Quat& quat) const{
00126        return Quat(
00127               y * quat.z - z * quat.y + quat.w * x + w * quat.x,
00128               z * quat.x - x * quat.z + quat.w * y + w * quat.y,
00129               x * quat.y - y * quat.x + quat.w * z + w * quat.z,
00130               w * quat.w - x * quat.x - y * quat.y - z * quat.z
00131        );
00132 }
00133 
00134 
00135 
00136 Quat 
00137 Quat::interpolate(const Quat& q, Quat r,double t )
00138 {
00139        Quat res;
00140     double cosphi;
00141     double sinphi;
00142     double phi;
00143     double scale0, scale1;
00144 
00145     cosphi = q.x * r.x + q.y * r.y + q.z * r.z + q.w * r.w;
00146 
00147     // adjust signs (if necessary) 
00148     if ( cosphi < 0.0 ) {
00149        cosphi = -cosphi;
00150        r.x = -r.x;
00151        r.y = -r.y;
00152        r.z = -r.z;
00153        r.w = -r.w;
00154     }
00155 
00156     if ( 1.0 - cosphi > EPS ) {
00157        // standard case -- slerp 
00158        phi = acos( cosphi );
00159        sinphi = sin( phi );
00160        scale0 = sin( phi * ( 1.0 - t ) ) / sinphi;
00161        scale1 = sin( phi * t ) / sinphi;
00162     } else {
00163        // use linear interpolation to avoid division by zero
00164        scale0 = 1.0 - t;
00165        scale1 = t;
00166     }
00167 
00168     res.x = scale0 * q.x + scale1 * r.x; 
00169     res.y = scale0 * q.y + scale1 * r.y; 
00170     res.z = scale0 * q.z + scale1 * r.z; 
00171     res.w = scale0 * q.w + scale1 * r.w; 
00172 
00173     return res;
00174 }
00175        
00176 } //namespace pp