Permalink
Cannot retrieve contributors at this time
/***************************************************************************** | |
* Copyright (C) 2004-2018 The pykep development team, * | |
* Advanced Concepts Team (ACT), European Space Agency (ESA) * | |
* * | |
* https://gitter.im/esa/pykep * | |
* https://github.com/esa/pykep * | |
* * | |
* act@esa.int * | |
* * | |
* 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 2 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, write to the * | |
* Free Software Foundation, Inc., * | |
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * | |
*****************************************************************************/ | |
#include <boost/math/special_functions/acosh.hpp> | |
#include <boost/math/special_functions/asinh.hpp> | |
#include <keplerian_toolbox/core_functions/array3D_operations.hpp> | |
#include <keplerian_toolbox/exceptions.hpp> | |
#include <keplerian_toolbox/lambert_problem.hpp> | |
namespace kep_toolbox | |
{ | |
const array3D lambert_problem::default_r1 = {{1.0, 0.0, 0.0}}; | |
const array3D lambert_problem::default_r2 = {{0.0, 1.0, 0.0}}; | |
/// Constructor | |
/** Constructs and solves a Lambert problem. | |
* | |
* \param[in] R1 first cartesian position | |
* \param[in] R2 second cartesian position | |
* \param[in] tof time of flight | |
* \param[in] mu gravity parameter | |
* \param[in] cw when 1 a retrograde orbit is assumed | |
* \param[in] multi_revs maximum number of multirevolutions to compute | |
*/ | |
lambert_problem::lambert_problem(const array3D &r1, const array3D &r2, const double &tof, const double &mu, | |
const int &cw, const int &multi_revs) | |
: m_r1(r1), m_r2(r2), m_tof(tof), m_mu(mu), m_has_converged(true), m_multi_revs(multi_revs) | |
{ | |
// 0 - Sanity checks | |
if (tof <= 0) { | |
throw_value_error("Time of flight is negative!"); | |
} | |
if (mu <= 0) { | |
throw_value_error("Gravity parameter is zero or negative!"); | |
} | |
// 1 - Getting lambda and T | |
m_c = sqrt((r2[0] - r1[0]) * (r2[0] - r1[0]) + (r2[1] - r1[1]) * (r2[1] - r1[1]) | |
+ (r2[2] - r1[2]) * (r2[2] - r1[2])); | |
double R1 = norm(m_r1); | |
double R2 = norm(m_r2); | |
m_s = (m_c + R1 + R2) / 2.0; | |
array3D ir1, ir2, ih, it1, it2; | |
vers(ir1, r1); | |
vers(ir2, r2); | |
cross(ih, ir1, ir2); | |
vers(ih, ih); | |
if (ih[2] == 0) { | |
throw_value_error("The angular momentum vector has no z component, impossible to define automatically clock or " | |
"counterclockwise"); | |
} | |
double lambda2 = 1.0 - m_c / m_s; | |
m_lambda = sqrt(lambda2); | |
if (ih[2] < 0.0) // Transfer angle is larger than 180 degrees as seen from abive the z axis | |
{ | |
m_lambda = -m_lambda; | |
cross(it1, ir1, ih); | |
cross(it2, ir2, ih); | |
} else { | |
cross(it1, ih, ir1); | |
cross(it2, ih, ir2); | |
} | |
vers(it1, it1); | |
vers(it2, it2); | |
if (cw) { // Retrograde motion | |
m_lambda = -m_lambda; | |
it1[0] = -it1[0]; | |
it1[1] = -it1[1]; | |
it1[2] = -it1[2]; | |
it2[0] = -it2[0]; | |
it2[1] = -it2[1]; | |
it2[2] = -it2[2]; | |
} | |
double lambda3 = m_lambda * lambda2; | |
double T = sqrt(2.0 * m_mu / m_s / m_s / m_s) * m_tof; | |
// 2 - We now have lambda, T and we will find all x | |
// 2.1 - Let us first detect the maximum number of revolutions for which there exists a solution | |
m_Nmax = static_cast<int>(T / M_PI); | |
double T00 = acos(m_lambda) + m_lambda * sqrt(1.0 - lambda2); | |
double T0 = (T00 + m_Nmax * M_PI); | |
double T1 = 2.0 / 3.0 * (1.0 - lambda3), DT = 0.0, DDT = 0.0, DDDT = 0.0; | |
if (m_Nmax > 0) { | |
if (T < T0) { // We use Halley iterations to find xM and TM | |
int it = 0; | |
double err = 1.0; | |
double T_min = T0; | |
double x_old = 0.0, x_new = 0.0; | |
while (1) { | |
dTdx(DT, DDT, DDDT, x_old, T_min); | |
if (DT != 0.0) { | |
x_new = x_old - DT * DDT / (DDT * DDT - DT * DDDT / 2.0); | |
} | |
err = fabs(x_old - x_new); | |
if ((err < 1e-13) || (it > 12)) { | |
break; | |
} | |
x2tof(T_min, x_new, m_Nmax); | |
x_old = x_new; | |
it++; | |
} | |
if (T_min > T) { | |
m_Nmax -= 1; | |
} | |
} | |
} | |
// We exit this if clause with Nmax being the maximum number of revolutions | |
// for which there exists a solution. We crop it to m_multi_revs | |
m_Nmax = std::min(m_multi_revs, m_Nmax); | |
// 2.2 We now allocate the memory for the output variables | |
m_v1.resize(m_Nmax * 2 + 1); | |
m_v2.resize(m_Nmax * 2 + 1); | |
m_iters.resize(m_Nmax * 2 + 1); | |
m_x.resize(m_Nmax * 2 + 1); | |
// 3 - We may now find all solutions in x,y | |
// 3.1 0 rev solution | |
// 3.1.1 initial guess | |
if (T >= T00) { | |
m_x[0] = -(T - T00) / (T - T00 + 4); | |
} else if (T <= T1) { | |
m_x[0] = T1 * (T1 - T) / (2.0 / 5.0 * (1 - lambda2 * lambda3) * T) + 1; | |
} else { | |
m_x[0] = pow((T / T00), 0.69314718055994529 / log(T1 / T00)) - 1.0; | |
} | |
// 3.1.2 Householder iterations | |
m_iters[0] = householder(T, m_x[0], 0, 1e-5, 15); | |
// 3.2 multi rev solutions | |
double tmp; | |
for (int i = 1; i < m_Nmax + 1; ++i) { | |
// 3.2.1 left Householder iterations | |
tmp = pow((i * M_PI + M_PI) / (8.0 * T), 2.0 / 3.0); | |
m_x[2 * i - 1] = (tmp - 1) / (tmp + 1); | |
m_iters[2 * i - 1] = householder(T, m_x[2 * i - 1], i, 1e-8, 15); | |
// 3.2.1 right Householder iterations | |
tmp = pow((8.0 * T) / (i * M_PI), 2.0 / 3.0); | |
m_x[2 * i] = (tmp - 1) / (tmp + 1); | |
m_iters[2 * i] = householder(T, m_x[2 * i], i, 1e-8, 15); | |
} | |
// 4 - For each found x value we reconstruct the terminal velocities | |
double gamma = sqrt(m_mu * m_s / 2.0); | |
double rho = (R1 - R2) / m_c; | |
double sigma = sqrt(1 - rho * rho); | |
double vr1, vt1, vr2, vt2, y; | |
for (size_t i = 0; i < m_x.size(); ++i) { | |
y = sqrt(1.0 - lambda2 + lambda2 * m_x[i] * m_x[i]); | |
vr1 = gamma * ((m_lambda * y - m_x[i]) - rho * (m_lambda * y + m_x[i])) / R1; | |
vr2 = -gamma * ((m_lambda * y - m_x[i]) + rho * (m_lambda * y + m_x[i])) / R2; | |
double vt = gamma * sigma * (y + m_lambda * m_x[i]); | |
vt1 = vt / R1; | |
vt2 = vt / R2; | |
for (int j = 0; j < 3; ++j) | |
m_v1[i][j] = vr1 * ir1[j] + vt1 * it1[j]; | |
for (int j = 0; j < 3; ++j) | |
m_v2[i][j] = vr2 * ir2[j] + vt2 * it2[j]; | |
} | |
} | |
int lambert_problem::householder(const double T, double &x0, const int N, const double eps, const int iter_max) | |
{ | |
int it = 0; | |
double err = 1.0; | |
double xnew = 0.0; | |
double tof = 0.0, delta = 0.0, DT = 0.0, DDT = 0.0, DDDT = 0.0; | |
while ((err > eps) && (it < iter_max)) { | |
x2tof(tof, x0, N); | |
dTdx(DT, DDT, DDDT, x0, tof); | |
delta = tof - T; | |
double DT2 = DT * DT; | |
xnew = x0 - delta * (DT2 - delta * DDT / 2.0) / (DT * (DT2 - delta * DDT) + DDDT * delta * delta / 6.0); | |
err = fabs(x0 - xnew); | |
x0 = xnew; | |
it++; | |
} | |
return it; | |
} | |
void lambert_problem::dTdx(double &DT, double &DDT, double &DDDT, const double x, const double T) | |
{ | |
double l2 = m_lambda * m_lambda; | |
double l3 = l2 * m_lambda; | |
double umx2 = 1.0 - x * x; | |
double y = sqrt(1.0 - l2 * umx2); | |
double y2 = y * y; | |
double y3 = y2 * y; | |
DT = 1.0 / umx2 * (3.0 * T * x - 2.0 + 2.0 * l3 * x / y); | |
DDT = 1.0 / umx2 * (3.0 * T + 5.0 * x * DT + 2.0 * (1.0 - l2) * l3 / y3); | |
DDDT = 1.0 / umx2 * (7.0 * x * DDT + 8.0 * DT - 6.0 * (1.0 - l2) * l2 * l3 * x / y3 / y2); | |
} | |
void lambert_problem::x2tof2(double &tof, const double x, const int N) | |
{ | |
double a = 1.0 / (1.0 - x * x); | |
if (a > 0) // ellipse | |
{ | |
double alfa = 2.0 * acos(x); | |
double beta = 2.0 * asin(sqrt(m_lambda * m_lambda / a)); | |
if (m_lambda < 0.0) beta = -beta; | |
tof = ((a * sqrt(a) * ((alfa - sin(alfa)) - (beta - sin(beta)) + 2.0 * M_PI * N)) / 2.0); | |
} else { | |
double alfa = 2.0 * boost::math::acosh(x); | |
double beta = 2.0 * boost::math::asinh(sqrt(-m_lambda * m_lambda / a)); | |
if (m_lambda < 0.0) beta = -beta; | |
tof = (-a * sqrt(-a) * ((beta - sinh(beta)) - (alfa - sinh(alfa))) / 2.0); | |
} | |
} | |
void lambert_problem::x2tof(double &tof, const double x, const int N) | |
{ | |
double battin = 0.01; | |
double lagrange = 0.2; | |
double dist = fabs(x - 1); | |
if (dist < lagrange && dist > battin) { // We use Lagrange tof expression | |
x2tof2(tof, x, N); | |
return; | |
} | |
double K = m_lambda * m_lambda; | |
double E = x * x - 1.0; | |
double rho = fabs(E); | |
double z = sqrt(1 + K * E); | |
if (dist < battin) { // We use Battin series tof expression | |
double eta = z - m_lambda * x; | |
double S1 = 0.5 * (1.0 - m_lambda - x * eta); | |
double Q = hypergeometricF(S1, 1e-11); | |
Q = 4.0 / 3.0 * Q; | |
tof = (eta * eta * eta * Q + 4.0 * m_lambda * eta) / 2.0 + N * M_PI / pow(rho, 1.5); | |
return; | |
} else { // We use Lancaster tof expresion | |
double y = sqrt(rho); | |
double g = x * z - m_lambda * E; | |
double d = 0.0; | |
if (E < 0) { | |
double l = acos(g); | |
d = N * M_PI + l; | |
} else { | |
double f = y * (z - m_lambda * x); | |
d = log(f + g); | |
} | |
tof = (x - m_lambda * z - d / y) / E; | |
return; | |
} | |
} | |
double lambert_problem::hypergeometricF(double z, double tol) | |
{ | |
double Sj = 1.0; | |
double Cj = 1.0; | |
double err = 1.0; | |
double Cj1 = 0.0; | |
double Sj1 = 0.0; | |
int j = 0; | |
while (err > tol) { | |
Cj1 = Cj * (3.0 + j) * (1.0 + j) / (2.5 + j) * z / (j + 1); | |
Sj1 = Sj + Cj1; | |
err = fabs(Cj1); | |
Sj = Sj1; | |
Cj = Cj1; | |
j = j + 1; | |
} | |
return Sj; | |
} | |
/// Gets velocity at r1 | |
/** | |
* | |
* \return an std::vector containing 3-d arrays with the cartesian components of the velocities at r1 for all 2N_max+1 | |
* solutions | |
*/ | |
const std::vector<array3D> &lambert_problem::get_v1() const | |
{ | |
return m_v1; | |
} | |
/// Gets velocity at r2 | |
/** | |
* | |
* \return an std::vector containing 3-d arrays with the cartesian components of the velocities at r2 for all 2N_max+1 | |
* solutions | |
*/ | |
const std::vector<array3D> &lambert_problem::get_v2() const | |
{ | |
return m_v2; | |
} | |
/// Gets r1 | |
/** | |
* | |
* \return a 3-d array with the cartesian components of r1 | |
*/ | |
const array3D &lambert_problem::get_r1() const | |
{ | |
return m_r1; | |
} | |
/// Gets r2 | |
/** | |
* | |
* \return a 3-d array with the cartesian components of r2 | |
*/ | |
const array3D &lambert_problem::get_r2() const | |
{ | |
return m_r2; | |
} | |
/// Gets the time of flight between r1 and r2 | |
/** | |
* | |
* \return the time of flight | |
*/ | |
const double &lambert_problem::get_tof() const | |
{ | |
return m_tof; | |
} | |
/// Gets the x variable | |
/** | |
* Gets the x variable for each solution found (0 revs, 1,1,2,2,3,3 .... N,N) | |
* | |
* \return the x variables in an std::vector | |
*/ | |
const std::vector<double> &lambert_problem::get_x() const | |
{ | |
return m_x; | |
} | |
/// Gets gravitational parameter | |
/** | |
* | |
* \return the gravitational parameter | |
*/ | |
const double &lambert_problem::get_mu() const | |
{ | |
return m_mu; | |
} | |
/// Gets number of iterations | |
/** | |
* | |
* \return an std::vector containing the iterations taken to compute each one of the solutions | |
*/ | |
const std::vector<int> &lambert_problem::get_iters() const | |
{ | |
return m_iters; | |
} | |
/// Gets N_max | |
/** | |
* | |
* \return the maximum number of revolutions. The number of solutions to the problem will be Nmax*2 +1 | |
*/ | |
int lambert_problem::get_Nmax() const | |
{ | |
return m_Nmax; | |
} | |
/// Streaming operator | |
std::ostream &operator<<(std::ostream &s, const lambert_problem &lp) | |
{ | |
s << std::setprecision(14) << "Lambert's problem:" << std::endl; | |
s << "mu = " << lp.m_mu << std::endl; | |
s << "r1 = " << lp.m_r1 << std::endl; | |
s << "r2 = " << lp.m_r2 << std::endl; | |
s << "Time of flight: " << lp.m_tof << std::endl << std::endl; | |
s << "chord = " << lp.m_c << std::endl; | |
s << "semiperimeter = " << lp.m_s << std::endl; | |
s << "lambda = " << lp.m_lambda << std::endl; | |
s << "non dimensional time of flight = " << lp.m_tof * sqrt(2 * lp.m_mu / lp.m_s / lp.m_s / lp.m_s) << std::endl | |
<< std::endl; | |
s << "Maximum number of revolutions: " << lp.m_Nmax << std::endl; | |
s << "Solutions: " << std::endl; | |
s << "0 revs, Iters: " << lp.m_iters[0] << ", x: " << lp.m_x[0] | |
<< ", a: " << lp.m_s / 2.0 / (1 - lp.m_x[0] * lp.m_x[0]) << std::endl; | |
s << "\tv1= " << lp.m_v1[0] << " v2= " << lp.m_v2[0] << std::endl; | |
for (int i = 0; i < lp.m_Nmax; ++i) { | |
s << i + 1 << " revs, left. Iters: " << lp.m_iters[1 + 2 * i] << ", x: " << lp.m_x[1 + 2 * i] | |
<< ", a: " << lp.m_s / 2.0 / (1 - lp.m_x[1 + 2 * i] * lp.m_x[1 + 2 * i]) << std::endl; | |
s << "\tv1= " << lp.m_v1[1 + 2 * i] << " v2= " << lp.m_v2[1 + 2 * i] << std::endl; | |
s << i + 1 << " revs, right. Iters: " << lp.m_iters[2 + 2 * i] << ", a: " << lp.m_x[2 + 2 * i] | |
<< ", a: " << lp.m_s / 2.0 / (1 - lp.m_x[2 + 2 * i] * lp.m_x[2 + 2 * i]) << std::endl; | |
s << "\tv1= " << lp.m_v1[2 + 2 * i] << " v2= " << lp.m_v2[2 + 2 * i] << std::endl; | |
} | |
return s; | |
} | |
} // namespace kep_toolbox |