1 // ------------------------------------------------------------------------ //
2 // This source file is part of the 'ESA Advanced Concepts Team's //
3 // Space Mechanics Toolbox' software. //
5 // The source files are for research use only, //
6 // and are distributed WITHOUT ANY WARRANTY. Use them on your own risk. //
8 // Copyright (c) 2004-2007 European Space Agency //
9 // ------------------------------------------------------------------------ //
13 #include "Astro_Functions.h"
16 #include "propagateKEP.h"
17 #include "time2distance.h"
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
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] = {
45 //TODO: move somewhere else
46 void vector_normalize(const double in
[3], double out
[3])
48 double norm
= norm2(in
);
49 for(int i
= 0; i
< 3; i
++) {
50 out
[i
] = in
[i
] / norm
;
55 * Compute velocity and position of an celestial object of interest at specified time.
57 * problem - concerned problem
59 * i_count - hop number (starting from 0)
60 * r - [output] object's position
61 * v - [output] object's velocity
63 void get_celobj_r_and_v(const mgadsmproblem
& problem
, const double T
, const int i_count
, double* r
, double* v
)
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
69 Custom_Eph(T
+ 2451544.5, problem
.asteroid
.epoch
, problem
.asteroid
.keplerian
,
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.
78 * problem - concerned problem
79 * r - [output] array of position vectors
80 * v - [output] array of velocity vectors
82 void precalculate_ers_and_vees(const vector
<double>& t
, const mgadsmproblem
& problem
, std::vector
<double*>& r
, std::vector
<double*>& v
)
84 double T
= t
[0]; //time of departure
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
93 * Get gravitational constant of an celestial object of interest.
95 * problem - concerned problem
96 * i_count - hop number (starting from 0)
98 double get_celobj_mu(const mgadsmproblem
& problem
, const int i_count
)
100 if (problem
.sequence
[i_count
] < 10) { //normal planet
101 return MU
[problem
.sequence
[i_count
]];
103 return problem
.asteroid
.mu
;
107 // FIRST BLOCK (P1 to P2)
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
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])
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];
127 int i
; //loop counter
129 // Spacecraft position and velocity at departure
131 cross(r
[0], v
[0], vtemp
);
134 vector_normalize(vtemp
, zP1
);
137 vector_normalize(v
[0], iP1
);
140 cross(zP1
, iP1
, jP1
);
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
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
]);
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
153 for (i
= 0; i
< 3; i
++)
155 //v_sc_pl_in[i] = v[0][i];
156 v_sc_pl_out
[i
] = v
[0][i
] + vinf
[i
];
159 // Computing S/C position and absolute incoming velocity at DSM1
160 double rd
[3], v_sc_dsm_in
[3];
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.
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
);
169 int lw
= (Dum_Vec
[2] > 0) ? 0 : 1;
171 int iter_unused
; // [MR] unused variable
173 double v_sc_dsm_out
[3]; // DSM output speed
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
178 // First Contribution to DV (the 1st deep space maneuver)
179 for (i
= 0; i
< 3; i
++)
181 Dum_Vec
[i
] = v_sc_dsm_out
[i
] - v_sc_dsm_in
[i
]; // [MR] Temporary variable reused. Dirty.
184 DV
[0] = norm2(Dum_Vec
);
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
)
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
;
201 int i
; //loop counter
203 // Evaluation of the state immediately after Pi
207 for (i
= 0; i
< 3; i
++)
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
];
213 // Hop object's gravitional constant
214 double hopobj_mu
= get_celobj_mu(problem
, i_count
+ 1);
216 double e
= 1.0 + rp_non_dim
[i_count
] * RPL
[sequence
[i_count
+ 1] - 1] * vrelin
/ hopobj_mu
;
218 double beta_rot
= 2 * asin(1 / e
); // velocity rotation
221 vector_normalize(v_rel_in
, ix
);
224 vector_normalize(v
[i_count
+1], vpervnorm
);
227 vett(ix
, vpervnorm
, iy
);
228 vector_normalize(iy
, iy
); // [MR]this *might* not work properly...
233 double v_rel_in_norm
= norm2(v_rel_in
);
235 double v_sc_pl_out
[3]; // TODO: document me!
237 for (i
= 0; i
< 3; i
++)
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
;
244 // Computing S/C position and absolute incoming velocity at DSMi
245 double rd
[3], v_sc_dsm_in
[3];
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
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
);
254 int lw
= (Dum_Vec
[2] > 0) ? 0 : 1;
256 int iter_unused
; // [MR] unused variable
258 double v_sc_dsm_out
[3]; // DSM output speed
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.
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.
268 DV
[i_count
+ 1] = norm2(Dum_Vec
);
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
)
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
;
281 int i
; //loop counter
283 // Evaluation of the arrival DV
285 for (i
= 0; i
< 3; i
++) {
286 Dum_Vec
[i
] = v
[n
-1][i
] - v_sc_pl_in
[i
];
290 DVrel
= norm2(Dum_Vec
); // Relative velocity at target planet
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
){
298 } else if (problem
.type
== total_DV_rndv
){
301 DVarr
= 0.0; // no DVarr is considered
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
,
317 //[MR] A bunch of helper variables to simplify the code
318 const int n
= problem
.sequence
.size();
320 int i
; //loop counter
322 //References to objects pre-allocated in the mgadsm struct
323 std::vector
<double*>& r
= problem
.r
;
324 std::vector
<double*>& v
= problem
.v
;
326 std::vector
<double>& DV
= problem
.DV
; //DV contributions
328 precalculate_ers_and_vees(t
, problem
, r
, v
);
330 double inter_pl_in_v
[3], inter_pl_out_v
[3]; //inter-hop velocities
333 first_block(t
, problem
, r
, v
,
334 DV
, inter_pl_out_v
); // [MR] output
336 // INTERMEDIATE BLOCK
337 for (int i_count
=0; i_count
< n
- 2; i_count
++) {
338 //copy previous output velocity to current input velocity
339 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 intermediate_block(t
, problem
, r
, v
, i_count
, inter_pl_in_v
,
345 //copy previous output velocity to current input velocity
346 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(problem
, r
, v
, inter_pl_in_v
,
351 // **************************************************************************
352 // Evaluation of total DV spent by the propulsion system
353 // **************************************************************************
356 for (i
= 0; i
< n
; i
++) {
360 // %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
361 //[MR] Calculation of the actual procedure output (DVvec and J)
363 const double& VINF
= t
[1]; // Variable renaming: Hyperbolic escape velocity (km/sec)
365 for (i
= n
; i
> 0; i
--) {
371 // Finally our objective function (J) is:
373 if (problem
.type
== orbit_insertion
) {
375 } else if (problem
.type
== total_DV_orbit_insertion
) {
377 } else if (problem
.type
== rndv
) {
379 } else if (problem
.type
== total_DV_rndv
) {
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];
391 // non dimensional units
392 const double AU
= 149597870.66;
393 const double V
= sqrt(MU
[0] / AU
);
394 const double T
= AU
/ V
;
396 //evaluate the state of the spacecraft after the last fly-by
401 v_rel_in
[i
] = inter_pl_in_v
[i
] - v
[n
-1][i
];
402 vrelin
+= v_rel_in
[i
] * v_rel_in
[i
];
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
407 double beta_rot
=2*asin(1/e
); // velocity rotation
409 double vrelinn
= norm2(v_rel_in
);
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
416 double vnorm
= norm2(v
[n
-1]);
420 vtemp
[i
] = v
[n
-1][i
]/vnorm
;
425 double iynorm
= norm2(iy
);
427 iy
[i
] = iy
[i
]/iynorm
;
431 double v_rel_in_norm
= norm2(v_rel_in
);
433 double v_sc_pl_out
[3]; // TODO: document me!
434 for (i
= 0; i
< 3; i
++)
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
;
442 double v_sc_pl_out_per_V
[3];
443 for (i
= 0; i
< 3; i
++)
445 r_per_AU
[i
] = r
[n
- 1][i
] / AU
;
446 v_sc_pl_out_per_V
[i
] = v_sc_pl_out
[i
] / V
;
449 double time
= time2distance(r_per_AU
, v_sc_pl_out_per_V
, AUdist
);
450 // if (time == -1) cout << " ERROR" << endl;
457 for (i
=0; i
<n
+1; i
++)
461 DVpen
+= DVpen
+(sum
-DVtotal
);
464 for (i
=1; i
<n
+1; i
++)
468 DVpen
= DVpen
+ (sum
- DVonboard
);
471 for (i
=0; i
<n
-1; i
++)
474 J
= (time
*T
/60/60/24 + sum
)/365.25 + DVpen
*100;
477 J
= 100000; // there was an ERROR in time2distance