00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
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
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
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
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
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
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
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
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
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
00571
00572
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
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
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
00647
00648
00649
00650
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__