00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
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
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
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
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