/***********************************************************************/
/* _______ ___________ ______ ______ ___ ___
/ _____||___ ____| / __ \ | _ \ \ \ / /
| | __ | | | | | | | |_) | \ V /
| | |_ | | | | | | | | ___/ > <
| |__| | | | | |__| | | | / _ \
\______| |__| \______/ |__| /__/ \__\
Version 1.0
GTOPX - Space Mission Benchmarks
--------------------------------
This file contains the source code for the GTOPX benchmarks.
In order to run the GTOPX benchmark problems, you may want
to use the main program available at:
http://www.midaco-solver.com/index.php/about/benchmarks/gtopx
Note: The GTOPX function call is located in this file in Line 555
Note: If used with a plain C main program, Line 555
must contain the additional statement: extern "C"
For further information on ESA's original GTOP, see here:
https://www.esa.int/gsp/ACT/projects/gtop/
For questions/suggestions on GTOPX, please email: info@midaco-solver.com
*/
/***********************************************************************/
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see .
*/
// ------------------------------------------------------------------------ //
// This source file is part of the 'ESA Advanced Concepts Team's //
// Space Mechanics Toolbox' software. //
// //
// The source files are for research use only, //
// and are distributed WITHOUT ANY WARRANTY. Use them on your own risk. //
// //
// Copyright (c) 2004-2007 European Space Agency //
// ------------------------------------------------------------------------ //
#ifndef ASTRO_FUNCTIONS_H
#define ASTRO_FUNCTIONS_H
#include
#include
#include
#include
#include
// #include "ZeroFinder.h"
using namespace std;
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
// Conversion from Mean Anomaly to Eccentric Anomaly via Kepler's equation
double Mean2Eccentric (const double ,const double);
void Conversion(const double*, double*, double*, const double);
double norm(const double*, const double*);
double norm2(const double*);
void vett(const double*, const double*, double*);
double asinh (double);
double acosh (double );
double tofabn(const double&, const double&, const double&);
void vers(const double*, double*);
double x2tof(const double&, const double&, const double&, const int);
#endif
// ------------------------------------------------------------------------ //
// This source file is part of the 'ESA Advanced Concepts Team's //
// Space Mechanics Toolbox' software. //
// //
// The source files are for research use only, //
// and are distributed WITHOUT ANY WARRANTY. Use them on your own risk. //
// //
// Copyright (c) 2004-2007 European Space Agency //
// ------------------------------------------------------------------------ //
#ifndef LAMBERT_H
#define LAMBERT_H
// #include "Astro_Functions.h"
void LambertI (const double*, const double*, double, const double, const int, //INPUT
double*, double*, double&, double&, double& , int&);//OUTPUT
#endif
// ------------------------------------------------------------------------ //
// This source file is part of the 'ESA Advanced Concepts Team's //
// Space Mechanics Toolbox' software. //
// //
// The source files are for research use only, //
// and are distributed WITHOUT ANY WARRANTY. Use them on your own risk. //
// //
// Copyright (c) 2004-2007 European Space Agency //
// ------------------------------------------------------------------------ //
#ifndef MISSION_H
#define MISSION_H
#include
// #include "Pl_Eph_An.h"
using namespace std;
// problem types
#define orbit_insertion 0 // Tandem
#define total_DV_orbit_insertion 1 // Cassini 1
#define rndv 2 // Rosetta
#define total_DV_rndv 3 // Cassini 2 and Messenger
#define asteroid_impact 4 // gtoc1
#define time2AUs 5 // SAGAS
struct customobject
{
double keplerian[6];
double epoch;
double mu;
};
struct mgaproblem {
int type; //problem type
vector sequence; //fly-by sequence (ex: 3,2,3,3,5,is Earth-Venus-Earth-Earth-Jupiter)
vector rev_flag; //vector of flags for clockwise legs
double e; //insertion e (only in case total_DV_orbit_insertion)
double rp; //insertion rp in km (only in case total_DV_orbit_insertion)
customobject asteroid; //asteroid data (in case fly-by sequence has a final number = 10)
double Isp;
double mass;
double DVlaunch;
};
int MGA(
//INPUTS
vector,
mgaproblem,
//OUTPUTS
vector &, vector&, double&, double&);
#endif
// ------------------------------------------------------------------------ //
// This source file is part of the 'ESA Advanced Concepts Team's //
// Space Mechanics Toolbox' software. //
// //
// The source files are for research use only, //
// and are distributed WITHOUT ANY WARRANTY. Use them on your own risk. //
// //
// Copyright (c) 2004-2007 European Space Agency //
// ------------------------------------------------------------------------ //
#ifndef MGA_DSM_H
#define MGA_DSM_H
#include
// #include "Pl_Eph_An.h"
// #include "Astro_Functions.h"
// #include "propagateKEP.h"
// #include "Lambert.h"
// #include "time2distance.h"
// #include "mga.h"
using namespace std;
struct mgadsmproblem {
int type; //problem type
vector sequence; //fly-by sequence (ex: 3,2,3,3,5,is Earth-Venus-Earth-Earth-Jupiter)
double e; //insertion e (only in case total_DV_orbit_insertion)
double rp; //insertion rp in km (only in case total_DV_orbit_insertion)
customobject asteroid; //asteroid data (in case fly-by sequence has a final number = 10)
double AUdist; //Distance to reach in AUs (only in case of time2AUs)
double DVtotal; //Total DV allowed in km/s (only in case of time2AUs)
double DVonboard; //Total DV on the spacecraft in km/s (only in case of time2AUs)
//Pre-allocated memory, in order to remove allocation of heap space in MGA_DSM calls
//The DV vector serves also as output containing all the values for the impulsive DV
std::vector r;// = std::vector(n);
std::vector v;// = std::vector(n);
std::vector DV;// = std::vector(n+1);
};
int MGA_DSM(
/* INPUT values: */
vector x , // it is the decision vector
mgadsmproblem& mgadsm, // contains the problem specific data, passed as reference as mgadsm.DV is an output
/* OUTPUT values: */
double& J, double&, double&, double& // J output
);
#endif
// ------------------------------------------------------------------------ //
// This source file is part of the 'ESA Advanced Concepts Team's //
// Space Mechanics Toolbox' software. //
// //
// The source files are for research use only, //
// and are distributed WITHOUT ANY WARRANTY. Use them on your own risk. //
// //
// Copyright (c) 2004-2007 European Space Agency //
// ------------------------------------------------------------------------ //
#ifndef miscTandem_H
#define miscTandem_H
int xant(const double(&X)[5] , const double &x);
int yant(const double(&Y)[15] , const double &y);
int xantA5(const double(&X)[9] , const double &x);
int yantA5(const double(&Y)[13] , const double &y);
double interp2SF(const double (&X)[5] , double(&Y)[15] , const double &VINF, const double &declination);
double interp2A5(const double (&X)[5] , double(&Y)[15] , const double &VINF, const double &declination);
double SoyuzFregat (const double &VINF, const double &declination) ;
double Atlas501 (const double &VINF, const double &declination) ;
void ecl2equ (double (&ecl)[3],double (&equ)[3]);
#endif
// ------------------------------------------------------------------------ //
// This source file is part of the 'ESA Advanced Concepts Team's //
// Space Mechanics Toolbox' software. //
// //
// The source files are for research use only, //
// and are distributed WITHOUT ANY WARRANTY. Use them on your own risk. //
// //
// Copyright (c) 2004-2007 European Space Agency //
// ------------------------------------------------------------------------ //
#ifndef PL_EPH_AN_H
#define PL_EPH_AN_H
#include
using namespace std;
void Planet_Ephemerides_Analytical (const double, const int, double*, double*);
void Custom_Eph(const double, const double, const double[], double*, double*);
#endif
// ------------------------------------------------------------------------ //
// This source file is part of the 'ESA Advanced Concepts Team's //
// Space Mechanics Toolbox' software. //
// //
// The source files are for research use only, //
// and are distributed WITHOUT ANY WARRANTY. Use them on your own risk. //
// //
// Copyright (c) 2004-2007 European Space Agency //
// ------------------------------------------------------------------------ //
#ifndef POWSWINGBYINV_H
#define POWSWINGBYINV_H
void PowSwingByInv (const double, const double, const double, double&, double&);
#endif
// ------------------------------------------------------------------------ //
// This source file is part of the 'ESA Advanced Concepts Team's //
// Space Mechanics Toolbox' software. //
// //
// The source files are for research use only, //
// and are distributed WITHOUT ANY WARRANTY. Use them on your own risk. //
// //
// Copyright (c) 2004-2006 European Space Agency //
// ------------------------------------------------------------------------ //
#ifndef PROPAGATEKEP_H
#define PROPAGATEKEP_H
// #include "Astro_Functions.h"
void propagateKEP(const double *, const double *, double, double,
double *, double *);
void IC2par(const double*, const double*, double, double*);
void par2IC(const double*, double, double*, double*);
// Returns the cross product of the vectors X and Y.
// That is, z = X x Y. X and Y must be 3 element
// vectors.
void cross(const double*, const double*, double*);
#endif
// ------------------------------------------------------------------------ //
// This source file is part of the 'ESA Advanced Concepts Team's //
// Space Mechanics Toolbox' software. //
// //
// The source files are for research use only, //
// and are distributed WITHOUT ANY WARRANTY. Use them on your own risk. //
// //
// Copyright (c) 2004-2007 European Space Agency //
// ------------------------------------------------------------------------ //
#ifndef TIME_2_DISTANCE_H
#define TIME_2_DISTANCE_H
#include
#include
#include
#include
#include
// #include "propagateKEP.h"
using namespace std;
double time2distance(const double*, const double*, double);
#endif
/*
* GOProblems.h
* SeGMO, a Sequential Global Multiobjective Optimiser
*
* Created by Dario Izzo on 5/17/08.
* Copyright 2008 ¿dvanced Concepts Team (European Space Agency). All rights reserved.
*
*/
#ifndef TRAJOBJFUNS_H
#define TRAJOBJFUNS_H
#include
//NOTE: the functions here have passing by reference + const as they are called a lot of time during execution and thus
//it is worth trying to save time by avoiding to make a copy of the variable passed
double gtoc1 ( const std::vector& x, std::vector& rp);
double cassini1 ( const std::vector& x, std::vector& rp, double &launchDV);
double sagas (const std::vector& x, double& DVtot, double& DVonboard);
double rosetta (const std::vector& x, double &launchDV, double &flightDV, double &arrivalDV);
double cassini2 (const std::vector& x, double &launchDV, double &flightDV, double &arrivalDV);
double messenger(const std::vector& x, double &launchDV, double &flightDV, double &arrivalDV);
double messengerfull(const std::vector& x, double &launchDV, double &flightDV, double &arrivalDV);
double cassini1minlp ( const std::vector& x, std::vector& rp, double &launchDV);
double tandem (const std::vector& x, double& tof, const int sequence_[]);
#endif
// ------------------------------------------------------------------------ //
// This source file is part of the 'ESA Advanced Concepts Team's //
// Space Mechanics Toolbox' software. //
// //
// The source files are for research use only, //
// and are distributed WITHOUT ANY WARRANTY. Use them on your own risk. //
// //
// Copyright (c) 2004-2007 European Space Agency //
// ------------------------------------------------------------------------ //
#ifndef ZEROFINDER_H
#define ZEROFINDER_H
using namespace std;
/// Namespace
namespace ZeroFinder
{
/// Class for one dimensional functions
// with some parameters
/** The ()-operator with one double argument
* has to be overloaded for a derived class
* The return value is the ordinate computed for
* the abscissa-argument.
*/
class Function1D
{
public:
//virtual double Compute(double x)=0;
virtual double operator()(double x)=0;
// parameters
double p1,p2;
void SetParameters(double a, double b);
};
class Function1D_7param
{
public:
//virtual double Compute(double x)=0;
virtual double operator()(double x)=0;
// parameters
double p1,p2,p3,p4,p5,p6,p7;
void SetParameters(double a, double b, double c, double d, double e, double f, double g);
};
class FZero
{
private:
double a, c; // lower and upper bound
public:
FZero(double a, double b); // constructor
// fzero procedure
double FindZero(Function1D& f);
double FindZero7(Function1D_7param& f);
void SetBounds(double a, double b);
};
}
#endif
#include
#include
#include "math.h"
#define VENUS 2
#define EARTH 3
#define MARS 4
#define JUPITER 5
#define SATURN 6
/***********************************************************************/
/***********************************************************************/
/***********************************************************************/
extern "C" void gtopx(int GTOP_PROBLEM, double *f, double *g, double *x)
{
double DVtot, DVonboard, launchDV, flightDV, arrivalDV, dummy;
std::vector solution, rp;
int n;
if( GTOP_PROBLEM == 1 ){ n=6; }
if( GTOP_PROBLEM == 2 ){ n=22; }
if( GTOP_PROBLEM == 3 ){ n=18; }
if( GTOP_PROBLEM == 4 ){ n=26; }
if( GTOP_PROBLEM == 5 ){ n=8; }
if( GTOP_PROBLEM == 6 ){ n=22; }
if( GTOP_PROBLEM == 7 ){ n=12; }
if( GTOP_PROBLEM == 8 ){ n=10; }
if( GTOP_PROBLEM == 9 ){ n=6; }
if( GTOP_PROBLEM == 10){ n=10; }
for (int i=0; i
#include
// #include "Astro_Functions.h"
class CZF : public ZeroFinder::Function1D
{
public:
double operator()(double x)
{
return p1*tan(x) - log(tan(0.5*x + 0.25*M_PI)) - p2;
}
};
double Mean2Eccentric (const double M, const double e)
{
double tolerance = 1e-13;
int n_of_it = 0; // Number of iterations
double Eccentric,Ecc_New;
double err = 1.0;
if (e < 1.0)
{
Eccentric = M + e* cos(M); // Initial guess
while ( (err > tolerance) && (n_of_it < 100))
{
Ecc_New = Eccentric - (Eccentric - e* sin(Eccentric) -M )/(1.0 - e * cos(Eccentric));
err = fabs(Eccentric - Ecc_New);
Eccentric = Ecc_New;
n_of_it++;
}
}
else
{
CZF FF; // function to find its zero point
ZeroFinder::FZero fz(-0.5*3.14159265358979 + 1e-8, 0.5*3.14159265358979-1e-8);
FF.SetParameters(e, M);
Ecc_New = fz.FindZero(FF);
Eccentric = Ecc_New;
}
return Eccentric;
}
void Conversion (const double *E,
double *pos,
double *vel,
const double mu)
{
double a,e,ii,omg,omp,theta;
double b,n;
double X_per[3],X_dotper[3];
double R[3][3];
a = E[0];
e = E[1];
ii = E[2];
omg = E[3];
omp = E[4];
theta = E[5];
b = a * sqrt (1 - e*e);
n = sqrt (mu/pow(a,3));
const double sin_theta = sin(theta);
const double cos_theta = cos(theta);
X_per[0] = a * (cos_theta - e);
X_per[1] = b * sin_theta;
X_dotper[0] = -(a * n * sin_theta)/(1 - e * cos_theta);
X_dotper[1] = (b * n * cos_theta)/(1 - e * cos_theta);
const double cosomg = cos(omg);
const double cosomp = cos(omp);
const double sinomg = sin(omg);
const double sinomp = sin(omp);
const double cosi = cos(ii);
const double sini = sin(ii);
R[0][0] = cosomg*cosomp-sinomg*sinomp*cosi;
R[0][1] = -cosomg*sinomp-sinomg*cosomp*cosi;
R[1][0] = sinomg*cosomp+cosomg*sinomp*cosi;
R[1][1] = -sinomg*sinomp+cosomg*cosomp*cosi;
R[2][0] = sinomp*sini;
R[2][1] = cosomp*sini;
// evaluate position and velocity
for (int i = 0;i < 3;i++)
{
pos[i] = 0;
vel[i] = 0;
for (int j = 0;j < 2;j++)
{
pos[i] += R[i][j] * X_per[j];
vel[i] += R[i][j] * X_dotper[j];
}
}
return;
}
double norm(const double *vet1, const double *vet2)
{
double Vin = 0;
for (int i = 0; i < 3; i++)
{
Vin += (vet1[i] - vet2[i])*(vet1[i] - vet2[i]);
}
return sqrt(Vin);
}
double norm2(const double *vet1)
{
double temp = 0.0;
for (int i = 0; i < 3; i++) {
temp += vet1[i] * vet1[i];
}
return sqrt(temp);
}
//subfunction that evaluates vector product
void vett(const double *vet1, const double *vet2, double *prod )
{
prod[0]=(vet1[1]*vet2[2]-vet1[2]*vet2[1]);
prod[1]=(vet1[2]*vet2[0]-vet1[0]*vet2[2]);
prod[2]=(vet1[0]*vet2[1]-vet1[1]*vet2[0]);
}
double asinh (double x) { return log(x + sqrt(x*x + 1)); }
double acosh (double x) { return log(x + sqrt(x*x - 1)); }
double x2tof(const double &x,const double &s,const double &c,const int lw)
{
double am,a,alfa,beta;
am = s/2;
a = am/(1-x*x);
if (x < 1)//ellpise
{
beta = 2 * asin (sqrt((s - c)/(2*a)));
if (lw) beta = -beta;
alfa = 2 * acos(x);
}
else
{
alfa = 2 * acosh(x);
beta = 2 * asinh(sqrt ((s - c)/(-2 * a)));
if (lw) beta = -beta;
}
if (a > 0)
{
return (a * sqrt (a)* ( (alfa - sin(alfa)) - (beta - sin(beta)) ));
}
else
{
return (-a * sqrt(-a)*( (sinh(alfa) - alfa) - ( sinh(beta) - beta)) );
}
}
// Subfunction that evaluates the time of flight as a function of x
double tofabn(const double &sigma,const double &alfa,const double &beta)
{
if (sigma > 0)
{
return (sigma * sqrt (sigma)* ( (alfa - sin(alfa)) - (beta - sin(beta)) ));
}
else
{
return (-sigma * sqrt(-sigma)*( (sinh(alfa) - alfa) - ( sinh(beta) - beta)) );
}
}
// subfunction that evaluates unit vectors
void vers(const double *V_in, double *Ver_out)
{
double v_mod = 0;
int i;
for (i = 0;i < 3;i++)
{
v_mod += V_in[i]*V_in[i];
}
double sqrtv_mod = sqrt(v_mod);
for (i = 0;i < 3;i++)
{
Ver_out[i] = V_in[i]/sqrtv_mod;
}
}
// ------------------------------------------------------------------------ //
// This source file is part of the 'ESA Advanced Concepts Team's //
// Space Mechanics Toolbox' software. //
// //
// The source files are for research use only, //
// and are distributed WITHOUT ANY WARRANTY. Use them on your own risk. //
// //
// Copyright (c) 2004-2007 European Space Agency //
// ------------------------------------------------------------------------ //
/*
This routine implements a new algorithm that solves Lambert's problem. The
algorithm has two major characteristics that makes it favorable to other
existing ones.
1) It describes the generic orbit solution of the boundary condition
problem through the variable X=log(1+cos(alpha/2)). By doing so the
graphs of the time of flight become defined in the entire real axis and
resembles a straight line. Convergence is granted within few iterations
for all the possible geometries (except, of course, when the transfer
angle is zero). When multiple revolutions are considered the variable is
X=tan(cos(alpha/2)*pi/2).
2) Once the orbit has been determined in the plane, this routine
evaluates the velocity vectors at the two points in a way that is not
singular for the transfer angle approaching to pi (Lagrange coefficient
based methods are numerically not well suited for this purpose).
As a result Lambert's problem is solved (with multiple revolutions
being accounted for) with the same computational effort for all
possible geometries. The case of near 180 transfers is also solved
efficiently.
We note here that even when the transfer angle is exactly equal to pi
the algorithm does solve the problem in the plane (it finds X), but it
is not able to evaluate the plane in which the orbit lies. A solution
to this would be to provide the direction of the plane containing the
transfer orbit from outside. This has not been implemented in this
routine since such a direction would depend on which application the
transfer is going to be used in.
Inputs:
r1=Position vector at departure (column)
r2=Position vector at arrival (column, same units as r1)
t=Transfer time (scalar)
mu=gravitational parameter (scalar, units have to be
consistent with r1,t units)
lw=1 if long way is chosen
Outputs:
v1=Velocity at departure (consistent units)
v2=Velocity at arrival
a=semi major axis of the solution
p=semi latus rectum of the solution
theta=transfer angle in rad
iter=number of iteration made by the newton solver (usually 6)
*/
#include
#include
// #include "Lambert.h"
using namespace std;
void LambertI (const double *r1_in, const double *r2_in, double t, const double mu, //INPUT
const int lw, //INPUT
double *v1, double *v2, double &a, double &p, double &theta, int &iter)//OUTPUT
{
double V,T,
r2_mod = 0.0, // R2 module
dot_prod = 0.0, // dot product
c, // non-dimensional chord
s, // non dimesnional semi-perimeter
am, // minimum energy ellipse semi major axis
lambda, //lambda parameter defined in Battin's Book
x,x1,x2,y1,y2,x_new=0,y_new,err,alfa,beta,psi,eta,eta2,sigma1,vr1,vt1,vt2,vr2,R=0.0;
int i_count, i;
const double tolerance = 1e-11;
double r1[3], r2[3], r2_vers[3];
double ih_dum[3], ih[3], dum[3];
// Increasing the tolerance does not bring any advantage as the
// precision is usually greater anyway (due to the rectification of the tof
// graph) except near particular cases such as parabolas in which cases a
// lower precision allow for usual convergence.
if (t <= 0)
{
cout << "ERROR in Lambert Solver: Negative Time in input." << endl;
return;
}
for (i = 0; i < 3; i++)
{
r1[i] = r1_in[i];
r2[i] = r2_in[i];
R += r1[i]*r1[i];
}
R = sqrt(R);
V = sqrt(mu/R);
T = R/V;
// working with non-dimensional radii and time-of-flight
t /= T;
for (i = 0;i <3;i++) // r1 dimension is 3
{
r1[i] /= R;
r2[i] /= R;
r2_mod += r2[i]*r2[i];
}
// Evaluation of the relevant geometry parameters in non dimensional units
r2_mod = sqrt(r2_mod);
for (i = 0;i < 3;i++)
dot_prod += (r1[i] * r2[i]);
theta = acos(dot_prod/r2_mod);
if (lw)
theta=2*acos(-1.0)-theta;
c = sqrt(1 + r2_mod*(r2_mod - 2.0 * cos(theta)));
s = (1 + r2_mod + c)/2.0;
am = s/2.0;
lambda = sqrt (r2_mod) * cos (theta/2.0)/s;
// We start finding the log(x+1) value of the solution conic:
// NO MULTI REV --> (1 SOL)
// inn1=-.5233; //first guess point
// inn2=.5233; //second guess point
x1=log(0.4767);
x2=log(1.5233);
y1=log(x2tof(-.5233,s,c,lw))-log(t);
y2=log(x2tof(.5233,s,c,lw))-log(t);
// Newton iterations
err=1;
i_count=0;
while ((err>tolerance) && (y1 != y2))
{
i_count++;
x_new=(x1*y2-y1*x2)/(y2-y1);
y_new=logf(x2tof(expf(x_new)-1,s,c,lw))-logf(t); //[MR] Why ...f() functions? Loss of data!
x1=x2;
y1=y2;
x2=x_new;
y2=y_new;
err = fabs(x1-x_new);
}
iter = i_count;
x = expf(x_new)-1; //[MR] Same here... expf -> exp
// The solution has been evaluated in terms of log(x+1) or tan(x*pi/2), we
// now need the conic. As for transfer angles near to pi the lagrange
// coefficient technique goes singular (dg approaches a zero/zero that is
// numerically bad) we here use a different technique for those cases. When
// the transfer angle is exactly equal to pi, then the ih unit vector is not
// determined. The remaining equations, though, are still valid.
a = am/(1 - x*x);
// psi evaluation
if (x < 1) // ellipse
{
beta = 2 * asin (sqrt( (s-c)/(2*a) ));
if (lw) beta = -beta;
alfa=2*acos(x);
psi=(alfa-beta)/2;
eta2=2*a*pow(sin(psi),2)/s;
eta=sqrt(eta2);
}
else // hyperbola
{
beta = 2*asinh(sqrt((c-s)/(2*a)));
if (lw) beta = -beta;
alfa = 2*acosh(x);
psi = (alfa-beta)/2;
eta2 = -2 * a * pow(sinh(psi),2)/s;
eta = sqrt(eta2);
}
// parameter of the solution
p = ( r2_mod / (am * eta2) ) * pow (sin (theta/2),2);
sigma1 = (1/(eta * sqrt(am)) )* (2 * lambda * am - (lambda + x * eta));
vett(r1,r2,ih_dum);
vers(ih_dum,ih) ;
if (lw)
{
for (i = 0; i < 3;i++)
ih[i]= -ih[i];
}
vr1 = sigma1;
vt1 = sqrt(p);
vett(ih,r1,dum);
for (i = 0;i < 3 ;i++)
v1[i] = vr1 * r1[i] + vt1 * dum[i];
vt2 = vt1 / r2_mod;
vr2 = -vr1 + (vt1 - vt2)/tan(theta/2);
vers(r2,r2_vers);
vett(ih,r2_vers,dum);
for (i = 0;i < 3 ;i++)
v2[i] = vr2 * r2[i] / r2_mod + vt2 * dum[i];
for (i = 0;i < 3;i++)
{
v1[i] *= V;
v2[i] *= V;
}
a *= R;
p *= R;
}
// ------------------------------------------------------------------------ //
// This source file is part of the 'ESA Advanced Concepts Team's //
// Space Mechanics Toolbox' software. //
// //
// The source files are for research use only, //
// and are distributed WITHOUT ANY WARRANTY. Use them on your own risk. //
// //
// Copyright (c) 2004-2007 European Space Agency //
// ------------------------------------------------------------------------ //
#include
#include
#include
#include
#include
// #include "Pl_Eph_An.h"
// #include "mga.h"
// #include "Lambert.h"
// #include "PowSwingByInv.h"
// #include "Astro_Functions.h"
#include
#define MAX(a, b) (a > b ? a : b)
using namespace std;
//the function return 0 if the input is right or -1 it there is something wrong
int MGA(vector t, // it is the vector which provides time in modified julian date 2000.
// The first entry is launch date, the next entries represent the time needed to
// fly from last swing-by to current swing-by.
mgaproblem problem,
/* OUTPUT values: */
vector& rp, // periplanets radius
vector& DV, // final delta-Vs
double &obj_funct, //objective function
double &martin_launch) //objective function
{
const int n = problem.sequence.size();
const vector sequence = problem.sequence;
const vector rev_flag = problem.rev_flag;// array containing 0 clockwise, 1 un-clockwise
customobject cust_obj = problem.asteroid;
double MU[9] = {//1.32712440018e11, //SUN = 0
1.32712428e11,
22321, // Gravitational constant of Mercury = 1
324860, // Gravitational constant of Venus = 2
398601.19, // Gravitational constant of Earth = 3
42828.3, // Gravitational constant of Mars = 4
126.7e6, // Gravitational constant of Jupiter = 5
37.9e6, // Gravitational constant of Saturn = 6
5.78e6, // Gravitational constant of Uranus = 7
6.8e6 // Gravitational constant of Neptune = 8
};
double penalty[9] = {0,
0, // Mercury
6351.8, // Venus
6778.1, // Earth
6000, // Mars
//671492, // Jupiter
600000, // Jupiter
70000, // Saturn
0, // Uranus
0 // Neptune
};
double penalty_coeffs[9] = {0,
0, // Mercury
0.01, // Venus
0.01, // Earth
0.01, // Mars
0.001, // Jupiter
0.01, // Saturn
0, // Uranus
0 // Neptune
};
double DVtot = 0;
double Dum_Vec[3],Vin,Vout;
double V_Lamb[2][2][3],dot_prod;
double a,p,theta,alfa;
double DVrel, DVarr=0;
//only used for orbit insertion (ex: cassini)
double DVper, DVper2;
const double rp_target = problem.rp;
const double e_target = problem.e;
const double DVlaunch = problem.DVlaunch;
//only used for asteroid impact (ex: gtoc1)
const double initial_mass = problem.mass; // Satellite initial mass [Kg]
double final_mass; // satelite final mass
const double Isp = problem.Isp; // Satellite specific impulse [s]
const double g = 9.80665 / 1000.0; // Gravity
double *vec, *rec;
vector r; // {0...n-1} position
vector v; // {0...n-1} velocity
double T = 0.0; // total time
int i_count, j_count, lw;
int iter = 0;
if (n >= 2)
{
for ( i_count = 0; i_count < n; i_count++)
{
vec = new double [3]; // velocity and position are 3 D vector
rec = new double [3];
r.push_back(vec);
v.push_back(rec);
DV [i_count] = 0.0;
}
T = 0;
for (i_count = 0; i_count < n; i_count++)
{
T += t[i_count];
if (sequence[i_count]<10)
Planet_Ephemerides_Analytical (T, sequence[i_count],
r[i_count], v[i_count]); //r and v in heliocentric coordinate system
else
{
Custom_Eph(T+2451544.5, cust_obj.epoch, cust_obj.keplerian, r[i_count], v[i_count]);
}
}
vett(r[0], r[1], Dum_Vec);
if (Dum_Vec[2] > 0)
lw = (rev_flag[0] == 0) ? 0 : 1;
else
lw = (rev_flag[0] == 0) ? 1 : 0;
LambertI(r[0],r[1],t[1]*24*60*60,MU[0],lw, // INPUT
V_Lamb[0][0],V_Lamb[0][1],a,p,theta,iter); // OUTPUT
DV[0] = norm(V_Lamb[0][0], v[0]); // Earth launch
for (i_count = 1; i_count <= n-2; i_count++)
{
vett(r[i_count], r[i_count+1], Dum_Vec);
if (Dum_Vec[2] > 0)
lw = (rev_flag[i_count] == 0) ? 0 : 1;
else
lw = (rev_flag[i_count] == 0) ? 1 : 0;
/*if (i_count%2 != 0) {*/
LambertI(r[i_count],r[i_count+1],t[i_count + 1]*24*60*60,MU[0],lw, // INPUT
V_Lamb[1][0],V_Lamb[1][1],a,p,theta,iter); // OUTPUT
// norm first perform the subtraction of vet1-vet2 and the evaluate ||...||
Vin = norm(V_Lamb[0][1], v[i_count]);
Vout = norm(V_Lamb[1][0], v[i_count]);
dot_prod = 0.0;
for (int i = 0; i < 3; i++)
{
dot_prod += (V_Lamb[0][1][i] - v[i_count][i]) * (V_Lamb[1][0][i] - v[i_count][i]);
}
alfa = acos ( dot_prod /(Vin * Vout) );
// calculation of delta V at pericenter
PowSwingByInv(Vin, Vout, alfa, DV[i_count], rp[i_count - 1]);
rp[i_count - 1] *= MU[sequence[i_count]];
if (i_count != n-2) //swap
for (j_count = 0; j_count < 3; j_count++)
{
V_Lamb[0][0][j_count] = V_Lamb[1][0][j_count]; // [j_count];
V_Lamb[0][1][j_count] = V_Lamb[1][1][j_count]; // [j_count];
}
}
}
else
{
return -1;
}
for (i_count = 0; i_count < 3; i_count++)
Dum_Vec[i_count] = v[n-1][i_count] - V_Lamb[1][1][i_count];
DVrel = norm2(Dum_Vec);
if (problem.type == total_DV_orbit_insertion){
DVper = sqrt(DVrel*DVrel + 2*MU[sequence[n-1]]/rp_target);
DVper2 = sqrt(2*MU[sequence[n-1]]/rp_target - MU[sequence[n-1]]/rp_target*(1-e_target));
DVarr = fabs(DVper - DVper2);
}
else if (problem.type == asteroid_impact){
DVarr = DVrel;
}
DVtot = 0;
for (i_count = 1; i_count < n-1; i_count++)
DVtot += DV[i_count];
if (problem.type == total_DV_orbit_insertion){
DVtot += DVarr;
}
// Build Penalty
for (i_count = 0;i_count < n-2; i_count++)
if (rp[i_count] < penalty[sequence[i_count+1]])
DVtot += penalty_coeffs[sequence[i_count+1]]*fabs(rp[i_count] - penalty[sequence[i_count+1]]);
// Launcher Constraint
if (DV[0] > DVlaunch)
DVtot += (DV[0] - DVlaunch);
if (problem.type == total_DV_orbit_insertion){
obj_funct = DVtot;
}
else if (problem.type == asteroid_impact){
// Evaluation of satellite final mass
obj_funct = final_mass = initial_mass * exp(- DVtot/ (Isp * g));
// V asteroid - V satellite
for (i_count = 0; i_count < 3; i_count++)
Dum_Vec[i_count] = v[n-1][i_count] - V_Lamb[1][1][i_count];// arrival relative velocity at the asteroid;
dot_prod = 0;
for (i_count = 0; i_count < 3 ; i_count++)
dot_prod += Dum_Vec[i_count] * v[n-1][i_count];
obj_funct = 2000000 - (final_mass)* fabs(dot_prod);
}
// final clean
for ( i_count = 0;i_count < n;i_count++)
{
delete [] r[i_count];
delete [] v[i_count];
}
r.clear();
v.clear();
martin_launch = DV[0];
// printf("\n DV[0] = %f",DV[0]);
// printf("\n DVlaunch = %f",DVlaunch);
// printf("\n DVarr = %f",DVarr);
// printf("\n DVtot = %f \n",DVtot);
return 0;
}
// ------------------------------------------------------------------------ //
// This source file is part of the 'ESA Advanced Concepts Team's //
// Space Mechanics Toolbox' software. //
// //
// The source files are for research use only, //
// and are distributed WITHOUT ANY WARRANTY. Use them on your own risk. //
// //
// Copyright (c) 2004-2007 European Space Agency //
// ------------------------------------------------------------------------ //
#include
#include
#include
// #include "Pl_Eph_An.h"
// #include "mga_dsm.h"
// [MR] TODO: exctract these somewhere...
const double MU[9] = {
1.32712428e11, // SUN = 0
22321, // Gravitational constant of Mercury = 1
324860, // Gravitational constant of Venus = 2
398601.19, // Gravitational constant of Earth = 3
42828.3, // Gravitational constant of Mars = 4
126.7e6, // Gravitational constant of Jupiter = 5
0.37939519708830e8, // Gravitational constant of Saturn = 6
5.78e6, // Gravitational constant of Uranus = 7
6.8e6 // Gravitational constant of Neptune = 8
};
// Definition of planetari radii
// TODO: maybe put missing values here so that indices correspond to those from MU[]?
const double RPL[6] = {
2440, // Mercury
6052, // Venus
6378, // Earth
3397, // Mars
71492, // Jupiter
60330 // Saturn
};
//TODO: move somewhere else
void vector_normalize(const double in[3], double out[3])
{
double norm = norm2(in);
for(int i = 0; i < 3; i++) {
out[i] = in[i] / norm;
}
}
/**
* Compute velocity and position of an celestial object of interest at specified time.
*
* problem - concerned problem
* T - time
* i_count - hop number (starting from 0)
* r - [output] object's position
* v - [output] object's velocity
*/
void get_celobj_r_and_v(const mgadsmproblem& problem, const double T, const int i_count, double* r, double* v)
{
if (problem.sequence[i_count] < 10) { //normal planet
Planet_Ephemerides_Analytical (T, problem.sequence[i_count],
r, v); // r and v in heliocentric coordinate system
} else { //asteroid
Custom_Eph(T + 2451544.5, problem.asteroid.epoch, problem.asteroid.keplerian,
r, v);
}
}
/**
* Precomputes all velocities and positions of celestial objects of interest for the problem.
* Before calling this function, r and v verctors must be pre-allocated with sufficient amount of entries.
*
* problem - concerned problem
* r - [output] array of position vectors
* v - [output] array of velocity vectors
*/
void precalculate_ers_and_vees(const vector& t, const mgadsmproblem& problem, std::vector& r, std::vector& v)
{
double T = t[0]; //time of departure
for(unsigned int i_count = 0; i_count < problem.sequence.size(); i_count++) {
get_celobj_r_and_v(problem, T, i_count, r[i_count], v[i_count]);
T += t[4 + i_count]; //time of flight
}
}
/**
* Get gravitational constant of an celestial object of interest.
*
* problem - concerned problem
* i_count - hop number (starting from 0)
*/
double get_celobj_mu(const mgadsmproblem& problem, const int i_count)
{
if (problem.sequence[i_count] < 10) { //normal planet
return MU[problem.sequence[i_count]];
} else { //asteroid
return problem.asteroid.mu;
}
}
// FIRST BLOCK (P1 to P2)
/**
* t - decision vector
* problem - problem parameters
* r - planet positions
* v - planet velocities
* DV - [output] velocity contributions table
* v_sc_pl_in - [output] next hop input speed
*/
void first_block(const vector& t, const mgadsmproblem& problem, const std::vector& r, std::vector& v, std::vector& DV, double v_sc_nextpl_in[3])
{
//First, some helper constants to make code more readable
const int n = problem.sequence.size();
const double VINF = t[1]; // Hyperbolic escape velocity (km/sec)
const double udir = t[2]; // Hyperbolic escape velocity var1 (non dim)
const double vdir = t[3]; // Hyperbolic escape velocity var2 (non dim)
// [MR] {LITTLE HACKER TRICK} Instead of copying (!) arrays let's just introduce pointers to appropriate positions in the decision vector.
const double *tof = &t[4];
const double *alpha = &t[n+3];
int i; //loop counter
// Spacecraft position and velocity at departure
double vtemp[3];
cross(r[0], v[0], vtemp);
double zP1[3];
vector_normalize(vtemp, zP1);
double iP1[3];
vector_normalize(v[0], iP1);
double jP1[3];
cross(zP1, iP1, jP1);
double theta, phi;
theta = 2 * M_PI * udir; // See Picking a Point on a Sphere
phi = acos(2 * vdir - 1) - M_PI / 2; // In this way: -pi/2 0) ? 0 : 1;
double a, p, theta2;
int iter_unused; // [MR] unused variable
double v_sc_dsm_out[3]; // DSM output speed
LambertI(rd, r[1], tof[0] * (1 - alpha[0]) * 86400, MU[0], lw,
v_sc_dsm_out, v_sc_nextpl_in, a, p, theta2, iter_unused); // [MR] last 6 are output
// First Contribution to DV (the 1st deep space maneuver)
for (i = 0; i < 3; i++)
{
Dum_Vec[i] = v_sc_dsm_out[i] - v_sc_dsm_in[i]; // [MR] Temporary variable reused. Dirty.
}
DV[0] = norm2(Dum_Vec);
}
// ------
// INTERMEDIATE BLOCK
// WARNING: i_count starts from 0
void intermediate_block(const vector& t, const mgadsmproblem& problem, const std::vector& r, const std::vector& v, int i_count, const double v_sc_pl_in[], std::vector& DV, double* v_sc_nextpl_in)
{
//[MR] A bunch of helper variables to simplify the code
const int n = problem.sequence.size();
// [MR] {LITTLE HACKER TRICK} Instead of copying (!) arrays let's just introduce pointers to appropriate positions in the decision vector.
const double *tof = &t[4];
const double *alpha = &t[n+3];
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)
const double *gamma = &t[3*n]; // rotation of the bplane-component of the swingby outgoing
const vector& sequence = problem.sequence;
int i; //loop counter
// Evaluation of the state immediately after Pi
double v_rel_in[3];
double vrelin = 0.0;
for (i = 0; i < 3; i++)
{
v_rel_in[i] = v_sc_pl_in[i] - v[i_count+1][i];
vrelin += v_rel_in[i] * v_rel_in[i];
}
// Hop object's gravitional constant
double hopobj_mu = get_celobj_mu(problem, i_count + 1);
double e = 1.0 + rp_non_dim[i_count] * RPL[sequence[i_count + 1] - 1] * vrelin / hopobj_mu;
double beta_rot = 2 * asin(1 / e); // velocity rotation
double ix[3];
vector_normalize(v_rel_in, ix);
double vpervnorm[3];
vector_normalize(v[i_count+1], vpervnorm);
double iy[3];
vett(ix, vpervnorm, iy);
vector_normalize(iy, iy); // [MR]this *might* not work properly...
double iz[3];
vett(ix, iy, iz);
double v_rel_in_norm = norm2(v_rel_in);
double v_sc_pl_out[3]; // TODO: document me!
for (i = 0; i < 3; i++)
{
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];
double v_rel_out = v_rel_in_norm * iVout;
v_sc_pl_out[i] = v[i_count + 1][i] + v_rel_out;
}
// Computing S/C position and absolute incoming velocity at DSMi
double rd[3], v_sc_dsm_in[3];
propagateKEP(r[i_count + 1], v_sc_pl_out, alpha[i_count+1] * tof[i_count+1] * 86400, MU[0],
rd, v_sc_dsm_in); // [MR] last two are output
// Evaluating the Lambert arc from DSMi to Pi+1
double Dum_Vec[3]; // [MR] Rename it to something sensible...
vett(rd, r[i_count + 2], Dum_Vec);
int lw = (Dum_Vec[2] > 0) ? 0 : 1;
double a, p, theta;
int iter_unused; // [MR] unused variable
double v_sc_dsm_out[3]; // DSM output speed
LambertI(rd, r[i_count + 2], tof[i_count + 1] * (1 - alpha[i_count + 1]) * 86400, MU[0], lw,
v_sc_dsm_out, v_sc_nextpl_in, a, p, theta, iter_unused); // [MR] last 6 are output.
// DV contribution
for (i = 0; i < 3; i++) {
Dum_Vec[i] = v_sc_dsm_out[i] - v_sc_dsm_in[i]; // [MR] Temporary variable reused. Dirty.
}
DV[i_count + 1] = norm2(Dum_Vec);
}
// FINAL BLOCK
//
void final_block(const mgadsmproblem& problem, const std::vector& v, const double v_sc_pl_in[], std::vector& DV)
{
//[MR] A bunch of helper variables to simplify the code
const int n = problem.sequence.size();
const double rp_target = problem.rp;
const double e_target = problem.e;
const vector& sequence = problem.sequence;
int i; //loop counter
// Evaluation of the arrival DV
double Dum_Vec[3];
for (i = 0; i < 3; i++) {
Dum_Vec[i] = v[n-1][i] - v_sc_pl_in[i];
}
double DVrel, DVarr;
DVrel = norm2(Dum_Vec); // Relative velocity at target planet
if ((problem.type == orbit_insertion) || (problem.type == total_DV_orbit_insertion)) {
double DVper = sqrt(DVrel * DVrel + 2 * MU[sequence[n - 1]] / rp_target); //[MR] should MU be changed to get_... ?
double DVper2 = sqrt(2 * MU[sequence[n - 1]] / rp_target - MU[sequence[n - 1]] / rp_target * (1 - e_target));
DVarr = fabs(DVper - DVper2);
} else if (problem.type == rndv){
DVarr = DVrel;
} else if (problem.type == total_DV_rndv){
DVarr = DVrel;
} else {
DVarr = 0.0; // no DVarr is considered
}
DV[n - 1] = DVarr;
}
int MGA_DSM(
/* INPUT values: */ //[MR] make this parameters const, if they are not modified and possibly references (especially 'problem').
vector t, // it is the vector which provides time in modified julian date 2000. [MR] ??? Isn't it the decision vetor ???
mgadsmproblem& problem,
/* OUTPUT values: */
double &J , double &launch_dv, double &flight_dv, double &arrival_dv )
{
//[MR] A bunch of helper variables to simplify the code
const int n = problem.sequence.size();
int i; //loop counter
//References to objects pre-allocated in the mgadsm struct
std::vector& r = problem.r;
std::vector& v = problem.v;
std::vector& DV = problem.DV; //DV contributions
precalculate_ers_and_vees(t, problem, r, v);
double inter_pl_in_v[3], inter_pl_out_v[3]; //inter-hop velocities
// FIRST BLOCK
first_block(t, problem, r, v,
DV, inter_pl_out_v); // [MR] output
// INTERMEDIATE BLOCK
for (int i_count=0; i_count < n - 2; i_count++) {
//copy previous output velocity to current input velocity
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];
intermediate_block(t, problem, r, v, i_count, inter_pl_in_v,
DV, inter_pl_out_v);
}
//copy previous output velocity to current input velocity
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];
// FINAL BLOCK
final_block(problem, v, inter_pl_in_v,
DV);
// **************************************************************************
// Evaluation of total DV spent by the propulsion system
// **************************************************************************
double DVtot = 0.0;
for (i = 0; i < n; i++) {
DVtot += DV[i];
}
// %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
//[MR] Calculation of the actual procedure output (DVvec and J)
const double& VINF = t[1]; // Variable renaming: Hyperbolic escape velocity (km/sec)
for (i = n; i > 0; i--) {
DV[i] = DV[i - 1];
}
DV[0] = VINF;
// Finally our objective function (J) is:
if (problem.type == orbit_insertion) {
J = DVtot;
} else if (problem.type == total_DV_orbit_insertion) {
J = DVtot + VINF;
} else if (problem.type == rndv) {
J = DVtot;
} else if (problem.type == total_DV_rndv) {
J = DVtot + VINF;
} else if (problem.type == time2AUs) { // [MR] TODO: extract method
// [MR] helper constants
const vector& sequence = problem.sequence;
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)
const double *gamma = &t[3*n]; // rotation of the bplane-component of the swingby outgoing
const double AUdist = problem.AUdist;
const double DVtotal = problem.DVtotal;
const double DVonboard = problem.DVonboard;
const double *tof = &t[4];
// non dimensional units
const double AU = 149597870.66;
const double V = sqrt(MU[0] / AU);
const double T = AU / V;
//evaluate the state of the spacecraft after the last fly-by
double vrelin = 0.0;
double v_rel_in[3];
for (i=0; i<3; i++)
{
v_rel_in[i] = inter_pl_in_v[i] - v[n-1][i];
vrelin += v_rel_in[i] * v_rel_in[i];
}
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
double beta_rot=2*asin(1/e); // velocity rotation
double vrelinn = norm2(v_rel_in);
double ix[3];
for (i=0; i<3; i++)
ix[i] = v_rel_in[i]/vrelinn;
// ix=r_rel_in/norm(v_rel_in); // activating this line and disactivating the one above
// shifts the singularity for r_rel_in parallel to v_rel_in
double vnorm = norm2(v[n-1]);
double vtemp[3];
for (i=0; i<3; i++)
vtemp[i] = v[n-1][i]/vnorm;
double iy[3];
vett(ix, vtemp, iy);
double iynorm = norm2(iy);
for (i=0; i<3; i++)
iy[i] = iy[i]/iynorm;
double iz[3];
vett(ix, iy, iz);
double v_rel_in_norm = norm2(v_rel_in);
double v_sc_pl_out[3]; // TODO: document me!
for (i = 0; i < 3; i++)
{
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];
double v_rel_out = v_rel_in_norm * iVout;
v_sc_pl_out[i] = v[n - 1][i] + v_rel_out;
}
double r_per_AU[3];
double v_sc_pl_out_per_V[3];
for (i = 0; i < 3; i++)
{
r_per_AU[i] = r[n - 1][i] / AU;
v_sc_pl_out_per_V[i] = v_sc_pl_out[i] / V;
}
double time = time2distance(r_per_AU, v_sc_pl_out_per_V, AUdist);
// if (time == -1) cout << " ERROR" << endl;
if (time != -1)
{
double DVpen=0;
double sum = 0.0;
for (i=0; i DVtotal)
//DVpen += DVpen+(sum-DVtotal);
sum = 0.0;
for (i=1; i DVonboard)
//DVpen = DVpen + (sum - DVonboard);
sum = 0.0;
for (i=0; i
//The following data refer to the Launcher Atlas501 performances as given by NASA
static const double x_atl[9] = {2.5,3,3.5,4,4.5,5,5.5,5.75,6};
static const double y_atl[13] = {-40, -30,-29,-28.5, -20, -10, 0, 10, 20,28.5,29,30, 40};
static const double data_atl[13][9] = {
{0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0},
{1160,1100,1010,930,830,740,630,590,550},
{2335.0 ,2195.0,2035.0,1865.0,1675.0,1480.0,1275.0,1175.0,1075.0},
{2335.0 ,2195.0,2035.0,1865.0,1675.0,1480.0,1275.0,1175.0,1075.0},
{2335.0 ,2195.0,2035.0,1865.0,1675.0,1480.0,1275.0,1175.0,1075.0},
{2335.0 ,2195.0,2035.0,1865.0,1675.0,1480.0,1275.0,1175.0,1075.0},
{2335.0 ,2195.0,2035.0,1865.0,1675.0,1480.0,1275.0,1175.0,1075.0},
{2335.0 ,2195.0,2035.0,1865.0,1675.0,1480.0,1275.0,1175.0,1075.0},
{2335.0 ,2195.0,2035.0,1865.0,1675.0,1480.0,1275.0,1175.0,1075.0},
{1160,1100,1010,930,830,740,630,590,550},
{0,0,0,0,0,0,0,0,0},
{0,0,0,0,0,0,0,0,0}
};
//The following data refer to the Launcher Soyuz-Fregat performances as given by ESOC. The data here and consider
// an elaborated five impulse strategy to exploit the launcher performances as much as possible.
static const double x_sf[5] = {1,2,3,4,5};
static const double y_sf[15] = {-90, -65, -50, -40, -30, -20, -10, 0, 10, 20, 30, 40, 50, 65, 90};
static const double data_sf[15][5] = {
{0.00000,0.00000,0.00000,0.00000,0.00000},
{100.00000,100.00000,100.00000,100.00000,100.00000},
{1830.50000,1815.90000,1737.70000,1588.00000,1344.30000},
{1910.80000,1901.90000,1819.00000,1636.40000,1369.30000},
{2001.80000,1995.30000,1891.30000,1673.90000,1391.90000},
{2108.80000,2088.60000,1947.90000,1708.00000,1409.50000},
{2204.00000,2167.30000,1995.50000,1734.50000,1419.60000},
{2270.80000,2205.80000,2013.60000,1745.10000,1435.20000},
{2204.70000,2133.60000,1965.40000,1712.80000,1413.60000},
{2087.90000,2060.60000,1917.70000,1681.10000,1392.50000},
{1979.17000,1975.40000,1866.50000,1649.00000,1371.70000},
{1886.90000,1882.20000,1801.00000,1614.60000,1350.50000},
{1805.90000,1796.00000,1722.70000,1571.60000,1327.60000},
{100.00000,100.00000,100.00000,100.00000,100.00000},
{0.00000,0.00000,0.00000,0.00000,0.00000}
};
int xant(const double &x) {
int i;
for(i=1; i<4; i++) {
if (x_sf[i]>x) break;
}
return i-1;
}
int yant(const double &y) {
int i;
for(i=1; i<14; i++) {
if (y_sf[i]>y) break;
}
return i-1;
}
int xantA5(const double &x) {
int i;
for(i=1; i<8; i++) {
if (x_atl[i]>x) break;
}
return i-1;
}
int yantA5(const double &y) {
int i;
for(i=1; i<12; i++) {
if (y_atl[i]>y) break;
}
return i-1;
}
double interp2SF(const double &VINF, const double &declination) {
double retval;
int v_indx=xant(VINF);
int dec_indx=yant(declination);
if (fabs(declination)>=90) return 0;
if ((VINF<1)||(VINF>5)) return 0;
double dx = x_sf[v_indx+1]-x_sf[v_indx];
double dydx = dx * (y_sf[dec_indx+1]-y_sf[dec_indx]);
retval = data_sf[dec_indx][v_indx] / dydx * (x_sf[v_indx+1]-VINF) * (y_sf[dec_indx+1]-declination);
retval += data_sf[dec_indx][v_indx+1] / dydx * (VINF - x_sf[v_indx]) * (y_sf[dec_indx+1]-declination);
retval += data_sf[dec_indx+1][v_indx] / dydx * (x_sf[v_indx+1]-VINF) * (declination - y_sf[dec_indx]);
retval += data_sf[dec_indx+1][v_indx+1] / dydx * (VINF - x_sf[v_indx]) * (declination - y_sf[dec_indx]);
return retval;
}
double interp2A5(const double &VINF, const double &declination) {
double retval;
int v_indx=xantA5(VINF);
int dec_indx=yantA5(declination);
if ((VINF<2.5)||(VINF>6)) return 0;
if (fabs(declination)>40) return 0;
double dx = x_atl[v_indx+1]-x_atl[v_indx];
double dydx = dx * (y_atl[dec_indx+1]-y_atl[dec_indx]);
retval = data_atl[dec_indx][v_indx] / dydx * (x_atl[v_indx+1]-VINF) * (y_atl[dec_indx+1]-declination);
retval += data_atl[dec_indx][v_indx+1] / dydx * (VINF - x_atl[v_indx]) * (y_atl[dec_indx+1]-declination);
retval += data_atl[dec_indx+1][v_indx] / dydx * (x_atl[v_indx+1]-VINF) * (declination - y_atl[dec_indx]);
retval += data_atl[dec_indx+1][v_indx+1] / dydx * (VINF - x_atl[v_indx]) * (declination - y_atl[dec_indx]);
return retval;
}
double SoyuzFregat (const double &VINF, const double &declination) {
//This function returns the mass that a Soyuz-Fregat launcher can inject
//into a given escape velocity and asymptote declination. The data here
//refer to ESOC WP-521 and consider an elaborated five impulse strategy to
//exploit the launcher performances as much as possible.
return interp2SF(VINF,declination);
}
double Atlas501 (const double &VINF, const double &declination) {
//This function returns the mass that a Atlas501 Launcher
return interp2A5(VINF,declination);
}
void ecl2equ (double (&ecl)[3],double (&equ)[3]){
static const double incl=0.409072975;
double temp[3];
temp[0]=ecl[0];
temp[1]=ecl[1];
temp[2]=ecl[2];
equ[0]=temp[0];
equ[1]=temp[1]*cos(incl)-temp[2]*sin(incl);
equ[2]=temp[1]*sin(incl)+temp[2]*cos(incl);
}
// ------------------------------------------------------------------------ //
// This source file is part of the 'ESA Advanced Concepts Team's //
// Space Mechanics Toolbox' software. //
// //
// The source files are for research use only, //
// and are distributed WITHOUT ANY WARRANTY. Use them on your own risk. //
// //
// Copyright (c) 2004-2007 European Space Agency //
// ------------------------------------------------------------------------ //
#include
#include
// #include "Pl_Eph_An.h"
// #include "Astro_Functions.h"
void Planet_Ephemerides_Analytical (const double mjd2000,
const int planet,
double *position,
double *velocity)
{
const double pi = acos(-1.0);
const double RAD = pi/180.0;
const double AU = 149597870.66; // Astronomical Unit
const double KM = AU;
//const double MuSun = 1.32712440018e+11; //Gravitational constant of Sun);
const double MuSun = 1.327124280000000e+011; //Gravitational constant of Sun);
double Kepl_Par[6];
double XM;
double T =(mjd2000 + 36525.00)/36525.00;
switch (planet)
{
case(1):// Mercury
Kepl_Par[0]=(0.38709860);
Kepl_Par[1]=(0.205614210 + 0.000020460*T - 0.000000030*T*T);
Kepl_Par[2]=(7.002880555555555560 + 1.86083333333333333e-3*T - 1.83333333333333333e-5*T*T);
Kepl_Par[3]=(4.71459444444444444e+1 + 1.185208333333333330*T + 1.73888888888888889e-4*T*T);
Kepl_Par[4]=(2.87537527777777778e+1 + 3.70280555555555556e-1*T +1.20833333333333333e-4*T*T);
XM = 1.49472515288888889e+5 + 6.38888888888888889e-6*T;
Kepl_Par[5]=(1.02279380555555556e2 + XM*T);
break;
case(2):// Venus
Kepl_Par[0]=(0.72333160);
Kepl_Par[1]=(0.006820690 - 0.000047740*T + 0.0000000910*T*T);
Kepl_Par[2]=(3.393630555555555560 + 1.00583333333333333e-3*T - 9.72222222222222222e-7*T*T);
Kepl_Par[3]=(7.57796472222222222e+1 + 8.9985e-1*T + 4.1e-4*T*T);
Kepl_Par[4]=(5.43841861111111111e+1 + 5.08186111111111111e-1*T -1.38638888888888889e-3*T*T);
XM =5.8517803875e+4 + 1.28605555555555556e-3*T;
Kepl_Par[5]=(2.12603219444444444e2 + XM*T);
break;
case(3):// Earth
Kepl_Par[0]=(1.000000230);
Kepl_Par[1]=(0.016751040 - 0.000041800*T - 0.0000001260*T*T);
Kepl_Par[2]=(0.00);
Kepl_Par[3]=(0.00);
Kepl_Par[4]=(1.01220833333333333e+2 + 1.7191750*T + 4.52777777777777778e-4*T*T + 3.33333333333333333e-6*T*T*T);
XM = 3.599904975e+4 - 1.50277777777777778e-4*T - 3.33333333333333333e-6*T*T;
Kepl_Par[5]=(3.58475844444444444e2 + XM*T);
break;
case(4):// Mars
Kepl_Par[0]=(1.5236883990);
Kepl_Par[1]=(0.093312900 + 0.0000920640*T - 0.0000000770*T*T);
Kepl_Par[2]=(1.850333333333333330 - 6.75e-4*T + 1.26111111111111111e-5*T*T);
Kepl_Par[3]=(4.87864416666666667e+1 + 7.70991666666666667e-1*T - 1.38888888888888889e-6*T*T - 5.33333333333333333e-6*T*T*T);
Kepl_Par[4]=(2.85431761111111111e+2 + 1.069766666666666670*T + 1.3125e-4*T*T + 4.13888888888888889e-6*T*T*T);
XM = 1.91398585e+4 + 1.80805555555555556e-4*T + 1.19444444444444444e-6*T*T;
Kepl_Par[5]=(3.19529425e2 + XM*T);
break;
case(5):// Jupiter
Kepl_Par[0]=(5.2025610);
Kepl_Par[1]=(0.048334750 + 0.000164180*T - 0.00000046760*T*T -0.00000000170*T*T*T);
Kepl_Par[2]=(1.308736111111111110 - 5.69611111111111111e-3*T + 3.88888888888888889e-6*T*T);
Kepl_Par[3]=(9.94433861111111111e+1 + 1.010530*T + 3.52222222222222222e-4*T*T - 8.51111111111111111e-6*T*T*T);
Kepl_Par[4]=(2.73277541666666667e+2 + 5.99431666666666667e-1*T + 7.0405e-4*T*T + 5.07777777777777778e-6*T*T*T);
XM = 3.03469202388888889e+3 - 7.21588888888888889e-4*T + 1.78444444444444444e-6*T*T;
Kepl_Par[5]=(2.25328327777777778e2 + XM*T);
break;
case(6):// Saturn
Kepl_Par[0]=(9.5547470);
Kepl_Par[1]=(0.055892320 - 0.00034550*T - 0.0000007280*T*T + 0.000000000740*T*T*T);
Kepl_Par[2]=(2.492519444444444440 - 3.91888888888888889e-3*T - 1.54888888888888889e-5*T*T + 4.44444444444444444e-8*T*T*T);
Kepl_Par[3]=(1.12790388888888889e+2 + 8.73195138888888889e-1*T -1.52180555555555556e-4*T*T - 5.30555555555555556e-6*T*T*T);
Kepl_Par[4]=(3.38307772222222222e+2 + 1.085220694444444440*T + 9.78541666666666667e-4*T*T + 9.91666666666666667e-6*T*T*T);
XM = 1.22155146777777778e+3 - 5.01819444444444444e-4*T - 5.19444444444444444e-6*T*T;
Kepl_Par[5]=(1.75466216666666667e2 + XM*T);
break;
case(7):// Uranus
Kepl_Par[0]=(19.218140);
Kepl_Par[1]=(0.04634440 - 0.000026580*T + 0.0000000770*T*T);
Kepl_Par[2]=(7.72463888888888889e-1 + 6.25277777777777778e-4*T + 3.95e-5*T*T);
Kepl_Par[3]=(7.34770972222222222e+1 + 4.98667777777777778e-1*T + 1.31166666666666667e-3*T*T);
Kepl_Par[4]=(9.80715527777777778e+1 + 9.85765e-1*T - 1.07447222222222222e-3*T*T - 6.05555555555555556e-7*T*T*T);
XM = 4.28379113055555556e+2 + 7.88444444444444444e-5*T + 1.11111111111111111e-9*T*T;
Kepl_Par[5]=(7.26488194444444444e1 + XM*T);
break;
case(8)://Neptune
Kepl_Par[0]=(30.109570);
Kepl_Par[1]=(0.008997040 + 0.0000063300*T - 0.0000000020*T*T);
Kepl_Par[2]=(1.779241666666666670 - 9.54361111111111111e-3*T - 9.11111111111111111e-6*T*T);
Kepl_Par[3]=(1.30681358333333333e+2 + 1.0989350*T + 2.49866666666666667e-4*T*T - 4.71777777777777778e-6*T*T*T);
Kepl_Par[4]=(2.76045966666666667e+2 + 3.25639444444444444e-1*T + 1.4095e-4*T*T + 4.11333333333333333e-6*T*T*T);
XM = 2.18461339722222222e+2 - 7.03333333333333333e-5*T;
Kepl_Par[5]=(3.77306694444444444e1 + XM*T);
break;
case(9):// Pluto
//Fifth order polynomial least square fit generated by Dario Izzo
//(ESA ACT). JPL405 ephemerides (Charon-Pluto barycenter) have been used to produce the coefficients.
//This approximation should not be used outside the range 2000-2100;
T =mjd2000/36525.00;
Kepl_Par[0]=(39.34041961252520 + 4.33305138120726*T - 22.93749932403733*T*T + 48.76336720791873*T*T*T - 45.52494862462379*T*T*T*T + 15.55134951783384*T*T*T*T*T);
Kepl_Par[1]=(0.24617365396517 + 0.09198001742190*T - 0.57262288991447*T*T + 1.39163022881098*T*T*T - 1.46948451587683*T*T*T*T + 0.56164158721620*T*T*T*T*T);
Kepl_Par[2]=(17.16690003784702 - 0.49770248790479*T + 2.73751901890829*T*T - 6.26973695197547*T*T*T + 6.36276927397430*T*T*T*T - 2.37006911673031*T*T*T*T*T);
Kepl_Par[3]=(110.222019291707 + 1.551579150048*T - 9.701771291171*T*T + 25.730756810615*T*T*T - 30.140401383522*T*T*T*T + 12.796598193159 * T*T*T*T*T);
Kepl_Par[4]=(113.368933916592 + 9.436835192183*T - 35.762300003726*T*T + 48.966118351549*T*T*T - 19.384576636609*T*T*T*T - 3.362714022614 * T*T*T*T*T);
Kepl_Par[5]=(15.17008631634665 + 137.023166578486*T + 28.362805871736*T*T - 29.677368415909*T*T*T - 3.585159909117*T*T*T*T + 13.406844652829 * T*T*T*T*T);
break;
}
// conversion of AU into KM
Kepl_Par[0] *= KM;
// conversion of DEG into RAD
Kepl_Par[2] *= RAD;
Kepl_Par[3] *= RAD;
Kepl_Par[4] *= RAD;
Kepl_Par[5] *= RAD;
Kepl_Par[5] = fmod(Kepl_Par[5], 2.0*pi);
// Conversion from Mean Anomaly to Eccentric Anomaly via Kepler's equation
Kepl_Par[5] = Mean2Eccentric(Kepl_Par[5], Kepl_Par[1]);
// Position and Velocity evaluation according to j2000 system
Conversion(Kepl_Par, position, velocity, MuSun);
}
void Custom_Eph(const double jd,
const double epoch,
const double keplerian[],
double *position,
double *velocity)
{
const double pi = acos(-1.0);
const double RAD = pi/180.0;
const double AU = 149597870.66; // Astronomical Unit
const double muSUN = 1.32712428e+11; // Gravitational constant of Sun
double a,e,i,W,w,M,jdepoch,DT,n,E;
double V[6];
a=keplerian[0]*AU; // in km
e=keplerian[1];
i=keplerian[2];
W=keplerian[3];
w=keplerian[4];
M=keplerian[5];
jdepoch = epoch +2400000.5;
DT=(jd-jdepoch)*86400;
n=sqrt(muSUN/pow(a,3));
M=M/180.0*pi;
M+=n*DT;
M=fmod(M,2*pi);
E=Mean2Eccentric(M,e);
V[0] = a; V[1] = e; V[2] = i*RAD;
V[3] = W*RAD; V[4] = w*RAD; V[5] = E;
Conversion(V,position,velocity,muSUN);
}
// ------------------------------------------------------------------------ //
// This source file is part of the 'ESA Advanced Concepts Team's //
// Space Mechanics Toolbox' software. //
// //
// The source files are for research use only, //
// and are distributed WITHOUT ANY WARRANTY. Use them on your own risk. //
// //
// Copyright (c) 2004-2007 European Space Agency //
// ------------------------------------------------------------------------ //
// #include "PowSwingByInv.h"
#include
void PowSwingByInv (const double Vin,const double Vout,const double alpha,
double &DV,double &rp)
{
const int maxiter = 30;
int i = 0;
double err = 1.0;
double f,df; // function and its derivative
double rp_new;
const double tolerance = 1e-8;
double aIN = 1.0/pow(Vin,2); // semimajor axis of the incoming hyperbola
double aOUT = 1.0/pow(Vout,2); // semimajor axis of the incoming hyperbola
rp = 1.0;
while ((err > tolerance)&&(i < maxiter))
{
i++;
f = asin(aIN/(aIN + rp)) + asin(aOUT/(aOUT + rp)) - alpha;
df = -aIN/sqrt((rp + 2 * aIN) * rp)/(aIN+rp) -
aOUT/sqrt((rp + 2 * aOUT) * rp)/(aOUT+rp);
rp_new = rp - f/df;
if (rp_new > 0)
{
err = fabs(rp_new - rp);
rp = rp_new;
}
else
rp /= 2.0;
}
// Evaluation of the DV
DV = fabs (sqrt(Vout*Vout + (2.0/rp)) - sqrt(Vin*Vin + (2.0/rp)));
}
// ------------------------------------------------------------------------ //
// This source file is part of the 'ESA Advanced Concepts Team's //
// Space Mechanics Toolbox' software. //
// //
// The source files are for research use only, //
// and are distributed WITHOUT ANY WARRANTY. Use them on your own risk. //
// //
// Copyright (c) 2004-2007 European Space Agency //
// ------------------------------------------------------------------------ //
//
// File: propageteKEP.cpp
//
// #include "propagateKEP.h"
/*
Origin: MATLAB code programmed by Dario Izzo (ESA/ACT)
C++ version by Tamas Vinko (ESA/ACT)
Inputs:
r0: column vector for the non dimensional position
v0: column vector for the non dimensional velocity
t: non dimensional time
Outputs:
r: column vector for the non dimensional position
v: column vector for the non dimensional velocity
Comments: The function works in non dimensional units, it takes an
initial condition and it propagates it as in a kepler motion analytically.
*/
void propagateKEP(const double *r0_in, const double *v0_in, double t, double mu,
double *r, double *v)
{
/*
The matrix DD will be almost always the unit matrix, except for orbits
with little inclination in which cases a rotation is performed so that
par2IC is always defined
*/
double DD[9] = {1, 0, 0,
0, 1, 0,
0, 0, 1};
double h[3];
double ih[3] = {0,0,0};
double temp1[3] = {0,0,0}, temp2[3] = {0,0,0};
double E[6];
double normh, M, M0;
double r0[3], v0[3];
int i;
for (i=0; i<3; i++)
{
r0[i] = r0_in[i];
v0[i] = v0_in[i];
}
vett(r0, v0, h);
normh=norm2(h);
for (i=0; i<3; i++)
ih[i] = h[i]/normh;
if (fabs(fabs(ih[2])-1.0) < 1e-3) // the abs is needed in cases in which the orbit is retrograde,
{ // that would held ih=[0,0,-1]!!
DD[0] = 1; DD[1] = 0; DD[2] = 0;
DD[3] = 0; DD[4] = 0; DD[5] = 1;
DD[6] = 0; DD[7] = -1; DD[8] = 0;
// Random rotation matrix that make the Euler angles well defined for the case
// For orbits with little inclination another ref. frame is used.
for ( i=0; i<3; i++)
{
temp1[0] += DD[i]*r0[i];
temp1[1] += DD[i+3]*r0[i];
temp1[2] += DD[i+6]*r0[i];
temp2[0] += DD[i]*v0[i];
temp2[1] += DD[i+3]*v0[i];
temp2[2] += DD[i+6]*v0[i];
}
for ( i=0; i<3; i++)
{
r0[i] = temp1[i];
temp1[i] = 0.0;
v0[i] = temp2[i];
temp2[i] = 0.0;
}
// for practical reason we take the transpose of the matrix DD here (will be used at the end of the function)
DD[0] = 1; DD[1] = 0; DD[2] = 0;
DD[3] = 0; DD[4] = 0; DD[5] = -1;
DD[6] = 0; DD[7] = 1; DD[8] = 0;
}
IC2par(r0, v0, mu, E);
if (E[1] < 1.0)
{
M0 = E[5] - E[1]*sin(E[5]);
M=M0+sqrt(mu/pow(E[0],3))*t;
}
else
{
M0 = E[1]*tan(E[5]) - log(tan(0.5*E[5] + 0.25*M_PI));
M=M0+sqrt(mu/pow(-E[0],3))*t;
}
E[5]=Mean2Eccentric(M, E[1]);
par2IC(E, mu, r, v);
for (int j=0; j<3; j++)
{
temp1[0] += DD[j]*r[j];
temp1[1] += DD[j+3]*r[j];
temp1[2] += DD[j+6]*r[j];
temp2[0] += DD[j]*v[j];
temp2[1] += DD[j+3]*v[j];
temp2[2] += DD[j+6]*v[j];
}
for ( i=0; i<3; i++)
{
r[i] = temp1[i];
v[i] = temp2[i];
}
return;
}
/*
Origin: MATLAB code programmed by Dario Izzo (ESA/ACT)
C++ version by Tamas Vinko (ESA/ACT) 12/09/2006
Inputs:
r0: column vector for the position
v0: column vector for the velocity
Outputs:
E: Column Vectors containing the six keplerian parameters,
(a,e,i,OM,om,Eccentric Anomaly (or Gudermannian whenever e>1))
Comments: The parameters returned are, of course, referred to the same
ref. frame in which r0,v0 are given. Units have to be consistent, and
output angles are in radians
The algorithm used is quite common and can be found as an example in Bate,
Mueller and White. It goes singular for zero inclination
*/
void IC2par(const double *r0, const double *v0, double mu, double *E)
{
double k[3];
double h[3];
double Dum_Vec[3];
double n[3];
double evett[3];
double p = 0.0;
double temp =0.0;
double R0, ni;
int i;
vett(r0, v0, h);
for (i=0; i<3; i++)
p += h[i]*h[i];
p/=mu;
k[0] = 0; k[1] = 0; k[2] = 1;
vett(k, h, n);
for (i=0; i<3; i++)
temp += pow(n[i], 2);
temp = sqrt(temp);
for (i=0; i<3; i++)
n[i] /= temp;
R0 = norm2(r0);
vett(v0, h, Dum_Vec);
for (i=0; i<3; i++)
evett[i] = Dum_Vec[i]/mu - r0[i]/R0;
double e = 0.0;
for (i=0; i<3; i++)
e += pow(evett[i], 2);
E[0] = p/(1-e);
E[1] = sqrt(e);
e = E[1];
E[2] = acos(h[2]/norm2(h));
temp = 0.0;
for (i=0; i<3; i++)
temp+=n[i]*evett[i];
E[4] = acos(temp/e);
if (evett[2] < 0) E[4] = 2*M_PI - E[4];
E[3] = acos(n[0]);
if (n[1] < 0) E[3] = 2*M_PI-E[3];
temp = 0.0;
for (i=0; i<3; i++)
temp+=evett[i]*r0[i];
ni = acos(temp/e/R0); // danger, the argument could be improper.
temp = 0.0;
for (i=0; i<3; i++)
temp+=r0[i]*v0[i];
if (temp<0.0) ni = 2*M_PI - ni;
if (e<1.0)
E[5] = 2.0*atan(sqrt((1-e)/(1+e))*tan(ni/2.0)); // algebraic kepler's equation
else
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
}
/*
Origin: MATLAB code programmed by Dario Izzo (ESA/ACT)
C++ version by Tamas Vinko (ESA/ACT)
Usage: [r0,v0] = IC2par(E,mu)
Outputs:
r0: column vector for the position
v0: column vector for the velocity
Inputs:
E: Column Vectors containing the six keplerian parameters,
(a (negative for hyperbolas),e,i,OM,om,Eccentric Anomaly)
mu: gravitational constant
Comments: The parameters returned are, of course, referred to the same
ref. frame in which r0,v0 are given. a can be given either in kms or AUs,
but has to be consistent with mu.All the angles must be given in radians.
This function does work for hyperbolas as well.
*/
void par2IC(const double *E, double mu, double *r0, double *v0)
{
double a = E[0];
double e = E[1];
double i = E[2];
double omg = E[3];
double omp = E[4];
double EA = E[5];
double b, n, xper, yper, xdotper, ydotper;
double R[3][3];
double cosomg, cosomp, sinomg, sinomp, cosi, sini;
double dNdZeta;
// Grandezze definite nel piano dell'orbita
if (e<1.0)
{
b = a*sqrt(1-e*e);
n = sqrt(mu/(a*a*a));
xper=a*(cos(EA)-e);
yper=b*sin(EA);
xdotper = -(a*n*sin(EA))/(1-e*cos(EA));
ydotper=(b*n*cos(EA))/(1-e*cos(EA));
}
else
{
b = -a*sqrt(e*e-1);
n = sqrt(-mu/(a*a*a));
dNdZeta = e * (1+tan(EA)*tan(EA))-(0.5+0.5*pow(tan(0.5*EA + 0.25*M_PI),2))/tan(0.5*EA+ 0.25*M_PI);
xper = a/cos(EA) - a*e;
yper = b*tan(EA);
xdotper = a*tan(EA)/cos(EA)*n/dNdZeta;
ydotper = b/pow(cos(EA), 2)*n/dNdZeta;
}
// Matrice di trasformazione da perifocale a ECI
cosomg = cos(omg);
cosomp = cos(omp);
sinomg = sin(omg);
sinomp = sin(omp);
cosi = cos(i);
sini = sin(i);
R[0][0]=cosomg*cosomp-sinomg*sinomp*cosi;
R[0][1]=-cosomg*sinomp-sinomg*cosomp*cosi;
R[0][2]=sinomg*sini;
R[1][0]=sinomg*cosomp+cosomg*sinomp*cosi;
R[1][1]=-sinomg*sinomp+cosomg*cosomp*cosi;
R[1][2]=-cosomg*sini;
R[2][0]=sinomp*sini;
R[2][1]=cosomp*sini;
R[2][2]=cosi;
// Posizione nel sistema inerziale
double temp[3] = {xper, yper, 0.0};
double temp2[3] = {xdotper, ydotper, 0};
for (int j = 0; j<3; j++)
{
r0[j] = 0.0; v0[j] = 0.0;
for (int k = 0; k<3; k++)
{
r0[j]+=R[j][k]*temp[k];
v0[j]+=R[j][k]*temp2[k];
}
}
return;
}
// Returns the cross product of the vectors X and Y.
// That is, z = X x Y. X and Y must be 3 element
// vectors.
void cross(const double *x, const double *y, double *z)
{
z[0] = x[1]*y[2] - x[2]*y[1];
z[1] = x[2]*y[0] - x[0]*y[2];
z[2] = x[0]*y[1] - x[1]*y[0];
}
// ------------------------------------------------------------------------ //
// This source file is part of the 'ESA Advanced Concepts Team's //
// Space Mechanics Toolbox' software. //
// //
// The source files are for research use only, //
// and are distributed WITHOUT ANY WARRANTY. Use them on your own risk. //
// //
// Copyright (c) 2004-2007 European Space Agency //
// ------------------------------------------------------------------------ //
/*
%Inputs:
% r0: column vector for the position (mu=1)
% v0: column vector for the velocity (mu=1)
% rtarget: distance to be reached
%
%Outputs:
% t: time taken to reach a given distance
%
%Comments: everything works in non dimensional units
*/
// #include "time2distance.h"
#include
double time2distance(const double *r0, const double *v0, double rtarget)
{
double temp = 0.0;
double E[6];
double r0norm = norm2(r0);
double a, e, E0, p, ni, Et;
int i;
if (r0norm < rtarget)
{
for (i=0; i<3; i++)
temp += r0[i]*v0[i];
IC2par(r0,v0,1,E);
a = E[0];
e = E[1];
E0 = E[5];
p = a * (1-e*e);
// If the solution is an ellipse
if (e<1)
{
double ra = a * (1+e);
if (rtarget>ra)
return -1; // NaN;
else // we find the anomaly where the target distance is reached
{
ni = acos((p/rtarget-1)/e); //in 0-pi
Et = 2*atan(sqrt((1-e)/(1+e))*tan(ni/2)); // algebraic kepler's equation
if (temp>0)
return sqrt(pow(a,3))*(Et-e*sin(Et)-E0 + e*sin(E0));
else
{
E0 = -E0;
return sqrt(pow(a,3))*(Et-e*sin(Et)+E0 - e*sin(E0));
}
}
}
else // the solution is a hyperbolae
{
ni = acos((p/rtarget-1)/e); // in 0-pi
Et = 2*atan(sqrt((e-1)/(e+1))*tan(ni/2)); // algebraic equivalent of kepler's equation in terms of the Gudermannian
if (temp>0) // out==1
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)));
else
{
E0 = -E0;
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)));
}
}
}
else
return 12;
}
/*
* GOProblems.cpp
* SeGMO, a Sequential Global Multiobjective Optimiser
*
* Created by Dario Izzo on 5/17/08.
* Copyright 2008 ¿dvanced Concepts Team (European Space Agency). All rights reserved.
*
*/
#include
// #include "mga.h"
// #include "mga_dsm.h"
// #include "misc4Tandem.h"
using namespace std;
double gtoc1( const vector& x, std::vector& rp)
{
const int GTOC1_DIM = 8;
vector Delta_V(GTOC1_DIM);
rp.resize(GTOC1_DIM-2);
vector t(GTOC1_DIM);
mgaproblem problem;
//Filling up the problem parameters
problem.type = asteroid_impact;
problem.mass = 1500.0; // Satellite initial mass [Kg]
problem.Isp = 2500.0; // Satellite specific impulse [s]
problem.DVlaunch = 2.5; // Launcher DV in km/s
int sequence_[GTOC1_DIM] = {3,2,3,2,3,5,6,10}; // sequence of planets
vector sequence(GTOC1_DIM);
problem.sequence.insert(problem.sequence.begin(), sequence_, sequence_+GTOC1_DIM);
const int rev_[GTOC1_DIM] = {0,0,0,0,0,0,1,0}; // sequence of clockwise legs
vector rev(GTOC1_DIM);
problem.rev_flag.insert(problem.rev_flag.begin(), rev_, rev_+GTOC1_DIM);
problem.asteroid.keplerian[0] = 2.5897261; // Asteroid data
problem.asteroid.keplerian[1] = 0.2734625;
problem.asteroid.keplerian[2] = 6.40734;
problem.asteroid.keplerian[3] = 128.34711;
problem.asteroid.keplerian[4] = 264.78691;
problem.asteroid.keplerian[5] = 320.479555;
problem.asteroid.epoch = 53600;
double obj = 0;
double launch_dv = 0;
MGA(x,problem,rp,Delta_V,obj,launch_dv);
return obj;
}
double cassini1( const vector& x, std::vector& rp, double &launchDV)
{
const int CASSINI_DIM = 6;
vector Delta_V(CASSINI_DIM);
rp.resize(CASSINI_DIM-2);
vector t(CASSINI_DIM);
mgaproblem problem;
//Filling up the problem parameters
problem.type = total_DV_orbit_insertion;
int sequence_[CASSINI_DIM] = {3,2,2,3,5,6}; // sequence of planets
vector sequence(CASSINI_DIM);
problem.sequence.insert(problem.sequence.begin(), sequence_, sequence_+CASSINI_DIM);
const int rev_[CASSINI_DIM] = {0,0,0,0,0,0}; // sequence of clockwise legs
vector rev(CASSINI_DIM);
problem.rev_flag.insert(problem.rev_flag.begin(), rev_, rev_+CASSINI_DIM);
problem.e = 0.98; // Final orbit eccentricity
problem.rp = 108950; // Final orbit pericenter
problem.DVlaunch = 0; // Launcher DV
double obj = 0;
double launch_dv = 0;
MGA(x,problem,rp,Delta_V,obj,launch_dv);
launchDV = launch_dv;
return obj;
}
double cassini1minlp( const vector& x, std::vector& rp, double &launchDV)
{
const int CASSINI_DIM = 6;
vector Delta_V(CASSINI_DIM);
rp.resize(CASSINI_DIM-2);
vector t(CASSINI_DIM);
mgaproblem problem;
//Filling up the problem parameters
problem.type = total_DV_orbit_insertion;
int sequence_[CASSINI_DIM] = {3,(int) round(x[6]),(int) round(x[7]),(int) round(x[8]),(int) round(x[9]),6}; // sequence of planets
vector sequence(CASSINI_DIM);
problem.sequence.insert(problem.sequence.begin(), sequence_, sequence_+CASSINI_DIM);
const int rev_[CASSINI_DIM] = {0,0,0,0,0,0}; // sequence of clockwise legs
vector rev(CASSINI_DIM);
problem.rev_flag.insert(problem.rev_flag.begin(), rev_, rev_+CASSINI_DIM);
problem.e = 0.98; // Final orbit eccentricity
problem.rp = 108950; // Final orbit pericenter
problem.DVlaunch = 0; // Launcher DV
double obj = 0;
double launch_dv = 0;
MGA(x,problem,rp,Delta_V,obj,launch_dv);
launchDV = launch_dv;
return obj;
}
double messenger(const vector& x, double &launchDV, double &flightDV, double &arrivalDV){
mgadsmproblem problem;
int sequence_[5] = {3, 3, 2, 2, 1}; // sequence of planets
problem.sequence.insert(problem.sequence.begin(), sequence_, sequence_+ 5 );
problem.type = total_DV_rndv;
//Memory allocation
problem.r = std::vector(5);
problem.v = std::vector(5);
problem.DV = std::vector(5+1);
for(int i = 0; i < 5; i++) {
problem.r[i] = new double[3];
problem.v[i] = new double[3];
}
double obj = 0;
double launch_dv = 0;
double flight_dv = 0;
double arrival_dv = 0;
MGA_DSM(
/* INPUT values: */
x,
problem,
/* OUTPUT values: */
obj,launch_dv,flight_dv,arrival_dv);
launchDV = launch_dv;
flightDV = flight_dv;
arrivalDV = arrival_dv;
//Memory release
for(int i = 0; i < 5; i++) {
delete[] problem.r[i];
delete[] problem.v[i];
}
problem.r.clear();
problem.v.clear();
return obj;
}
double messengerfull(const vector& x, double &launchDV, double &flightDV, double &arrivalDV){
mgadsmproblem problem;
int sequence_[7] = {3, 2, 2, 1, 1, 1, 1};; // sequence of planets
problem.sequence.insert(problem.sequence.begin(), sequence_, sequence_+ 7 );
problem.type = orbit_insertion;
problem.e = 0.704;
problem.rp = 2640.0;
//Memory allocation
problem.r = std::vector(7);
problem.v = std::vector(7);
problem.DV = std::vector(7+1);
for(int i = 0; i < 7; i++) {
problem.r[i] = new double[3];
problem.v[i] = new double[3];
}
double obj = 0;
double launch_dv = 0;
double flight_dv = 0;
double arrival_dv = 0;
MGA_DSM(
/* INPUT values: */
x,
problem,
/* OUTPUT values: */
obj,launch_dv,flight_dv,arrival_dv);
launchDV = launch_dv;
flightDV = flight_dv;
arrivalDV = arrival_dv;
//Memory release
for(int i = 0; i < 7; i++) {
delete[] problem.r[i];
delete[] problem.v[i];
}
problem.r.clear();
problem.v.clear();
return obj;
}
double cassini2(const vector& x, double &launchDV, double &flightDV, double &arrivalDV){
mgadsmproblem problem;
int sequence_[6] = {3, 2, 2, 3, 5, 6}; // sequence of planets
problem.sequence.insert(problem.sequence.begin(), sequence_, sequence_+ 6 );
problem.type = total_DV_rndv;
double obj = 0;
double launch_dv = 0;
double flight_dv = 0;
double arrival_dv = 0;
//Allocate temporary memory for MGA_DSM
problem.r = std::vector(6);
problem.v = std::vector(6);
problem.DV = std::vector(6+1);
for(int i = 0; i < 6; i++) {
problem.r[i] = new double[3];
problem.v[i] = new double[3];
}
MGA_DSM(
/* INPUT values: */
x,
problem,
/* OUTPUT values: */
obj,launch_dv,flight_dv,arrival_dv);
launchDV = launch_dv;
flightDV = flight_dv;
arrivalDV = arrival_dv;
//Free temporary memory for MGA_DSM
for(int i = 0; i < 6; i++) {
delete[] problem.r[i];
delete[] problem.v[i];
}
problem.r.clear();
problem.v.clear();
return obj;
}
double rosetta(const vector& x, double &launchDV, double &flightDV, double &arrivalDV){
mgadsmproblem problem;
int sequence_[6] = {3, 3, 4, 3, 3, 10}; // sequence of planets
problem.sequence.insert(problem.sequence.begin(), sequence_, sequence_+ 6 );
problem.type = rndv;
problem.asteroid.keplerian[0] = 3.50294972836275;
problem.asteroid.keplerian[1] = 0.6319356;
problem.asteroid.keplerian[2] = 7.12723;
problem.asteroid.keplerian[3] = 50.92302;
problem.asteroid.keplerian[4] = 11.36788;
problem.asteroid.keplerian[5] = 0.0;
problem.asteroid.epoch = 52504.23754000012;
problem.asteroid.mu = 0.0;
//Allocate temporary memory for MGA_DSM
problem.r = std::vector(6);
problem.v = std::vector(6);
problem.DV = std::vector(6+1);
for(int i = 0; i < 6; i++) {
problem.r[i] = new double[3];
problem.v[i] = new double[3];
}
double obj = 0;
double launch_dv = 0;
double flight_dv = 0;
double arrival_dv = 0;
MGA_DSM(
/* INPUT values: */
x,
problem,
/* OUTPUT values: */
obj,launch_dv,flight_dv,arrival_dv);
launchDV = launch_dv;
flightDV = flight_dv;
arrivalDV = arrival_dv;
//Free temporary memory for MGA_DSM
for(int i = 0; i < 6; i++) {
delete[] problem.r[i];
delete[] problem.v[i];
}
problem.r.clear();
problem.v.clear();
return obj;
}
double sagas(const vector& x, double& DVtot, double& DVonboard){
mgadsmproblem problem;
int sequence_[3] = {3,3,5}; // sequence of planets
problem.sequence.insert(problem.sequence.begin(), sequence_, sequence_+ 3 );
problem.type = time2AUs;
problem.AUdist = 50.0;
problem.DVtotal = 6.782;
problem.DVonboard = 1.782;
//Allocate temporary memory for MGA_DSM
problem.r = std::vector(3);
problem.v = std::vector(3);
problem.DV = std::vector(3+2);
for(int i = 0; i < 3; i++) {
problem.r[i] = new double[3];
problem.v[i] = new double[3];
}
double obj = 0;
double launch_dv = 0;
double flight_dv = 0;
double arrival_dv = 0;
MGA_DSM(
/* INPUT values: */
x,
problem,
/* OUTPUT values: */
obj,launch_dv,flight_dv,arrival_dv);
DVtot = problem.DV[0] +problem.DV[1]+problem.DV[2]+problem.DV[3]+problem.DV[4];
DVonboard = DVtot - problem.DV[0];
// launchDV = launch_dv;
// flightDV = flight_dv;
// arrivalDV = arrival_dv;
//Free temporary memory for MGA_DSM
for(int i = 0; i < 3; i++) {
delete[] problem.r[i];
delete[] problem.v[i];
}
problem.r.clear();
problem.v.clear();
return obj;
}
double tandem(const vector& x, double& tof, const int sequence_[]){
mgadsmproblem problem;
problem.sequence.insert(problem.sequence.begin(), sequence_, sequence_+ 5 );
problem.type = orbit_insertion;
problem.rp = 80330.0;
problem.e = 0.98531407996358;
//Allocate temporary memory for MGA_DSM
problem.r = std::vector(5);
problem.v = std::vector(5);
problem.DV = std::vector(5+1);
for(int i = 0; i < 5; i++) {
problem.r[i] = new double[3];
problem.v[i] = new double[3];
}
double obj = 0;
double launch_dv = 0;
double flight_dv = 0;
double arrival_dv = 0;
MGA_DSM(
/* INPUT values: */
x,
problem,
/* OUTPUT values: */
obj,launch_dv,flight_dv,arrival_dv);
//evaluating the mass from the dvs
double rE[3];
double vE[3];
Planet_Ephemerides_Analytical (x[0],3,rE,vE);
double VINFE = x[1];
double udir = x[2];
double vdir = x[3];
double vtemp[3];
vtemp[0]= rE[1]*vE[2]-rE[2]*vE[1];
vtemp[1]= rE[2]*vE[0]-rE[0]*vE[2];
vtemp[2]= rE[0]*vE[1]-rE[1]*vE[0];
double iP1[3];
double normvE=sqrt(vE[0]*vE[0]+vE[1]*vE[1]+vE[2]*vE[2]);
iP1[0]= vE[0]/normvE;
iP1[1]= vE[1]/normvE;
iP1[2]= vE[2]/normvE;
double zP1[3];
double normvtemp=sqrt(vtemp[0]*vtemp[0]+vtemp[1]*vtemp[1]+vtemp[2]*vtemp[2]);
zP1[0]= vtemp[0]/normvtemp;
zP1[1]= vtemp[1]/normvtemp;
zP1[2]= vtemp[2]/normvtemp;
double jP1[3];
jP1[0]= zP1[1]*iP1[2]-zP1[2]*iP1[1];
jP1[1]= zP1[2]*iP1[0]-zP1[0]*iP1[2];
jP1[2]= zP1[0]*iP1[1]-zP1[1]*iP1[0];
double theta=2*M_PI*udir; //See Picking a Point on a Sphere
double phi=acos(2*vdir-1)-M_PI/2; //In this way: -pi/2
#include
// #include "ZeroFinder.h"
#include
#define NUMERATOR(dab,dcb,fa,fb,fc) fb*(dab*fc*(fc-fb)-fa*dcb*(fa-fb))
#define DENOMINATOR(fa,fb,fc) (fc-fb)*(fa-fb)*(fa-fc)
void ZeroFinder::Function1D::SetParameters(double a, double b)
{
p1 = a;
p2 = b;
}
void ZeroFinder::Function1D_7param::SetParameters(double a, double b, double c, double d, double e, double f, double g)
{
p1 = a;
p2 = b;
p3 = c;
p4 = d;
p5 = e;
p6 = f;
p7 = g;
}
//
ZeroFinder::FZero::FZero(double a, double b)
{
SetBounds(a, b);
}
//
void ZeroFinder::FZero::SetBounds(double lb, double ub)
{
a = lb;
c = ub;
}
//-------------------------------------------------------------------------------------//
// This part is an adaption of the 'Amsterdam method', which is an inversre quadratic //
// interpolation - bisection method //
// See http://mymathlib.webtrellis.net/roots/amsterdam.html //
//
//-------------------------------------------------------------------------------------//
double ZeroFinder::FZero::FindZero(Function1D &f)
{
int max_iterations = 500;
double tolerance = 1e-15;
double fa = f(a), b = 0.5 * ( a + c ), fc = f(c), fb = fa * fc;
double delta, dab, dcb;
int i;
// If the initial estimates do not bracket a root, set the err flag and //
// return. If an initial estimate is a root, then return the root. //
//double err = 0;
if ( fb >= 0.0 ){
if ( fb > 0.0 ) { return 0.0; }
else return ( fa == 0.0 ) ? a : c;}
// Insure that the initial estimate a < c. //
if ( a > c ) {
delta = a; a = c; c = delta; delta = fa; fa = fc; fc = delta;
}
// If the function at the left endpoint is positive, and the function //
// at the right endpoint is negative. Iterate reducing the length //
// of the interval by either bisection or quadratic inverse //
// interpolation. Note that the function at the left endpoint //
// remains nonnegative and the function at the right endpoint remains //
// nonpositive. //
if ( fa > 0.0 )
for ( i = 0; i < max_iterations; i++) {
// Are the two endpoints within the user specified tolerance ?
if ( ( c - a ) < tolerance ) return 0.5 * ( a + c);
// No, Continue iteration.
fb = f(b);
// Check that we are converging or that we have converged near //
// the left endpoint of the inverval. If it appears that the //
// interval is not decreasing fast enough, use bisection. //
if ( ( c - a ) < tolerance ) return 0.5 * ( a + c);
if ( ( b - a ) < tolerance ){
if ( fb > 0 ) {
a = b; fa = fb; b = 0.5 * ( a + c ); continue;
}
else return b;
}
// Check that we are converging or that we have converged near //
// the right endpoint of the inverval. If it appears that the //
// interval is not decreasing fast enough, use bisection. //
if ( ( c - b ) < tolerance ){
if ( fb < 0 ) {
c = b; fc = fb; b = 0.5 * ( a + c ); continue;
}
else return b;}
// If quadratic inverse interpolation is feasible, try it. //
if ( ( fa > fb ) && ( fb > fc ) ) {
delta = DENOMINATOR(fa,fb,fc);
if ( delta != 0.0 ) {
dab = a - b;
dcb = c - b;
delta = NUMERATOR(dab,dcb,fa,fb,fc) / delta;
// Will the new estimate of the root be within the //
// interval? If yes, use it and update interval. //
// If no, use the bisection method. //
if ( delta > dab && delta < dcb ) {
if ( fb > 0.0 ) { a = b; fa = fb; }
else if ( fb < 0.0 ) { c = b; fc = fb; }
else return b;
b += delta;
continue;
}
}
}
// If not, use the bisection method. //
fb > 0 ? ( a = b, fa = fb ) : ( c = b, fc = fb );
b = 0.5 * ( a + c );
}
else
// If the function at the left endpoint is negative, and the function //
// at the right endpoint is positive. Iterate reducing the length //
// of the interval by either bisection or quadratic inverse //
// interpolation. Note that the function at the left endpoint //
// remains nonpositive and the function at the right endpoint remains //
// nonnegative. //
for ( i = 0; i < max_iterations; i++) {
if ( ( c - a ) < tolerance ) return 0.5 * ( a + c);
fb = f(b);
if ( ( b - a ) < tolerance ){
if ( fb < 0 ) {
a = b; fa = fb; b = 0.5 * ( a + c ); continue;
}
else return b;}
if ( ( c - b ) < tolerance ){
if ( fb > 0 ) {
c = b; fc = fb; b = 0.5 * ( a + c ); continue;
}
else return b;}
if ( ( fa < fb ) && ( fb < fc ) ) {
delta = DENOMINATOR(fa,fb,fc);
if ( delta != 0.0 ) {
dab = a - b;
dcb = c - b;
delta = NUMERATOR(dab,dcb,fa,fb,fc) / delta;
if ( delta > dab && delta < dcb ) {
if ( fb < 0.0 ) { a = b; fa = fb; }
else if ( fb > 0.0 ) { c = b; fc = fb; }
else return b;
b += delta;
continue;
}
}
}
fb < 0 ? ( a = b, fa = fb ) : ( c = b, fc = fb );
b = 0.5 * ( a + c );
}
//err = -2;
return b;
}
//-----------------------------------------------------------------------------------
double ZeroFinder::FZero::FindZero7(Function1D_7param &f)
{
int max_iterations = 500;
double tolerance = 1e-15;
double fa = f(a), b = 0.5 * ( a + c ), fc = f(c), fb = fa * fc;
double delta, dab, dcb;
int i;
// If the initial estimates do not bracket a root, set the err flag and //
// return. If an initial estimate is a root, then return the root. //
//double err = 0;
if ( fb >= 0.0 ){
if ( fb > 0.0 ) { return 0.0; }
else return ( fa == 0.0 ) ? a : c;}
// Insure that the initial estimate a < c. //
if ( a > c ) {
delta = a; a = c; c = delta; delta = fa; fa = fc; fc = delta;
}
// If the function at the left endpoint is positive, and the function //
// at the right endpoint is negative. Iterate reducing the length //
// of the interval by either bisection or quadratic inverse //
// interpolation. Note that the function at the left endpoint //
// remains nonnegative and the function at the right endpoint remains //
// nonpositive. //
if ( fa > 0.0 )
for ( i = 0; i < max_iterations; i++) {
// Are the two endpoints within the user specified tolerance ?
if ( ( c - a ) < tolerance ) return 0.5 * ( a + c);
// No, Continue iteration.
fb = f(b);
// Check that we are converging or that we have converged near //
// the left endpoint of the inverval. If it appears that the //
// interval is not decreasing fast enough, use bisection. //
if ( ( c - a ) < tolerance ) return 0.5 * ( a + c);
if ( ( b - a ) < tolerance ){
if ( fb > 0 ) {
a = b; fa = fb; b = 0.5 * ( a + c ); continue;
}
else return b;}
// Check that we are converging or that we have converged near //
// the right endpoint of the inverval. If it appears that the //
// interval is not decreasing fast enough, use bisection. //
if ( ( c - b ) < tolerance ){
if ( fb < 0 ) {
c = b; fc = fb; b = 0.5 * ( a + c ); continue;
}
else return b;}
// If quadratic inverse interpolation is feasible, try it. //
if ( ( fa > fb ) && ( fb > fc ) ) {
delta = DENOMINATOR(fa,fb,fc);
if ( delta != 0.0 ) {
dab = a - b;
dcb = c - b;
delta = NUMERATOR(dab,dcb,fa,fb,fc) / delta;
// Will the new estimate of the root be within the //
// interval? If yes, use it and update interval. //
// If no, use the bisection method. //
if ( delta > dab && delta < dcb ) {
if ( fb > 0.0 ) { a = b; fa = fb; }
else if ( fb < 0.0 ) { c = b; fc = fb; }
else return b;
b += delta;
continue;
}
}
}
// If not, use the bisection method. //
fb > 0 ? ( a = b, fa = fb ) : ( c = b, fc = fb );
b = 0.5 * ( a + c );
}
else
// If the function at the left endpoint is negative, and the function //
// at the right endpoint is positive. Iterate reducing the length //
// of the interval by either bisection or quadratic inverse //
// interpolation. Note that the function at the left endpoint //
// remains nonpositive and the function at the right endpoint remains //
// nonnegative. //
for ( i = 0; i < max_iterations; i++) {
if ( ( c - a ) < tolerance ) return 0.5 * ( a + c);
fb = f(b);
if ( ( b - a ) < tolerance ){
if ( fb < 0 ) {
a = b; fa = fb; b = 0.5 * ( a + c ); continue;
}
else return b;}
if ( ( c - b ) < tolerance ){
if ( fb > 0 ) {
c = b; fc = fb; b = 0.5 * ( a + c ); continue;
}
else return b;}
if ( ( fa < fb ) && ( fb < fc ) ) {
delta = DENOMINATOR(fa,fb,fc);
if ( delta != 0.0 ) {
dab = a - b;
dcb = c - b;
delta = NUMERATOR(dab,dcb,fa,fb,fc) / delta;
if ( delta > dab && delta < dcb ) {
if ( fb < 0.0 ) { a = b; fa = fb; }
else if ( fb > 0.0 ) { c = b; fc = fb; }
else return b;
b += delta;
continue;
}
}
}
fb < 0 ? ( a = b, fa = fb ) : ( c = b, fc = fb );
b = 0.5 * ( a + c );
}
//err = -2;
return b;
}