ctvspat.h Source File

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 #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