AstroToolbox/mga_dsm.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 #include <cmath>
  12
  13 #include "Astro_Functions.h"
  14 #include "Lambert.h"
  15 #include "mga_dsm.h"
  16 #include "propagateKEP.h"
  17 #include "time2distance.h"
  18
  19 using namespace std;
  20
  21 // [MR] TODO: exctract these somewhere...
  22 const double MU[9] = {
  23         1.32712428e11,          // SUN                                  = 0
  24         22321,                                  // Gravitational constant of Mercury    = 1
  25         324860,                                 // Gravitational constant of Venus              = 2
  26         398601.19,                              // Gravitational constant of Earth              = 3
  27         42828.3,                                // Gravitational constant of Mars               = 4
  28         126.7e6,                                // Gravitational constant of Jupiter    = 5
  29         0.37939519708830e8,             // Gravitational constant of Saturn             = 6
  30         5.78e6,                                 // Gravitational constant of Uranus             = 7
  31         6.8e6                                   // Gravitational constant of Neptune    = 8
  32 };
  33
  34 //  Definition of planetari radii
  35 //  TODO: maybe put missing values here so that indices correspond to those from MU[]?
  36 const double RPL[6] = {
  37         2440,   // Mercury
  38         6052,   // Venus
  39         6378,   // Earth
  40         3397,   // Mars
  41         71492,  // Jupiter
  42         60330   // Saturn
  43 };
  44
  45 //TODO: move somewhere else
  46 void vector_normalize(const double in[3], double out[3])
  47 {
  48         double norm = norm2(in);
  49         for(int i = 0; i < 3; i++) {
  50                 out[i] = in[i] / norm;
  51         }
  52 }
  53
  54 /**
  55  * Compute velocity and position of an celestial object of interest at specified time.
  56  *
  57  * problem - concerned problem
  58  * T       - time
  59  * i_count - hop number (starting from 0)
  60  * r       - [output] object's position
  61  * v       - [output] object's velocity
  62  */
  63 void get_celobj_r_and_v(const mgadsmproblem& problem, const double T, const int i_count, double* r, double* v)
  64 {
  65         if (problem.sequence[i_count] < 10) { //normal planet
  66                 Planet_Ephemerides_Analytical (T, problem.sequence[i_count],
  67                         r, v); // r and  v in heliocentric coordinate system
  68         } else { //asteroid
  69                 Custom_Eph(T + 2451544.5, problem.asteroid.epoch, problem.asteroid.keplerian,
  70                         r, v);
  71         }
  72 }
  73
  74 /**
  75  * Precomputes all velocities and positions of celestial objects of interest for the problem.
  76  * Before calling this function, r and v verctors must be pre-allocated with sufficient amount of entries.
  77  *
  78  * problem - concerned problem
  79  * r       - [output] array of position vectors
  80  * v       - [output] array of velocity vectors
  81  */
  82 void precalculate_ers_and_vees(const vector<double>& t, const mgadsmproblem& problem, std::vector<double*>& r, std::vector<double*>& v)
  83 {
  84         double T = t[0]; //time of departure
  85
  86         for(unsigned int i_count = 0; i_count < problem.sequence.size(); i_count++) {
  87                 get_celobj_r_and_v(problem, T, i_count, r[i_count], v[i_count]);
  88                 T += t[4 + i_count]; //time of flight
  89         }
  90 }
  91
  92 /**
  93  * Get gravitational constant of an celestial object of interest.
  94  *
  95  * problem - concerned problem
  96  * i_count - hop number (starting from 0)
  97  */
  98 double get_celobj_mu(const mgadsmproblem& problem, const int i_count)
  99 {
 100         if (problem.sequence[i_count] < 10) { //normal planet
 101                 return MU[problem.sequence[i_count]];
 102         } else { //asteroid
 103                 return problem.asteroid.mu;
 104         }
 105 }
 106
 107 // FIRST BLOCK (P1 to P2)
 108 /**
 109  * t          - decision vector
 110  * problem    - problem parameters
 111  * r          - planet positions
 112  * v          - planet velocities
 113  * DV         - [output] velocity contributions table
 114  * v_sc_pl_in - [output] next hop input speed
 115  */
 116 void first_block(const vector<double>& t, const mgadsmproblem& problem, const std::vector<double*>& r, std::vector<double*>& v, std::vector<double>& DV, double v_sc_nextpl_in[3])
 117 {
 118         //First, some helper constants to make code more readable
 119         const int n = problem.sequence.size();
 120         const double VINF = t[1];         // Hyperbolic escape velocity (km/sec)
 121         const double udir = t[2];         // Hyperbolic escape velocity var1 (non dim)
 122         const double vdir = t[3];         // Hyperbolic escape velocity var2 (non dim)
 123         // [MR] {LITTLE HACKER TRICK} Instead of copying (!) arrays let's just introduce pointers to appropriate positions in the decision vector.
 124         const double *tof = &t[4];
 125         const double *alpha = &t[n+3];
 126
 127         int i; //loop counter
 128
 129         // Spacecraft position and velocity at departure
 130         double vtemp[3];
 131         cross(r[0], v[0], vtemp);
 132
 133         double zP1[3];
 134         vector_normalize(vtemp, zP1);
 135
 136         double iP1[3];
 137         vector_normalize(v[0], iP1);
 138
 139         double jP1[3];
 140         cross(zP1, iP1, jP1);
 141
 142         double theta, phi;
 143         theta = 2 * M_PI * udir;             // See Picking a Point on a Sphere
 144         phi = acos(2 * vdir - 1) - M_PI / 2; // In this way: -pi/2<phi<pi/2 so phi can be used as out-of-plane rotation
 145
 146         double vinf[3];
 147         for (i = 0; i < 3; i++)
 148                 vinf[i] = VINF * (cos(theta) * cos(phi) * iP1[i] + sin(theta) * cos(phi) * jP1[i] + sin(phi) * zP1[i]);
 149
 150         //double v_sc_pl_in[3];  // Spacecraft absolute incoming velocity at P1 [MR] not needed?
 151         double v_sc_pl_out[3]; // Spacecraft absolute outgoing velocity at P1
 152
 153         for (i = 0; i < 3; i++)
 154         {
 155                 //v_sc_pl_in[i]  = v[0][i];
 156                 v_sc_pl_out[i] = v[0][i] + vinf[i];
 157         }
 158
 159         // Computing S/C position and absolute incoming velocity at DSM1
 160         double rd[3], v_sc_dsm_in[3];
 161
 162         propagateKEP(r[0], v_sc_pl_out, alpha[0] * tof[0] * 86400, MU[0],
 163                         rd, v_sc_dsm_in); // [MR] last two are output.
 164
 165         // Evaluating the Lambert arc from DSM1 to P2
 166         double Dum_Vec[3]; // [MR] Rename it to something sensible...
 167         vett(rd, r[1], Dum_Vec);
 168
 169         int lw = (Dum_Vec[2] > 0) ? 0 : 1;
 170         double a, p, theta2;
 171         int iter_unused; // [MR] unused variable
 172
 173         double v_sc_dsm_out[3]; // DSM output speed
 174
 175         LambertI(rd, r[1], tof[0] * (1 - alpha[0]) * 86400, MU[0], lw,
 176                 v_sc_dsm_out, v_sc_nextpl_in, a, p, theta2, iter_unused);       // [MR] last 6 are output
 177
 178         // First Contribution to DV (the 1st deep space maneuver)
 179         for (i = 0; i < 3; i++)
 180         {
 181                 Dum_Vec[i] = v_sc_dsm_out[i] - v_sc_dsm_in[i]; // [MR] Temporary variable reused. Dirty.
 182         }
 183
 184         DV[0] = norm2(Dum_Vec);
 185 }
 186
 187 // ------
 188 // INTERMEDIATE BLOCK
 189 // WARNING: i_count starts from 0
 190 void intermediate_block(const vector<double>& t, const mgadsmproblem& problem, const std::vector<double*>& r, const std::vector<double*>& v, int i_count, const double v_sc_pl_in[], std::vector<double>& DV, double* v_sc_nextpl_in)
 191 {
 192         //[MR] A bunch of helper variables to simplify the code
 193         const int n = problem.sequence.size();
 194         // [MR] {LITTLE HACKER TRICK} Instead of copying (!) arrays let's just introduce pointers to appropriate positions in the decision vector.
 195         const double *tof = &t[4];
 196         const double *alpha = &t[n+3];
 197         const double *rp_non_dim = &t[2*n+2]; // non-dim perigee fly-by radius of planets P2..Pn(-1) (i=1 refers to the second planet)
 198         const double *gamma = &t[3*n];        // rotation of the bplane-component of the swingby outgoing
 199         const vector<int>& sequence = problem.sequence;
 200
 201         int i; //loop counter
 202
 203         // Evaluation of the state immediately after Pi
 204         double v_rel_in[3];
 205         double vrelin = 0.0;
 206
 207         for (i = 0; i < 3; i++)
 208         {
 209                 v_rel_in[i] = v_sc_pl_in[i] - v[i_count+1][i];
 210                 vrelin += v_rel_in[i] * v_rel_in[i];
 211         }
 212
 213         // Hop object's gravitional constant
 214         double hopobj_mu = get_celobj_mu(problem, i_count + 1);
 215
 216         double e = 1.0 + rp_non_dim[i_count] * RPL[sequence[i_count + 1] - 1] * vrelin / hopobj_mu;
 217
 218         double beta_rot = 2 * asin(1 / e); // velocity rotation
 219
 220         double ix[3];
 221         vector_normalize(v_rel_in, ix);
 222
 223         double vpervnorm[3];
 224         vector_normalize(v[i_count+1], vpervnorm);
 225
 226         double iy[3];
 227         vett(ix, vpervnorm, iy);
 228         vector_normalize(iy, iy); // [MR]this *might* not work properly...
 229
 230         double iz[3];
 231         vett(ix, iy, iz);
 232
 233         double v_rel_in_norm = norm2(v_rel_in);
 234
 235         double v_sc_pl_out[3]; // TODO: document me!
 236
 237         for (i = 0; i < 3; i++)
 238         {
 239                 double iVout = cos(beta_rot) * ix[i] + cos(gamma[i_count]) * sin(beta_rot) * iy[i] + sin(gamma[i_count]) * sin(beta_rot) * iz[i];
 240                 double v_rel_out = v_rel_in_norm * iVout;
 241                 v_sc_pl_out[i] = v[i_count + 1][i] + v_rel_out;
 242         }
 243
 244         // Computing S/C position and absolute incoming velocity at DSMi
 245         double rd[3], v_sc_dsm_in[3];
 246
 247         propagateKEP(r[i_count + 1], v_sc_pl_out, alpha[i_count+1] * tof[i_count+1] * 86400, MU[0],
 248                                         rd, v_sc_dsm_in); // [MR] last two are output
 249
 250         // Evaluating the Lambert arc from DSMi to Pi+1
 251         double Dum_Vec[3]; // [MR] Rename it to something sensible...
 252         vett(rd, r[i_count + 2], Dum_Vec);
 253
 254         int lw = (Dum_Vec[2] > 0) ? 0 : 1;
 255         double a, p, theta;
 256         int iter_unused; // [MR] unused variable
 257
 258         double v_sc_dsm_out[3]; // DSM output speed
 259
 260         LambertI(rd, r[i_count + 2], tof[i_count + 1] * (1 - alpha[i_count + 1]) * 86400, MU[0], lw,
 261                 v_sc_dsm_out, v_sc_nextpl_in, a, p, theta, iter_unused); // [MR] last 6 are output.
 262
 263         // DV contribution
 264         for (i = 0; i < 3; i++) {
 265                 Dum_Vec[i] = v_sc_dsm_out[i] - v_sc_dsm_in[i]; // [MR] Temporary variable reused. Dirty.
 266         }
 267
 268         DV[i_count + 1] = norm2(Dum_Vec);
 269 }
 270
 271 // FINAL BLOCK
 272 //
 273 void final_block(const mgadsmproblem& problem, const std::vector<double*>& , const std::vector<double*>& v, const double v_sc_pl_in[], std::vector<double>& DV)
 274 {
 275         //[MR] A bunch of helper variables to simplify the code
 276         const int n = problem.sequence.size();
 277         const double rp_target =  problem.rp;
 278         const double e_target = problem.e;
 279         const vector<int>& sequence = problem.sequence;
 280
 281         int i; //loop counter
 282
 283         // Evaluation of the arrival DV
 284         double Dum_Vec[3];
 285         for (i = 0; i < 3; i++) {
 286                 Dum_Vec[i] = v[n-1][i] - v_sc_pl_in[i];
 287         }
 288
 289         double DVrel, DVarr;
 290         DVrel = norm2(Dum_Vec);  // Relative velocity at target planet
 291
 292         if ((problem.type  == orbit_insertion) || (problem.type == total_DV_orbit_insertion)) {
 293                 double DVper = sqrt(DVrel * DVrel + 2 * MU[sequence[n - 1]] / rp_target); //[MR] should MU be changed to get_... ?
 294             double DVper2 = sqrt(2 * MU[sequence[n - 1]] / rp_target - MU[sequence[n - 1]] / rp_target * (1 - e_target));
 295             DVarr = fabs(DVper - DVper2);
 296         } else if (problem.type == rndv){
 297                 DVarr = DVrel;
 298         } else if (problem.type == total_DV_rndv){
 299                 DVarr = DVrel;
 300         } else {
 301                 DVarr = 0.0;  // no DVarr is considered
 302         }
 303
 304         DV[n - 1] = DVarr;
 305 }
 306
 307
 308 int MGA_DSM(
 309                         /* INPUT values: */ //[MR] make this parameters const, if they are not modified and possibly references (especially 'problem').
 310                         vector<double> t,       // it is the vector which provides time in modified julian date 2000. [MR] ??? Isn't it the decision vetor ???
 311                         mgadsmproblem problem,
 312
 313                         /* OUTPUT values: */
 314                         double &J,    // output
 315                         double *DVvec // output
 316                         )
 317 {
 318         //[MR] A bunch of helper variables to simplify the code
 319         const int n = problem.sequence.size();
 320
 321         int i; //loop counter
 322
 323         //References to objects pre-allocated in the mgadsm struct
 324         std::vector<double*>& r = problem.r;
 325         std::vector<double*>& v = problem.v;
 326
 327         std::vector<double>& DV = problem.DV; //DV contributions
 328
 329         precalculate_ers_and_vees(t, problem, r, v);
 330
 331         double inter_pl_in_v[3], inter_pl_out_v[3]; //inter-hop velocities
 332
 333         // FIRST BLOCK
 334         first_block(t, problem, r, v,
 335                 DV, inter_pl_out_v); // [MR] output
 336
 337         // INTERMEDIATE BLOCK
 338         for (int i_count=0; i_count < n - 2; i_count++) {
 339                 //copy previous output velocity to current input velocity
 340                 inter_pl_in_v[0] = inter_pl_out_v[0]; inter_pl_in_v[1] = inter_pl_out_v[1]; inter_pl_in_v[2] = inter_pl_out_v[2];
 341
 342                 intermediate_block(t, problem, r, v, i_count, inter_pl_in_v,
 343                         DV, inter_pl_out_v);
 344         }
 345
 346         //copy previous output velocity to current input velocity
 347         inter_pl_in_v[0] = inter_pl_out_v[0]; inter_pl_in_v[1] = inter_pl_out_v[1]; inter_pl_in_v[2] = inter_pl_out_v[2];
 348         // FINAL BLOCK
 349         final_block(problem, r, v, inter_pl_in_v,
 350                 DV);
 351
 352         // **************************************************************************
 353         // Evaluation of total DV spent by the propulsion system
 354         // **************************************************************************
 355         double DVtot = 0.0;
 356
 357         for (i = 0; i < n; i++) {
 358                 DVtot += DV[i];
 359         }
 360
 361         // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 362         //[MR] Calculation of the actual procedure output (DVvec and J)
 363
 364         const double VINF = t[1];         // Hyperbolic escape velocity (km/sec)
 365
 366         DVvec[0] = VINF;
 367         for (i = 1; i < n + 1; i++) {
 368                 DVvec[i] = DV[i - 1];
 369         }
 370
 371         // Finally our objective function (J) is:
 372
 373         if (problem.type == orbit_insertion) {
 374                 J = DVtot;
 375         } else if (problem.type == total_DV_orbit_insertion) {
 376                 J = DVtot + VINF;
 377         } else if (problem.type == rndv) {
 378                 J = DVtot;
 379         } else if (problem.type == total_DV_rndv) {
 380                 J = DVtot + VINF;
 381         } else if (problem.type == time2AUs) { // [MR] TODO: extract method
 382                 // [MR] helper constants
 383                 const vector<int>& sequence = problem.sequence;
 384                 const double *rp_non_dim = &t[2*n+2]; // non-dim perigee fly-by radius of planets P2..Pn(-1) (i=1 refers to the second planet)
 385                 const double *gamma = &t[3*n];        // rotation of the bplane-component of the swingby outgoing
 386                 const double AUdist = problem.AUdist;
 387                 const double DVtotal = problem.DVtotal;
 388                 const double DVonboard = problem.DVonboard;
 389                 const double *tof = &t[4];
 390
 391                 // non dimensional units
 392                 const double AU  = 149597870.66;
 393                 const double V = sqrt(MU[0] / AU);
 394                 const double T = AU / V;
 395
 396                 //evaluate the state of the spacecraft after the last fly-by
 397                 double vrelin = 0.0;
 398                 double v_rel_in[3];
 399                 for (i=0; i<3; i++)
 400                 {
 401                         v_rel_in[i] = inter_pl_in_v[i] - v[n-1][i];
 402                         vrelin += v_rel_in[i] * v_rel_in[i];
 403                 }
 404
 405                 double e = 1.0 + rp_non_dim[n - 2] * RPL[sequence[n - 2 + 1] - 1] * vrelin / get_celobj_mu(problem, n - 1); //I hope the planet index (n - 1) is OK
 406
 407                 double beta_rot=2*asin(1/e);              // velocity rotation
 408
 409                 double vrelinn = norm2(v_rel_in);
 410                 double ix[3];
 411                 for (i=0; i<3; i++)
 412                         ix[i] = v_rel_in[i]/vrelinn;
 413                         // ix=r_rel_in/norm(v_rel_in);  // activating this line and disactivating the one above
 414                                  // shifts the singularity for r_rel_in parallel to v_rel_in
 415
 416                 double vnorm = norm2(v[n-1]);
 417
 418         double vtemp[3];
 419                 for (i=0; i<3; i++)
 420                         vtemp[i] = v[n-1][i]/vnorm;
 421
 422                 double iy[3];
 423                 vett(ix, vtemp, iy);
 424
 425                 double iynorm = norm2(iy);
 426                 for (i=0; i<3; i++)
 427                         iy[i] = iy[i]/iynorm;
 428
 429                 double iz[3];
 430                 vett(ix, iy, iz);
 431                 double v_rel_in_norm = norm2(v_rel_in);
 432
 433                 double v_sc_pl_out[3]; // TODO: document me!
 434                 for (i = 0; i < 3; i++)
 435                 {
 436                         double iVout = cos(beta_rot) * ix[i] + cos(gamma[n - 2]) * sin(beta_rot) * iy[i] + sin(gamma[n - 2]) * sin(beta_rot) * iz[i];
 437                         double v_rel_out = v_rel_in_norm * iVout;
 438                         v_sc_pl_out[i] = v[n - 1][i] + v_rel_out;
 439                 }
 440
 441                 double r_per_AU[3];
 442                 double v_sc_pl_out_per_V[3];
 443                 for (i = 0; i < 3; i++)
 444                 {
 445                         r_per_AU[i] = r[n - 1][i] / AU;
 446                         v_sc_pl_out_per_V[i] = v_sc_pl_out[i] / V;
 447                 }
 448
 449                 double time = time2distance(r_per_AU, v_sc_pl_out_per_V, AUdist);
 450                 // if (time == -1) cout << " ERROR" << endl;
 451
 452                 if (time != -1)
 453                 {
 454                         double DVpen=0;
 455                         double sum = 0.0;
 456
 457                         for (i=0; i<n+1; i++)
 458                                 sum += DVvec[i];
 459
 460                         if (sum > DVtotal)
 461                                 DVpen += DVpen+(sum-DVtotal);
 462
 463                         sum = 0.0;
 464                         for (i=1; i<n+1; i++)
 465                                 sum+=DVvec[i];
 466
 467                         if (sum > DVonboard)
 468                                 DVpen = DVpen + (sum - DVonboard);
 469
 470                         sum = 0.0;
 471                         for (i=0; i<n-1; i++)
 472                                 sum += tof[i];
 473
 474                         J= (time*T/60/60/24 + sum)/365.25 + DVpen*100;
 475                 }
 476                 else
 477                         J = 100000;   // there was an ERROR in time2distance
 478         } // time2AU
 479
 480         return 0;
 481 }