|
ctvspat.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_SPATIALVECTOR 00022 #define CT_SPATIALVECTOR 00023 00024 #include "csphyzik/ctvector.h" 00025 #include "qsqrt.h" 00026 00027 class ctSpatialVector6; 00028 class ctSpatialMatrix6; 00029 00030 #define ctSpatialVector ctSpatialVector6 00031 00032 class ctVectorTranspose6 00033 { 00034 public: 00035 ctVectorTranspose6 () 00036 { 00037 elements[0] = elements[1] = elements[2] = 0.0; 00038 elements[3] = elements[4] = elements[5] = 0.0; 00039 } 00040 00041 ctVectorTranspose6 ( real pfirst, real psecond, real pthird, 00042 real p2first, real p2second, real p2third ) 00043 { 00044 elements[0] = pfirst; 00045 elements[1] = psecond; 00046 elements[2] = pthird; 00047 elements[3] = p2first; 00048 elements[4] = p2second; 00049 elements[5] = p2third; 00050 } 00051 00052 void set ( real pfirst, real psecond, real pthird, 00053 real p2first, real p2second, real p2third ) 00054 { 00055 elements[0] = pfirst; 00056 elements[1] = psecond; 00057 elements[2] = pthird; 00058 elements[3] = p2first; 00059 elements[4] = p2second; 00060 elements[5] = p2third; 00061 00062 } 00063 00064 void set ( int pnum, real *pele ) 00065 { 00066 int idx; 00067 for(idx = 0; idx < pnum; idx++ ) 00068 { 00069 elements[idx] = *pele; 00070 pele++; 00071 } 00072 } 00073 00074 void set ( real *pele ) 00075 { 00076 int idx; 00077 for(idx = 0; idx < 6; idx++ ) 00078 { 00079 elements[idx] = *pele; 00080 pele++; 00081 } 00082 } 00083 00084 real operator[] (const int index) const 00085 { return elements[index]; } 00086 00087 real& operator[] (const int index) 00088 { return elements[index]; } 00089 00090 ctVectorTranspose6 operator* ( const real pk ) 00091 { 00092 ctVectorTranspose6 scaled; 00093 int idx; 00094 for(idx = 0; idx < 6; idx++) 00095 scaled.elements[idx] = elements[idx] * pk; 00096 return scaled; 00097 } 00098 00099 void operator*= (const real p) 00100 { int idx; for (idx=0; idx<6; ++idx) elements[idx] *= p; } 00101 00102 void operator/= (const real p) 00103 { int idx; for (idx=0; idx<6; ++idx) elements[idx] /= p; } 00104 00105 real operator* ( const ctSpatialVector6 &bs ); 00106 00107 protected: 00108 real elements[ 6 ]; 00109 }; 00110 00113 class ctSpatialVector6 00114 { 00115 public: 00116 00117 ctSpatialVector6 ( const ctVector3 &pa, const ctVector3 &pb ) 00118 { 00119 elements[0] = pa[0]; 00120 elements[1] = pa[1]; 00121 elements[2] = pa[2]; 00122 elements[3] = pb[0]; 00123 elements[4] = pb[1]; 00124 elements[5] = pb[2]; 00125 } 00126 00127 ctSpatialVector6 () 00128 { 00129 elements[0] = elements[1] = elements[2] = 0.0; 00130 elements[3] = elements[4] = elements[5] = 0.0; 00131 } 00132 00133 ctSpatialVector6 ( real pone, real ptwo, real pthree, 00134 real p2one, real p2two, real p2three ) 00135 { 00136 elements[0] = pone; 00137 elements[1] = ptwo; 00138 elements[2] = pthree; 00139 elements[3] = p2one; 00140 elements[4] = p2two; 00141 elements[5] = p2three; 00142 } 00143 00144 real operator[] (const int index) const 00145 { return elements[index]; } 00146 00147 real& operator[](const int index) 00148 { return elements[index]; } 00149 00151 ctVectorTranspose6 transpose () 00152 { 00153 ctVectorTranspose6 trans; 00154 trans[0] = elements[3]; 00155 trans[1] = elements[4]; 00156 trans[2] = elements[5]; 00157 trans[3] = elements[0]; 00158 trans[4] = elements[1]; 00159 trans[5] = elements[2]; 00160 return trans; 00161 } 00162 00164 real spatial_dot( ctSpatialVector6 &pb ) 00165 { 00166 return ( elements[3]*pb[0] + elements[4]*pb[1] + elements[5]*pb[2] + 00167 elements[0]*pb[3] + elements[1]*pb[4] + elements[2]*pb[5] ); 00168 } 00169 00170 ctVectorTranspose6 operator!() 00171 { return transpose(); } 00172 00175 void set_a ( const ctVector3 &pa ) 00176 { 00177 elements[0] = pa[0]; 00178 elements[1] = pa[1]; 00179 elements[2] = pa[2]; 00180 } 00181 00183 void set_b ( const ctVector3 &pb ) 00184 { 00185 elements[3] = pb[0]; 00186 elements[4] = pb[1]; 00187 elements[5] = pb[2]; 00188 } 00189 00192 ctVector3 get_a () 00193 { return ctVector3 (elements[0], elements[1], elements[2]); } 00194 00196 ctVector3 get_b() 00197 { return ctVector3 ( elements[3], elements[4], elements[5]); } 00198 00199 void operator= ( const ctSpatialVector6 &pm ) 00200 { 00201 int idx; 00202 for (idx = 0; idx < 6; idx++ ) 00203 elements[idx] = pm[idx]; 00204 } 00205 00207 real length (); 00208 00210 ctSpatialVector6 unit (); 00211 void normalize (); 00212 00214 void zero () 00215 { int idx; for(idx = 0; idx < 6; idx++ ) elements[idx] = 0.0; } 00216 00217 // this = this + x 00218 void add ( const ctSpatialVector6 & px ) 00219 { 00220 elements[0] += px.elements[0]; 00221 elements[1] += px.elements[1]; 00222 elements[2] += px.elements[2]; 00223 elements[3] += px.elements[3]; 00224 elements[4] += px.elements[4]; 00225 elements[5] += px.elements[5]; 00226 } 00227 00228 // this = x + y 00229 void add2 (const ctSpatialVector6 & px, const ctSpatialVector6 & py) 00230 { 00231 elements[0] = px.elements[0] + py.elements[0]; 00232 elements[1] = px.elements[1] + py.elements[1]; 00233 elements[2] = px.elements[2] + py.elements[2]; 00234 elements[3] = px.elements[3] + py.elements[3]; 00235 elements[4] = px.elements[4] + py.elements[4]; 00236 elements[5] = px.elements[5] + py.elements[5]; 00237 } 00238 00239 // dest = x + y 00240 void add3 ( ctSpatialVector6 & pdest, 00241 const ctSpatialVector6 & px, const ctSpatialVector6 & py ) 00242 { 00243 pdest.elements[0] = px.elements[0] + py.elements[0]; 00244 pdest.elements[1] = px.elements[1] + py.elements[1]; 00245 pdest.elements[2] = px.elements[2] + py.elements[2]; 00246 pdest.elements[3] = px.elements[3] + py.elements[3]; 00247 pdest.elements[4] = px.elements[4] + py.elements[4]; 00248 pdest.elements[5] = px.elements[5] + py.elements[5]; 00249 00250 } 00251 00252 void add_scaled ( ctSpatialVector6 & padme, real pk ) 00253 { 00254 elements[0] += pk*padme.elements[0]; 00255 elements[1] += pk*padme.elements[1]; 00256 elements[2] += pk*padme.elements[2]; 00257 elements[3] += pk*padme.elements[3]; 00258 elements[4] += pk*padme.elements[4]; 00259 elements[5] += pk*padme.elements[5]; 00260 } 00261 00262 void add_scaled ( real pk, ctSpatialVector6 & padme ) 00263 { 00264 elements[0] += pk*padme.elements[0]; 00265 elements[1] += pk*padme.elements[1]; 00266 elements[2] += pk*padme.elements[2]; 00267 elements[3] += pk*padme.elements[3]; 00268 elements[4] += pk*padme.elements[4]; 00269 elements[5] += pk*padme.elements[5]; 00270 } 00271 00272 void operator += (const ctSpatialVector6 & p) 00273 { int idx; for(idx = 0; idx < 6; idx++ ) elements[idx] += p.elements[idx]; } 00274 00275 ctSpatialVector6 operator + ( const ctSpatialVector6 & p) const 00276 { 00277 ctSpatialVector6 sum; 00278 int idx; 00279 for (idx = 0; idx < 6; idx++) 00280 sum.elements[idx] = elements[idx] + p.elements[idx]; 00281 return sum; 00282 } 00283 00285 void subtract ( const ctSpatialVector6 & px ) 00286 { int idx; for(idx = 0; idx < 6; idx++ ) elements[idx] -= px.elements[idx]; } 00287 00289 void subtract2 ( const ctSpatialVector6 & px, const ctSpatialVector6 & py ) 00290 { 00291 int idx; 00292 for(idx = 0; idx < 6; idx++) 00293 elements[idx] = px.elements[idx] - py.elements[idx]; 00294 } 00295 00297 void subtract3 (ctSpatialVector6 & pdest, 00298 const ctSpatialVector6 & px, const ctSpatialVector6 & py) 00299 { 00300 int idx; 00301 for (idx = 0; idx < 6; idx++) 00302 pdest.elements[idx] = px.elements[idx] - py.elements[idx]; 00303 } 00304 00305 void operator -= (const ctSpatialVector6 & p) 00306 { 00307 int idx; 00308 for (idx = 0; idx < 6; idx++ ) 00309 elements[idx] -= p.elements[idx]; 00310 } 00311 00312 ctSpatialVector6 operator - (const ctSpatialVector6 & p) 00313 { 00314 ctSpatialVector6 sum; 00315 int idx; 00316 for (idx = 0; idx < 6; idx++) 00317 sum.elements[idx] = elements[idx] - p.elements[idx]; 00318 return sum; 00319 } 00320 00321 ctSpatialVector6 operator - (const ctSpatialVector6 & p) const 00322 { 00323 ctSpatialVector6 sum; 00324 int idx; 00325 for (idx = 0; idx < 6; idx++) 00326 sum.elements[idx] = elements[idx] - p.elements[idx]; 00327 return sum; 00328 } 00329 00330 real operator * ( const ctSpatialVector6 & p ) 00331 { 00332 real dotp = 0.0; 00333 int idx; 00334 for (idx = 0; idx < 6; idx++ ) dotp += elements[idx] * p.elements[idx]; 00335 return dotp; 00336 } 00337 00338 real operator * ( const ctSpatialVector6 & p ) const 00339 { 00340 real dotp = 0.0; 00341 int idx; 00342 for (idx = 0; idx < 6; idx++ ) dotp += elements[idx] * p.elements[idx]; 00343 return dotp; 00344 } 00345 00346 ctSpatialVector6 operator * ( const real pk ) 00347 { 00348 ctSpatialVector6 scaled; 00349 int idx; 00350 for (idx = 0; idx < 6; idx++) 00351 scaled.elements[idx] = elements[idx] * pk; 00352 return scaled; 00353 } 00354 00355 ctSpatialVector6 operator * ( const real pk ) const 00356 { 00357 ctSpatialVector6 scaled; 00358 int idx; 00359 for (idx = 0; idx < 6; idx++) 00360 scaled.elements[idx] = elements[idx] * pk; 00361 return scaled; 00362 } 00363 00364 ctSpatialVector6 operator / ( const real pk ) 00365 { 00366 ctSpatialVector6 scaled; 00367 int idx; 00368 for(idx = 0; idx < 6; idx++) 00369 scaled.elements[idx] = elements[idx] / pk; 00370 return scaled; 00371 } 00372 00373 void operator *= (const real p) 00374 { int idx; for (idx=0; idx<6; ++idx) elements[idx] *= p;} 00375 00376 void operator /= (const real p) 00377 { int idx; for (idx=0; idx<6; ++idx) elements[idx] /= p;} 00378 00379 ctSpatialMatrix6 operator * ( const ctVectorTranspose6 &pvt ); 00380 00381 int get_dimension () 00382 { return 6; } 00383 00384 real *get_elements () 00385 { return elements; } 00386 00387 protected: 00388 real elements[ 6 ]; 00389 00390 }; 00391 00392 inline real ctSpatialVector6::length () 00393 { 00394 return qsqrt ( elements[0] * elements[0] 00395 + elements[1] * elements[1] 00396 + elements[2] * elements[2] 00397 + elements[3] * elements[3] 00398 + elements[4] * elements[4] 00399 + elements[5] * elements[5] ); 00400 } 00401 00402 inline ctSpatialVector6 ctSpatialVector6::unit () 00403 { 00404 return ((*this)/this->length() ); 00405 } 00406 00407 inline void ctSpatialVector6::normalize() 00408 { 00409 real len; 00410 len = this->length (); 00411 if ( len > MIN_REAL ) 00412 *this /= len; 00413 } 00414 00415 inline real ctVectorTranspose6::operator* ( const ctSpatialVector6 &pv ) 00416 { 00417 real dotp = 0.0; 00418 int idx; 00419 for(idx = 0; idx < 6; idx++) dotp += elements[idx] * pv[idx]; 00420 return dotp; 00421 } 00422 00423 #endif Generated for Crystal Space by doxygen 1.2.5 written by Dimitri van Heesch, ©1997-2000 |