Main Page   Class Hierarchy   Compound List   File List   Compound Members  

ctmatrix.h

00001 /*
00002     Dynamics/Kinematics modeling and simulation library.
00003     Copyright (C) 1999 by Michael Alexander Ewert
00004 
00005     This library is free software; you can redistribute it and/or
00006     modify it under the terms of the GNU Library General Public
00007     License as published by the Free Software Foundation; either
00008     version 2 of the License, or (at your option) any later version.
00009 
00010     This library is distributed in the hope that it will be useful,
00011     but WITHOUT ANY WARRANTY; without even the implied warranty of
00012     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00013     Library General Public License for more details.
00014 
00015     You should have received a copy of the GNU Library General Public
00016     License along with this library; if not, write to the Free
00017     Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
00018 
00019 */
00020 
00021 #ifndef __CT_MATRIX__
00022 #define __CT_MATRIX__
00023 
00024 #include <stdlib.h>
00025 
00026 #include "csphyzik/phyztype.h"
00027 #include "csphyzik/ctvector.h"
00028 #include "csphyzik/mtrxutil.h"
00029 #include "csphyzik/debug.h"
00030 
00031 class ctMatrix
00032 {
00033 public:
00034   int get_dim() { return dimen; }
00035 
00036 protected:
00037   int dimen;
00038 
00039 
00040 };
00041 
00042 // NxN matrix
00043 class ctMatrixN
00044 {
00045 protected:
00046   ctMatrixN ()
00047   { rows = NULL; dimen = 0; }
00048 
00049   real **rows;
00050   int dimen;
00051 
00052 public:
00053   ctMatrixN ( long pdim, real scl = 1.0 )
00054   {
00055         int i, j;
00056     dimen = pdim;
00057     rows = new real *[pdim];
00058     for(i = 0; i < pdim; i++ )
00059     {
00060       rows[i] = new real[pdim];
00061       for(j = 0; j < pdim; j++ )
00062         rows[i][j] = 0.0;
00063       rows[i][i] = scl;
00064     }
00065   }
00066   
00067   ctMatrixN ( const ctMatrixN &pM )
00068   {
00069     int i,j;
00070     dimen = pM.dimen;
00071     rows = new real *[dimen];
00072     for( i = 0; i < dimen; i++ )
00073     {
00074       rows[i] = new real[dimen];
00075       for( j = 0; j < dimen; j++ )
00076         rows[i][j] = 0.0;
00077       rows[i][i] = 1.0;
00078     }
00079 
00080     for( i = 0; i < dimen; i++ )
00081       for( j = 0; j < dimen; j++ )
00082         rows[i][j] = pM.rows[i][j];
00083   }
00084 
00085   void operator = ( const ctMatrixN &pM )
00086   {
00087     int i,j;
00088     int lowdim;
00089     if ( pM.dimen < dimen)
00090       lowdim = pM.dimen;
00091     else
00092       lowdim = dimen;
00093 
00094     for( i = 0; i < lowdim; i++ )
00095       for( j = 0; j < lowdim; j++ )
00096         rows[i][j] = pM.rows[i][j];
00097   }
00098 
00099   virtual ~ctMatrixN ()
00100   {
00101         int i;
00102     for(i = 0; i < dimen; i++)
00103       delete [] (rows[i]);
00104     delete [] rows;
00105   }
00106 
00107   real **access_elements ()
00108   { return rows; }
00109 
00110   void identity ()
00111   { 
00112         int i, j;
00113     for(i = 0; i < dimen; i++ )
00114     {
00115       for(j = 0; j < dimen; j++ )
00116         rows[i][j] = 0.0;
00117       rows[i][i] = 1.0;
00118     }
00119   }
00120 
00121   real *operator[] ( const int index )
00122   { return rows[index]; }
00123 
00124   real *operator[] ( const int index ) const 
00125   { return rows[index]; }
00126 
00127   ctMatrixN get_transpose () const 
00128   {
00129     ctMatrixN Mret;
00130         int idx, idy;
00131     for(idx = 0; idx < dimen; idx++)
00132       for(idy = 0; idy < dimen; idy++)
00133         Mret[idx][idy] = rows[idy][idx];
00134     return Mret;
00135   }
00136 
00137   void orthonormalize();
00138 
00139   // better be same size vector....
00140   void mult_v ( real *pdest, const real *pv )
00141   {
00142         int idx, idy;
00143     for(idx = 0; idx < dimen; idx++)
00144     {
00145       pdest[idx] = 0;
00146       for(idy = 0; idy < dimen; idy++)
00147         pdest[idx] += rows[idx][idy]*pv[idy];
00148     }
00149   }
00150 
00151   ctMatrixN operator* ( const ctMatrixN &MM ) const 
00152   {
00153     ctMatrixN Mret;
00154         int idr, idc, adder;
00155     for(idr = 0; idr < dimen; idr++)
00156       for(idc = 0; idc < dimen; idc++)
00157       {
00158         Mret[idr][idc] = 0.0;
00159         for(adder = 0; adder < dimen; adder++)
00160           Mret[idr][idc] += rows[idr][adder]*(MM[adder][idc]);
00161       }
00162 
00163     return Mret;
00164   }
00165 
00166   ctMatrixN operator* ( const real pk ) const 
00167   {
00168     ctMatrixN Mret;
00169         int idr, idc;
00170     for(idr = 0; idr < dimen; idr++)
00171       for(idc = 0; idc < dimen; idc++)
00172         Mret[idr][idc] = rows[idr][idc]*pk;
00173     return Mret;
00174   }
00175 
00176   void operator*= ( const real pm )
00177   {
00178         int idx, idy;
00179     for(idx = 0; idx < dimen; idx++)
00180       for(idy = 0; idy < dimen; idy++)
00181         rows[idx][idy] *= pm;
00182   }
00183 
00184   // addition
00185   void add ( const ctMatrixN &pm )
00186   {
00187         int idx, idy;
00188     for(idx = 0; idx < dimen; idx++)
00189       for(idy = 0; idy < dimen; idy++)
00190         rows[idx][idy] += pm.rows[idx][idy];
00191   }
00192 
00193   void add2 ( const ctMatrixN &pm1, const ctMatrixN &pm2 )
00194   {
00195         int idx, idy;
00196     for(idx = 0; idx < dimen; idx++)
00197       for(idy = 0; idy < dimen; idy++)
00198         rows[idx][idy] = pm1.rows[idx][idy] + pm2.rows[idx][idy];
00199   }
00200 
00201   void add3 ( ctMatrixN &pmdest, const ctMatrixN &pm1, const ctMatrixN &pm2 )
00202   {
00203         int idx, idy;
00204     for(idx = 0; idx < dimen; idx++ )
00205       for(idy = 0; idy < dimen; idy++ )
00206         pmdest.rows[idx][idy] = pm1.rows[idx][idy] + pm2.rows[idx][idy];
00207   }
00208 
00209   void operator+=( const ctMatrixN &pm )
00210   {
00211         int idx, idy;
00212     for(idx = 0; idx < dimen; idx++)
00213       for(idy = 0; idy < dimen; idy++)
00214         rows[idx][idy] += pm.rows[idx][idy];
00215   }
00216 
00217   ctMatrixN operator+( const ctMatrixN &pm )
00218   {
00219     ctMatrixN Mret;
00220 
00221         int idx, idy;
00222     for(idx = 0; idx < dimen; idx++)
00223       for(idy = 0; idy < dimen; idy++)
00224         Mret.rows[idx][idy] = rows[idx][idy] + pm.rows[idx][idy];
00225 
00226     return Mret;
00227   }
00228 
00229   // subtraction
00230   void subtract ( const ctMatrixN &pm )
00231   {
00232         int idx, idy;
00233     for(idx = 0; idx < dimen; idx++)
00234       for(idy = 0; idy < dimen; idy++)
00235         rows[idx][idy] -= pm.rows[idx][idy];
00236   }
00237 
00238   void subtract2 ( const ctMatrixN &pm1, const ctMatrixN &pm2 )
00239   {
00240         int idx, idy;
00241     for(idx = 0; idx < dimen; idx++)
00242       for(idy = 0; idy < dimen; idy++)
00243         rows[idx][idy] = pm1.rows[idx][idy] - pm2.rows[idx][idy];
00244   }
00245 
00246   void subtract3 ( ctMatrixN &pmdest, const ctMatrixN &pm1, const ctMatrixN &pm2 )
00247   {
00248         int idx, idy;
00249     for(idx = 0; idx < dimen; idx++)
00250       for(idy = 0; idy < dimen; idy++)
00251         pmdest.rows[idx][idy] = pm1.rows[idx][idy] - pm2.rows[idx][idy];
00252   }
00253 
00254   void operator-= ( const ctMatrixN &pm )
00255   {
00256         int idx, idy;
00257     for(idx = 0; idx < dimen; idx++)
00258       for(idy = 0; idy < dimen; idy++)
00259         rows[idx][idy] -= pm.rows[idx][idy];
00260   }
00261 
00262   ctMatrixN operator- ( ctMatrixN &pm )
00263   {
00264     ctMatrixN Mret;
00265 
00266         int idx, idy;
00267     for(idx = 0; idx < dimen; idx++)
00268       for(idy = 0; idy < dimen; idy++)
00269         Mret.rows[idx][idy] = rows[idx][idy] - pm.rows[idx][idy];
00270 
00271     return Mret;
00272   }
00273 
00279   void solve( real *px, const real *pb )
00280   {
00281     real *x;
00282     real *b;
00283     int idx;  
00284     b = (real *)malloc( sizeof( real )*dimen );
00285     x = px;
00286 
00287     for( idx = 0; idx < dimen; idx++ )
00288       b[idx] = pb[idx];
00289 
00290     // solve this sucker
00291     linear_solve ( rows, dimen, x, b );
00292     free(b);
00293   }
00294 
00295   void debug_print()
00296   {
00297         int i, j;
00298     for(i = 0; i < dimen; i++)
00299     {
00300       for(j = 0; j < dimen; j++)
00301       {
00302         logf( "%f :: ", rows[i][j] ); 
00303       }
00304       logf( "\n" );
00305     }
00306   }
00307 };
00308 
00309 
00310 class ctMatrix3 : public ctMatrix
00311 {
00312 protected:
00313   real rows[3][3];
00314 
00315   real cofactor(int i, int j) 
00316   {
00317     real sign = ((i + j) % 2) ? -1 : 1;
00318 
00319     // Set which rows/columns the cofactor will use
00320     int r1, r2, c1, c2;
00321     r1 = (i == 0) ? 1 : 0;
00322     r2 = (i == 2) ? 1 : 2;
00323     c1 = (j == 0) ? 1 : 0;
00324     c2 = (j == 2) ? 1 : 2;
00325 
00326     real C = rows[r1][c1] * rows[r2][c2] - rows[r2][c1] * rows[r1][c2];
00327     return sign * C;
00328   }
00329 
00330   real determinant() 
00331   {
00332     return rows[0][0]*rows[1][1]*rows[2][2] + rows[0][1]*rows[1][2]*rows[2][0]
00333       + rows[0][2]*rows[1][0]*rows[2][1] - rows[0][0]*rows[1][2]*rows[2][1]
00334       - rows[0][1]*rows[1][0]*rows[2][2] - rows[0][2]*rows[1][1]*rows[2][0];
00335   }
00336 
00337 public:
00338 
00339   ctMatrix3( real scl = 1.0 )
00340   {
00341     dimen = 3;
00342     rows[0][0] = rows[1][1] = rows[2][2] = scl;
00343     rows[0][1] = rows[0][2] = rows[1][0] = 0.0;
00344     rows[1][2] = rows[2][0] = rows[2][1] = 0.0;
00345   }
00346 
00347   ctMatrix3( real p00, real p01, real p02,
00348     real p10, real p11, real p12,
00349     real p20, real p21, real p22 )
00350   {
00351     rows[0][0] = p00; 
00352     rows[0][1] = p01; 
00353     rows[0][2] = p02;
00354     rows[1][0] = p10; 
00355     rows[1][1] = p11; 
00356     rows[1][2] = p12;
00357     rows[2][0] = p20; 
00358     rows[2][1] = p21; 
00359     rows[2][2] = p22;
00360   }
00361 
00362   void set( real p00, real p01, real p02,
00363             real p10, real p11, real p12,
00364             real p20, real p21, real p22 );
00365 
00366   void identity()
00367   { 
00368     rows[0][0] = rows[1][1] = rows[2][2] = 1.0;
00369     rows[0][1] = rows[0][2] = rows[1][0] = 0.0;
00370     rows[1][2] = rows[2][0] = rows[2][1] = 0.0;
00371   }
00372 
00373   real *operator[]( const int index )
00374   { return (real *)(rows[index]); }
00375 
00376   real *operator[]( const int index ) const 
00377   { return (real *)(rows[index]); }
00378 
00379   ctMatrix3 get_transpose () const 
00380   {
00381     ctMatrix3 Mret;
00382         int idx, idy;
00383     for (idx = 0; idx < 3; idx++)
00384       for (idy = 0; idy < 3; idy++)
00385         Mret[idx][idy] = (*this)[idy][idx];
00386     return Mret;
00387   }
00388 
00389   void put_transpose( ctMatrix3 &Mret ) const 
00390   {
00391         int idx, idy;
00392     for (idx = 0; idx < 3; idx++)
00393       for (idy = 0; idy < 3; idy++)
00394         Mret[idx][idy] = (*this)[idy][idx];
00395   }
00396 
00398   void similarity_transform( ctMatrix3 &Mret, const ctMatrix3 &pA ) const 
00399   {
00400         int idr, idc, adder;
00401     for (idr = 0; idr < 3; idr++)
00402       for (idc = 0; idc < 3; idc++)
00403       {
00404         Mret[idr][idc] = 0.0;
00405         for (adder = 0; adder < 3; adder++)
00406           Mret[idr][idc] += ( rows[idr][0]*pA[0][adder] + 
00407                               rows[idr][1]*pA[1][adder] + 
00408                               rows[idr][2]*pA[2][adder] ) *
00409                               rows[idc][adder];
00410       }
00411   }
00412 
00413   void orthonormalize ();
00414 
00415   void mult_v ( ctVector3 &pdest, const ctVector3 &pv )
00416   {
00417         int idx, idy;
00418     for (idx = 0; idx < 3; idx++)
00419     {
00420       pdest[idx] = 0;
00421       for (idy = 0; idy < 3; idy++)
00422         pdest[idx] += rows[idx][idy]*pv[idy];
00423     }
00424   }
00425 
00426   ctVector3 operator* ( const ctVector3 &pv ) 
00427   {
00428     ctVector3 rv(0,0,0);
00429     int idx, idx2;
00430     for ( idx = 0; idx < 3; idx++ )
00431       for ( idx2 = 0; idx2 < 3; idx2++ )
00432         rv[idx] += rows[idx][idx2]*pv[idx2];
00433     return rv;
00434   }
00435 
00436   ctVector3 operator* ( const ctVector3 &pv ) const 
00437   {
00438     ctVector3 rv( 0,0,0);
00439     int idx, idx2;
00440     for ( idx = 0; idx < 3; idx++ )
00441       for ( idx2 = 0; idx2 < 3; idx2++ )
00442         rv[idx] += rows[idx][idx2]*pv[idx2];
00443     return rv;
00444   }
00445 
00446   ctMatrix3 operator* ( const ctMatrix3 &MM ) const 
00447   {
00448     ctMatrix3 Mret;
00449         int idr, idc, adder;
00450     for (idr = 0; idr < 3; idr++)
00451       for (idc = 0; idc < 3; idc++ )
00452       {
00453         Mret[idr][idc] = 0.0;
00454         for (adder = 0; adder < 3; adder++)
00455           Mret[idr][idc] += (*this)[idr][adder]*MM[adder][idc];
00456       }
00457 
00458     return Mret;
00459   }
00460 
00461   ctMatrix3 operator* ( const real pk ) const 
00462   {
00463     ctMatrix3 Mret;
00464         int idr, idc;
00465     for (idr = 0; idr < 3; idr++)
00466       for ( idc = 0; idc < 3; idc++)
00467         Mret[idr][idc] = (*this)[idr][idc]*pk;
00468 
00469     return Mret;
00470   }
00471 
00472   void operator*= ( const real pm )
00473   {
00474         int idx, idy;
00475     for (idx = 0; idx < 3; idx++)
00476       for (idy = 0; idy < 3; idy++)
00477         rows[idx][idy] *= pm;
00478   }
00479 
00480   // addition
00481   void add ( const ctMatrix3 &pm )
00482   {
00483         int idx, idy;
00484     for (idx = 0; idx < 3; idx++)
00485       for (idy = 0; idy < 3; idy++)
00486         (*this)[idx][idy] += pm[idx][idy];
00487   }
00488 
00489   void add2 ( const ctMatrix3 &pm1, const ctMatrix3 &pm2 )
00490   {
00491         int idx, idy;
00492     for (idx = 0; idx < 3; idx++)
00493       for (idy = 0; idy < 3; idy++)
00494         (*this)[idx][idy] = pm1[idx][idy] + pm2[idx][idy];
00495   }
00496 
00497   void add3 ( ctMatrix3 &pmdest, const ctMatrix3 &pm1, const ctMatrix3 &pm2 )
00498   {
00499         int idx, idy;
00500     for (idx = 0; idx < 3; idx++)
00501       for (idy = 0; idy < 3; idy++)
00502         pmdest[idx][idy] = pm1[idx][idy] + pm2[idx][idy];
00503   }
00504 
00505   void operator+= ( const ctMatrix3 &pm )
00506   {
00507         int idx, idy;
00508     for (idx = 0; idx < 3; idx++)
00509       for (idy = 0; idy < 3; idy++)
00510         (*this)[idx][idy] += pm[idx][idy];
00511   }
00512 
00513   ctMatrix3 operator+ ( const ctMatrix3 &pm )
00514   {
00515     ctMatrix3 Mret;
00516 
00517         int idx, idy;
00518     for (idx = 0; idx < 3; idx++)
00519       for (idy = 0; idy < 3; idy++)
00520         Mret[idx][idy] = (*this)[idx][idy] + pm[idx][idy];
00521 
00522     return Mret;
00523   }
00524 
00525   // subtraction
00526   void subtract ( const ctMatrix3 &pm )
00527   {
00528         int idx, idy;
00529     for (idx = 0; idx < 3; idx++ )
00530       for (idy = 0; idy < 3; idy++ )
00531         (*this)[idx][idy] -= pm[idx][idy];
00532   }
00533 
00534   void subtract2 ( const ctMatrix3 &pm1, const ctMatrix3 &pm2 )
00535   {
00536         int idx, idy;
00537     for (idx = 0; idx < 3; idx++)
00538       for (idy = 0; idy < 3; idy++)
00539         (*this)[idx][idy] = pm1[idx][idy] - pm2[idx][idy];
00540   }
00541 
00542   void subtract3 ( ctMatrix3 &pmdest, const ctMatrix3 &pm1, const ctMatrix3 &pm2 )
00543   {
00544         int idx, idy;
00545     for (idx = 0; idx < 3; idx++)
00546       for (idy = 0; idy < 3; idy++)
00547         pmdest[idx][idy] = pm1[idx][idy] - pm2[idx][idy];
00548   }
00549 
00550   void operator-=( const ctMatrix3 &pm )
00551   {
00552         int idx, idy;
00553     for (idx = 0; idx < 3; idx++)
00554       for (idy = 0; idy < 3; idy++)
00555         (*this)[idx][idy] -= pm[idx][idy];
00556   }
00557 
00558   ctMatrix3 operator-( ctMatrix3 &pm )
00559   {
00560     ctMatrix3 Mret;
00561 
00562         int idx, idy;
00563     for (idx = 0; idx < 3; idx++)
00564       for (idy = 0; idy < 3; idy++)
00565         Mret[idx][idy] = (*this)[idx][idy] - pm[idx][idy];
00566 
00567     return Mret;
00568   }
00569 
00570   // solve the linear system Ax = b where x is an unknown vector
00571   // b is a known vector and A is this matrix
00572   // solved x will be returned in px
00573   void solve ( ctVector3 &px, const ctVector3 &pb )
00574   {
00575     real **A;
00576     real *b;
00577     real *x;
00578     int idx, idy;
00579     
00580     b = (real *)malloc( sizeof( real )*3 );
00581     A = (real **)malloc( sizeof( real * )*3 );
00582 //    x = px.get_elements();
00583     x = (real *)malloc( sizeof( real )*3 );
00584     x[0] = px[0]; x[1] = px[1]; x[2] = px[2];
00585 
00586     for(idx = 0; idx < 3; idx++)
00587     {
00588       b[idx] = pb[idx];
00589       A[idx] = (real *)malloc( sizeof( real )*3 );
00590       for(idy = 0; idy < 3; idy++)
00591         A[idx][idy] = (*this)[idx][idy];
00592     }
00593 
00594     // solve this sucker
00595     linear_solve ( A, 3, x, b );
00596 
00597     px[0] = x[0]; px[1] = x[1]; px[2] = x[2];
00598     free( x );
00599     free( b );
00600     free( A );
00601   }
00602 
00603   ctMatrix3 inverse () 
00604   {
00605     ctMatrix3 inv;
00606     real det = determinant();
00607 
00608     int i,j;
00609     for(i = 0; i < 3; i++) 
00610       for(j = 0; j < 3; j++)
00611         inv[i][j] = cofactor (j,i) / det;
00612 
00613     return inv;
00614   }
00615 
00616   void debug_print ()
00617   {
00618         int i, j;
00619     for (i = 0; i < dimen; i++)
00620     {
00621       for (j = 0; j < dimen; j++)
00622         DEBUGLOGF( "%f :: ", rows[i][j] ); 
00623       DEBUGLOG( "\n" );
00624     }
00625   }
00626 };
00627 
00628 
00629 inline void ctMatrix3::set ( real p00, real p01, real p02,
00630                              real p10, real p11, real p12,
00631                              real p20, real p21, real p22 )
00632 {
00633   rows[0][0] = p00; 
00634   rows[0][1] = p01; 
00635   rows[0][2] = p02;
00636   rows[1][0] = p10; 
00637   rows[1][1] = p11; 
00638   rows[1][2] = p12;
00639   rows[2][0] = p20; 
00640   rows[2][1] = p21; 
00641   rows[2][2] = p22;
00642   
00643 }
00644 
00645 /*
00646 inline void ctMatrix3::orthonormalize()
00647 {
00648   rows[0].Normalize();
00649   rows[1] -= rows[0]*(rows[1] * rows[0]);
00650   rows[2] = rows[0] % rows[1];
00651 }
00652 */
00653 inline void ctMatrix3::orthonormalize ()
00654 {
00655   real len = sqrt (  rows[0][0] * rows[0][0]  
00656                    + rows[0][1] * rows[0][1]  
00657                    + rows[0][2] * rows[0][2] ); 
00658   
00659   rows[0][0] /= len;   rows[0][1] /= len;   rows[0][2] /= len;
00660   
00661   real abdot =   rows[1][0] * rows[0][0] 
00662                + rows[1][1] * rows[0][1] 
00663                + rows[1][2] * rows[0][2]; 
00664 
00665   rows[1][0] -= rows[0][0] * abdot;
00666   rows[1][1] -= rows[0][1] * abdot;
00667   rows[1][2] -= rows[0][2] * abdot;
00668 
00669   rows[2][0] = rows[0][1] * rows[1][2] - rows[0][2] * rows[1][1];
00670   rows[2][1] = rows[0][2] * rows[1][0] - rows[0][0] * rows[1][2];
00671   rows[2][2] = rows[0][0] * rows[1][1] - rows[0][1] * rows[1][0];
00672 }
00673 
00674 #endif // __CT_MATRIX__

Generated for Crystal Space by doxygen 1.2.5 written by Dimitri van Heesch, ©1997-2000