AstroToolbox/Astro_Functions.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 #include <cmath>
  13
  14 #include "Astro_Functions.h"
  15 #include "ZeroFinder.h"
  16
  17 using namespace std;
  18
  19 class CZF : public ZeroFinder::Function1D
  20 {
  21 public:
  22     CZF(const double &a, const double &b):Function1D(a,b) {}
  23         double operator()(const double &x) const
  24         {
  25                 return (p1*tan(x) - log(tan(0.5*x + M_PI_4)) - p2);
  26         }
  27 };
  28
  29 double Mean2Eccentric (const double &M, const double &e)
  30 {
  31
  32         static const double tolerance = 1e-13;
  33         int n_of_it = 0; // Number of iterations
  34         double Eccentric,Ecc_New;
  35         double err = 1.0;
  36
  37
  38
  39         if (e < 1.0) {
  40                 Eccentric = M + e* cos(M); // Initial guess
  41                 while ( (err > tolerance) && (n_of_it < 100))
  42                 {
  43                         Ecc_New = Eccentric - (Eccentric - e* sin(Eccentric) -M )/(1.0 - e * cos(Eccentric));
  44                         err = fabs(Eccentric - Ecc_New);
  45                         Eccentric = Ecc_New;
  46                         n_of_it++;
  47                 }
  48         } else {
  49                 CZF FF(e,M);  // function to find its zero point
  50                 ZeroFinder::FZero fz(-M_PI_2 + 1e-8, M_PI_2 - 1e-8);
  51                 Ecc_New = fz.FindZero(FF);
  52                 Eccentric = Ecc_New;
  53         }
  54         return Eccentric;
  55 }
  56
  57
  58
  59 void Conversion (const double *E,
  60                                  double *pos,
  61                                  double *vel,
  62                                  const double &mu)
  63 {
  64         double a,e,i,omg,omp,theta;
  65         double b,n;
  66         double X_per[3],X_dotper[3];
  67         double R[3][3];
  68
  69         a = E[0];
  70         e = E[1];
  71         i = E[2];
  72         omg = E[3];
  73         omp = E[4];
  74         theta = E[5];
  75
  76         b = a * sqrt (1 - e*e);
  77         n = sqrt (mu/pow(a,3));
  78
  79         const double sin_theta = sin(theta);
  80         const double cos_theta = cos(theta);
  81
  82         X_per[0] = a * (cos_theta - e);
  83         X_per[1] = b * sin_theta;
  84
  85         X_dotper[0] = -(a * n * sin_theta)/(1 - e * cos_theta);
  86         X_dotper[1] = (b * n * cos_theta)/(1 - e * cos_theta);
  87
  88         const double cosomg = cos(omg);
  89         const double cosomp = cos(omp);
  90         const double sinomg = sin(omg);
  91         const double sinomp = sin(omp);
  92         const double cosi = cos(i);
  93         const double sini = sin(i);
  94
  95         R[0][0] =  cosomg*cosomp-sinomg*sinomp*cosi;
  96         R[0][1] =  -cosomg*sinomp-sinomg*cosomp*cosi;
  97
  98         R[1][0] =  sinomg*cosomp+cosomg*sinomp*cosi;
  99         R[1][1] =  -sinomg*sinomp+cosomg*cosomp*cosi;
 100
 101         R[2][0] =  sinomp*sini;
 102         R[2][1] =  cosomp*sini;
 103
 104         // evaluate position and velocity
 105         for (int i = 0;i < 3;i++)
 106         {
 107                 pos[i] = 0;
 108                 vel[i] = 0;
 109                 for (int j = 0;j < 2;j++)
 110                 {
 111                                 pos[i] += R[i][j] * X_per[j];
 112                                 vel[i] += R[i][j] * X_dotper[j];
 113                 }
 114         }
 115         return;
 116 }
 117
 118
 119 double norm(const double *vet1, const double *vet2)
 120 {
 121         double Vin = 0;
 122         for (int i = 0; i < 3; i++)
 123         {
 124                 Vin += (vet1[i] - vet2[i])*(vet1[i] - vet2[i]);
 125         }
 126         return sqrt(Vin);
 127 }
 128
 129
 130 double norm2(const double *vet1)
 131 {
 132     double temp = 0.0;
 133         for (int i = 0; i < 3; i++) {
 134                 temp += vet1[i] * vet1[i];
 135         }
 136         return sqrt(temp);
 137 }
 138
 139
 140 //subfunction that evaluates vector product
 141 void vett(const double *vet1, const double *vet2, double *prod )
 142 {
 143         prod[0]=(vet1[1]*vet2[2]-vet1[2]*vet2[1]);
 144         prod[1]=(vet1[2]*vet2[0]-vet1[0]*vet2[2]);
 145         prod[2]=(vet1[0]*vet2[1]-vet1[1]*vet2[0]);
 146 }
 147
 148 double x2tof(const double &x,const double &s,const double &c,const int &lw)
 149 {
 150         double am,a,alfa,beta;
 151
 152         am = s/2;
 153         a = am/(1-x*x);
 154         if (x < 1)//ellpise
 155     {
 156                 beta = 2 * asin (sqrt((s - c)/(2*a)));
 157                 if (lw) beta = -beta;
 158                 alfa = 2 * acos(x);
 159     }
 160         else
 161     {
 162                 alfa = 2 * acosh(x);
 163                 beta = 2 * asinh(sqrt ((s - c)/(-2 * a)));
 164                 if (lw) beta = -beta;
 165     }
 166
 167         if (a > 0)
 168     {
 169       return (a * sqrt (a)* ( (alfa - sin(alfa)) - (beta - sin(beta)) ));
 170     }
 171         else
 172     {
 173       return (-a * sqrt(-a)*( (sinh(alfa) - alfa) - ( sinh(beta) - beta)) );
 174     }
 175
 176 }
 177
 178
 179 // Subfunction that evaluates the time of flight as a function of x
 180 double tofabn(const double &sigma,const double &alfa,const double &beta)
 181 {
 182         if (sigma > 0)
 183     {
 184       return (sigma * sqrt (sigma)* ( (alfa - sin(alfa)) - (beta - sin(beta)) ));
 185     }
 186         else
 187     {
 188       return (-sigma * sqrt(-sigma)*( (sinh(alfa) - alfa) - ( sinh(beta) - beta)) );
 189     }
 190 }
 191
 192 // subfunction that evaluates unit vectors
 193 void vers(const double *V_in, double *Ver_out)
 194 {
 195         double v_mod = 0;
 196         int i;
 197
 198         for (i = 0;i < 3;i++)
 199     {
 200                 v_mod += V_in[i]*V_in[i];
 201         }
 202
 203         double sqrtv_mod = sqrt(v_mod);
 204
 205         for (i = 0;i < 3;i++)
 206         {
 207                 Ver_out[i] = V_in[i]/sqrtv_mod;
 208         }
 209 }
 210
 211
 212
 213