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