ctmatrix.h Source File

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