Geodesics.hpp

#ifndef __GEODESCICS_HPP_
#define __GEODESCICS_HPP_

#include <fish/fiber/vector/CubicIpol.hpp>
#include <fish/fiber/vector/LinearIpol.hpp>
#include <eagle/QuadraticMatrix.hpp> 
#include <eagle/Coordinates.hpp>
#include <fish/fiber/baseop/LocalFromWorldPoint.hpp>
#include <ode/dop853.hpp>

//#define VERBOSE

namespace Fiber
{

template <>
struct  LinearIpolZeroDerivativeTrait<metric33>
{
static  metric33 zero()
        {
        metric33 m;
                m.set(0.0);
                return m;
        }
};

template <>
struct  LinearIpolZeroDerivativeTrait<metric44>
{
static  metric44 zero()
        {
        metric44 m;
                m.set(0.0);
                return m;
        }
};

//template <>
// struct  CubicIpolZeroDerivativeTrait<metric33>
// {
// static       metric33 zero()
//         {
//         metric33 m;
//              m.set(0.0);
//                 return m;
//         }
// };

// template <>
// struct  CubicIpolZeroDerivativeTrait<metric44>
// {
// static       metric44 zero()
//         {
//         metric44 m;
//              m.set(0.0);
//                 return m;
//         }
// };

}


namespace FieldLines
{

struct Geodesic{};

struct GeodesicSpatial{};

template<class T>
double getDet(const Eagle::Quadratic<T::Dims,double>&q)
{
        return  Eagle::Determinantor<T::Dims>::compute(q);
}

template<class T>
class ChristoffelXYZ : public MemCore::ReferenceBase< ChristoffelXYZ<T> >
{
        typedef typename T::Chart_t Chart_t;
        typedef typename Eagle::Coordinates<Chart_t>::point point;
        // store metric tensor
        T g;
        
        Eagle::Quadratic<T::Dims, double> g_inv;
 
        // store precomputed finite differences of g // array size 3
        T g_diff[T::Dims];


        bool ok;
        // consts for access
//      enum { MU = 0, NU, LAMBDA  };
//      enum { X = 0, Y, Z};

public:
        ChristoffelXYZ( const T&gP, const T&g_xP, const T&g_yP, const T&g_zP,
                        const double dxP, const double dyP, const double dzP  )
        : MemCore::ReferenceBase< ChristoffelXYZ >(this)
        , g(gP)
        , ok(true)
        {
                std::cout << "aber jetzt nit im ersnst!!!";
                if(T::Dims == 3)
                {
                        enum { X = 0, Y, Z};

                if(dxP != 0.0)
                        for(unsigned i = 0; i < T::SIZE; i++)
                                g_diff[X][i] = ( g_xP[i] - g[i] ) / dxP; 

                if(dyP != 0.0)
                        for(unsigned i = 0; i < T::SIZE; i++)
                                g_diff[Y][i] = ( g_yP[i] - g[i] ) / dyP; 

                if(dzP != 0.0)
                        for(unsigned i = 0; i < T::SIZE; i++)
                                g_diff[Z][i] = ( g_zP[i] - g[i] ) / dzP;

                } 

                if(T::Dims == 4)
                {
                        enum {  t = Eagle::STA::CartesianChart4D<>::T, 
                                X = Eagle::STA::CartesianChart4D<>::X,
                                Y = Eagle::STA::CartesianChart4D<>::Y,
                                Z = Eagle::STA::CartesianChart4D<>::Z }; 

                        if(dxP != 0.0)
                                for(unsigned i = 0; i < T::SIZE; i++)
                                        g_diff[X][i] = ( g_xP[i] - g[i] ) / dxP; 

                        if(dyP != 0.0)
                                for(unsigned i = 0; i < T::SIZE; i++)
                                        g_diff[Y][i] = ( g_yP[i] - g[i] ) / dyP; 

                        if(dzP != 0.0)
                                for(unsigned i = 0; i < T::SIZE; i++)
                                        g_diff[Z][i] = ( g_zP[i] - g[i] ) / dzP;
                        
                }
        }

