|
ctmatrix.h00001 /* 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__ Generated for Crystal Space by doxygen 1.2.5 written by Dimitri van Heesch, ©1997-2000 |