diff --git a/src/core_astro/ic2par.cpp b/src/core_astro/ic2par.cpp index 69eb758a..d3088093 100644 --- a/src/core_astro/ic2par.cpp +++ b/src/core_astro/ic2par.cpp @@ -23,10 +23,10 @@ using xt::linalg::dot; namespace kep3 { -// r,v,mu -> keplerian osculating elements [a,e,i,W,w,E]. The last -// is the eccentric anomaly or the Gudermannian according to e. The -// semi-major axis a is positive for ellipses, negative for hyperbolae. -// The anomalies W, w and E are in [0, 2pi]. Inclination is in [0, pi]. +// 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, +// pi]. std::array ic2par(const std::array &rin, const std::array &vin, double mu) { @@ -34,8 +34,8 @@ std::array ic2par(const std::array &rin, std::array retval{}; // 0 - We prepare a few xtensor constants. xt::xtensor_fixed> k{0.0, 0.0, 1.0}; - xt::xtensor_fixed> r0 = xt::adapt(rin); - xt::xtensor_fixed> v0 = xt::adapt(vin); + auto r0 = xt::adapt(rin); + auto v0 = xt::adapt(vin); // 1 - We compute the orbital angular momentum vector auto h = cross(r0, v0); // h = r0 x v0 @@ -61,43 +61,32 @@ std::array ic2par(const std::array &rin, retval[0] = p(0) / (1 - retval[1] * retval[1]); // Inclination is calculated and stored as the third orbital element - // i = acos(hy/h) + // i = acos(hy/h) in [0, pi] retval[2] = std::acos(h(2) / xt::linalg::norm(h)); // Argument of pericentrum is calculated and stored as the fifth orbital - // elemen.t w = acos(n.e)\|n||e| + // element. w = acos(n.e)\|n||e| in [0, 2pi] auto temp = dot(n, evett); retval[4] = std::acos(temp(0) / retval[1]); if (evett(2) < 0) { retval[4] = 2 * pi() - retval[4]; } // Argument of longitude is calculated and stored as the fourth orbital - // element + // element in [0, 2pi] retval[3] = std::acos(n(0)); if (n(1) < 0) { retval[3] = 2 * boost::math::constants::pi() - retval[3]; } - // 4 - We compute ni: the true anomaly (in 0, 2*PI) + // 4 - We compute ni: the true anomaly in [0, 2pi] temp = dot(evett, r0); - auto ni = std::acos(temp(0) / retval[1] / R0); + auto f = std::acos(temp(0) / retval[1] / R0); temp = dot(r0, v0); if (temp(0) < 0.0) { - ni = 2 * boost::math::constants::pi() - ni; - } - - // Eccentric anomaly or the gudermannian is calculated and stored as the - // sixth orbital element - if (retval[1] < 1.0) { - retval[5] = 2.0 * atan(sqrt((1 - retval[1]) / (1 + retval[1])) * - tan(ni / 2.0)); // algebraic Kepler's equation - } else { - retval[5] = - 2.0 * atan(sqrt((retval[1] - 1) / (retval[1] + 1)) * - tan(ni / 2.0)); // algebraic equivalent of Kepler's - // equation in terms of the Gudermannian + f = 2 * boost::math::constants::pi() - f; } + retval[5] = f; return retval; } } // namespace kep3 \ No newline at end of file diff --git a/src/core_astro/par2ic.cpp b/src/core_astro/par2ic.cpp index ee774124..35351ba9 100644 --- a/src/core_astro/par2ic.cpp +++ b/src/core_astro/par2ic.cpp @@ -8,6 +8,7 @@ // with this file, You can obtain one at http://mozilla.org/MPL/2.0/. #include +#include #include #include @@ -24,9 +25,9 @@ namespace kep3 { constexpr double pi4{boost::math::constants::quarter_pi()}; -// keplerian osculating elements [a,e,i,W,w,E] -> r,v. -// The last osculating elements needs to be the eccentric anomaly or -// the Gudermannian according to e. The semi-major axis a needs to be positive +// keplerian osculating elements [a,e,i,W,w,f] -> r,v. +// The last osculating elements is the true anomaly. +// The semi-major axis a needs to be positive // for ellipses, negative for hyperbolae. // The anomalies W, w and E must be in [0, 2pi] and inclination in [0, pi]. @@ -39,45 +40,36 @@ std::array, 2> par2ic(const std::array &par, auto vel_xt = xt::adapt(vel, {3u, 1u}); // Rename some variables for readibility - double acc = par[0]; + double sma = par[0]; double ecc = par[1]; double inc = par[2]; double omg = par[3]; double omp = par[4]; - double EA = par[5]; + double f = par[5]; - // TODO(darioizzo): Check a<0 if e>1 + if (sma * (1 - ecc) < 0) { + throw std::domain_error("par2ic was called with ecc and sma not compatible " + "with the convention a<0 -> e>1 [a>0 -> e<1]."); + } + double cosf = std::cos(f); + if (ecc > 1 && cosf < -1 / ecc) { + throw std::domain_error("par2ic was called for an hyperbola but the true " + "anomaly is beyond asymptotes (cosf<-1/e)"); + } // 1 - We start by evaluating position and velocity in the perifocal reference // system - double xper = 0., yper = 0., xdotper = 0., ydotper = 0.; - if (ecc < 1.0) // EA is the eccentric anomaly - { - double b = acc * std::sqrt(1 - ecc * ecc); - double n = std::sqrt(mu / (acc * acc * acc)); - xper = acc * (std::cos(EA) - ecc); - yper = b * std::sin(EA); - xdotper = -(acc * n * std::sin(EA)) / (1 - ecc * std::cos(EA)); - ydotper = (b * n * std::cos(EA)) / (1 - ecc * std::cos(EA)); - } else // EA is the Gudermannian - { - double b = -acc * std::sqrt(ecc * ecc - 1); - double n = std::sqrt(-mu / (acc * acc * acc)); - - double dNdZeta = ecc * (1 + std::tan(EA) * std::tan(EA)) - - (0.5 + 0.5 * std::pow(std::tan(0.5 * EA + pi4), 2)) / - std::tan(0.5 * EA + pi4); - - xper = acc / std::cos(EA) - acc * ecc; - yper = b * std::tan(EA); - - xdotper = acc * tan(EA) / cos(EA) * n / dNdZeta; - ydotper = b / pow(cos(EA), 2) * n / dNdZeta; - } + double p = sma * (1.0 - ecc * ecc); + double r = p / (1.0 + ecc * std::cos(f)); + double h = std::sqrt(p * mu); + double sinf = std::sin(f); + double x_per = r * cosf; + double y_per = r * sinf; + double xdot_per = -mu / h * sinf; + double ydot_per = mu / h * (ecc + cosf); // 2 - We then built the rotation matrix from perifocal reference frame to // inertial - double cosomg = std::cos(omg); double cosomp = std::cos(omp); double sinomg = std::sin(omg); @@ -93,14 +85,14 @@ std::array, 2> par2ic(const std::array &par, {sinomp * sini, cosomp * sini, cosi}}; // 3 - We end by transforming according to this rotation matrix - xt::xtensor_fixed> temp1{{xper}, {yper}, {0.0}}; - xt::xtensor_fixed> temp2{ - {xdotper}, {ydotper}, {0.0}}; - - pos_xt = R * temp1; - vel_xt = R * temp2; - fmt::print("\npar2ic: {}, {}", pos_xt, vel_xt); - fmt::print("\npar2ic: {}, {}", pos, vel); + xt::xtensor_fixed> pos_per{{x_per}, {y_per}, {0.0}}; + xt::xtensor_fixed> vel_per{ + {xdot_per}, {ydot_per}, {0.0}}; + + // The following lines, since use xtensors adapted to pos and vel, will change + // pos and vel. + pos_xt = xt::linalg::dot(R, pos_per); + vel_xt = xt::linalg::dot(R, vel_per); return {pos, vel}; } diff --git a/test/convert_anomalies_test.cpp b/test/convert_anomalies_test.cpp index 934a043f..0af282a4 100644 --- a/test/convert_anomalies_test.cpp +++ b/test/convert_anomalies_test.cpp @@ -70,7 +70,7 @@ TEST_CASE("f2e") { // std::mt19937 rng_engine(rd()); // - // Distribtuions + // Distributions // std::uniform_real_distribution ecc_difficult_d(0.9, 0.99); std::uniform_real_distribution ecc_easy_d(0., 0.9); diff --git a/test/ic2par2ic_test.cpp b/test/ic2par2ic_test.cpp index 9ab4d6a9..126ff2a1 100644 --- a/test/ic2par2ic_test.cpp +++ b/test/ic2par2ic_test.cpp @@ -10,15 +10,178 @@ #include #include +#include + #include #include +#include #include "catch.hpp" +using Catch::Matchers::WithinAbs; +using Catch::Matchers::WithinRel; +using kep3::ic2par; +using kep3::par2ic; + +constexpr double pi{boost::math::constants::pi()}; + TEST_CASE("ic2par") { - fmt::print("\nResult: {}", kep3::ic2par({1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, 1.0)); - fmt::print("\nResult: {}", kep3::par2ic({1.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 1.0)); - fmt::print("\nResult: {}", kep3::ic2par({1.0, 0.0, 0.0}, {0.0, 0.0, 1.1}, 1.0)); - fmt::print("\nResult: {}", kep3::par2ic({1.0, 0.0, 0.0, 0.0, 0.0, 0.0}, 1.0)); + // Singular orbit zero inclination and eccentricity + { + auto par = ic2par({1.0, 0.0, 0.0}, {0.0, 1.0, 0.0}, 1.0); + REQUIRE(par[0] == 1.); // sma is 1 + REQUIRE(par[1] == 0.); // ecc is zero + REQUIRE(par[2] == 0.); // incl is zero + REQUIRE(!std::isfinite(par[3])); + REQUIRE(!std::isfinite(par[4])); + REQUIRE(!std::isfinite(par[5])); + } + // Orbit at 90 degrees inclination + { + auto par = ic2par({1.0, 0.0, 0.0}, {0.0, 0.0, 1.1}, 1.0); + REQUIRE_THAT(par[0], WithinRel(1.2658227848101269, 1e-14)); + REQUIRE_THAT(par[1], WithinRel(0.21, 1e-14)); + REQUIRE_THAT(par[2], WithinRel(pi / 2, 1e-14)); // incl at 90 degrees + REQUIRE(par[3] == 0.); // Omega is zero + REQUIRE(par[4] == 0.); // omega is zero + REQUIRE(par[5] == 0.); // true anomaly is zero + } + // Orbit at 90 degrees inclination + { + auto par = ic2par({1.0, 0.0, 0.0}, {0.0, 0.0, -1.1}, 1.0); + REQUIRE_THAT(par[0], WithinRel(1.2658227848101269, 1e-14)); + REQUIRE_THAT(par[1], WithinRel(0.21, 1e-14)); + REQUIRE_THAT(par[2], WithinRel(pi / 2, 1e-14)); // incl at 90 degrees + REQUIRE_THAT(par[3], WithinRel(pi, 1e-14)); // Omega at 180 degrees + REQUIRE_THAT(par[4], WithinRel(pi, 1e-14)); // omeg at 180 degrees + REQUIRE(par[5] == 0.); // true anomaly is zero + } + // Retrogade orbit + { + auto par = ic2par({1.0, 0.0, 0.0}, {0.0, -1.0, 0.1}, 1.0); + REQUIRE_THAT(par[0], WithinRel(1.01010101010101, 1e-14)); + REQUIRE_THAT(par[1], WithinRel(0.01, 1e-14)); + REQUIRE_THAT(par[2], WithinRel(174.289406862500 / 180.0 * pi, + 1e-14)); // incl + REQUIRE(par[3] == 0.); // Omega is zero + REQUIRE(par[4] == 0.); // omega is zero + REQUIRE(par[5] == 0.); // true anomaly is zero + } + // A random orbit + { + auto par = ic2par({-1.1823467354129, 0.0247369349235, -0.014848484784}, + {0.00232349642367, 1.1225625625625, -0.34678634567}, 1.0); + REQUIRE_THAT(par[0], WithinRel(3.21921322281178, 1e-14)); + REQUIRE_THAT(par[1], WithinAbs(0.63283595179672, 1e-14)); + REQUIRE_THAT(par[2], WithinRel(162.82902986040048 / 180.0 * pi, + 1e-14)); // incl + REQUIRE_THAT(par[3], WithinAbs(1.13023105373051 / 180.0 * pi, + 1e-14)); // Omega + REQUIRE_THAT(par[4], WithinRel(179.22698703370386 / 180.0 * pi, + 1e-14)); // omega + REQUIRE_THAT(par[5], WithinAbs(3.21031850920605 / 180.0 * pi, + 1e-14)); // true anomaly + } +} +TEST_CASE("par2ic") { + // Orbit at 90 degrees inclination + { + auto [r, v] = + par2ic({1.2658227848101269, 0.21, pi / 2, 0.0, 0.0, 0.0}, 1.0); + REQUIRE_THAT(r[0], WithinRel(1., 1e-14)); + REQUIRE_THAT(r[1], WithinAbs(0., 1e-14)); + REQUIRE_THAT(r[2], WithinAbs(0., 1e-14)); + REQUIRE_THAT(v[0], WithinAbs(0., 1e-14)); + REQUIRE_THAT(v[1], WithinAbs(0., 1e-14)); + REQUIRE_THAT(v[2], WithinRel(1.1, 1e-14)); + } + // Orbit at 90 degrees inclination + { + auto [r, v] = par2ic({1.2658227848101269, 0.21, pi / 2, pi, pi, 0.0}, 1.0); + REQUIRE_THAT(r[0], WithinRel(1., 1e-14)); + REQUIRE_THAT(r[1], WithinAbs(0., 1e-14)); + REQUIRE_THAT(r[2], WithinAbs(0., 1e-14)); + REQUIRE_THAT(v[0], WithinAbs(0., 1e-14)); + REQUIRE_THAT(v[1], WithinAbs(0., 1e-14)); + REQUIRE_THAT(v[2], WithinRel(-1.1, 1e-14)); + } + // Retrogade orbit + { + auto [r, v] = par2ic( + {1.01010101010101, 0.01, 174.289406862500 / 180.0 * pi, 1e-14, 0., 0.0}, + 1.0); + REQUIRE_THAT(r[0], WithinRel(1., 1e-14)); + REQUIRE_THAT(r[1], WithinAbs(0., 1e-14)); + REQUIRE_THAT(r[2], WithinAbs(0., 1e-14)); + REQUIRE_THAT(v[0], WithinAbs(0., 1e-14)); + REQUIRE_THAT(v[1], WithinRel(-1., 1e-14)); + REQUIRE_THAT(v[2], WithinAbs(0.1, 1e-14)); + } + // A random orbit + { + auto [r, v] = + par2ic({3.21921322281178, 0.63283595179672, + 162.82902986040048 / 180.0 * pi, 1.13023105373051 / 180.0 * pi, + 179.22698703370386 / 180.0 * pi, 3.21031850920605 / 180.0 * pi}, + 1.0); + + REQUIRE_THAT(r[0], WithinRel(-1.1823467354129, 1e-14)); + REQUIRE_THAT(r[1], WithinAbs(0.0247369349235, 1e-13)); + REQUIRE_THAT(r[2], WithinAbs(-0.014848484784, 1e-14)); + REQUIRE_THAT(v[0], WithinAbs(0.00232349642367, 1e-13)); + REQUIRE_THAT(v[1], WithinRel(1.1225625625625, 1e-14)); + REQUIRE_THAT(v[2], WithinAbs(-0.34678634567, 1e-14)); + } + // We check the convention a<0 -> e>1 is followed. + REQUIRE_THROWS_AS(par2ic({1.3, 1.3, 0., 0., 0., 0.}, 1), std::domain_error); + REQUIRE_THROWS_AS(par2ic({-1.3, 0.4, 0., 0., 0., 0.}, 1), std::domain_error); + REQUIRE_THROWS_AS(par2ic({11.1, 1.4, 0., 0., 0., 5.23}, 1), std::domain_error); + +} +TEST_CASE("ic2par2ic") { + // Engines + // NOLINTNEXTLINE(cert-msc32-c, cert-msc51-cpp) + std::mt19937 rng_engine(122012203u); + // Distributions for the elements + std::uniform_real_distribution sma_d(1.1, 100.); + std::uniform_real_distribution ecc_d(0, 0.99); + 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); + { + // Testing on N random calls on ellipses + unsigned N = 10000; + 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); + auto [r, v] = par2ic({sma, ecc, incl, Omega, omega, f}, 1.); + auto par = ic2par(r, v, 1.0); + REQUIRE_THAT(par[0], WithinAbs(sma, 1e-10)); + } + } + { + // Testing on N random calls on hyperbolas + unsigned N = 10000; + for (auto i = 0u; i < N; ++i) { + double sma = -sma_d(rng_engine); + double ecc = ecc_d(rng_engine) + 1.1; + double incl = incl_d(rng_engine); + double Omega = Omega_d(rng_engine); + double omega = omega_d(rng_engine); + double f = f_d(rng_engine); + // Skipping if true anomaly out of asymptotes + if (std::cos(f) < -1 / ecc) { + continue; + } + auto [r, v] = par2ic({sma, ecc, incl, Omega, omega, f}, 1.); + auto par = ic2par(r, v, 1.0); + REQUIRE_THAT(par[0], WithinAbs(sma, 1e-10)); + } + } }