Main Page   Class Hierarchy   Compound List   File List   Compound Members  

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

Generated for Crystal Space by doxygen 1.2.5 written by Dimitri van Heesch, ©1997-2000