        ChristoffelXYZ( const T&gP, const T&g_dxP, const T&g_dyP, const T&g_dzP )
        : MemCore::ReferenceBase< ChristoffelXYZ<T> >(this)
        , g(gP)
        , ok(true)      
        {
                //std::cout << g_dxP << endl << g_dyP << endl << g_dzP << endl;
                //std::cout << "------";
                if(T::Dims == 3)
                {
                        enum { X = 0, Y, Z};
                        g_diff[X] = g_dxP; 
                        g_diff[Y] = g_dyP; 
                        g_diff[Z] = g_dzP; 
                } 
                if(T::Dims == 4)
                {
                        enum {  t = Eagle::STA::CartesianChart4D<>::T, 
                                X = Eagle::STA::CartesianChart4D<>::X,
                                Y = Eagle::STA::CartesianChart4D<>::Y,
                                Z = Eagle::STA::CartesianChart4D<>::Z }; 

                        g_diff[t].set(0.0); 
                        g_diff[X] = g_dxP; 
                        g_diff[Y] = g_dyP; 
                        g_diff[Z] = g_dzP; 
                }
                        try
                        {
                                Eagle::Quadratic<T::Dims, double> q (g); 

                                Eagle::ComputeInverse(g_inv, q); 

                                Eagle::Matrix<T::Dims,T::Dims, double> &mg = q ; 
                                Eagle::Matrix<T::Dims,T::Dims, double> &mg_i = g_inv; 
                                Eagle::Matrix<T::Dims,T::Dims, double> I = mg * mg_i; 
                                //std::cout << "controll inversion: " << I << std::endl; 
                        } 
                        catch(const Eagle::DegeneratedMatrix&)
                        {
                                puts("ChristoffelXYZ<>::ChristoffelXYZ ERROR Degenerated Matrix in inversion."); fflush(stdout); 
                                ok = false;
                        } 
        }

        bool isOk(){ return ok;}

        void  getMetricfromQuadratic(const Eagle::Quadratic<T::Dims,double>&q, T&g)
                {
                        for(unsigned i = 0; T::SIZE; i++)
                                g[i] = q[i];
                }



        // christoffel symbols function for on demand symbol computing, later there may be the need to 
        // store them and reuse them when possible.
        const double operator()(const unsigned mu, const unsigned nu, const unsigned lambda) const 
        {
        double gamma = 0.0; 

                if(mu > T::Dims-1 || nu > T::Dims-1 || lambda > T::Dims-1)
                        return 0.0; 

                for(unsigned i = 0; i < T::Dims; i++)
                        gamma += g_inv(lambda, i) * ( g_diff[nu](mu,i) + g_diff[mu](nu,i) - g_diff[i](mu, nu) ); 

                gamma /= 2;

                return gamma;
        }
        
        const double gammaDebug(const unsigned mu, const unsigned nu, const unsigned lambda) const 
        {
        double gamma = 0.0; 

                if(mu > T::Dims-1 || nu > T::Dims-1 || lambda > T::Dims-1)
                        return 0.0; 
                        printf( "mu nu lam %d %d %d", mu, nu, lambda);
                for(unsigned i = 0; i < T::Dims; i++)
                {
                        
                double tmp = g_inv(lambda, i) * ( g_diff[nu](mu,i) + g_diff[mu](nu,i) - g_diff[i](mu, nu) );
                    gamma += tmp; 
                printf( "g_inv: %f, g1: %f, g2:% f, g3: %f, term: %f\n", g_inv(lambda, i), g_diff[nu](mu,i), g_diff[mu](nu,i), g_diff[i](mu, nu), tmp);
                }
                gamma /= 2; 
                printf( "gamma: %f\n", gamma);
                return gamma;
        }


        const T& getG() { return g; }
        
