00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00030 #ifndef __GEODESIC853_HPP
00031 #define __GEODESIC853_HPP "Created 27.03.2004 13:48:45 by werner benger"
00032
00033 #include "Geodesic.hpp"
00034 #include <ode/Integrate853.hpp>
00035
00036 namespace Traum
00037 {
00038 using namespace VecAl;
00039
00040 template <class Acceleration>
00041
00042 class Geodesic853 : public IntegratePath853<float>
00043 {
00044 int components;
00045 public:
00046 const Acceleration&myAcceleration;
00047
00048 typedef typename Acceleration::Point_t Point_t;
00049 typedef typename Acceleration::Vector_t Vector_t;
00050
00051 enum { Dims = Acceleration::Dims };
00052
00053 Geodesic853(const Acceleration&A)
00054 : myAcceleration(A)
00055 , components(0)
00056 {}
00057
00058 void restart(int N, long double s, const Point_t*x0, const Vector_t*v0)
00059 {
00060 components = N*Dims;
00061 init(components, s, x0->const_ptr(), v0->const_ptr() );
00062 }
00063
00064 void restart1(long double s, const Point_t&x0, const Vector_t&v0)
00065 {
00066 components = Dims;
00067 init(components, s, x0.const_ptr(), v0.const_ptr() );
00068 }
00069
00070 void Accel(real, const real *x, const real *v, real *d2x_ds2)
00071 {
00072 for(int c=0; c<components/Dims; c++)
00073 {
00074 Point_t P;
00075 Vector_t V;
00076
00077 for(int i=0; i<Dims; i++)
00078 {
00079 P[i] = x[c*Dims + i];
00080 V[i] = v[c*Dims + i];
00081 }
00082
00083 const Vector_t&a = myAcceleration(P, V);
00084
00085 for(int j=0; j<Dims; j++)
00086 d2x_ds2[j+c*Dims] = -a[j];
00087 }
00088 }
00089
00090 Point_t position(int c=0) const
00091 {
00092 Point_t retval;
00093 for(int i=0; i<Dims; i++)
00094 retval[i] = q(i+c*Dims);
00095 return retval;
00096 }
00097
00098 Vector_t velocity(int c=0) const
00099 {
00100 Vector_t retval;
00101 for(int i=0; i<Dims; i++)
00102 retval[i] = dq(i+c*Dims);
00103 return retval;
00104 }
00105
00106 Point_t position(real s, int c=0) const
00107 {
00108 Point_t retval;
00109 for(int i=0; i<Dims; i++)
00110 retval[i] = q(i+c*Dims, s);
00111 return retval;
00112 }
00113
00114 Vector_t velocity(real s, int c=0) const
00115 {
00116 Vector_t retval;
00117 for(int i=0; i<Dims; i++)
00118 retval[i] = dq(i+c*Dims, s);
00119 return retval;
00120 }
00121 };
00122
00123 }
00124
00125 #endif // __GEODESIC853_HPP