AstroToolbox/time2distance.cpp

   1 // ------------------------------------------------------------------------ //
   2 // This source file is part of the 'ESA Advanced Concepts Team's                        //
   3 // Space Mechanics Toolbox' software.                                       //
   4 //                                                                          //
   5 // The source files are for research use only,                              //
   6 // and are distributed WITHOUT ANY WARRANTY. Use them on your own risk.     //
   7 //                                                                          //
   8 // Copyright (c) 2004-2007 European Space Agency                            //
   9 // ------------------------------------------------------------------------ //
  10
  11 /*
  12 %Inputs:
  13 %           r0:    column vector for the position (mu=1)
  14 %           v0:    column vector for the velocity (mu=1)
  15 %           rtarget: distance to be reached
  16 %
  17 %Outputs:
  18 %           t:     time taken to reach a given distance
  19 %
  20 %Comments:  everything works in non dimensional units
  21 */
  22
  23 #include <cmath>
  24
  25 #include "Astro_Functions.h"
  26 #include "propagateKEP.h"
  27 #include "time2distance.h"
  28
  29 using namespace std;
  30
  31 double time2distance(const double *r0, const double *v0, const double &rtarget)
  32 {
  33         double temp = 0.0;
  34         double E[6];
  35         double r0norm = norm2(r0);
  36         double a, e, E0, p, ni, Et;
  37         int i;
  38
  39         if (r0norm < rtarget)
  40         {
  41                 for (i=0; i<3; i++)
  42                         temp += r0[i]*v0[i];
  43
  44                 IC2par(r0,v0,1,E);
  45                 a = E[0];
  46                 e = E[1];
  47                 E0 = E[5];
  48                 p = a * (1-e*e);
  49                 // If the solution is an ellipse
  50                 if (e<1)
  51                 {
  52                         double ra = a * (1+e);
  53                         if (rtarget>ra)
  54                                 return -1; // NaN;
  55             else // we find the anomaly where the target distance is reached
  56                         {
  57                                 ni = acos((p/rtarget-1)/e);         //in 0-pi
  58                                 Et = 2*atan(sqrt((1-e)/(1+e))*tan(ni/2)); // algebraic kepler's equation
  59
  60                                 if (temp>0)
  61                                         return sqrt(pow(a,3))*(Et-e*sin(Et)-E0 + e*sin(E0));
  62                                 else
  63                                 {
  64                                         E0 = -E0;
  65                                         return sqrt(pow(a,3))*(Et-e*sin(Et)+E0 - e*sin(E0));
  66                                 }
  67                         }
  68                 }
  69                 else // the solution is a hyperbolae
  70                 {
  71                         ni = acos((p/rtarget-1)/e);         // in 0-pi
  72                         Et = 2*atan(sqrt((e-1)/(e+1))*tan(ni/2)); // algebraic equivalent of kepler's equation in terms of the Gudermannian
  73
  74                         if (temp>0) // out==1
  75                                 return sqrt(pow((-a),3))*(e*tan(Et)-log(tan(Et/2+M_PI/4))-e*tan(E0)+log(tan(E0/2+M_PI/4)));
  76                         else
  77                         {
  78                                 E0 = -E0;
  79                                 return sqrt(pow((-a),3))*(e*tan(Et)-log(tan(Et/2+M_PI/4))+e*tan(E0)-log(tan(E0/2+M_PI/4)));
  80                         }
  81                 }
  82         }
  83         else
  84                         return 12;
  85 }