From 51fbf7f4e0bc5559a44a4d9b3b41ae996665cfd0 Mon Sep 17 00:00:00 2001 From: Dario Izzo Date: Thu, 21 Sep 2023 16:21:03 +0200 Subject: [PATCH 01/11] lambert problem port --- CMakeLists.txt | 1 + include/kep3/{detail => }/exceptions.hpp | 0 include/kep3/lambert_problem.hpp | 115 ++++++ include/kep3/planet.hpp | 2 +- src/core_astro/ic2par2ic.cpp | 4 +- src/lambert_problem.cpp | 433 +++++++++++++++++++++++ src/planet.cpp | 21 +- test/planet_keplerian_test.cpp | 8 +- test/planet_test.cpp | 2 +- 9 files changed, 568 insertions(+), 18 deletions(-) rename include/kep3/{detail => }/exceptions.hpp (100%) create mode 100644 include/kep3/lambert_problem.hpp create mode 100644 src/lambert_problem.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 52b3ff37..752df921 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -131,6 +131,7 @@ endif() set(kep3_SRC_FILES "${CMAKE_CURRENT_SOURCE_DIR}/src/epoch.cpp" "${CMAKE_CURRENT_SOURCE_DIR}/src/planet.cpp" + "${CMAKE_CURRENT_SOURCE_DIR}/src/lambert_problem.cpp" "${CMAKE_CURRENT_SOURCE_DIR}/src/planets/keplerian.cpp" "${CMAKE_CURRENT_SOURCE_DIR}/src/core_astro/ic2par2ic.cpp" "${CMAKE_CURRENT_SOURCE_DIR}/src/core_astro/ic2eq2ic.cpp" diff --git a/include/kep3/detail/exceptions.hpp b/include/kep3/exceptions.hpp similarity index 100% rename from include/kep3/detail/exceptions.hpp rename to include/kep3/exceptions.hpp diff --git a/include/kep3/lambert_problem.hpp b/include/kep3/lambert_problem.hpp new file mode 100644 index 00000000..316eddcc --- /dev/null +++ b/include/kep3/lambert_problem.hpp @@ -0,0 +1,115 @@ +// Copyright 2023, 2024 Dario Izzo (dario.izzo@gmail.com), Francesco Biscani +// (bluescarni@gmail.com) +// +// This file is part of the kep3 library. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#ifndef kep3_LAMBERT_PROBLEM_H +#define kep3_LAMBERT_PROBLEM_H + +#include +#include +#include + +#include +#include +#include + +namespace kep3 { + +/// Lambert Problem +/** + * This class represent a Lambert's problem. When instantiated it assumes a + * prograde orbit (unless otherwise stated) and evaluates all the solutions up + * to a maximum number of multiple revolutions. After the object is instantiated + * the solutions can be retreived using the appropriate getters. Note that the + * number of solutions will be N_max*2 + 1, where N_max is the maximum number of + * revolutions. + * + * NOTE: The class has been tested extensively via monte carlo runs checked with + * numerical propagation. Compared to the previous Lambert Solver in the + * keplerian_toolbox it is 1.7 times faster (on average as defined by + * lambert_test.cpp). With respect to Gooding algorithm it is 1.3 - 1.5 times + * faster (zero revs - multi revs). The algorithm is described in detail in: + * + * Izzo, Dario. "Revisiting Lambert’s problem." Celestial Mechanics and + * Dynamical Astronomy 121 (2015): 1-15. + * + * @author Dario Izzo (dario.izzo _AT_ googlemail.com) + */ + +class kep3_DLL_PUBLIC lambert_problem; + +// Streaming operator for the class kep_toolbox::lambert_problem. +kep3_DLL_PUBLIC std::ostream &operator<<(std::ostream &, + const lambert_problem &); + +class kep3_DLL_PUBLIC lambert_problem { + static const std::array default_r1; + static const std::array default_r2; + +public: + friend kep3_DLL_PUBLIC std::ostream &operator<<(std::ostream &, + const lambert_problem &); + explicit lambert_problem(const std::array &r1 = default_r1, + const std::array &r2 = default_r2, + double tof = kep3::pi / 2, double mu = 1., + bool cw = false, unsigned multi_revs = 5); + [[nodiscard]] const std::vector> &get_v1() const; + [[nodiscard]] const std::vector> &get_v2() const; + [[nodiscard]] const std::array &get_r1() const; + [[nodiscard]] const std::array &get_r2() const; + [[nodiscard]] const double &get_tof() const; + [[nodiscard]] const double &get_mu() const; + [[nodiscard]] const std::vector &get_x() const; + [[nodiscard]] const std::vector &get_iters() const; + [[nodiscard]] unsigned get_Nmax() const; + +private: + unsigned householder(double, double &, unsigned, double, unsigned) const; + void dTdx(double &, double &, double &, double, double) const; + void x2tof(double &tof, double x0, unsigned N) const; + void x2tof2(double &tof, double x0, unsigned N) const ; + [[nodiscard]] double hypergeometricF(double z, double tol) const; + friend class boost::serialization::access; + template void serialize(Archive &ar, const unsigned int) { + ar &m_r1; + ar &m_r2; + ar &m_tof; + ar &m_mu; + ar &m_v1; + ar &m_v2; + ar &m_iters; + ar &m_x; + ar &m_s; + ar &m_c; + ar &m_lambda; + ar &m_iters; + ar &m_Nmax; + ar &m_has_converged; + ar &m_multi_revs; + } + + std::array m_r1, m_r2; + double m_tof; + double m_mu; + std::vector> m_v1; + std::vector> m_v2; + std::vector m_iters; + std::vector m_x; + double m_s, m_c, m_lambda; + unsigned m_Nmax; + bool m_has_converged; + unsigned m_multi_revs; +}; + +// Streaming operator for the class kep3::lambert_problem. +kep3_DLL_PUBLIC std::ostream &operator<<(std::ostream &, + const lambert_problem &); + +} // namespace kep3 + +#endif // kep3_LAMBERT_PROBLEM_H \ No newline at end of file diff --git a/include/kep3/planet.hpp b/include/kep3/planet.hpp index 8c376eaa..308f7990 100644 --- a/include/kep3/planet.hpp +++ b/include/kep3/planet.hpp @@ -23,11 +23,11 @@ #endif #include #include -#include #include #include #include #include +#include #define kep3_S11N_PLANET_EXPORT_KEY(PLA) \ BOOST_CLASS_EXPORT_KEY2(kep3::detail::planet_inner, "udpla " #PLA) \ diff --git a/src/core_astro/ic2par2ic.cpp b/src/core_astro/ic2par2ic.cpp index 622c9fac..6d19ae0d 100644 --- a/src/core_astro/ic2par2ic.cpp +++ b/src/core_astro/ic2par2ic.cpp @@ -16,11 +16,11 @@ #include #include +namespace kep3 { + using xt::linalg::cross; using xt::linalg::dot; -namespace kep3 { - // r,v,mu -> keplerian osculating elements [a,e,i,W,w,f]. The last // is the true anomaly. The semi-major axis a is positive for ellipses, negative // for hyperbolae. The anomalies W, w, f are in [0, 2pi]. Inclination is in [0, diff --git a/src/lambert_problem.cpp b/src/lambert_problem.cpp new file mode 100644 index 00000000..c1f26653 --- /dev/null +++ b/src/lambert_problem.cpp @@ -0,0 +1,433 @@ +// Copyright 2023, 2024 Dario Izzo (dario.izzo@gmail.com), Francesco Biscani +// (bluescarni@gmail.com) +// +// This file is part of the kep3 library. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace kep3 { + +using xt::linalg::cross; +using xt::linalg::dot; +using xt::linalg::norm; + +const std::array lambert_problem::default_r1 = {{1.0, 0.0, 0.0}}; +const std::array 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 std::array &r1_a, + const std::array &r2_a, + double tof, // NOLINT + double mu, bool cw, unsigned multi_revs) + : m_r1(r1_a), m_r2(r2_a), m_tof(tof), m_mu(mu), m_has_converged(true), + m_multi_revs(multi_revs) { + // 0 - Sanity checks + if (tof <= 0) { + throw std::domain_error("lambert_problem: Time of flight is negative!"); + } + if (mu <= 0) { + throw std::domain_error( + "lambert_problem: Gravity parameter is zero or negative!"); + } + + // Creating xtensor objects binded to the kep3 arrays + const auto r1 = xt::adapt(r1_a); + const auto r2 = xt::adapt(r2_a); + + // 1 - Getting lambda and T + m_c = norm(r2 - r1)(0); + double R1 = norm(r1)(0); + double R2 = norm(r2)(0); + m_s = (m_c + R1 + R2) / 2.0; + + auto ir1 = r1 / R1; + auto ir2 = r2 / R2; + auto ih = cross(ir1, ir2); + ih = ih / norm(ih); + + if (ih(2) == 0) { + throw std::domain_error( + "lambert_problem: 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 = std::sqrt(lambda2); + + xt::xtensor_fixed> it1{0.0, 0.0, 0.0}; + xt::xtensor_fixed> it2{0.0, 0.0, 0.0}; + + if (ih(2) < 0.0) // Transfer angle is larger than 180 degrees as seen from + // above the z axis + { + m_lambda = -m_lambda; + it1 = cross(ir1, ih); + it2 = cross(ir2, ih); + } else { + it1 = cross(ih, ir1); + it2 = cross(ih, ir2); + } + it1 = it1 / norm(it1); + it2 = it2 / norm(it2); + + if (cw) { // Retrograde motion + m_lambda = -m_lambda; + it1 = -it1; + it2 = -it2; + } + double lambda3 = m_lambda * lambda2; + double T = std::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 = std::floor(T / kep3::pi); + double T00 = std::acos(m_lambda) + m_lambda * std::sqrt(1.0 - lambda2); + double T0 = (T00 + m_Nmax * kep3::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 (true) { + 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(static_cast(m_Nmax) * 2 + 1); + m_v2.resize(static_cast(m_Nmax) * 2 + 1); + m_iters.resize(static_cast(m_Nmax) * 2 + 1); + m_x.resize(static_cast(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] = + std::pow((T / T00), 0.69314718055994529 / std::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 = 0.; + for (decltype(m_Nmax) i = 1; i < m_Nmax + 1; ++i) { + // 3.2.1 left Householder iterations + tmp = std::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 = std::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 = 0., vt1 = 0., vr2 = 0., vt2 = 0., y = 0.; + 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]; + } + } +} + +unsigned lambert_problem::householder(double T, double &x0, + unsigned N, // NOLINT + double eps, unsigned iter_max) const { + 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, double x, + double T) const { + 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, double x, // NOLINT + unsigned N) const { + double a = 1.0 / (1.0 - x * x); + if (a > 0) // ellipse + { + double alfa = 2.0 * std::acos(x); + double beta = 2.0 * std::asin(std::sqrt(m_lambda * m_lambda / a)); + if (m_lambda < 0.0) { + beta = -beta; + } + tof = ((a * std::sqrt(a) * + ((alfa - std::sin(alfa)) - (beta - std::sin(beta)) + + 2.0 * M_PI * N)) / + 2.0); + } else { + double alfa = 2.0 * std::acosh(x); + double beta = 2.0 * std::asinh(std::sqrt(-m_lambda * m_lambda / a)); + if (m_lambda < 0.0) { + beta = -beta; + } + tof = (-a * std::sqrt(-a) * + ((beta - std::sinh(beta)) - (alfa - std::sinh(alfa))) / 2.0); + } +} + +void lambert_problem::x2tof(double &tof, double x, unsigned N) const { + 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) const { // NOLINT + 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> &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> &lambert_problem::get_v2() const { + return m_v2; +} + +/// Gets r1 +/** + * + * \return a 3-d array with the cartesian components of r1 + */ +const std::array &lambert_problem::get_r1() const { return m_r1; } + +/// Gets r2 +/** + * + * \return a 3-d array with the cartesian components of r2 + */ +const std::array &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 &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 &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 + */ +unsigned lambert_problem::get_Nmax() const { return m_Nmax; } + +/// Streaming operator +std::ostream &operator<<(std::ostream &s, const lambert_problem &lp) { + s << std::setprecision(16) << "Lambert's problem:" << std::endl; + s << "mu = " << lp.m_mu << std::endl; + s << "r1 = " + << "[" << lp.m_r1[0] << ", " << lp.m_r1[1] << ", " << lp.m_r1[2] << "]" + << std::endl; + s << "r1 = " + << "[" << lp.m_r2[0] << ", " << lp.m_r2[1] << ", " << lp.m_r2[2] << "]" + << 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][0] << ", " << lp.m_v1[0][1] << ", " << lp.m_v1[0][2] + << "]"; + s << " v2 = " + << "[" << lp.m_v2[0][0] << ", " << lp.m_v2[0][1] << ", " << lp.m_v2[0][2] + << "]" << 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][0] << ", " << lp.m_v1[1 + 2 * i][1] << ", " + << lp.m_v1[1 + 2 * i][2] << "]"; + s << " v2 = " + << "[" << lp.m_v2[1 + 2 * i][0] << ", " << lp.m_v2[1 + 2 * i][1] << ", " + << lp.m_v2[1 + 2 * i][2] << "]" << 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][0] << ", " << lp.m_v1[2 + 2 * i][1] << ", " + << lp.m_v1[2 + 2 * i][2] << "]"; + s << " v2 = " + << "[" << lp.m_v2[2 + 2 * i][0] << ", " << lp.m_v2[2 + 2 * i][1] << ", " + << lp.m_v2[2 + 2 * i][2] << "]" << std::endl; + } + return s; +} + +} // namespace kep3 \ No newline at end of file diff --git a/src/planet.cpp b/src/planet.cpp index 72b63bd5..a121c379 100644 --- a/src/planet.cpp +++ b/src/planet.cpp @@ -7,11 +7,9 @@ // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. - -#include +#include #include - namespace kep3::detail { std::array, 2> null_udpla::eph(const epoch &) { @@ -59,9 +57,7 @@ std::array, 2> planet::eph(const epoch &ep) { return ptr()->eph(ep); } -double planet::period(const epoch &ep) const { - return ptr()->period(ep); -} +double planet::period(const epoch &ep) const { return ptr()->period(ep); } std::string planet::get_name() const { return ptr()->get_name(); } @@ -70,7 +66,8 @@ std::string planet::get_extra_info() const { return ptr()->get_extra_info(); } double planet::get_mu_central_body() const { double mu = ptr()->get_mu_central_body(); if (mu == -1.) { - throw not_implemented_error("A central body mu has not been declared for '" + get_name() + "'"); + throw not_implemented_error( + "A central body mu has not been declared for '" + get_name() + "'"); } return mu; } @@ -78,7 +75,8 @@ double planet::get_mu_central_body() const { double planet::get_mu_self() const { double mu = ptr()->get_mu_self(); if (mu == -1.) { - throw not_implemented_error("A body mu has not been declared for '" + get_name() + "'"); + throw not_implemented_error("A body mu has not been declared for '" + + get_name() + "'"); } return mu; } @@ -86,7 +84,8 @@ double planet::get_mu_self() const { double planet::get_radius() const { double mu = ptr()->get_radius(); if (mu == -1.) { - throw not_implemented_error("A body radius has not been declared for '" + get_name() + "'"); + throw not_implemented_error("A body radius has not been declared for '" + + get_name() + "'"); } return mu; } @@ -94,7 +93,9 @@ double planet::get_radius() const { double planet::get_safe_radius() const { double mu = ptr()->get_safe_radius(); if (mu == -1.) { - throw not_implemented_error("A body safe radius has has not been declared for '" + get_name() + "'"); + throw not_implemented_error( + "A body safe radius has has not been declared for '" + get_name() + + "'"); } return mu; } diff --git a/test/planet_keplerian_test.cpp b/test/planet_keplerian_test.cpp index 69f1810f..9d94a045 100644 --- a/test/planet_keplerian_test.cpp +++ b/test/planet_keplerian_test.cpp @@ -9,15 +9,15 @@ #include #include +#include #include -#include +#include +#include +#include #include -#include #include "catch.hpp" -#include "kep3/core_astro/convert_anomalies.hpp" -#include "kep3/core_astro/ic2eq2ic.hpp" #include "test_helpers.hpp" using kep3::udpla::keplerian; diff --git a/test/planet_test.cpp b/test/planet_test.cpp index 1280c42e..0a725b12 100644 --- a/test/planet_test.cpp +++ b/test/planet_test.cpp @@ -12,8 +12,8 @@ #include -#include #include +#include #include #include "catch.hpp" From e53a1b26877ba07f157453cfacec5f63ee7551b1 Mon Sep 17 00:00:00 2001 From: Dario Izzo Date: Thu, 21 Sep 2023 17:35:50 +0200 Subject: [PATCH 02/11] lambert problem port code finished --- src/lambert_problem.cpp | 42 +++++++++++++++++------------------ test/CMakeLists.txt | 3 ++- test/lambert_problem_test.cpp | 21 ++++++++++++++++++ 3 files changed, 44 insertions(+), 22 deletions(-) create mode 100644 test/lambert_problem_test.cpp diff --git a/src/lambert_problem.cpp b/src/lambert_problem.cpp index c1f26653..8a24bb49 100644 --- a/src/lambert_problem.cpp +++ b/src/lambert_problem.cpp @@ -22,7 +22,6 @@ namespace kep3 { using xt::linalg::cross; using xt::linalg::dot; -using xt::linalg::norm; const std::array lambert_problem::default_r1 = {{1.0, 0.0, 0.0}}; const std::array lambert_problem::default_r2 = {{0.0, 1.0, 0.0}}; @@ -57,15 +56,16 @@ lambert_problem::lambert_problem(const std::array &r1_a, const auto r2 = xt::adapt(r2_a); // 1 - Getting lambda and T - m_c = norm(r2 - r1)(0); - double R1 = norm(r1)(0); - double R2 = norm(r2)(0); + m_c = xt::linalg::norm(r2 - r1); + fmt::print("DEBUG {}, {}:", m_c, r2-r1); + double R1 = xt::linalg::norm(r1); + double R2 = xt::linalg::norm(r2); m_s = (m_c + R1 + R2) / 2.0; auto ir1 = r1 / R1; auto ir2 = r2 / R2; auto ih = cross(ir1, ir2); - ih = ih / norm(ih); + ih = ih / xt::linalg::norm(ih); if (ih(2) == 0) { throw std::domain_error( @@ -89,8 +89,8 @@ lambert_problem::lambert_problem(const std::array &r1_a, it1 = cross(ih, ir1); it2 = cross(ih, ir2); } - it1 = it1 / norm(it1); - it2 = it2 / norm(it2); + it1 = it1 / xt::linalg::norm(it1); + it2 = it2 / xt::linalg::norm(it2); if (cw) { // Retrograde motion m_lambda = -m_lambda; @@ -168,12 +168,12 @@ lambert_problem::lambert_problem(const std::array &r1_a, } // 4 - For each found x value we reconstruct the terminal velocities - double gamma = sqrt(m_mu * m_s / 2.0); + double gamma = std::sqrt(m_mu * m_s / 2.0); double rho = (R1 - R2) / m_c; - double sigma = sqrt(1 - rho * rho); + double sigma = std::sqrt(1 - rho * rho); double vr1 = 0., vt1 = 0., vr2 = 0., vt2 = 0., y = 0.; for (size_t i = 0; i < m_x.size(); ++i) { - y = sqrt(1.0 - lambda2 + lambda2 * m_x[i] * m_x[i]); + y = std::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 = @@ -204,7 +204,7 @@ unsigned lambert_problem::householder(double T, double &x0, 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); + err = std::abs(x0 - xnew); x0 = xnew; it++; } @@ -216,7 +216,7 @@ void lambert_problem::dTdx(double &DT, double &DDT, double &DDDT, double x, 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 y = std::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); @@ -253,33 +253,33 @@ void lambert_problem::x2tof2(double &tof, double x, // NOLINT void lambert_problem::x2tof(double &tof, double x, unsigned N) const { double battin = 0.01; double lagrange = 0.2; - double dist = fabs(x - 1); + double dist = std::abs(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); + double rho = std::abs(E); + double z = std::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); + N * kep3::pi / std::pow(rho, 1.5); return; } else { // We use Lancaster tof expresion - double y = sqrt(rho); + double y = std::sqrt(rho); double g = x * z - m_lambda * E; double d = 0.0; if (E < 0) { - double l = acos(g); + double l = std::acos(g); d = N * M_PI + l; } else { double f = y * (z - m_lambda * x); - d = log(f + g); + d = std::log(f + g); } tof = (x - m_lambda * z - d / y) / E; return; @@ -296,7 +296,7 @@ double lambert_problem::hypergeometricF(double z, double tol) const { // NOLINT while (err > tol) { Cj1 = Cj * (3.0 + j) * (1.0 + j) / (2.5 + j) * z / (j + 1); Sj1 = Sj + Cj1; - err = fabs(Cj1); + err = std::abs(Cj1); Sj = Sj1; Cj = Cj1; j = j + 1; @@ -420,7 +420,7 @@ std::ostream &operator<<(std::ostream &s, const lambert_problem &lp) { << ", 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 = " + s << "\tv1 = " << "[" << lp.m_v1[2 + 2 * i][0] << ", " << lp.m_v1[2 + 2 * i][1] << ", " << lp.m_v1[2 + 2 * i][2] << "]"; s << " v2 = " diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 112b9864..ede3e724 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -33,4 +33,5 @@ ADD_kep3_TESTCASE(planet_keplerian_test) ADD_kep3_TESTCASE(ic2par2ic_test) ADD_kep3_TESTCASE(ic2eq2ic_test) ADD_kep3_TESTCASE(eq2par2eq_test) -ADD_kep3_TESTCASE(propagate_lagrangian_test) \ No newline at end of file +ADD_kep3_TESTCASE(propagate_lagrangian_test) +ADD_kep3_TESTCASE(lambert_problem_test) \ No newline at end of file diff --git a/test/lambert_problem_test.cpp b/test/lambert_problem_test.cpp new file mode 100644 index 00000000..9b984117 --- /dev/null +++ b/test/lambert_problem_test.cpp @@ -0,0 +1,21 @@ +// Copyright 2023, 2024 Dario Izzo (dario.izzo@gmail.com), Francesco Biscani +// (bluescarni@gmail.com) +// +// This file is part of the kep3 library. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include + +#include + +#include "catch.hpp" + + +TEST_CASE("construct") { + kep3::lambert_problem lp{{1.,0.,0.}, {0.,1.,0.}, kep3::pi/2,1.}; + std::cout << lp << std::endl; + +} \ No newline at end of file From d0eb4d5ac6ff2e01a8a569729451318730b0cdf4 Mon Sep 17 00:00:00 2001 From: Dario Izzo Date: Thu, 21 Sep 2023 18:40:00 +0200 Subject: [PATCH 03/11] signed unsigned maddness --- src/lambert_problem.cpp | 24 +++++++++++++----------- 1 file changed, 13 insertions(+), 11 deletions(-) diff --git a/src/lambert_problem.cpp b/src/lambert_problem.cpp index 8a24bb49..e1d09a7d 100644 --- a/src/lambert_problem.cpp +++ b/src/lambert_problem.cpp @@ -21,7 +21,6 @@ namespace kep3 { using xt::linalg::cross; -using xt::linalg::dot; const std::array lambert_problem::default_r1 = {{1.0, 0.0, 0.0}}; const std::array lambert_problem::default_r2 = {{0.0, 1.0, 0.0}}; @@ -57,7 +56,9 @@ lambert_problem::lambert_problem(const std::array &r1_a, // 1 - Getting lambda and T m_c = xt::linalg::norm(r2 - r1); - fmt::print("DEBUG {}, {}:", m_c, r2-r1); + fmt::print("\nxt::norm {}:", xt::norm(r2 - r1)); + fmt::print("\nxt::linalg::norm {}:", xt::linalg::norm(r2 - r1)); + double R1 = xt::linalg::norm(r1); double R2 = xt::linalg::norm(r2); m_s = (m_c + R1 + R2) / 2.0; @@ -103,7 +104,7 @@ lambert_problem::lambert_problem(const std::array &r1_a, // 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 = std::floor(T / kep3::pi); + m_Nmax = static_cast(T / kep3::pi); double T00 = std::acos(m_lambda) + m_lambda * std::sqrt(1.0 - lambda2); double T0 = (T00 + m_Nmax * kep3::pi); double T1 = 2.0 / 3.0 * (1.0 - lambda3), DT = 0.0, DDT = 0.0, DDDT = 0.0; @@ -156,15 +157,16 @@ lambert_problem::lambert_problem(const std::array &r1_a, m_iters[0] = householder(T, m_x[0], 0, 1e-5, 15); // 3.2 multi rev solutions double tmp = 0.; - for (decltype(m_Nmax) i = 1; i < m_Nmax + 1; ++i) { + for (std::vector::size_type i = 1u; i < m_Nmax + 1; ++i) { // 3.2.1 left Householder iterations - tmp = std::pow((i * M_PI + M_PI) / (8.0 * T), 2.0 / 3.0); + tmp = std::pow((static_cast(i) * kep3::pi + kep3::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 = std::pow((8.0 * T) / (i * M_PI), 2.0 / 3.0); + tmp = std::pow((8.0 * T) / (static_cast(i) * kep3::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); + m_iters[2ul * i] = householder(T, m_x[2 * i], i, 1e-8, 15); } // 4 - For each found x value we reconstruct the terminal velocities @@ -181,10 +183,10 @@ lambert_problem::lambert_problem(const std::array &r1_a, double vt = gamma * sigma * (y + m_lambda * m_x[i]); vt1 = vt / R1; vt2 = vt / R2; - for (int j = 0; j < 3; ++j) { + for (auto j = 0lu; j < 3lu; ++j) { m_v1[i][j] = vr1 * ir1[j] + vt1 * it1[j]; } - for (int j = 0; j < 3; ++j) { + for (auto j = 0lu; j < 3lu; ++j) { m_v2[i][j] = vr2 * ir2[j] + vt2 * it2[j]; } } @@ -237,7 +239,7 @@ void lambert_problem::x2tof2(double &tof, double x, // NOLINT } tof = ((a * std::sqrt(a) * ((alfa - std::sin(alfa)) - (beta - std::sin(beta)) + - 2.0 * M_PI * N)) / + 2.0 * kep3::pi * N)) / 2.0); } else { double alfa = 2.0 * std::acosh(x); @@ -276,7 +278,7 @@ void lambert_problem::x2tof(double &tof, double x, unsigned N) const { double d = 0.0; if (E < 0) { double l = std::acos(g); - d = N * M_PI + l; + d = N * kep3::pi + l; } else { double f = y * (z - m_lambda * x); d = std::log(f + g); From 73f0a0912af7dbc6b292e5bf00b94d1cd7de529e Mon Sep 17 00:00:00 2001 From: Dario Izzo Date: Thu, 21 Sep 2023 19:00:25 +0200 Subject: [PATCH 04/11] osx green --- src/lambert_problem.cpp | 8 ++++---- test/propagate_lagrangian_test.cpp | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/lambert_problem.cpp b/src/lambert_problem.cpp index e1d09a7d..0ff5b6e1 100644 --- a/src/lambert_problem.cpp +++ b/src/lambert_problem.cpp @@ -162,11 +162,11 @@ lambert_problem::lambert_problem(const std::array &r1_a, tmp = std::pow((static_cast(i) * kep3::pi + kep3::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); + m_iters[2 * i - 1] = householder(T, m_x[2 * i - 1], static_cast(i), 1e-8, 15); // 3.2.1 right Householder iterations tmp = std::pow((8.0 * T) / (static_cast(i) * kep3::pi), 2.0 / 3.0); m_x[2 * i] = (tmp - 1) / (tmp + 1); - m_iters[2ul * i] = householder(T, m_x[2 * i], i, 1e-8, 15); + m_iters[2ul * i] = householder(T, m_x[2 * i], static_cast(i), 1e-8, 15); } // 4 - For each found x value we reconstruct the terminal velocities @@ -195,7 +195,7 @@ lambert_problem::lambert_problem(const std::array &r1_a, unsigned lambert_problem::householder(double T, double &x0, unsigned N, // NOLINT double eps, unsigned iter_max) const { - int it = 0; + unsigned 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; @@ -407,7 +407,7 @@ std::ostream &operator<<(std::ostream &s, const lambert_problem &lp) { s << " v2 = " << "[" << lp.m_v2[0][0] << ", " << lp.m_v2[0][1] << ", " << lp.m_v2[0][2] << "]" << std::endl; - for (int i = 0; i < lp.m_Nmax; ++i) { + for (std::vector::size_type i = 0lu; 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]) diff --git a/test/propagate_lagrangian_test.cpp b/test/propagate_lagrangian_test.cpp index 5c8edc2c..373e0667 100644 --- a/test/propagate_lagrangian_test.cpp +++ b/test/propagate_lagrangian_test.cpp @@ -88,13 +88,13 @@ void test_propagate_lagrangian( // We test orbital parameters are unchanged for random propagations // Engines // NOLINTNEXTLINE(cert-msc32-c, cert-msc51-cpp) - std::mt19937 rng_engine(12201203u); + std::mt19937 rng_engine(1220202343u); { // Targeting Ellipses std::uniform_real_distribution sma_d(1.1, 10.); std::uniform_real_distribution ecc_d(0, 0.9); - std::uniform_real_distribution incl_d(0., pi); - std::uniform_real_distribution Omega_d(0, 2 * pi); + std::uniform_real_distribution incl_d(0., kep3::pi); + std::uniform_real_distribution Omega_d(0, 2 * kep3::pi); std::uniform_real_distribution omega_d(0., pi); std::uniform_real_distribution f_d(0, 2 * pi); std::uniform_real_distribution time_d(-2. * kep3::pi, From d7d0f5162818919b8164a9cf884f42125fa6c6ac Mon Sep 17 00:00:00 2001 From: Dario Izzo Date: Thu, 21 Sep 2023 21:20:23 +0200 Subject: [PATCH 05/11] proto-benchmark added to lambert --- benchmark/CMakeLists.txt | 2 + benchmark/lambert_problem_benchmark.cpp | 102 ++++++++++++++++++++++++ src/lambert_problem.cpp | 4 +- 3 files changed, 105 insertions(+), 3 deletions(-) create mode 100644 benchmark/lambert_problem_benchmark.cpp diff --git a/benchmark/CMakeLists.txt b/benchmark/CMakeLists.txt index 1530f755..355edfc7 100644 --- a/benchmark/CMakeLists.txt +++ b/benchmark/CMakeLists.txt @@ -18,3 +18,5 @@ endfunction() ADD_kep3_BENCHMARK(convert_anomalies_benchmark) ADD_kep3_BENCHMARK(propagate_lagrangian_benchmark) +ADD_kep3_BENCHMARK(lambert_problem_benchmark) + diff --git a/benchmark/lambert_problem_benchmark.cpp b/benchmark/lambert_problem_benchmark.cpp new file mode 100644 index 00000000..cab893ce --- /dev/null +++ b/benchmark/lambert_problem_benchmark.cpp @@ -0,0 +1,102 @@ +/***************************************************************************** + * 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 +#include +#include + +#include +#include + +#include +#include +#include + +int main() { + // Preamble + std::array r1{{0, 0, 0}}, r2{{0, 0, 0}}; + double tof = 0.; + // NOLINTNEXTLINE(cert-msc32-c, cert-msc51-cpp) + std::mt19937 rng_engine(122012203u); + std::uniform_int_distribution dist(0, 1); + std::uniform_real_distribution dist1(-2, 2); + + double acc = 0, err_max = 0; + unsigned count = 0u; + + // Experiment Settings + unsigned int Ntrials = 120000; + + // Start Experiment + for (unsigned int i = 0; i < Ntrials; ++i) { + // 1 - generate a random problem geometry + r1[0] = dist1(rng_engine); + r1[1] = dist1(rng_engine); + r1[2] = dist1(rng_engine); + r2[0] = dist1(rng_engine); + r2[1] = dist1(rng_engine); + r2[2] = dist1(rng_engine); + tof = (dist1(rng_engine) + 2) / 4 * 100 + 0.1; + + // 2 - Solve the lambert problem + try { + double mu = 1.0; + int revs_max = 20; + bool cw = static_cast(dist(rng_engine)); + kep3::lambert_problem lp(r1, r2, tof, mu, cw, revs_max); + + // 3 - Check its precision using propagate_lagrangian + for (const auto & v1 : lp.get_v1()) { + std::array, 2> pos_vel{{r1, v1}}; + kep3::propagate_lagrangian(pos_vel, tof, mu); + double err = + std::sqrt((pos_vel[0][0] - r2[0]) * (pos_vel[0][0] - r2[0]) + + (pos_vel[0][1] - r2[1]) * (pos_vel[0][1] - r2[1]) + + (pos_vel[0][2] - r2[2]) * (pos_vel[0][2] - r2[2])); + if (err > 1e-2) { + fmt::print("r1: {}\n r2: {}\n, tof: {}", r1, r2, tof); + } + err_max = std::max(err_max, err); + acc += err; + } + count += (lp.get_Nmax() * 2 + 1); + } catch (...) { + std::cout << "failed: " << std::endl; + std::cout << "r1[0]=" << r1[0] << "; r1[1]=" << r1[1] + << "; r1[2]=" << r1[2] << std::endl; + std::cout << "r2[0]=" << r2[0] << "; r2[1]=" << r2[1] + << "; r2[2]=" << r1[2] << std::endl; + std::cout << "tof=" << tof << std::endl; + } + } + std::cout << "Max error: " << err_max << std::endl; + std::cout << "Average Error: " << acc / count << std::endl; + std::cout << "Number of Problems Solved: " << count << std::endl; + if (err_max < 1e-6) { + return 0; + } else { + return 1; + } +} \ No newline at end of file diff --git a/src/lambert_problem.cpp b/src/lambert_problem.cpp index e1d09a7d..021cba31 100644 --- a/src/lambert_problem.cpp +++ b/src/lambert_problem.cpp @@ -56,8 +56,6 @@ lambert_problem::lambert_problem(const std::array &r1_a, // 1 - Getting lambda and T m_c = xt::linalg::norm(r2 - r1); - fmt::print("\nxt::norm {}:", xt::norm(r2 - r1)); - fmt::print("\nxt::linalg::norm {}:", xt::linalg::norm(r2 - r1)); double R1 = xt::linalg::norm(r1); double R2 = xt::linalg::norm(r2); @@ -162,7 +160,7 @@ lambert_problem::lambert_problem(const std::array &r1_a, tmp = std::pow((static_cast(i) * kep3::pi + kep3::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); + m_iters[2 * i - 1] = householder(T, m_x[2 * i - 1], i, 1e-8, 15); // 3.2.1 right Householder iterations tmp = std::pow((8.0 * T) / (static_cast(i) * kep3::pi), 2.0 / 3.0); m_x[2 * i] = (tmp - 1) / (tmp + 1); From f3ec8f7066bbcad9be7403f2f1ff052b77c824ba Mon Sep 17 00:00:00 2001 From: Dario Izzo Date: Thu, 21 Sep 2023 22:16:24 +0200 Subject: [PATCH 06/11] wip --- src/lambert_problem.cpp | 30 ++++++++++++++---------------- test/lambert_problem_test.cpp | 2 +- 2 files changed, 15 insertions(+), 17 deletions(-) diff --git a/src/lambert_problem.cpp b/src/lambert_problem.cpp index cd86d27f..e25a7687 100644 --- a/src/lambert_problem.cpp +++ b/src/lambert_problem.cpp @@ -28,11 +28,11 @@ const std::array 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] r1_a first cartesian position + * \param[in] r2_a 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] cw when true a retrograde orbit is assumed * \param[in] multi_revs maximum number of multirevolutions to compute */ lambert_problem::lambert_problem(const std::array &r1_a, @@ -75,22 +75,18 @@ lambert_problem::lambert_problem(const std::array &r1_a, double lambda2 = 1.0 - m_c / m_s; m_lambda = std::sqrt(lambda2); - xt::xtensor_fixed> it1{0.0, 0.0, 0.0}; - xt::xtensor_fixed> it2{0.0, 0.0, 0.0}; + auto it1 = cross(ih, ir1); + auto it2 = cross(ih, ir2); + it1 = it1 / xt::linalg::norm(it1); + it2 = it2 / xt::linalg::norm(it2); if (ih(2) < 0.0) // Transfer angle is larger than 180 degrees as seen from // above the z axis { m_lambda = -m_lambda; - it1 = cross(ir1, ih); - it2 = cross(ir2, ih); - } else { - it1 = cross(ih, ir1); - it2 = cross(ih, ir2); + it1 = -it1; + it2 = -it2; } - it1 = it1 / xt::linalg::norm(it1); - it2 = it2 / xt::linalg::norm(it2); - if (cw) { // Retrograde motion m_lambda = -m_lambda; it1 = -it1; @@ -117,7 +113,7 @@ lambert_problem::lambert_problem(const std::array &r1_a, if (DT != 0.0) { x_new = x_old - DT * DDT / (DDT * DDT - DT * DDDT / 2.0); } - err = fabs(x_old - x_new); + err = std::abs(x_old - x_new); if ((err < 1e-13) || (it > 12)) { break; } @@ -160,11 +156,13 @@ lambert_problem::lambert_problem(const std::array &r1_a, tmp = std::pow((static_cast(i) * kep3::pi + kep3::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], static_cast(i), 1e-8, 15); + m_iters[2 * i - 1] = + householder(T, m_x[2 * i - 1], static_cast(i), 1e-8, 15); // 3.2.1 right Householder iterations tmp = std::pow((8.0 * T) / (static_cast(i) * kep3::pi), 2.0 / 3.0); m_x[2 * i] = (tmp - 1) / (tmp + 1); - m_iters[2ul * i] = householder(T, m_x[2 * i], static_cast(i), 1e-8, 15); + m_iters[2ul * i] = + householder(T, m_x[2 * i], static_cast(i), 1e-8, 15); } // 4 - For each found x value we reconstruct the terminal velocities diff --git a/test/lambert_problem_test.cpp b/test/lambert_problem_test.cpp index 9b984117..1d096e7b 100644 --- a/test/lambert_problem_test.cpp +++ b/test/lambert_problem_test.cpp @@ -15,7 +15,7 @@ TEST_CASE("construct") { - kep3::lambert_problem lp{{1.,0.,0.}, {0.,1.,0.}, kep3::pi/2,1.}; + kep3::lambert_problem lp{{1.,0.,0.}, {0.,1.,0.}, kep3::pi/2 + 2*kep3::pi,1.,false,100}; std::cout << lp << std::endl; } \ No newline at end of file From f307021113d9f0672f330204e88aa6081af83336 Mon Sep 17 00:00:00 2001 From: Dario Izzo Date: Fri, 22 Sep 2023 16:46:10 +0200 Subject: [PATCH 07/11] delta guidance error used in Lambert tests --- benchmark/lambert_problem_benchmark.cpp | 47 +++++++------------- src/core_astro/propagate_lagrangian.cpp | 21 +++++---- src/lambert_problem.cpp | 2 +- test/CMakeLists.txt | 2 +- test/lambert_problem_test.cpp | 57 +++++++++++++++++++++++-- test/propagate_lagrangian_test.cpp | 22 ++++------ test/test_helpers.hpp | 32 ++++++++++++-- 7 files changed, 121 insertions(+), 62 deletions(-) diff --git a/benchmark/lambert_problem_benchmark.cpp b/benchmark/lambert_problem_benchmark.cpp index cab893ce..f8737cc7 100644 --- a/benchmark/lambert_problem_benchmark.cpp +++ b/benchmark/lambert_problem_benchmark.cpp @@ -33,6 +33,7 @@ #include #include #include +#include int main() { // Preamble @@ -61,42 +62,24 @@ int main() { tof = (dist1(rng_engine) + 2) / 4 * 100 + 0.1; // 2 - Solve the lambert problem - try { - double mu = 1.0; - int revs_max = 20; - bool cw = static_cast(dist(rng_engine)); - kep3::lambert_problem lp(r1, r2, tof, mu, cw, revs_max); + double mu = 1.0; + unsigned revs_max = 20; + bool cw = static_cast(dist(rng_engine)); + kep3::lambert_problem lp(r1, r2, tof, mu, cw, revs_max); - // 3 - Check its precision using propagate_lagrangian - for (const auto & v1 : lp.get_v1()) { - std::array, 2> pos_vel{{r1, v1}}; - kep3::propagate_lagrangian(pos_vel, tof, mu); - double err = - std::sqrt((pos_vel[0][0] - r2[0]) * (pos_vel[0][0] - r2[0]) + - (pos_vel[0][1] - r2[1]) * (pos_vel[0][1] - r2[1]) + - (pos_vel[0][2] - r2[2]) * (pos_vel[0][2] - r2[2])); - if (err > 1e-2) { - fmt::print("r1: {}\n r2: {}\n, tof: {}", r1, r2, tof); - } - err_max = std::max(err_max, err); - acc += err; - } - count += (lp.get_Nmax() * 2 + 1); - } catch (...) { - std::cout << "failed: " << std::endl; - std::cout << "r1[0]=" << r1[0] << "; r1[1]=" << r1[1] - << "; r1[2]=" << r1[2] << std::endl; - std::cout << "r2[0]=" << r2[0] << "; r2[1]=" << r2[1] - << "; r2[2]=" << r1[2] << std::endl; - std::cout << "tof=" << tof << std::endl; + // 3 - Check its precision using propagate_lagrangian + for (const auto &v1 : lp.get_v1()) { + std::array, 2> pos_vel{{r1, v1}}; + kep3::propagate_lagrangian(pos_vel, tof, mu); + double err = std::sqrt((pos_vel[0][0] - r2[0]) * (pos_vel[0][0] - r2[0]) + + (pos_vel[0][1] - r2[1]) * (pos_vel[0][1] - r2[1]) + + (pos_vel[0][2] - r2[2]) * (pos_vel[0][2] - r2[2])); + err_max = std::max(err_max, err); + acc += err; } + count += (lp.get_Nmax() * 2 + 1); } std::cout << "Max error: " << err_max << std::endl; std::cout << "Average Error: " << acc / count << std::endl; std::cout << "Number of Problems Solved: " << count << std::endl; - if (err_max < 1e-6) { - return 0; - } else { - return 1; - } } \ No newline at end of file diff --git a/src/core_astro/propagate_lagrangian.cpp b/src/core_astro/propagate_lagrangian.cpp index 0a92d505..3670d91d 100644 --- a/src/core_astro/propagate_lagrangian.cpp +++ b/src/core_astro/propagate_lagrangian.cpp @@ -11,6 +11,8 @@ #include #include +#include + #include #include @@ -46,7 +48,7 @@ void propagate_lagrangian(std::array, 2> &pos_vel_0, // Solve Kepler Equation for ellipses in DE (eccentric anomaly difference) const int digits = std::numeric_limits::digits; - std::uintmax_t max_iter = 100u; + std::uintmax_t max_iter = 50u; double DE = boost::math::tools::halley_iterate( [DM, sigma0, sqrta, a, R](double DE) { return std::make_tuple(kepDE(DE, DM, sigma0, sqrta, a, R), @@ -54,7 +56,7 @@ void propagate_lagrangian(std::array, 2> &pos_vel_0, dd_kepDE(DE, sigma0, sqrta, a, R)); }, IG, IG - pi, IG + pi, digits, max_iter); - if (max_iter == 100u) { + if (max_iter == 50u) { throw std::domain_error( "Maximum number of iterations exceeded when solving Kepler's " "equation for the eccentric anomaly in propagate_lagrangian."); @@ -78,15 +80,18 @@ void propagate_lagrangian(std::array, 2> &pos_vel_0, // Solve Kepler Equation for ellipses in DH (hyperbolic anomaly difference) const int digits = std::numeric_limits::digits; - std::uintmax_t max_iter = 100u; + std::uintmax_t max_iter = 50u; double DH = boost::math::tools::halley_iterate( [DN, sigma0, sqrta, a, R](double DH) { return std::make_tuple(kepDH(DH, DN, sigma0, sqrta, a, R), d_kepDH(DH, sigma0, sqrta, a, R), dd_kepDH(DH, sigma0, sqrta, a, R)); }, - IG, IG - pi, IG + pi, digits, max_iter); - if (max_iter == 100u) { + IG, IG - 20, IG + 20, digits, + max_iter); // TODO (dario): study this hyperbolic equation in more details as + // to provide decent and well proved bounds + //fmt::print("Solution DH: {}\nIters: {}\n", DH, max_iter); + if (max_iter == 50u) { throw std::domain_error( "Maximum number of iterations exceeded when solving Kepler's " "equation for the hyperbolic anomaly in propagate_lagrangian."); @@ -95,11 +100,11 @@ void propagate_lagrangian(std::array, 2> &pos_vel_0, double r = a + (R - a) * std::cosh(DH) + sigma0 * sqrta * std::sinh(DH); // Lagrange coefficients - F = 1 - a / R * (1 - std::cosh(DH)); - G = a * sigma0 / std::sqrt(mu) * (1 - std::cosh(DH)) + + F = 1. - a / R * (1. - std::cosh(DH)); + G = a * sigma0 / std::sqrt(mu) * (1. - std::cosh(DH)) + R * std::sqrt(-a / mu) * std::sinh(DH); Ft = -std::sqrt(-mu * a) / (r * R) * std::sinh(DH); - Gt = 1 - a / r * (1 - std::cosh(DH)); + Gt = 1. - a / r * (1. - std::cosh(DH)); } double temp[3] = {r0[0], r0[1], r0[2]}; diff --git a/src/lambert_problem.cpp b/src/lambert_problem.cpp index e25a7687..4010b5ad 100644 --- a/src/lambert_problem.cpp +++ b/src/lambert_problem.cpp @@ -383,7 +383,7 @@ std::ostream &operator<<(std::ostream &s, const lambert_problem &lp) { s << "r1 = " << "[" << lp.m_r1[0] << ", " << lp.m_r1[1] << ", " << lp.m_r1[2] << "]" << std::endl; - s << "r1 = " + s << "r2 = " << "[" << lp.m_r2[0] << ", " << lp.m_r2[1] << ", " << lp.m_r2[2] << "]" << std::endl; s << "Time of flight: " << lp.m_tof << std::endl << std::endl; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index ede3e724..7793a314 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -13,7 +13,7 @@ set_property(TARGET kep3_test PROPERTY CXX_EXTENSIONS NO) function(ADD_kep3_TESTCASE arg1) add_executable(${arg1} ${arg1}.cpp) - target_link_libraries(${arg1} PRIVATE kep3_test kep3 Boost::boost) + target_link_libraries(${arg1} PRIVATE kep3_test kep3 xtensor xtensor-blas Boost::boost) target_compile_options(${arg1} PRIVATE "$<$:${kep3_CXX_FLAGS_DEBUG}>" "$<$:${kep3_CXX_FLAGS_RELEASE}>" diff --git a/test/lambert_problem_test.cpp b/test/lambert_problem_test.cpp index 1d096e7b..59e09b94 100644 --- a/test/lambert_problem_test.cpp +++ b/test/lambert_problem_test.cpp @@ -9,13 +9,64 @@ #include +#include +#include +#include +#include + #include #include "catch.hpp" - +#include "test_helpers.hpp" TEST_CASE("construct") { - kep3::lambert_problem lp{{1.,0.,0.}, {0.,1.,0.}, kep3::pi/2 + 2*kep3::pi,1.,false,100}; - std::cout << lp << std::endl; + // Here we test construction of a few simple geometries + kep3::lambert_problem lp{{1., 0., 0.}, {0., 1., 0.}, 3 * kep3::pi / 2, + 1., true, 100}; +} + +TEST_CASE("delta_guidance") { + // Here we test that in a number of randomly generated Lambert Problems + // The boundary data must satisfy the Delta Guidance law + // see Battin: "An Introduction to the Mathematics and Methods of + // Astrodynamics, Revised Edition", Introduction. + // + // (v1 x r1).(v1 x (r2 - r1)) + mu r2 . (r2/|r2| - r1/{|r1|}) + + // Preamble + std::array r1{{0, 0, 0}}, r2{{0, 0, 0}}; + double tof = 0.; + using xt::linalg::cross; + using xt::linalg::dot; + + // NOLINTNEXTLINE(cert-msc32-c, cert-msc51-cpp) + std::mt19937 rng_engine(122012203u); + std::uniform_int_distribution cw_d(0, 1); + std::uniform_real_distribution r_d(-2, 2); + std::uniform_real_distribution tof_d(2., 40.); + std::uniform_real_distribution mu_d(0.9, 1.1); + unsigned revs_max = 20u; + + unsigned trials = 10000u; + + for (auto i = 0u; i < trials; ++i) { + // 1 - generate a random problem geometry + r1[0] = r_d(rng_engine); + r1[1] = r_d(rng_engine); + r1[2] = r_d(rng_engine); + r2[0] = r_d(rng_engine); + r2[1] = r_d(rng_engine); + r2[2] = r_d(rng_engine); + tof = tof_d(rng_engine); + bool cw = static_cast(cw_d(rng_engine)); + double mu = mu_d(rng_engine); + + // 2 - Solve the lambert problem + kep3::lambert_problem lp(r1, r2, tof, mu, cw, revs_max); + // 3 - Check the Delta guidance error + for (const auto &v1 : lp.get_v1()) { + REQUIRE(kep3_tests::delta_guidance_error(r1, r2, v1, mu) < 1e-12); + } + } } \ No newline at end of file diff --git a/test/propagate_lagrangian_test.cpp b/test/propagate_lagrangian_test.cpp index 373e0667..afc63ee7 100644 --- a/test/propagate_lagrangian_test.cpp +++ b/test/propagate_lagrangian_test.cpp @@ -156,18 +156,14 @@ TEST_CASE("propagate_lagrangian") { test_propagate_lagrangian(&propagate_lagrangian_u, 10000u); } -TEST_CASE("infinite loop") { +TEST_CASE("extreme_orbit_H") { std::array, 2> pos_vel = { - {{3.2479950146598147, 4.866100102242875, 0.8564969484971678}, - {0.3140399734911721, 0.5042257639093218, 0.09475180867356801}}}; - double tof = 6.023574175415248; - kep3::propagate_lagrangian(pos_vel, -tof, 1.); -} - -TEST_CASE("extreme_orbit") { - std::array, 2> pos_vel = { - {{0.8086322075411211, -1.3297145067523164, -2.4969299661382585}, - {-0.02869546877795607, 0.05765808202641542, -0.03999826575867087}}}; - double tof = 4.454030166101634; - propagate_lagrangian_u(pos_vel, tof, 1.); + {{-0.3167755980094844, -1.916113450769878, 0.899028670370861}, + {1.2231112281789407, 7.472926753229921, -3.5814204332202086}}}; + double tof = 0.5150723675394596; + double mu = 1.; + kep3::propagate_lagrangian(pos_vel, tof, mu); + REQUIRE(kep3_tests::floating_point_error_vector( + pos_vel[0], {0.6049892513157507, 1.314038087851452, + 1.747826097602214}) < 1e-11); } \ No newline at end of file diff --git a/test/test_helpers.hpp b/test/test_helpers.hpp index f1003561..ad27594f 100644 --- a/test/test_helpers.hpp +++ b/test/test_helpers.hpp @@ -14,20 +14,44 @@ #include #include +#include +#include + namespace kep3_tests { -// This is a float test which, while controversial, will test for abs differences in small numbers, -// relative otherwise. +using xt::linalg::cross; +using xt::linalg::dot; +// This is a float test which, while controversial, will test for abs +// differences in small numbers, relative otherwise. inline double floating_point_error(double a, double b) { return std::abs(a - b) / std::max(1., std::max(a, b)); } // This tests how close two vectors are in the euclidean metric. err = r2-r1 -inline double floating_point_error_vector(const std::array &r1, const std::array &r2) { +inline double floating_point_error_vector(const std::array &r1, + const std::array &r2) { double R1 = std::sqrt(r1[0] * r1[0] + r1[1] * r1[1] + r1[2] * r1[2]); - std::array r12 = {{r2[0]-r1[0], r2[1]-r1[1], r2[2]-r1[2]}}; + std::array r12 = {{r2[0] - r1[0], r2[1] - r1[1], r2[2] - r1[2]}}; double R12 = std::sqrt(r12[0] * r12[0] + r12[1] * r12[1] + r12[2] * r12[2]); return R12 / std::max(1., R1); } + +// see Battin: "An Introduction to the Mathematics and Methods of +// Astrodynamics, Revised Edition", Introduction. +// +// On Keplerian dynamics the following must hold. +// +// (v1 x r1).(v1 x (r2 - r1)) + mu r2 . (r2/|r2| - r1/{|r1|}) +inline double delta_guidance_error(const std::array &r1, + const std::array &r2, + const std::array &v1, double mu) { + const auto r1_x = xt::adapt(r1); + const auto r2_x = xt::adapt(r2); + const auto v1_x = xt::adapt(v1); + auto F = dot(cross(v1_x, r1_x), cross(v1_x, (r2_x - r1_x))) + + mu * dot(r2_x, (r2_x / xt::linalg::norm(r2_x) - + r1_x / xt::linalg::norm(r1_x))); + return F(0); +} } // namespace kep3_tests #endif // kep3_TEST_HELPERS_H From 5e3c8c8217b59ca0d3532c2ca27debb73faa93cb Mon Sep 17 00:00:00 2001 From: Dario Izzo Date: Fri, 22 Sep 2023 17:13:40 +0200 Subject: [PATCH 08/11] bench wip --- benchmark/lambert_problem_benchmark.cpp | 72 ++++++++++---------- benchmark/propagate_lagrangian_benchmark.cpp | 7 +- test/lambert_problem_test.cpp | 7 +- 3 files changed, 41 insertions(+), 45 deletions(-) diff --git a/benchmark/lambert_problem_benchmark.cpp b/benchmark/lambert_problem_benchmark.cpp index f8737cc7..8d4d3dec 100644 --- a/benchmark/lambert_problem_benchmark.cpp +++ b/benchmark/lambert_problem_benchmark.cpp @@ -23,6 +23,8 @@ * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * *****************************************************************************/ +#include +#include #include #include #include @@ -35,51 +37,49 @@ #include #include +using std::chrono::duration_cast; +using std::chrono::high_resolution_clock; +using std::chrono::microseconds; + int main() { // Preamble - std::array r1{{0, 0, 0}}, r2{{0, 0, 0}}; - double tof = 0.; + const unsigned trials = 10000u; + + std::array, trials> r1s{}, r2s{}; + std::array tof{}; + std::array cw{}; + std::array mu{}; + // NOLINTNEXTLINE(cert-msc32-c, cert-msc51-cpp) std::mt19937 rng_engine(122012203u); - std::uniform_int_distribution dist(0, 1); - std::uniform_real_distribution dist1(-2, 2); + std::uniform_int_distribution cw_d(0, 1); + std::uniform_real_distribution r_d(-2, 2); + std::uniform_real_distribution tof_d(2., 40.); + std::uniform_real_distribution mu_d(0.9, 1.1); + unsigned revs_max = 20u; - double acc = 0, err_max = 0; unsigned count = 0u; - // Experiment Settings - unsigned int Ntrials = 120000; - - // Start Experiment - for (unsigned int i = 0; i < Ntrials; ++i) { + for (auto i = 0u; i < trials; ++i) { // 1 - generate a random problem geometry - r1[0] = dist1(rng_engine); - r1[1] = dist1(rng_engine); - r1[2] = dist1(rng_engine); - r2[0] = dist1(rng_engine); - r2[1] = dist1(rng_engine); - r2[2] = dist1(rng_engine); - tof = (dist1(rng_engine) + 2) / 4 * 100 + 0.1; + r1s[i][0] = r_d(rng_engine); + r1s[i][1] = r_d(rng_engine); + r1s[i][2] = r_d(rng_engine); + r2s[i][0] = r_d(rng_engine); + r2s[i][1] = r_d(rng_engine); + r2s[i][2] = r_d(rng_engine); + tof[i] = tof_d(rng_engine); + cw[i] = static_cast(cw_d(rng_engine)); + mu[i] = mu_d(rng_engine); + } + auto start = high_resolution_clock::now(); + for (auto i = 0u; i < trials; ++i) { // 2 - Solve the lambert problem - double mu = 1.0; - unsigned revs_max = 20; - bool cw = static_cast(dist(rng_engine)); - kep3::lambert_problem lp(r1, r2, tof, mu, cw, revs_max); - - // 3 - Check its precision using propagate_lagrangian - for (const auto &v1 : lp.get_v1()) { - std::array, 2> pos_vel{{r1, v1}}; - kep3::propagate_lagrangian(pos_vel, tof, mu); - double err = std::sqrt((pos_vel[0][0] - r2[0]) * (pos_vel[0][0] - r2[0]) + - (pos_vel[0][1] - r2[1]) * (pos_vel[0][1] - r2[1]) + - (pos_vel[0][2] - r2[2]) * (pos_vel[0][2] - r2[2])); - err_max = std::max(err_max, err); - acc += err; - } - count += (lp.get_Nmax() * 2 + 1); + kep3::lambert_problem lp(r1s[i], r2s[i], tof[i], mu[i], cw[i], revs_max); + count++; } - std::cout << "Max error: " << err_max << std::endl; - std::cout << "Average Error: " << acc / count << std::endl; - std::cout << "Number of Problems Solved: " << count << std::endl; + auto stop = high_resolution_clock::now(); + auto duration = duration_cast(stop - start); + fmt::print("{:.3f}s\n", (static_cast(duration.count()) / 1e6)); } \ No newline at end of file diff --git a/benchmark/propagate_lagrangian_benchmark.cpp b/benchmark/propagate_lagrangian_benchmark.cpp index edeb6e45..6e79e590 100644 --- a/benchmark/propagate_lagrangian_benchmark.cpp +++ b/benchmark/propagate_lagrangian_benchmark.cpp @@ -7,10 +7,10 @@ // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. -#include "kep3/core_astro/ic2par2ic.hpp" -#include "kep3/core_astro/propagate_lagrangian.hpp" #include #include +#include +#include #include #include @@ -113,7 +113,8 @@ void perform_test_accuracy( } pos_vels[i] = kep3::par2ic({sma, ecc, incl_d(rng_engine), - Omega_d(rng_engine), omega_d(rng_engine), f}, 1.); + Omega_d(rng_engine), omega_d(rng_engine), f}, + 1.); tofs[i] = tof_d(rng_engine); } // We log progress diff --git a/test/lambert_problem_test.cpp b/test/lambert_problem_test.cpp index 59e09b94..d5992abc 100644 --- a/test/lambert_problem_test.cpp +++ b/test/lambert_problem_test.cpp @@ -28,16 +28,11 @@ TEST_CASE("construct") { TEST_CASE("delta_guidance") { // Here we test that in a number of randomly generated Lambert Problems // The boundary data must satisfy the Delta Guidance law - // see Battin: "An Introduction to the Mathematics and Methods of - // Astrodynamics, Revised Edition", Introduction. - // - // (v1 x r1).(v1 x (r2 - r1)) + mu r2 . (r2/|r2| - r1/{|r1|}) + // Preamble std::array r1{{0, 0, 0}}, r2{{0, 0, 0}}; double tof = 0.; - using xt::linalg::cross; - using xt::linalg::dot; // NOLINTNEXTLINE(cert-msc32-c, cert-msc51-cpp) std::mt19937 rng_engine(122012203u); From d639d034ebb78402d8aa18a63157278d49d4f8b7 Mon Sep 17 00:00:00 2001 From: Dario Izzo Date: Sat, 23 Sep 2023 13:25:05 +0200 Subject: [PATCH 09/11] accuracies adjusted and numerical stability improvements added --- benchmark/lambert_problem_benchmark.cpp | 28 ++++- benchmark/propagate_lagrangian_benchmark.cpp | 14 ++- include/kep3/core_astro/convert_anomalies.hpp | 54 ++++++++- .../kep3/core_astro/propagate_lagrangian.hpp | 4 + src/core_astro/propagate_lagrangian.cpp | 86 +++++++++++---- src/lambert_problem.cpp | 7 +- test/CMakeLists.txt | 1 + test/lambert_problem_test.cpp | 10 +- test/propagate_keplerian_test.cpp | 103 ++++++++++++++++++ 9 files changed, 267 insertions(+), 40 deletions(-) create mode 100644 test/propagate_keplerian_test.cpp diff --git a/benchmark/lambert_problem_benchmark.cpp b/benchmark/lambert_problem_benchmark.cpp index 8d4d3dec..97c3003a 100644 --- a/benchmark/lambert_problem_benchmark.cpp +++ b/benchmark/lambert_problem_benchmark.cpp @@ -42,8 +42,8 @@ using std::chrono::high_resolution_clock; using std::chrono::microseconds; int main() { - // Preamble - const unsigned trials = 10000u; + // Number of trials + const unsigned trials = 50000u; std::array, trials> r1s{}, r2s{}; std::array tof{}; @@ -57,8 +57,7 @@ int main() { std::uniform_real_distribution tof_d(2., 40.); std::uniform_real_distribution mu_d(0.9, 1.1); unsigned revs_max = 20u; - - unsigned count = 0u; + unsigned long count = 0lu; for (auto i = 0u; i < trials; ++i) { // 1 - generate a random problem geometry @@ -77,9 +76,26 @@ int main() { for (auto i = 0u; i < trials; ++i) { // 2 - Solve the lambert problem kep3::lambert_problem lp(r1s[i], r2s[i], tof[i], mu[i], cw[i], revs_max); - count++; + count = count + lp.get_v1().size(); } auto stop = high_resolution_clock::now(); auto duration = duration_cast(stop - start); - fmt::print("{:.3f}s\n", (static_cast(duration.count()) / 1e6)); + fmt::print("Lambert:\n{} solutions computed in {:.3f}s\n", count, + (static_cast(duration.count()) / 1e6)); + fmt::print("Projected number of solutions per second: {}\n", + static_cast(count) / ((static_cast(duration.count()) / 1e6))); + + count = 0; //reset counter + start = high_resolution_clock::now(); + for (auto i = 0u; i < trials; ++i) { + // 3 - Solve the lambert problem for the singe rev case + kep3::lambert_problem lp(r1s[i], r2s[i], tof[i], mu[i], cw[i], 0u); + count += 1u; + } + stop = high_resolution_clock::now(); + duration = duration_cast(stop - start); + fmt::print("\nLambert (0 revs only):\n{} solutions computed in {:.3f}s\n", count, + (static_cast(duration.count()) / 1e6)); + fmt::print("Projected number of solutions per second: {}\n", + static_cast(count) / ((static_cast(duration.count()) / 1e6))); } \ No newline at end of file diff --git a/benchmark/propagate_lagrangian_benchmark.cpp b/benchmark/propagate_lagrangian_benchmark.cpp index 6e79e590..21747bbf 100644 --- a/benchmark/propagate_lagrangian_benchmark.cpp +++ b/benchmark/propagate_lagrangian_benchmark.cpp @@ -90,7 +90,7 @@ void perform_test_accuracy( // // Distributions // - std::uniform_real_distribution sma_d(0.5, 20.); + std::uniform_real_distribution sma_d(0.5, 10.); std::uniform_real_distribution ecc_d(min_ecc, max_ecc); std::uniform_real_distribution incl_d(0., pi); std::uniform_real_distribution Omega_d(0, 2 * pi); @@ -163,4 +163,16 @@ int main() { perform_test_accuracy(0, 0.5, 100000, &kep3::propagate_lagrangian_u); perform_test_accuracy(0.5, 0.9, 100000, &kep3::propagate_lagrangian_u); perform_test_accuracy(0.9, 0.99, 100000, &kep3::propagate_lagrangian_u); + + fmt::print("\nComputes speed at different eccentricity ranges [keplerian " + "propagation]:\n"); + perform_test_speed(0, 0.5, 1000000, &kep3::propagate_keplerian); + perform_test_speed(0.5, 0.9, 1000000, &kep3::propagate_keplerian); + perform_test_speed(0.9, 0.99, 1000000, &kep3::propagate_keplerian); + + fmt::print("\nComputes error at different eccentricity ranges [keplerian " + "propagation]:\n"); + perform_test_accuracy(0, 0.5, 100000, &kep3::propagate_keplerian); + perform_test_accuracy(0.5, 0.9, 100000, &kep3::propagate_keplerian); + perform_test_accuracy(0.9, 0.99, 100000, &kep3::propagate_keplerian); } \ No newline at end of file diff --git a/include/kep3/core_astro/convert_anomalies.hpp b/include/kep3/core_astro/convert_anomalies.hpp index 3a9fb3b0..2b75ede9 100644 --- a/include/kep3/core_astro/convert_anomalies.hpp +++ b/include/kep3/core_astro/convert_anomalies.hpp @@ -34,15 +34,16 @@ inline double m2e(double M, double ecc) { if (M_cropped < 0) { M_cropped += 2 * kep3::pi; } - // The Initial guess follows from a third order expansion of Kepler's equation. + // The Initial guess follows from a third order expansion of Kepler's + // equation. double IG = M_cropped + ecc * sinM + ecc * ecc * sinM * cosM + ecc * ecc * ecc * sinM * (1.5 * cosM * cosM - 0.5); const int digits = std::numeric_limits::digits; std::uintmax_t max_iter = 100u; - // Newton-raphson or Halley iterates can be used here. Similar performances, thus - // we choose the simplest algorithm. + // Newton-raphson or Halley iterates can be used here. Similar performances, + // thus we choose the simplest algorithm. double sol = boost::math::tools::newton_raphson_iterate( [M_cropped, ecc](double E) { return std::make_tuple(kepE(E, M_cropped, ecc), d_kepE(E, ecc)); @@ -62,6 +63,7 @@ inline double e2m(double E, double ecc) { return (E - ecc * std::sin(E)); } inline double e2f(double E, double ecc) { return 2 * std::atan(std::sqrt((1 + ecc) / (1 - ecc)) * std::tan(E / 2)); } + // true to eccentric (only ellipses) e<1 (returns in range [-pi,pi]) inline double f2e(double f, double ecc) { return 2 * std::atan(std::sqrt((1 - ecc) / (1 + ecc)) * std::tan(f / 2)); @@ -69,6 +71,7 @@ inline double f2e(double f, double ecc) { // mean to true (only ellipses) e<1 (returns in range [-pi,pi]) inline double m2f(double M, double ecc) { return e2f(m2e(M, ecc), ecc); } + // true to mean (only ellipses) e<1 (returns in range [-pi,pi]) inline double f2m(double f, double ecc) { return e2m(f2e(f, ecc), ecc); } @@ -76,9 +79,54 @@ inline double f2m(double f, double ecc) { return e2m(f2e(f, ecc), ecc); } inline double zeta2f(double f, double ecc) { return 2 * std::atan(std::sqrt((1 + ecc) / (ecc - 1)) * std::tan(f / 2)); } + // true to gudermannian (only hyperbolas) e>1 (returns in range [-pi,pi]) inline double f2zeta(double zeta, double ecc) { return 2 * std::atan(std::sqrt((ecc - 1) / (1 + ecc)) * std::tan(zeta / 2)); } + +// mean to hyperbolic (only hyperbolas) e>1. +// NOLINTNEXTLINE(bugprone-easily-swappable-parameters) +inline double n2h(double N, double ecc) { + + // The Initial guess (TODO(darioizo) improve) + double IG = 1.; + + const int digits = std::numeric_limits::digits; + std::uintmax_t max_iter = 100u; + + // Newton-raphson iterates. + double sol = boost::math::tools::newton_raphson_iterate( + [N, ecc](double H) { + return std::make_tuple(kepH(H, N, ecc), d_kepH(H, ecc)); + }, + IG, IG - 20 * kep3::pi, IG + 20 * kep3::pi, digits, max_iter); + if (max_iter == 100u) { + throw std::domain_error( + "Maximum number of iterations exceeded when solving Kepler's " + "equation for the hyperbolic anomaly in m2h."); + } + return sol; +} + +// hyperbolic H to hyperbolic mean N (only hyperbolas) e>1 +inline double h2n(double H, double ecc) { return (ecc * std::sinh(H) - H); } + +// hyperbolic H to true (only hyperbolas) e>1 (returns in range [-pi,pi]) +inline double h2f(double H, double ecc) { + return 2 * std::atan(std::sqrt((1 + ecc) / (ecc - 1)) * std::tanh(H / 2)); +} + +// true to hyperbolic H (only hyperbolas) e>1 (returns in range [-pi,pi]) +inline double f2h(double f, double ecc) { + return 2 * std::atanh(std::sqrt((ecc - 1) / (1 + ecc)) * std::tan(f / 2)); +} + +// mean hyperbolic to true (only hyperbolas) e>1 (returns in range [-pi,pi]) +inline double n2f(double N, double ecc) { return h2f(n2h(N, ecc), ecc); } + +// true to mean hyperbolic (only hyperbolas) e>1 (returns in range [-pi,pi]) +inline double f2n(double f, double ecc) { return h2n(f2h(f, ecc), ecc); } + } // namespace kep3 #endif // kep3_TOOLBOX_M2E_H diff --git a/include/kep3/core_astro/propagate_lagrangian.hpp b/include/kep3/core_astro/propagate_lagrangian.hpp index 889fc434..6cee85e6 100644 --- a/include/kep3/core_astro/propagate_lagrangian.hpp +++ b/include/kep3/core_astro/propagate_lagrangian.hpp @@ -13,6 +13,7 @@ #include #include + #include namespace kep3 { @@ -30,6 +31,9 @@ kep3_DLL_PUBLIC void propagate_lagrangian(std::array, 2> & kep3_DLL_PUBLIC void propagate_lagrangian_u(std::array, 2> &pos_vel, double dt, double mu); + +kep3_DLL_PUBLIC void propagate_keplerian(std::array, 2> &pos_vel, + double dt, double mu); } // namespace kep3 #endif // KEP_TOOLBOX_PROPAGATE_LAGRANGIAN_H \ No newline at end of file diff --git a/src/core_astro/propagate_lagrangian.cpp b/src/core_astro/propagate_lagrangian.cpp index 3670d91d..aa61efe4 100644 --- a/src/core_astro/propagate_lagrangian.cpp +++ b/src/core_astro/propagate_lagrangian.cpp @@ -7,15 +7,16 @@ // Public License v. 2.0. If a copy of the MPL was not distributed // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. +#include "kep3/core_astro/ic2par2ic.hpp" #include #include #include -#include - #include +#include #include +#include #include #include #include @@ -48,18 +49,22 @@ void propagate_lagrangian(std::array, 2> &pos_vel_0, // Solve Kepler Equation for ellipses in DE (eccentric anomaly difference) const int digits = std::numeric_limits::digits; - std::uintmax_t max_iter = 50u; - double DE = boost::math::tools::halley_iterate( + std::uintmax_t max_iter = 100u; + // NOTE: Halley iterates may result into instabilities (specially with a + // poor IG) + + double DE = boost::math::tools::newton_raphson_iterate( [DM, sigma0, sqrta, a, R](double DE) { return std::make_tuple(kepDE(DE, DM, sigma0, sqrta, a, R), - d_kepDE(DE, sigma0, sqrta, a, R), - dd_kepDE(DE, sigma0, sqrta, a, R)); + d_kepDE(DE, sigma0, sqrta, a, R)); }, IG, IG - pi, IG + pi, digits, max_iter); - if (max_iter == 50u) { - throw std::domain_error( + if (max_iter == 100u) { + throw std::domain_error(fmt::format( "Maximum number of iterations exceeded when solving Kepler's " - "equation for the eccentric anomaly in propagate_lagrangian."); + "equation for the eccentric anomaly in propagate_lagrangian.\n" + "DM={}\nsigma0={}\nsqrta={}\na={}\nR={}\nDE={}", + DM, sigma0, sqrta, a, R, DE)); } double r = a + (R - a) * std::cos(DE) + sigma0 * sqrta * std::sin(DE); @@ -73,28 +78,30 @@ void propagate_lagrangian(std::array, 2> &pos_vel_0, sqrta = std::sqrt(-a); double DN = std::sqrt(-mu / a / a / a) * dt; double IG = 0.; - dt > 0. ? IG = 3. - : IG = -3.; // TODO(darioizzo): find a better initial guess. + dt > 0. ? IG = 1. + : IG = -1.; // TODO(darioizzo): find a better initial guess. // I tried with 0 and DN (both have numercial // problems and result in exceptions) // Solve Kepler Equation for ellipses in DH (hyperbolic anomaly difference) const int digits = std::numeric_limits::digits; - std::uintmax_t max_iter = 50u; - double DH = boost::math::tools::halley_iterate( + std::uintmax_t max_iter = 100u; + // NOTE: Halley iterates may result into instabilities (specially with a + // poor IG) + double DH = boost::math::tools::newton_raphson_iterate( [DN, sigma0, sqrta, a, R](double DH) { return std::make_tuple(kepDH(DH, DN, sigma0, sqrta, a, R), - d_kepDH(DH, sigma0, sqrta, a, R), - dd_kepDH(DH, sigma0, sqrta, a, R)); + d_kepDH(DH, sigma0, sqrta, a, R)); }, - IG, IG - 20, IG + 20, digits, - max_iter); // TODO (dario): study this hyperbolic equation in more details as - // to provide decent and well proved bounds - //fmt::print("Solution DH: {}\nIters: {}\n", DH, max_iter); - if (max_iter == 50u) { - throw std::domain_error( + IG, IG - 50, IG + 50, digits, + max_iter); // TODO (dario): study this hyperbolic equation in more + // details as to provide decent and well proved bounds + if (max_iter == 100u) { + throw std::domain_error(fmt::format( "Maximum number of iterations exceeded when solving Kepler's " - "equation for the hyperbolic anomaly in propagate_lagrangian."); + "equation for the hyperbolic anomaly in propagate_lagrangian.\n" + "DN={}\nsigma0={}\nsqrta={}\na={}\nR={}\nDH={}", + DN, sigma0, sqrta, a, R, DH)); } double r = a + (R - a) * std::cosh(DH) + sigma0 * sqrta * std::sinh(DH); @@ -152,8 +159,8 @@ void propagate_lagrangian_u(std::array, 2> &pos_vel_0, // Solve Kepler Equation in DS (univrsal anomaly difference) const int digits = std::numeric_limits::digits; std::uintmax_t max_iter = 100u; - // NOTE: Halley iterate here seems to introduce issues in corner cases - // and be slower anyway. + // NOTE: Halley iterates may result into instabilities (specially with a poor + // IG) double DS = boost::math::tools::newton_raphson_iterate( [dt_copy, R0, VR0, alpha, mu](double DS) { return std::make_tuple(kepDS(DS, dt_copy, R0, VR0, alpha, mu), @@ -198,4 +205,35 @@ void propagate_lagrangian_u(std::array, 2> &pos_vel_0, } } +/// Keplerian (not using the lagrangian coefficients) propagation +/** + * This function propagates an initial Cartesian state for a time t assuming a + * central body and a keplerian motion. Simple conversions are used to compute + * M0 then Mt, etc.. It only here for study purposes as its x10 slower (strange + * such a high factor ..investigate?) + */ +void propagate_keplerian(std::array, 2> &pos_vel_0, + const double dt, const double mu) { // NOLINT + + // 1 - Compute the orbital parameters at t0 + auto par = kep3::ic2par(pos_vel_0, mu); + if (par[0] > 0) { + // 2e - Compute the mean anomalies + double n = std::sqrt(mu / par[0] / par[0] / par[0]); + double M0 = kep3::f2m(par[5], par[1]); + double Mf = M0 + n * dt; + // 3e - Update elements (here Kepler's equation gets solved) + par[5] = kep3::m2f(Mf, par[1]); + } else { + // 2h - Compute the mean hyperbolic anomalies + double n = std::sqrt(-mu / par[0] / par[0] / par[0]); + double N0 = kep3::f2n(par[5], par[1]); + double Nf = N0 + n * dt; + // 3h - Update elements (here Kepler's equation gets solved in its hyperbolic version) + par[5] = kep3::n2f(Nf, par[1]); + } + // Update posvel + pos_vel_0 = kep3::par2ic(par, mu); +} + } // namespace kep3 \ No newline at end of file diff --git a/src/lambert_problem.cpp b/src/lambert_problem.cpp index 4010b5ad..93933539 100644 --- a/src/lambert_problem.cpp +++ b/src/lambert_problem.cpp @@ -99,6 +99,7 @@ lambert_problem::lambert_problem(const std::array &r1_a, // 2.1 - Let us first detect the maximum number of revolutions for which there // exists a solution m_Nmax = static_cast(T / kep3::pi); + m_Nmax = std::min(m_multi_revs, m_Nmax); double T00 = std::acos(m_lambda) + m_lambda * std::sqrt(1.0 - lambda2); double T0 = (T00 + m_Nmax * kep3::pi); double T1 = 2.0 / 3.0 * (1.0 - lambda3), DT = 0.0, DDT = 0.0, DDDT = 0.0; @@ -125,10 +126,10 @@ lambert_problem::lambert_problem(const std::array &r1_a, 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); } - // 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(static_cast(m_Nmax) * 2 + 1); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 7793a314..0bbc24fb 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -34,4 +34,5 @@ ADD_kep3_TESTCASE(ic2par2ic_test) ADD_kep3_TESTCASE(ic2eq2ic_test) ADD_kep3_TESTCASE(eq2par2eq_test) ADD_kep3_TESTCASE(propagate_lagrangian_test) +ADD_kep3_TESTCASE(propagate_keplerian_test) ADD_kep3_TESTCASE(lambert_problem_test) \ No newline at end of file diff --git a/test/lambert_problem_test.cpp b/test/lambert_problem_test.cpp index d5992abc..9ddf655f 100644 --- a/test/lambert_problem_test.cpp +++ b/test/lambert_problem_test.cpp @@ -29,13 +29,12 @@ TEST_CASE("delta_guidance") { // Here we test that in a number of randomly generated Lambert Problems // The boundary data must satisfy the Delta Guidance law - // Preamble std::array r1{{0, 0, 0}}, r2{{0, 0, 0}}; double tof = 0.; // NOLINTNEXTLINE(cert-msc32-c, cert-msc51-cpp) - std::mt19937 rng_engine(122012203u); + std::mt19937 rng_engine(12201203u); std::uniform_int_distribution cw_d(0, 1); std::uniform_real_distribution r_d(-2, 2); std::uniform_real_distribution tof_d(2., 40.); @@ -61,7 +60,12 @@ TEST_CASE("delta_guidance") { // 3 - Check the Delta guidance error for (const auto &v1 : lp.get_v1()) { - REQUIRE(kep3_tests::delta_guidance_error(r1, r2, v1, mu) < 1e-12); + double dg_err = kep3_tests::delta_guidance_error(r1, r2, v1, mu); + if (!(dg_err < 1e-12)) { + std::cout << lp << std::endl; + fmt::print("\nr1= {}\nr2= {}\ntof= {}\nmu= {}\ncw= {}\nrevs_max= {}", r1, r2,tof, mu, cw, revs_max); + } + REQUIRE(dg_err < 1e-12); } } } \ No newline at end of file diff --git a/test/propagate_keplerian_test.cpp b/test/propagate_keplerian_test.cpp new file mode 100644 index 00000000..22a7c561 --- /dev/null +++ b/test/propagate_keplerian_test.cpp @@ -0,0 +1,103 @@ +// Copyright 2023, 2024 Dario Izzo (dario.izzo@gmail.com), Francesco Biscani +// (bluescarni@gmail.com) +// +// This file is part of the kep3 library. +// +// This Source Code Form is subject to the terms of the Mozilla +// Public License v. 2.0. If a copy of the MPL was not distributed +// with this file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include +#include + +#include +#include + +#include + +#include +#include +#include +#include + +#include "catch.hpp" +#include "test_helpers.hpp" + +using kep3::propagate_keplerian; + +constexpr double pi{boost::math::constants::pi()}; + +void test_propagate_keplerian( + const std::function, 2> &, double, + double)> &propagate, + unsigned int N = 10000) { + + // NOLINTNEXTLINE(cert-msc32-c, cert-msc51-cpp) + std::mt19937 rng_engine(1220202343u); + + { // Targeting Ellipses + std::uniform_real_distribution sma_d(1.1, 10.); + std::uniform_real_distribution ecc_d(0, 0.9); + std::uniform_real_distribution incl_d(0., kep3::pi); + std::uniform_real_distribution Omega_d(0, 2 * kep3::pi); + std::uniform_real_distribution omega_d(0., pi); + std::uniform_real_distribution f_d(0, 2 * pi); + std::uniform_real_distribution time_d(-2. * kep3::pi, + 2. * kep3::pi); + + // Testing on N random calls on ellipses + for (auto i = 0u; i < N; ++i) { + double sma = sma_d(rng_engine); + double ecc = ecc_d(rng_engine); + double incl = incl_d(rng_engine); + double Omega = Omega_d(rng_engine); + double omega = omega_d(rng_engine); + double f = f_d(rng_engine); + + std::array par = {sma, ecc, incl, Omega, omega, f}; + auto pos_vel = kep3::par2ic(par, 1.); + double tof = time_d(rng_engine); + auto pos_vel_after = pos_vel; + propagate(pos_vel_after, tof, 1.); + propagate(pos_vel_after, -tof, 1.); + REQUIRE(kep3_tests::floating_point_error_vector(pos_vel[0], + pos_vel_after[0]) < + 1e-10); // Low precision requested. The funciton is only here for + // academic purposes + } + } + + { // Targeting Hyperbolas + std::uniform_real_distribution sma_d(1.1, 100.); + std::uniform_real_distribution ecc_d(2., 20.); + std::uniform_real_distribution incl_d(0., pi); + std::uniform_real_distribution Omega_d(0, 2 * pi); + std::uniform_real_distribution omega_d(0., pi); + std::uniform_real_distribution f_d(0, 2 * pi); + std::uniform_real_distribution time_d(0.1, 20.); + // Testing on N random calls on hyperbolas + for (auto i = 0u; i < N; ++i) { + double sma = sma_d(rng_engine); + double ecc = ecc_d(rng_engine); + double incl = incl_d(rng_engine); + double Omega = Omega_d(rng_engine); + double omega = omega_d(rng_engine); + double f = f_d(rng_engine); + if (std::cos(f) > -1 / ecc) { + std::array par = {-sma, ecc, incl, Omega, omega, f}; + auto pos_vel = kep3::par2ic(par, 1.); + double tof = time_d(rng_engine); + auto pos_vel_after = pos_vel; + propagate(pos_vel_after, tof, 1.); + propagate(pos_vel_after, -tof, 1.); + REQUIRE(kep3_tests::floating_point_error_vector( + pos_vel[0], pos_vel_after[0]) < 1e-10); + } + } + } +} + +TEST_CASE("propagate_keplerian") { + // We test both Normal and Universal variables version with the same data. + test_propagate_keplerian(&propagate_keplerian, 10000u); +} \ No newline at end of file From 6b78c760cdcab8175edef2b31cd3b3d77c6eb32f Mon Sep 17 00:00:00 2001 From: Dario Izzo Date: Sun, 24 Sep 2023 00:17:37 +0200 Subject: [PATCH 10/11] tests finished for lambert --- benchmark/propagate_lagrangian_benchmark.cpp | 56 ++++++++-------- src/core_astro/propagate_lagrangian.cpp | 26 ++++++-- test/lambert_problem_test.cpp | 68 ++++++++++++++++++-- 3 files changed, 112 insertions(+), 38 deletions(-) diff --git a/benchmark/propagate_lagrangian_benchmark.cpp b/benchmark/propagate_lagrangian_benchmark.cpp index 21747bbf..24c5ae19 100644 --- a/benchmark/propagate_lagrangian_benchmark.cpp +++ b/benchmark/propagate_lagrangian_benchmark.cpp @@ -46,7 +46,7 @@ void perform_test_speed( std::uniform_real_distribution Omega_d(0, 2 * pi); std::uniform_real_distribution omega_d(0., 2 * pi); std::uniform_real_distribution f_d(0, 2 * pi); - std::uniform_real_distribution tof_d(0.1, 20.); + std::uniform_real_distribution tof_d(10., 100.); // We generate the random dataset std::vector, 2>> pos_vels(N); @@ -86,7 +86,7 @@ void perform_test_accuracy( // Engines // // NOLINTNEXTLINE(cert-msc32-c, cert-msc51-cpp) - std::mt19937 rng_engine(122012203u); + std::mt19937 rng_engine(53534535u); // // Distributions // @@ -96,7 +96,7 @@ void perform_test_accuracy( std::uniform_real_distribution Omega_d(0, 2 * pi); std::uniform_real_distribution omega_d(0., 2 * pi); std::uniform_real_distribution f_d(0, 2 * pi); - std::uniform_real_distribution tof_d(0.1, 20.); + std::uniform_real_distribution tof_d(10, 100.); // We generate the random dataset std::vector, 2>> pos_vels(N); @@ -150,29 +150,29 @@ int main() { perform_test_accuracy(0, 0.5, 100000, &kep3::propagate_lagrangian); perform_test_accuracy(0.5, 0.9, 100000, &kep3::propagate_lagrangian); perform_test_accuracy(0.9, 0.99, 100000, &kep3::propagate_lagrangian); - - fmt::print("\nComputes speed at different eccentricity ranges [Universal " - "Anomaly]:\n"); - perform_test_speed(0, 0.5, 1000000, &kep3::propagate_lagrangian_u); - perform_test_speed(0.5, 0.9, 1000000, &kep3::propagate_lagrangian_u); - perform_test_speed(0.9, 0.99, 1000000, &kep3::propagate_lagrangian_u); - perform_test_speed(1.1, 10., 1000000, &kep3::propagate_lagrangian_u); - - fmt::print("\nComputes error at different eccentricity ranges [Universal " - "Anomaly]:\n"); - perform_test_accuracy(0, 0.5, 100000, &kep3::propagate_lagrangian_u); - perform_test_accuracy(0.5, 0.9, 100000, &kep3::propagate_lagrangian_u); - perform_test_accuracy(0.9, 0.99, 100000, &kep3::propagate_lagrangian_u); - - fmt::print("\nComputes speed at different eccentricity ranges [keplerian " - "propagation]:\n"); - perform_test_speed(0, 0.5, 1000000, &kep3::propagate_keplerian); - perform_test_speed(0.5, 0.9, 1000000, &kep3::propagate_keplerian); - perform_test_speed(0.9, 0.99, 1000000, &kep3::propagate_keplerian); - - fmt::print("\nComputes error at different eccentricity ranges [keplerian " - "propagation]:\n"); - perform_test_accuracy(0, 0.5, 100000, &kep3::propagate_keplerian); - perform_test_accuracy(0.5, 0.9, 100000, &kep3::propagate_keplerian); - perform_test_accuracy(0.9, 0.99, 100000, &kep3::propagate_keplerian); +// + //fmt::print("\nComputes speed at different eccentricity ranges [Universal " + // "Anomaly]:\n"); + //perform_test_speed(0, 0.5, 1000000, &kep3::propagate_lagrangian_u); + //perform_test_speed(0.5, 0.9, 1000000, &kep3::propagate_lagrangian_u); + //perform_test_speed(0.9, 0.99, 1000000, &kep3::propagate_lagrangian_u); + //perform_test_speed(1.1, 10., 1000000, &kep3::propagate_lagrangian_u); +// + //fmt::print("\nComputes error at different eccentricity ranges [Universal " + // "Anomaly]:\n"); + //perform_test_accuracy(0, 0.5, 100000, &kep3::propagate_lagrangian_u); + //perform_test_accuracy(0.5, 0.9, 100000, &kep3::propagate_lagrangian_u); + //perform_test_accuracy(0.9, 0.99, 100000, &kep3::propagate_lagrangian_u); +// + //fmt::print("\nComputes speed at different eccentricity ranges [keplerian " + // "propagation]:\n"); + //perform_test_speed(0, 0.5, 1000000, &kep3::propagate_keplerian); + //perform_test_speed(0.5, 0.9, 1000000, &kep3::propagate_keplerian); + //perform_test_speed(0.9, 0.99, 1000000, &kep3::propagate_keplerian); +// + //fmt::print("\nComputes error at different eccentricity ranges [keplerian " + // "propagation]:\n"); + //perform_test_accuracy(0, 0.5, 100000, &kep3::propagate_keplerian); + //perform_test_accuracy(0.5, 0.9, 100000, &kep3::propagate_keplerian); + //perform_test_accuracy(0.9, 0.99, 100000, &kep3::propagate_keplerian); } \ No newline at end of file diff --git a/src/core_astro/propagate_lagrangian.cpp b/src/core_astro/propagate_lagrangian.cpp index aa61efe4..8359f695 100644 --- a/src/core_astro/propagate_lagrangian.cpp +++ b/src/core_astro/propagate_lagrangian.cpp @@ -45,7 +45,24 @@ void propagate_lagrangian(std::array, 2> &pos_vel_0, if (a > 0) { // Solve Kepler's equation in DE, elliptical case sqrta = std::sqrt(a); double DM = std::sqrt(mu / std::pow(a, 3)) * dt; - double IG = DM; + double sinDM = std::sin(DM), cosDM = std::cos(DM); + // Here we use the atan2 to recover the mean anomaly difference in the + // [0,2pi] range. This makes sure that for high value of M no catastrophic + // cancellation occurs, as would be the case using std::fmod(DM, 2pi) + double DM_cropped = std::atan2(sinDM, cosDM); + if (DM_cropped < 0) { + DM_cropped += 2 * kep3::pi; + } + double s0 = sigma0 / sqrta; + double c0 = (1 - R / a); + // This initial guess was developed applying Lagrange expansion theorem to + // the Kepler's equation in DE. We stopped at 3rd order. + double IG = + DM_cropped + c0 * sinDM - s0 * (1 - cosDM) + + (c0 * cosDM - s0 * sinDM) * (c0 * sinDM + s0 * cosDM - s0) + + 0.5 * (c0 * sinDM + s0 * cosDM - s0) * + (2 * std::pow(c0 * cosDM - s0 * sinDM, 2) - + (c0 * sinDM + s0 * cosDM - s0) * (c0 * sinDM + s0 * cosDM)); // Solve Kepler Equation for ellipses in DE (eccentric anomaly difference) const int digits = std::numeric_limits::digits; @@ -54,8 +71,8 @@ void propagate_lagrangian(std::array, 2> &pos_vel_0, // poor IG) double DE = boost::math::tools::newton_raphson_iterate( - [DM, sigma0, sqrta, a, R](double DE) { - return std::make_tuple(kepDE(DE, DM, sigma0, sqrta, a, R), + [DM_cropped, sigma0, sqrta, a, R](double DE) { + return std::make_tuple(kepDE(DE, DM_cropped, sigma0, sqrta, a, R), d_kepDE(DE, sigma0, sqrta, a, R)); }, IG, IG - pi, IG + pi, digits, max_iter); @@ -229,7 +246,8 @@ void propagate_keplerian(std::array, 2> &pos_vel_0, double n = std::sqrt(-mu / par[0] / par[0] / par[0]); double N0 = kep3::f2n(par[5], par[1]); double Nf = N0 + n * dt; - // 3h - Update elements (here Kepler's equation gets solved in its hyperbolic version) + // 3h - Update elements (here Kepler's equation gets solved in its + // hyperbolic version) par[5] = kep3::n2f(Nf, par[1]); } // Update posvel diff --git a/test/lambert_problem_test.cpp b/test/lambert_problem_test.cpp index 9ddf655f..a74f515a 100644 --- a/test/lambert_problem_test.cpp +++ b/test/lambert_problem_test.cpp @@ -8,11 +8,10 @@ // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. #include +#include #include #include -#include -#include #include @@ -20,9 +19,22 @@ #include "test_helpers.hpp" TEST_CASE("construct") { - // Here we test construction of a few simple geometries - kep3::lambert_problem lp{{1., 0., 0.}, {0., 1., 0.}, 3 * kep3::pi / 2, - 1., true, 100}; + // Here we test construction for a simple geometry + REQUIRE_NOTHROW(kep3::lambert_problem{ + {1., 0., 0.}, {0., 1., 0.}, 3 * kep3::pi / 2, 1., true, 100}); + // And we test the throws + REQUIRE_THROWS_AS( + (kep3::lambert_problem{ + {1., 0., 0.}, {0., 1., 0.}, 3 * kep3::pi / 2, -1.2, true, 100}), + std::domain_error); + REQUIRE_THROWS_AS( + (kep3::lambert_problem{ + {1., 0., 0.}, {0., 1., 0.}, -3 * kep3::pi / 2, 1.2, true, 100}), + std::domain_error); + REQUIRE_THROWS_AS( + (kep3::lambert_problem{ + {0, 0., 1.}, {0., 1., 0.}, 3 * kep3::pi / 2, 1.2, true, 100}), + std::domain_error); } TEST_CASE("delta_guidance") { @@ -63,9 +75,53 @@ TEST_CASE("delta_guidance") { double dg_err = kep3_tests::delta_guidance_error(r1, r2, v1, mu); if (!(dg_err < 1e-12)) { std::cout << lp << std::endl; - fmt::print("\nr1= {}\nr2= {}\ntof= {}\nmu= {}\ncw= {}\nrevs_max= {}", r1, r2,tof, mu, cw, revs_max); + fmt::print("\nr1= {}\nr2= {}\ntof= {}\nmu= {}\ncw= {}\nrevs_max= {}", + r1, r2, tof, mu, cw, revs_max); } REQUIRE(dg_err < 1e-12); } } +} + +TEST_CASE("methods") { + // Here we test construction for a simple geometry + kep3::lambert_problem lp{{1., 0., 0.}, {0., 1., 0.}, 3. * kep3::pi / 2., + 1., true, 5}; + auto v1 = lp.get_v1()[0]; + auto v2 = lp.get_v2()[0]; + REQUIRE(kep3_tests::floating_point_error_vector(v1, {0, -1, 0}) < 1e-13); + REQUIRE(kep3_tests::floating_point_error_vector(v2, {1, 0, 0}) < 1e-13); + auto r1 = lp.get_r1(); + auto r2 = lp.get_r2(); + REQUIRE(r1 == std::array{1,0,0}); + REQUIRE(r2 == std::array{0,1,0}); + REQUIRE(lp.get_tof() == 3 * kep3::pi / 2); + REQUIRE(lp.get_mu() == 1.); + REQUIRE(kep3_tests::floating_point_error(lp.get_x()[0], -0.3826834323650896) < 1e-13); + REQUIRE(lp.get_iters()[0] == 3u); +} + +TEST_CASE("serialization_test") { + // Instantiate a generic lambert problem + kep3::lambert_problem lp{{1.23, 0.1253232342323, 0.57235553354}, {0.234233423, 1.8645645645, 0.234234234}, 1.254856435, + 1., true, 5}; + + // Store the string representation. + std::stringstream ss; + auto before = boost::lexical_cast(lp); + // Now serialize + { + boost::archive::binary_oarchive oarchive(ss); + oarchive << lp; + } + // Deserialize + // Create a new lambert problem object + kep3::lambert_problem lp2{}; + { + boost::archive::binary_iarchive iarchive(ss); + iarchive >> lp2; + } + auto after = boost::lexical_cast(lp2); + // Compare the string represetation + REQUIRE(before == after); } \ No newline at end of file From b4c86bbdc8497978210aa730f8cb331fc5e0d300 Mon Sep 17 00:00:00 2001 From: Dario Izzo Date: Sun, 24 Sep 2023 00:26:10 +0200 Subject: [PATCH 11/11] lambert coverage increased, codecov comments added --- codecov.yml | 7 ++++++- test/lambert_problem_test.cpp | 6 ++++-- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/codecov.yml b/codecov.yml index 6fd56d39..647e9183 100644 --- a/codecov.yml +++ b/codecov.yml @@ -4,4 +4,9 @@ coverage: codecov: token: 60883dbc-866d-4da4-9306-8df7ac9b1360 -comment: off \ No newline at end of file +comment: # this is a top-level key + layout: " diff, flags, files" + behavior: default + require_changes: false # if true: only post the comment if coverage changes + require_base: false # [true :: must have a base report to post] + require_head: true # [true :: must have a head report to post] \ No newline at end of file diff --git a/test/lambert_problem_test.cpp b/test/lambert_problem_test.cpp index a74f515a..53d7b6db 100644 --- a/test/lambert_problem_test.cpp +++ b/test/lambert_problem_test.cpp @@ -99,12 +99,14 @@ TEST_CASE("methods") { REQUIRE(lp.get_mu() == 1.); REQUIRE(kep3_tests::floating_point_error(lp.get_x()[0], -0.3826834323650896) < 1e-13); REQUIRE(lp.get_iters()[0] == 3u); + REQUIRE(lp.get_Nmax() == 0u); + } TEST_CASE("serialization_test") { // Instantiate a generic lambert problem - kep3::lambert_problem lp{{1.23, 0.1253232342323, 0.57235553354}, {0.234233423, 1.8645645645, 0.234234234}, 1.254856435, - 1., true, 5}; + kep3::lambert_problem lp{{1.23, 0.1253232342323, 0.57235553354}, {0.234233423, 1.8645645645, 0.234234234}, 25.254856435, + 1., true, 10}; // Store the string representation. std::stringstream ss;