        void speak() const
       {
               
               std::cout << "Christoffel content:\n" << "g " << endl << g << "g_inv: " << endl << g_inv; 

               if(T::Dims == 3)
                       std::cout << "g,x" << std::endl <<g_diff[0] 
                                 << "g,y" << std::endl <<g_diff[1] 
                                 << "g,z" << std::endl <<g_diff[2]; 
                               
               if(T::Dims == 4)
                       std::cout << "g,t" << std::endl <<g_diff[0] 
                                 << "g,x" << std::endl <<g_diff[1] 
                                 << "g,y" << std::endl <<g_diff[2] 
                                 << "g,z" << std::endl <<g_diff[3]; 

               cout << "Gamma (T)|X|Y|Z|:" << endl;

               for( unsigned lam = 0; lam <  T::Dims; lam++)
                {
                        for( unsigned mu = 0; mu <  T::Dims; mu++)
                        {
                                for( unsigned nu = 0; nu <  T::Dims; nu++)
                                {
                                        printf("%f ", (*this)(mu,nu,lam));
                                } 
                                printf("\n");
                        } 
                        printf("-----------------------------\n");
                }


                // for( unsigned lam = 0; lam <  T::Dims; lam++)
                // {
                //      for( unsigned mu = 0; mu <  T::Dims; mu++)
                //      {
                //              for( unsigned nu = 0; nu <  T::Dims; nu++)
                //              {
                //                      gammaDebug(mu, nu, lam);        
                //              } 
                //              printf("\n");
                //      } 
                //      printf("-----------------------------\n");
                // }
       }
        
};


template<class T>
typename Eagle::Coordinates<typename T::Chart_t >::vector getQddot( RefPtr< ChristoffelXYZ<T> >& Gamma, 
                                                                    typename Eagle::Coordinates<typename T::Chart_t >::vector&q_dot )
{
typename Eagle::Coordinates<typename T::Chart_t >::vector q_ddot; 

        q_ddot.set(0.0);

        // q-ddot^lamba = - Gamma^lambda_mu_nu * q-dot^mu * q-dot^nu 

        //cout << "getQddot  qdot:" << q_dot << endl;

        for(index_t lambda = 0; lambda < T::Dims; lambda++)
                for( index_t mu = 0; mu < T::Dims; mu++)
                        for(index_t nu = 0; nu < T::Dims; nu++)
                                q_ddot[lambda] -= (*Gamma)( mu, nu, lambda ) * q_dot[mu] * q_dot[nu]; 

        //cout << "getQddot  qddot:" << q_ddot << endl;

        return q_ddot;
}


template<class TT>
typename Eagle::Coordinates<typename TT::Chart_t >::vector getQdot( const typename Eagle::Coordinates<typename TT::Chart_t >::vector& q_dot, 
                                                                    const TT&g, const bool big = true)
{
        if (TT::Dims==3)
                return q_dot; 

        enum {  T = STA::CartesianChart4D<>::T, 
                X = STA::CartesianChart4D<>::X,
                Y = STA::CartesianChart4D<>::Y,
                Z = STA::CartesianChart4D<>::Z }; 

const double eps = 1.0e-4; 

        //std::cout << "qdot before:" << q_dot << std::endl;

typename Eagle::Coordinates<typename TT::Chart_t >::vector v = q_dot; 

        if( !isnormal(v[X]) ) v[X] = 0.0; 
        if( !isnormal(v[Y]) ) v[Y] = 0.0; 
        if( !isnormal(v[Z]) ) v[Z] = 0.0; 
                

        if(v[X] < eps && v[X] > -eps &&
           v[Y] < eps && v[Y] > -eps &&
           v[Z] < eps && v[Z] > -eps)
        {
                //puts("yep");
                v[T] = 1.0; 
                return v;
        } 

        //puts("hm");

double A = 2 * ( g(X,T)*v[X]    + g(Y,T)*v[Y]      + g(Z,T)*v[Z] ); 
double B = g(X,X)*v[X]*v[X]     + g(Y,Y)*v[Y]*v[Y] + g(Z,Z)*v[Z]*v[Z]+
           2*( g(X,Y)*v[X]*v[Y] + g(X,Z)*v[X]*v[Z] + g(Y,Z)*v[Y]*v[Z] ); 

        if(g(T,T) == 0.0)
        {
                puts("getQdot::Error g(T,T) == 0! Division by 0."); 
                return v;
        }

double p = A/g(T,T); 
double q= B/g(T,T); 

double w = p*p/4 - q; 

        if(w < 0.0)
        {
                puts("getQdot::Error direction initialization srqt < 0!!!!"); 
                return v;
        }

        double sw = sqrt( w ); 

        double v1 = -p/2 + sw; 
        double v2 = -p/2 - sw; 

                if(big)
                        v[T] = (v1 > v2)? v1 : v2; // use bigger value of the solution of the quadratic equation 
                else
                        v[T] = (v1 > v2)? v2 : v1; 

        return v;
} 


template<class T>
bool getChristoffelXYZ( const typename Eagle::Coordinates<typename T::Chart_t >::point& CurrentPoint, 
                        const MemCore::RefPtr<LocalFromWorldPoint>&LocalPointFinder, 
                        const FieldSelector&FieldSelection, const double step_size, 
                        RefPtr< ChristoffelXYZ<T> >& Christoffels,
                        T& CurrentMetric,
                        pair<Eagle::PhysicalSpace::point, string>& localPoint ) 
        {
        typedef typename T::Chart_t Chart_t;
        typedef typename Coordinates<Chart_t>::point point; 

                Eagle::PhysicalSpace::point current (CurrentPoint[0+T::Dims-3], CurrentPoint[1+T::Dims-3], CurrentPoint[2+T::Dims-3]); 

        bool test = LocalPointFinder->get( current, localPoint ); 

                if(!test)
                {
                        puts("getChristoffelXYZ()1 error finding local point at:"); 
                        cout << current << endl;
                        return false;
                } 
                
        const string&FragName = localPoint.second; 
        const Eagle::PhysicalSpace::point &float_index = localPoint.first; 



        RefPtr<Field> Field; 
                Field = FieldSelection.getField(); 

#ifdef VERBOSE
                cout << "FieldLines::getChristoffelXYZ local point: " << float_index; 
                cout << "in Fragment:" << FragName << endl;
                cout << "getting Field: " << FieldSelection.getFieldName() << endl;
#endif          
                if(!Field) 
                { 
                        puts("AtomicIntegrator<metric33, Geodesic>doit2(); ERROR: No Field0 found in Time iteration"); 
                        return false; 
                } 


         RefPtr<FragmentID> FragID;

                if(FragName.compare("") != 0 )
                        FragID   = new FragmentID( FragName ); 

                RefPtr<CreativeArrayBase> cBase = Field->getCreator( FragID ); 
                assert(cBase); 

                RefPtr<MemBase> mBase = cBase->create();                
                assert(mBase); 



                RefPtr<MemArray<3, T> > ToIntArr0 = mBase; 
                ToIntArr0   = Field->getCreator( FragID )->create(); 

                if(!ToIntArr0)
                {
                        cout << "getChristoffelXYZ<T> dims:" << T::Dims << endl; 
                        mBase.speak("getChristoffelXYZ<T> ------>");

                        puts("getChristoffelXYZ()<T>:ERROR retrieving matching field. Maybe Dims of instantiated metric and Dims of field data does not match. 33 44?"); 
                        return false;   
                } 
                
                assert( ToIntArr0 );



                        // NOTE: Here optionally use another interpolator, eventually cubic, or 
                        //       self-aligning for eigenvector fields.
                        // 

//              cout << "before interpol" << endl; 

        T g_x, g_y, g_z; 
        T g_xl, g_xp, g_xxp, g_yp, g_zp, g_yzp, g_yyzp; 

        Interpolate<3, T, CubicIpol<T> > myField0  ( *ToIntArr0,   float_index); 
        Interpolate<3, T, CubicIpol<T>, double, NoDelimiter<T>, 0  > myField0_x( *ToIntArr0, float_index); 
        Interpolate<3, T, CubicIpol<T>, double, NoDelimiter<T>, 1  > myField0_y( *ToIntArr0, float_index); 
        Interpolate<3, T, CubicIpol<T>, double, NoDelimiter<T>, 2  > myField0_z( *ToIntArr0, float_index); 
        // Interpolate<3, T, LinearIpol<T> > myField0  ( *ToIntArr0,   float_index); 
//      Interpolate<3, T, LinearIpol<T>, double, NoDelimiter<T>, 0  > myField0_xl( *ToIntArr0, float_index); 
        // Interpolate<3, T, LinearIpol<T>, double, NoDelimiter<T>, 1  > myField0_y( *ToIntArr0, float_index); 
        // Interpolate<3, T, LinearIpol<T>, double, NoDelimiter<T>, 2  > myField0_z( *ToIntArr0, float_index); 
        
                  CurrentMetric = myField0.eval(); 
                  g_x = myField0_x.eval(); 
                  g_y = myField0_y.eval(); 
                  g_z = myField0_z.eval(); 

//                g_xl= myField0_xl.eval();

          // double eps = -1; 

          // Eagle::point3 xp =  float_index + Eagle::tvector3(eps,0,0 ); 
          // Eagle::point3 xxp =  float_index + Eagle::tvector3(-eps,0,0 ); 

          // Eagle::point3 yp =  float_index + Eagle::tvector3(0,eps,0 ); 
          // Eagle::point3 zp =  float_index + Eagle::tvector3(0,0,eps ); 
          // Eagle::point3 yzp = float_index + Eagle::tvector3(0,0,eps ) + Eagle::tvector3(0,eps,0 ); 
          // Eagle::point3 yyzp = float_index + Eagle::tvector3(0,0,eps ) - Eagle::tvector3(0,eps,0 ); 

          //Interpolate<3, T, LinearIpol<T> >  myField0_xp( *ToIntArr0, xp ); 
          //Interpolate<3, T, LinearIpol<T> >  myField0_xxp( *ToIntArr0, xxp ); 
          //Interpolate<3, T, LinearIpol<T> >  myField0_yp( *ToIntArr0, yp ); 
          //Interpolate<3, T, LinearIpol<T> >  myField0_zp( *ToIntArr0, zp ); 
          // Interpolate<3, T, LinearIpol<T> >  myField0_yzp( *ToIntArr0, yzp ); 
          // Interpolate<3, T, LinearIpol<T> >  myField0_yyzp( *ToIntArr0, yyzp ); 

          //         g_xp  = myField0_xp.eval(); 
          //        g_xxp  = myField0_xxp.eval(); 

          //      g_yp  = myField0_yp.eval(); 
          //      g_zp  = myField0_zp.eval(); 
                  // g_yzp = myField0_yzp.eval(); 
                  // g_yyzp = myField0_yyzp.eval(); 

                  RefPtr<Fiber::Field> field = FieldSelection.getCartesianPositions(); 
                  RefPtr<Fiber::DirectProductMemArray<Eagle::point3,3> > FData = field->getData(); 
                  Eagle::point3 delta = FData->Delta(); 
                  

        Eagle::Quadratic<T::Dims, double> q (CurrentMetric); 
        double det = getDet<T>(q); 

                

                if(det < 0.0 && T::Dims == 3) // check if det gets negative 
                {
                        cout << "g:" << q << endl;
                        cout << "det: " << det << endl;
                        puts("getChristoffelXYZ()<T>:warning, Det of G < 0."); 
                        return false;
                } 

                g_x  /= delta[0]; // normalize index derivative by interval length  
                g_xl /= delta[0]; 
                g_y  /= delta[1]; 
                g_z  /= delta[2]; 

#ifdef VERBOSE 
/*
        cout << "det(g): " << det << endl; 
        cout << "delta: " << delta << endl;
        cout << "aus dem interpolator:" << endl << CurrentMetric << g_x << g_y << g_z; 

                cout << endl <<"g:" << endl 
                     << float_index << endl << CurrentMetric << "variiert xyz:" << endl 
                     << xp  << endl << g_xp  
                     << xxp  << endl << g_xxp  
                        
                     << yp  << endl << g_yp
                     << zp  << endl << g_zp 

                     << "g,x lin" << endl << g_xl;
                     // << yzp << endl << g_yzp  
                     // << yyzp << endl << g_yyzp  ; 
*/
#endif          
                  Christoffels = new ChristoffelXYZ<T>( CurrentMetric, g_x, g_y, g_z ); 

#ifdef VERBOSE
                  std::cout << "Hilfe Christoffel wo bist du?" << std::endl; 
                  cout << "current point: " << CurrentPoint;
                  Christoffels->speak();
#endif  
                  if( Christoffels->isOk() == false)
                  {
                          puts("getChristoffelXYZ()<T>:warning, christoffels probably degenerated matrix."); 
                          return false;
                  }

                  return true;
        }

template<class T>
struct  GeodesicDifferentialEquation : public Traum::VirtualSecondOrderDiffEquation <double>
{
typedef typename T::Chart_t Chart_t;
typedef typename Eagle::Coordinates<Chart_t>::point point;
typedef typename Eagle::Coordinates<Chart_t>::vector tvector;

RefPtr<LocalFromWorldPoint> LocalPointFinder;   
FieldSelector FieldSelection;

double step_size; // depricated, define stepsize for finite differential derivation of the metric tensors used for ChristoffelsXYZ 
        
double scale;

bool valid_point;

pair<Eagle::PhysicalSpace::point, std::string>localPoint;

T CurrentMetric;
tvector DataDir;
RefPtr<ChristoffelXYZ<T> > Gamma;

