AstroToolbox/propagateKEP.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 // File: propageteKEP.cpp
  12 //
  13
  14 #include <cmath>
  15
  16 #include "Astro_Functions.h"
  17 #include "propagateKEP.h"
  18
  19 /*
  20  Origin: MATLAB code programmed by Dario Izzo (ESA/ACT)
  21
  22  C++ version by Tamas Vinko (ESA/ACT)
  23
  24  Inputs:
  25            r0:    column vector for the non dimensional position
  26            v0:    column vector for the non dimensional velocity
  27            t:     non dimensional time
  28
  29  Outputs:
  30            r:    column vector for the non dimensional position
  31            v:    column vector for the non dimensional velocity
  32
  33  Comments:  The function works in non dimensional units, it takes an
  34  initial condition and it propagates it as in a kepler motion analytically.
  35 */
  36
  37 using namespace std;
  38
  39 void propagateKEP(const double *r0_in, const double *v0_in, const double &t, const double &mu,
  40                                   double *r, double *v)
  41 {
  42
  43 /*
  44    The matrix DD will be almost always the unit matrix, except for orbits
  45    with little inclination in which cases a rotation is performed so that
  46    par2IC is always defined
  47 */
  48
  49         double DD[9] = {1, 0, 0,
  50                             0, 1, 0,
  51                                         0, 0, 1};
  52         double h[3];
  53         double ih[3] = {0,0,0};
  54         double temp1[3] = {0,0,0}, temp2[3] = {0,0,0};
  55         double E[6];
  56         double normh, M, M0;
  57         double r0[3], v0[3];
  58
  59         int i;
  60
  61         for (i=0; i<3; i++)
  62         {
  63                         r0[i] = r0_in[i];
  64                         v0[i] = v0_in[i];
  65         }
  66
  67         vett(r0, v0, h);
  68
  69         normh=norm2(h);
  70
  71         for (i=0; i<3; i++)
  72                 ih[i] = h[i]/normh;
  73
  74         if (fabs(fabs(ih[2])-1.0) < 1e-3)     // the abs is needed in cases in which the orbit is retrograde,
  75         {                                 // that would held ih=[0,0,-1]!!
  76                 DD[0] = 1; DD[1] =  0;  DD[2] = 0;
  77                 DD[3] = 0; DD[4] =  0;  DD[5] = 1;
  78                 DD[6] = 0; DD[7] = -1; DD[8] = 0;
  79
  80                 // Random rotation matrix that make the Euler angles well defined for the case
  81                 // For orbits with little inclination another ref. frame is used.
  82
  83                 for (int i=0; i<3; i++)
  84                 {
  85                         temp1[0] += DD[i]*r0[i];
  86                         temp1[1] += DD[i+3]*r0[i];
  87                         temp1[2] += DD[i+6]*r0[i];
  88                         temp2[0] += DD[i]*v0[i];
  89                         temp2[1] += DD[i+3]*v0[i];
  90                         temp2[2] += DD[i+6]*v0[i];
  91                 }
  92                 for (int i=0; i<3; i++)
  93                 {
  94                         r0[i] = temp1[i];
  95                         temp1[i] = 0.0;
  96                         v0[i] = temp2[i];
  97                         temp2[i] = 0.0;
  98                 }
  99         // for practical reason we take the transpose of the matrix DD here (will be used at the end of the function)
 100                 DD[0] = 1; DD[1] =  0;  DD[2] = 0;
 101                 DD[3] = 0; DD[4] =  0;  DD[5] = -1;
 102                 DD[6] = 0; DD[7] =  1;  DD[8] = 0;
 103         }
 104
 105         IC2par(r0, v0, mu, E);
 106         if (E[1] < 1.0)
 107         {
 108                 M0 = E[5] - E[1]*sin(E[5]);
 109                 M=M0+sqrt(mu/pow(E[0],3))*t;
 110         }
 111         else
 112         {
 113                 M0 = E[1]*tan(E[5]) - log(tan(0.5*E[5] + M_PI_4));
 114                 M=M0+sqrt(mu/pow(-E[0],3))*t;
 115         }
 116
 117         E[5]=Mean2Eccentric(M, E[1]);
 118         par2IC(E, mu, r, v);
 119
 120
 121         for (int j=0; j<3; j++)
 122         {
 123                 temp1[0] += DD[j]*r[j];
 124                 temp1[1] += DD[j+3]*r[j];
 125                 temp1[2] += DD[j+6]*r[j];
 126                 temp2[0] += DD[j]*v[j];
 127                 temp2[1] += DD[j+3]*v[j];
 128                 temp2[2] += DD[j+6]*v[j];
 129         }
 130         for (int i=0; i<3; i++)
 131         {
 132                 r[i] = temp1[i];
 133                 v[i] = temp2[i];
 134         }
 135
 136         return;
 137 }
 138
 139
 140
 141
 142 /*
 143         Origin: MATLAB code programmed by Dario Izzo (ESA/ACT)
 144
 145         C++ version by Tamas Vinko (ESA/ACT) 12/09/2006
 146
 147         Inputs:
 148            r0:    column vector for the position
 149            v0:    column vector for the velocity
 150
 151         Outputs:
 152            E:     Column Vectors containing the six keplerian parameters,
 153                   (a,e,i,OM,om,Eccentric Anomaly (or Gudermannian whenever e>1))
 154
 155         Comments:  The parameters returned are, of course, referred to the same
 156         ref. frame in which r0,v0 are given. Units have to be consistent, and
 157         output angles are in radians
 158         The algorithm used is quite common and can be found as an example in Bate,
 159         Mueller and White. It goes singular for zero inclination
 160 */
 161
 162 void IC2par(const double *r0, const double *v0, const double &mu, double *E)
 163 {
 164         double k[3];
 165         double h[3];
 166         double Dum_Vec[3];
 167         double n[3];
 168         double evett[3];
 169
 170         double p = 0.0;
 171         double temp =0.0;
 172         double R0, ni;
 173         int i;
 174
 175         vett(r0, v0, h);
 176
 177         for (i=0; i<3; i++)
 178                 p += h[i]*h[i];
 179
 180         p/=mu;
 181
 182
 183         k[0] = 0; k[1] = 0; k[2] = 1;
 184         vett(k, h, n);
 185
 186
 187         for (i=0; i<3; i++)
 188                 temp += pow(n[i], 2);
 189
 190         temp = sqrt(temp);
 191
 192         for (i=0; i<3; i++)
 193                 n[i] /= temp;
 194
 195         R0 = norm2(r0);
 196
 197         vett(v0, h, Dum_Vec);
 198
 199         for (i=0; i<3; i++)
 200                 evett[i] = Dum_Vec[i]/mu - r0[i]/R0;
 201
 202         double e = 0.0;
 203         for (i=0; i<3; i++)
 204                 e += pow(evett[i], 2);
 205
 206         E[0] = p/(1-e);
 207         E[1] = sqrt(e);
 208         e = E[1];
 209
 210         E[2] = acos(h[2]/norm2(h));
 211
 212         temp = 0.0;
 213         for (i=0; i<3; i++)
 214                 temp+=n[i]*evett[i];
 215
 216         E[4] = acos(temp/e);
 217
 218         if (evett[2] < 0) E[4] = 2*M_PI - E[4];
 219
 220         E[3] = acos(n[0]);
 221         if (n[1] < 0) E[3] = 2*M_PI-E[3];
 222
 223     temp = 0.0;
 224         for (i=0; i<3; i++)
 225                 temp+=evett[i]*r0[i];
 226
 227         ni = acos(temp/e/R0);  // danger, the argument could be improper.
 228
 229         temp = 0.0;
 230         for (i=0; i<3; i++)
 231                 temp+=r0[i]*v0[i];
 232
 233         if (temp<0.0) ni = 2*M_PI - ni;
 234
 235         if (e<1.0)
 236                 E[5] = 2.0*atan(sqrt((1-e)/(1+e))*tan(ni/2.0));  // algebraic kepler's equation
 237         else
 238                 E[5] =2.0*atan(sqrt((e-1)/(e+1))*tan(ni/2.0));   // algebraic equivalent of kepler's equation in terms of the Gudermannian
 239 }
 240
 241 /*
 242         Origin: MATLAB code programmed by Dario Izzo (ESA/ACT)
 243
 244         C++ version by Tamas Vinko (ESA/ACT)
 245
 246         Usage: [r0,v0] = IC2par(E,mu)
 247
 248         Outputs:
 249            r0:    column vector for the position
 250            v0:    column vector for the velocity
 251
 252         Inputs:
 253            E:     Column Vectors containing the six keplerian parameters,
 254                   (a (negative for hyperbolas),e,i,OM,om,Eccentric Anomaly)
 255            mu:    gravitational constant
 256
 257         Comments:  The parameters returned are, of course, referred to the same
 258         ref. frame in which r0,v0 are given. a can be given either in kms or AUs,
 259         but has to be consistent with mu.All the angles must be given in radians.
 260         This function does work for hyperbolas as well.
 261 */
 262
 263 void par2IC(const double *E, const double &mu, double *r0, double *v0)
 264 {
 265         double a = E[0];
 266         double e = E[1];
 267         double i = E[2];
 268         double omg = E[3];
 269         double omp = E[4];
 270         double EA = E[5];
 271         double b, n, xper, yper, xdotper, ydotper;
 272         double R[3][3];
 273         double cosomg, cosomp, sinomg, sinomp, cosi, sini;
 274         double dNdZeta;
 275
 276         // Grandezze definite nel piano dell'orbita
 277
 278         if (e<1.0)
 279         {
 280                 b = a*sqrt(1-e*e);
 281                 n = sqrt(mu/(a*a*a));
 282                 xper=a*(cos(EA)-e);
 283                 yper=b*sin(EA);
 284
 285                 xdotper = -(a*n*sin(EA))/(1-e*cos(EA));
 286                 ydotper=(b*n*cos(EA))/(1-e*cos(EA));
 287         }
 288         else
 289         {
 290                 b = -a*sqrt(e*e-1);
 291                 n = sqrt(-mu/(a*a*a));
 292
 293                 dNdZeta = e * (1+tan(EA)*tan(EA))-(0.5+0.5*pow(tan(0.5*EA + M_PI_4),2))/tan(0.5*EA+ M_PI_4);
 294
 295                 xper = a/cos(EA) - a*e;
 296                 yper = b*tan(EA);
 297
 298                 xdotper = a*tan(EA)/cos(EA)*n/dNdZeta;
 299                 ydotper = b/pow(cos(EA), 2)*n/dNdZeta;
 300         }
 301
 302     // Matrice di trasformazione da perifocale a ECI
 303
 304         cosomg = cos(omg);
 305         cosomp = cos(omp);
 306         sinomg = sin(omg);
 307         sinomp = sin(omp);
 308         cosi = cos(i);
 309         sini = sin(i);
 310
 311
 312         R[0][0]=cosomg*cosomp-sinomg*sinomp*cosi;
 313         R[0][1]=-cosomg*sinomp-sinomg*cosomp*cosi;
 314         R[0][2]=sinomg*sini;
 315         R[1][0]=sinomg*cosomp+cosomg*sinomp*cosi;
 316         R[1][1]=-sinomg*sinomp+cosomg*cosomp*cosi;
 317         R[1][2]=-cosomg*sini;
 318         R[2][0]=sinomp*sini;
 319         R[2][1]=cosomp*sini;
 320         R[2][2]=cosi;
 321
 322         // Posizione nel sistema inerziale
 323
 324
 325         double temp[3] = {xper, yper, 0.0};
 326         double temp2[3] = {xdotper, ydotper, 0};
 327
 328         for (int j = 0; j<3; j++)
 329         {
 330                 r0[j] = 0.0; v0[j] = 0.0;
 331                 for (int k = 0; k<3; k++)
 332                 {
 333                         r0[j]+=R[j][k]*temp[k];
 334                         v0[j]+=R[j][k]*temp2[k];
 335                 }
 336         }
 337         return;
 338 }