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 #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
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
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
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
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
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
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
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
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
00572
00573
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
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
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
00648
00649
00650
00651
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__