ctmspat.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_SPATIALMATRIX
00022 #define CT_SPATIALMATRIX
00023
00024 #include "csphyzik/ctmatrix.h"
00025 #include "csphyzik/ctvspat.h"
00026
00027 #define ctSpatialMatrix ctSpatialMatrix6
00028
00029
00030 //************ MATRIX6
00031
00032 class ctSpatialMatrix6 : public ctMatrix
00033 {
00034 public:
00035 ctSpatialMatrix6 ()
00036 {
00037 dimen = 6;
00038 int idx, idy;
00039 for(idx = 0; idx < 6; idx++)
00040 for(idy = 0; idy < 6; idy++)
00041 rows[idx][idy] = ( idx == idy ) ? 1.0 : 0.0;
00042 }
00043
00049 void form_spatial_transformation ( const ctMatrix3 &pR, const ctVector3 &pr );
00050
00055 void form_spatial_I ( const ctMatrix3 &pI, real pmass );
00056
00057 real * operator[] ( const int index )
00058 { return (real *)(rows[index]); }
00059
00060 real * operator[] ( const int index ) const
00061 { return (real *)(rows[index]); }
00062
00063 void operator=( const ctSpatialMatrix6 &pm )
00064 {
00065 int idx, idy;
00066 for( idx = 0; idx < 6; idx++ )
00067 for( idy = 0; idy < 6; idy++ )
00068 rows[idx][idy] = pm[idx][idy];
00069 }
00070
00071 void identity ()
00072 {
00073 int idx, idy;
00074 for (idx = 0; idx < 6; idx++)
00075 for (idy = 0; idy < 6; idy++)
00076 {
00077 if( idx == idy )
00078 rows[idx][idy] = 1.0;
00079 else
00080 rows[idx][idy] = 0.0;
00081 }
00082 }
00083
00084 ctSpatialMatrix6 get_transpose () const
00085 {
00086 ctSpatialMatrix6 Mret;
00087 int idx, idy;
00088 for (idx = 0; idx < 6; idx++)
00089 for (idy = 0; idy < 6; idy++)
00090 Mret[idx][idy] = rows[idy][idx];
00091 return Mret;
00092 }
00093
00094 void orthonormalize ();
00095
00096 void mult_v ( ctSpatialVector6 &pdest, const ctSpatialVector6 &pv )
00097 {
00098 int idx, idy;
00099 for(idx = 0; idx < 6; idx++)
00100 {
00101 pdest[idx] = 0;
00102 for(idy = 0; idy < 6; idy++)
00103 pdest[idx] += rows[idx][idy]*pv[idy];
00104 }
00105 }
00106
00107 ctSpatialVector6 operator* ( const ctSpatialVector6 &pv )
00108 {
00109 ctSpatialVector6 rv (0,0,0,0,0,0);
00110 int idx, idx2;
00111 for (idx = 0; idx < 6; idx++)
00112 for(idx2 = 0; idx2 < 6; idx2++)
00113 rv[idx] += rows[idx][idx2]*pv[idx2];
00114
00115 return rv;
00116 }
00117
00118 ctSpatialVector6 operator* ( const ctSpatialVector6 &pv ) const
00119 {
00120 ctSpatialVector6 rv( 0,0,0,0,0,0);
00121 int idx, idx2;
00122 for( idx = 0; idx < 6; idx++ )
00123 for( idx2 = 0; idx2 < 6; idx2++ )
00124 rv[idx] += rows[idx][idx2]*pv[idx2];
00125 return rv;
00126 }
00127
00128 ctSpatialMatrix6 operator* ( const ctSpatialMatrix6 &MM ) const
00129 {
00130 ctSpatialMatrix6 Mret;
00131 int idr, idc, adder;
00132 for(idr = 0; idr < 6; idr++)
00133 for(idc = 0; idc < 6; idc++)
00134 {
00135 Mret[idr][idc] = 0.0;
00136 for(adder = 0; adder < 6; adder++)
00137 Mret[idr][idc] += rows[idr][adder]*MM[adder][idc];
00138 }
00139
00140 return Mret;
00141 }
00142
00143 void mult_M( ctSpatialMatrix6 &Mret, const ctSpatialMatrix6 &MM ) const
00144 {
00145 int idr, idc, adder;
00146 for (idr = 0; idr < 6; idr++)
00147 for (idc = 0; idc < 6; idc++)
00148 {
00149 Mret[idr][idc] = 0.0;
00150 for(adder = 0; adder < 6; adder++)
00151 Mret[idr][idc] += rows[idr][adder]*MM[adder][idc];
00152 }
00153 }
00154
00155 void operator*=( const ctSpatialMatrix6 &MM )
00156 {
00157 ctSpatialMatrix6 Mret;
00158 int idr, idc, adder;
00159 for (idr = 0; idr < 6; idr++)
00160 for (idc = 0; idc < 6; idc++)
00161 {
00162 Mret[idr][idc] = 0.0;
00163 for (adder = 0; adder < 6; adder++)
00164 Mret[idr][idc] += rows[idr][adder]*MM[adder][idc];
00165 }
00166 *this = Mret;
00167 }
00168
00169 ctSpatialMatrix6 operator* ( const real pk ) const
00170 {
00171 ctSpatialMatrix6 Mret;
00172 int idr, idc;
00173 for (idr = 0; idr < 6; idr++)
00174 for (idc = 0; idc < 6; idc++)
00175 Mret[idr][idc] = rows[idr][idc]*pk;
00176 return Mret;
00177 }
00178
00179 void operator*= ( const real pm )
00180 {
00181 int idx, idy;
00182 for (idx = 0; idx < 6; idx++)
00183 for (idy = 0; idy < 6; idy++)
00184 rows[idx][idy] *= pm;
00185 }
00186
00187 // addition
00188 void add ( const ctSpatialMatrix6 &pm )
00189 {
00190 int idx, idy;
00191 for (idx = 0; idx < 6; idx++)
00192 for (idy = 0; idy < 6; idy++)
00193 rows[idx][idy] += pm.rows[idx][idy];
00194 }
00195
00196 void add2 ( const ctSpatialMatrix6 &pm1, const ctSpatialMatrix6 &pm2 )
00197 {
00198 int idx, idy;
00199 for (idx = 0; idx < 6; idx++)
00200 for (idy = 0; idy < 6; idy++)
00201 rows[idx][idy] = pm1.rows[idx][idy] + pm2.rows[idx][idy];
00202 }
00203
00204 void add3 ( ctSpatialMatrix6 &pmdest,
00205 const ctSpatialMatrix6 &pm1, const ctSpatialMatrix6 &pm2 )
00206 {
00207 int idx, idy;
00208 for (idx = 0; idx < 6; idx++)
00209 for (idy = 0; idy < 6; idy++)
00210 pmdest.rows[idx][idy] = pm1.rows[idx][idy] + pm2.rows[idx][idy];
00211 }
00212
00213 void operator+= ( const ctSpatialMatrix6 &pm )
00214 {
00215 int idx, idy;
00216 for (idx = 0; idx < 6; idx++)
00217 for (idy = 0; idy < 6; idy++)
00218 rows[idx][idy] += pm.rows[idx][idy];
00219 }
00220
00221 ctSpatialMatrix6 operator+ ( const ctSpatialMatrix6 &pm )
00222 {
00223 ctSpatialMatrix6 Mret;
00224 int idx, idy;
00225 for (idx = 0; idx < 6; idx++)
00226 for (idy = 0; idy < 6; idy++)
00227 Mret.rows[idx][idy] = rows[idx][idy] + pm.rows[idx][idy];
00228
00229 return Mret;
00230 }
00231
00232 // subtraction
00233 void subtract ( const ctSpatialMatrix6 &pm )
00234 {
00235 int idx, idy;
00236 for (idx = 0; idx < 6; idx++)
00237 for (idy = 0; idy < 6; idy++)
00238 rows[idx][idy] -= pm.rows[idx][idy];
00239 }
00240
00241 void subtract2 ( const ctSpatialMatrix6 &pm1, const ctSpatialMatrix6 &pm2 )
00242 {
00243 int idx, idy;
00244 for (idx = 0; idx < 6; idx++)
00245 for (idy = 0; idy < 6; idy++)
00246 rows[idx][idy] = pm1.rows[idx][idy] - pm2.rows[idx][idy];
00247 }
00248
00249 void subtract3 ( ctSpatialMatrix6 &pmdest,
00250 const ctSpatialMatrix6 &pm1, const ctSpatialMatrix6 &pm2 )
00251 {
00252 int idx, idy;
00253 for (idx = 0; idx < 6; idx++)
00254 for (idy = 0; idy < 6; idy++)
00255 pmdest.rows[idx][idy] = pm1.rows[idx][idy] - pm2.rows[idx][idy];
00256 }
00257
00258 void operator-= ( const ctSpatialMatrix6 &pm )
00259 {
00260 int idx, idy;
00261 for (idx = 0; idx < 6; idx++)
00262 for (idy = 0; idy < 6; idy++)
00263 rows[idx][idy] -= pm.rows[idx][idy];
00264 }
00265
00266 ctSpatialMatrix6 operator- ( ctSpatialMatrix6 &pm )
00267 {
00268 ctSpatialMatrix6 Mret;
00269 int idx, idy;
00270 for (idx = 0; idx < 6; idx++)
00271 for (idy = 0; idy < 6; idy++)
00272 Mret.rows[idx][idy] = rows[idx][idy] - pm.rows[idx][idy];
00273 return Mret;
00274 }
00275
00277 // solve the linear system Ax = b where x is an unknown vector
00278 // b is a known vector and A is this matrix
00279 // solved x will be returned in px
00280 void solve ( ctSpatialVector6 &px, const ctSpatialVector6 &pb )
00281 {
00282 real **A;
00283 real *b;
00284 real *x;
00285 int idx, idy;
00286
00287 b = (real *)malloc ( sizeof( real )*6 );
00288 A = (real **)malloc ( sizeof( real * )*6 );
00289 x = px.get_elements();
00290
00291 for(idx = 0; idx < 6; idx++)
00292 {
00293 b[idx] = pb[idx];
00294 A[idx] = (real *)malloc( sizeof( real )*6 );
00295 for(idy = 0; idy < 6; idy++)
00296 A[idx][idy] = rows[idx][idy];
00297 }
00298
00299 // solve this sucker
00300 linear_solve( A, 6, x, b );
00301
00302 free( b );
00303 free( A );
00304 }
00305
00306 void debug_print ()
00307 {
00308 int i, j;
00309 for(i = 0; i < dimen; i++)
00310 {
00311 for(j = 0; j < dimen; j++)
00312 {
00313 // Debug::logf( CT_DEBUG_LEVEL, "%lf :: ", rows[i][j] );
00314 // Debug::logf( CT_DEBUG_LEVEL, "\n" );
00315 }
00316 }
00317 }
00318
00319 protected:
00320 real rows[6][6];
00321
00322 };
00323
00324 inline ctSpatialMatrix6 ctSpatialVector6::operator*
00325 ( const ctVectorTranspose6 &pv )
00326 {
00327 ctSpatialMatrix6 Mret;
00328 int idr, idc;
00329 for(idr = 0; idr < 6; idr++)
00330 for(idc = 0; idc < 6; idc++)
00331 Mret[idr][idc] = elements[idr]*pv[idc];
00332 return Mret;
00333 }
00334
00335 inline void ctSpatialMatrix::form_spatial_transformation
00336 ( const ctMatrix3 &pR, const ctVector3 &pr )
00337 {
00338 // -~rR ~r is the x-product matrix thing
00339 rows[3][0]/*M21.m11*/ = pr[2]*pR[1][0] - pr[1] * pR[2][0];
00340 rows[3][1]/*M21.m12*/ = pr[2]*pR[1][1] - pr[1] * pR[2][1];
00341 rows[3][2]/*M21.m13*/ = pr[2]*pR[1][2] - pr[1] * pR[2][2];
00342
00343 rows[4][0]/*M21.m21*/ = - pr[2]*pR[0][0] + pr[0] * pR[2][0];
00344 rows[4][1]/*M21.m22*/ = - pr[2]*pR[0][1] + pr[0] * pR[2][1];
00345 rows[4][2]/*M21.m23*/ = - pr[2]*pR[0][2] + pr[0] * pR[2][2];
00346
00347 rows[5][0]/*M21.m31*/ = pr[1]*pR[0][0] - pr[0] * pR[1][0];
00348 rows[5][1]/*M21.m32*/ = pr[1]*pR[0][1] - pr[0] * pR[1][1];
00349 rows[5][2]/*M21.m33*/ = pr[1]*pR[0][2] - pr[0] * pR[1][2];
00350
00351 int idr, idc;
00352 for (idr = 0; idr < 3; idr++)
00353 {
00354 for(idc = 0; idc < 3; idc++)
00355 {
00356 rows[idr][idc] = pR[idr][idc];
00357 rows[idr+3][idc+3] = pR[idr][idc];
00358 rows[idr][idc+3] = 0.0;
00359 }
00360 }
00361 }
00362
00363 inline void ctSpatialMatrix::form_spatial_I( const ctMatrix3 &pI, real pmass )
00364 {
00365 /* M11 *= 0;
00366 M12.Identity();
00367 M12 *= pmass;
00368 M21 = pI;
00369 M22 *= 0;
00370 */
00371
00372 int idr, idc;
00373 for (idr = 0; idr < 3; idr++)
00374 {
00375 for (idc = 0; idc < 3; idc++)
00376 {
00377 rows[idr][idc] = 0;
00378 rows[idr][idc+3] = ( idc == idr ) ? pmass : 0.0;
00379 rows[idr+3][idc] = pI[idr][idc];
00380 rows[idr+3][idc+3] = 0.0;
00381 }
00382 }
00383 }
00384
00385 #endif
Generated for Crystal Space by doxygen 1.2.5 written by Dimitri van Heesch, ©1997-2000