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 "time2distance.h"
  24 #include <complex>
  25
  26 double time2distance(const double *r0, const double *v0, double rtarget)
  27 {
  28         double temp = 0.0;
  29         double E[6];
  30         double r0norm = norm2(r0);
  31         double a, e, E0, p, ni, Et;
  32         int i;
  33
  34         if (r0norm < rtarget)
  35         {
  36                 for (i=0; i<3; i++)
  37                         temp += r0[i]*v0[i];
  38
  39                 IC2par(r0,v0,1,E);
  40                 a = E[0];
  41                 e = E[1];
  42                 E0 = E[5];
  43                 p = a * (1-e*e);
  44                 // If the solution is an ellipse
  45                 if (e<1)
  46                 {
  47                         double ra = a * (1+e);
  48                         if (rtarget>ra)
  49                                 return -1; // NaN;
  50             else // we find the anomaly where the target distance is reached
  51                         {
  52                                 ni = acos((p/rtarget-1)/e);         //in 0-pi
  53                                 Et = 2*atan(sqrt((1-e)/(1+e))*tan(ni/2)); // algebraic kepler's equation
  54
  55                                 if (temp>0)
  56                                         return sqrt(pow(a,3))*(Et-e*sin(Et)-E0 + e*sin(E0));
  57                                 else
  58                                 {
  59                                         E0 = -E0;
  60                                         return sqrt(pow(a,3))*(Et-e*sin(Et)+E0 - e*sin(E0));
  61                                 }
  62                         }
  63                 }
  64                 else // the solution is a hyperbolae
  65                 {
  66                         ni = acos((p/rtarget-1)/e);         // in 0-pi
  67                         Et = 2*atan(sqrt((e-1)/(e+1))*tan(ni/2)); // algebraic equivalent of kepler's equation in terms of the Gudermannian
  68
  69                         if (temp>0) // out==1
  70                                 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)));
  71                         else
  72                         {
  73                                 E0 = -E0;
  74                                 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)));
  75                         }
  76                 }
  77         }
  78         else
  79                         return 12;
  80 }