        enum { N = T::Dims * 2 };
        typedef double real;
        
        GeodesicDifferentialEquation()
        {}
        
        virtual ~GeodesicDifferentialEquation() {}
        
        struct RealArray_t : FixedArray<real, N>               
        {
                void init(int) {}
        };
        
        struct DOPVarsArray_t : FixedArray<Traum::dop_vars<real>, N >
        {
                void init(int) {}
        };
        
        void setData(const tvector&DataDirP, const RefPtr<LocalFromWorldPoint>&LocalPointFinderP, const FieldSelector&FieldSelectionP,
                     const double step_sizeP, const double scaleP)
        {
                DataDir = DataDirP; 
                LocalPointFinder = LocalPointFinderP; 
                FieldSelection = FieldSelectionP; 
                step_size = step_sizeP; 
                scale = scaleP;
        }

        void Accel(real s, const real *x, const real *v, real *d2x_ds2)
        {
                point Q; 
                tvector q_dot; 

                        for(int k=0; k < point::Dims; k++)
                        {
                                Q[k] = x[k]; 
                                q_dot[k] = v[k]; 
                                DataDir[k] = v[k];
                        } 
//              printf("Q: %f, %f, %f\n", Q[0], Q[1], Q[2] ); 
//              printf("V: %f, %f, %f\n", v[0], v[1], v[2] ); 

        bool test = FieldLines::getChristoffelXYZ<T>( Q, LocalPointFinder, FieldSelection, step_size, 
                                                     Gamma,
                                                     CurrentMetric,
                                                     localPoint ); 
                if(!test)
                {
                        puts("GeodesicDifferentialEquation::DifferentialEquation::Accel() No valid point found!");fflush(stdout); 
                        return;
                } 

        tvector qdd = getQddot<T>( Gamma, q_dot); 

                for(int k=0; k < point::Dims; k++)
                        d2x_ds2[k] = qdd[k];
        }

};

// double linearInterpolate(const double t0, const double d0, const double t1, const double d1, const double t)
// {
//      return d0 + (d1 - d0) / (t1 - t0) * (t - t0);
// }

// metric33 linearInterpolate(const double t0, const metric33&m0, const double t1, const metric33&m1, const double t )
// {
// metric33 tmp; 
//      for(unsigned i = 0; i < metric33::SIZE; i++)
//              tmp[i] = linearInterpolate(t0, m0[i], t1, m1[i], t); 

//      return tmp;
// }


}
#endif // __GEODESCICS_HPP_