Back to index

extremetuxracer  0.5beta
matrix.cpp
Go to the documentation of this file.
00001 /* 
00002  * Copyright (C) 2004-2005 Volker Stroebel <mmv1@planetpenguin.de>
00003  * 
00004  * Copyright (C) 1999-2001 Jasmin F. Patry
00005  *
00006  * This program is free software; you can redistribute it and/or
00007  * modify it under the terms of the GNU General Public License
00008  * as published by the Free Software Foundation; either version 2
00009  * of the License, or (at your option) any later version.
00010  * 
00011  * This program 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
00014  * GNU General Public License for more details.
00015  * 
00016  * You should have received a copy of the GNU General Public License
00017  * along with this program; if not, write to the Free Software
00018  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
00019  */
00020  
00021 #include "matrix.h"
00022 #include "defs.h"
00023 #include <math.h>
00024 
00025 namespace pp {
00026        
00027 Matrix::Matrix(const Quat quat)
00028 {
00029     data[0][0] = 1.0 - 2.0 * ( quat.y * quat.y + quat.z * quat.z );
00030     data[1][0] =       2.0 * ( quat.x * quat.y - quat.w * quat.z );
00031     data[2][0] =       2.0 * ( quat.x * quat.z + quat.w * quat.y );
00032 
00033     data[0][1] =       2.0 * ( quat.x * quat.y + quat.w * quat.z );
00034     data[1][1] = 1.0 - 2.0 * ( quat.x * quat.x + quat.z * quat.z );
00035     data[2][1] =       2.0 * ( quat.y * quat.z - quat.w * quat.x );
00036 
00037        data[0][2] =       2.0 * ( quat.x * quat.z - quat.w * quat.y );
00038     data[1][2] =       2.0 * ( quat.y * quat.z + quat.w * quat.x );
00039     data[2][2] = 1.0 - 2.0 * ( quat.x * quat.x + quat.y * quat.y );
00040 
00041     data[3][0] = data[3][1] = data[3][2] = 0.0;
00042     data[0][3] = data[1][3] = data[2][3] = 0.0;
00043     data[3][3] = 1.0;
00044 }
00045 
00046 void
00047 Matrix::makeIdentity(void)
00048 {
00049        for(int i=0; i<4; i++){
00050               for(int j=0; j<4; j++){
00051                      data[i][j]=(i==j);   
00052               }
00053        }
00054 }      
00055 
00056 void
00057 Matrix::makeRotation(const double angle, const char axis)
00058 {
00059        double sinv = sin( ANGLES_TO_RADIANS( angle ) );
00060     double cosv = cos( ANGLES_TO_RADIANS( angle ) );
00061 
00062     makeIdentity();
00063 
00064     switch( axis ) {
00065     case 'x':
00066         data[1][1] = cosv;
00067         data[2][1] = -sinv;
00068         data[1][2] = sinv;
00069         data[2][2] = cosv;
00070         break;
00071 
00072     case 'y':
00073         data[0][0] = cosv;
00074         data[2][0] = sinv;
00075         data[0][2] = -sinv;
00076         data[2][2] = cosv;
00077         break;
00078 
00079     case 'z': 
00080         data[0][0] = cosv;
00081         data[1][0] = -sinv;
00082         data[0][1] = sinv;
00083         data[1][1] = cosv;
00084         break;
00085 
00086     default:
00087               {
00088         //code_not_reached();  /* shouldn't get here */
00089               }
00090     }
00091 }
00092 
00093 void
00094 Matrix::makeTranslation(const double x, const double y, const double z)
00095 {
00096        makeIdentity();
00097     data[3][0] = x;
00098     data[3][1] = y;
00099     data[3][2] = z;
00100 }
00101 
00102 void
00103 Matrix::makeScaling(const double x, const double y, const double z )
00104 {
00105        makeIdentity();
00106     data[0][0] = x;
00107     data[1][1] = y;
00108     data[2][2] = z;
00109 }
00110 
00111 void
00112 Matrix::makeRotationAboutVector(const Vec3d u, const double angle )
00113 {
00114        Matrix rx,irx, ry, iry;
00115        
00116     double a = u.x;
00117     double b = u.y;
00118     double c = u.z;
00119        double d = sqrt( b*b + c*c );
00120 
00121     if ( d < EPS ) {
00122         if ( a < 0 ) 
00123             makeRotation( -angle, 'x' );
00124         else
00125             makeRotation( angle, 'x' );
00126         return;
00127     } 
00128 
00129     rx.makeIdentity();
00130     irx.makeIdentity();
00131     ry.makeIdentity();
00132     iry.makeIdentity();
00133 
00134     rx.data[1][1] = c/d;
00135     rx.data[2][1] = -b/d;
00136     rx.data[1][2] = b/d;
00137     rx.data[2][2] = c/d;
00138 
00139     irx.data[1][1] = c/d;
00140     irx.data[2][1] = b/d;
00141     irx.data[1][2] = -b/d;
00142     irx.data[2][2] = c/d;
00143 
00144     ry.data[0][0] = d;
00145     ry.data[2][0] = -a;
00146     ry.data[0][2] = a;
00147     ry.data[2][2] = d;
00148 
00149     iry.data[0][0] = d;
00150     iry.data[2][0] = a;
00151     iry.data[0][2] = -a;
00152     iry.data[2][2] = d;
00153 
00154     makeRotation( angle, 'z' );
00155 
00156     *this=(*this)*ry;
00157     *this=(*this)*rx;
00158     *this=iry*(*this);
00159     *this=irx*(*this);
00160 }
00161 
00162 
00163 void
00164 Matrix::transpose(const Matrix& matrix)
00165 {
00166        for( int i= 0 ; i< 4 ; i++ ){
00167               for( int j= 0 ; j< 4 ; j++ ){
00168               data[j][i] = matrix.data[i][j];
00169               }
00170        }
00171 }
00172 
00173 Matrix
00174 Matrix::operator*(const Matrix matrix) const
00175 {
00176        Matrix ret;
00177  
00178     for( int i= 0 ; i< 4 ; i++ )
00179        for( int j= 0 ; j< 4 ; j++ )
00180            ret.data[j][i]=
00181               data[0][i] * matrix.data[j][0] +
00182               data[1][i] * matrix.data[j][1] +
00183               data[2][i] * matrix.data[j][2] +
00184               data[3][i] * matrix.data[j][3];
00185 
00186        return ret;
00187 }
00188 
00189 Vec3d
00190 Matrix::transformVector(const Vec3d v) const
00191 {   
00192        return Vec3d(
00193               v.x * data[0][0] + v.y * data[1][0] + v.z * data[2][0],
00194        v.x * data[0][1] + v.y * data[1][1] + v.z * data[2][1],
00195        v.x * data[0][2] + v.y * data[1][2] + v.z * data[2][2]
00196        );
00197 }
00198        
00199               
00200 Vec3d
00201 Matrix::transformPoint(const Vec3d p) const
00202 {
00203        return Vec3d(
00204        p.x * data[0][0] + p.y * data[1][0] + p.z * data[2][0] + data[3][0],
00205        p.x * data[0][1] + p.y * data[1][1] + p.z * data[2][1] + data[3][1],
00206        p.x * data[0][2] + p.y * data[1][2] + p.z * data[2][2] + data[3][2]
00207     );
00208 }
00209 
00210 void
00211 Matrix::makeChangeOfBasisMatrix(Matrix& mat,
00212                             Matrix& invMat,
00213                             const Vec3d w1,
00214                             const Vec3d w2,
00215                             const Vec3d w3)
00216 {
00217        mat.makeIdentity();
00218     mat.data[0][0] = w1.x;
00219     mat.data[0][1] = w1.y;
00220     mat.data[0][2] = w1.z;
00221     mat.data[1][0] = w2.x;
00222     mat.data[1][1] = w2.y;
00223     mat.data[1][2] = w2.z;
00224     mat.data[2][0] = w3.x;
00225     mat.data[2][1] = w3.y;
00226     mat.data[2][2] = w3.z;
00227 
00228     invMat.makeIdentity();
00229     invMat.data[0][0] = w1.x;
00230     invMat.data[1][0] = w1.y;
00231     invMat.data[2][0] = w1.z;
00232     invMat.data[0][1] = w2.x;
00233     invMat.data[1][1] = w2.y;
00234     invMat.data[2][1] = w2.z;
00235     invMat.data[0][2] = w3.x;
00236     invMat.data[1][2] = w3.y;
00237     invMat.data[2][2] = w3.z;
00238 }
00239 
00240 } //namespace pp