From 4b0bebee7c342a6ee84d7c79920389712719529b Mon Sep 17 00:00:00 2001 From: tteil Date: Sun, 11 May 2025 20:16:14 -0600 Subject: [PATCH 1/5] add cpp orbital motion --- src/architecture/utilities/orbitalMotion.cpp | 582 +++++++++++++++++++ src/architecture/utilities/orbitalMotion.hpp | 91 +++ 2 files changed, 673 insertions(+) create mode 100644 src/architecture/utilities/orbitalMotion.cpp create mode 100644 src/architecture/utilities/orbitalMotion.hpp diff --git a/src/architecture/utilities/orbitalMotion.cpp b/src/architecture/utilities/orbitalMotion.cpp new file mode 100644 index 000000000..3adbae229 --- /dev/null +++ b/src/architecture/utilities/orbitalMotion.cpp @@ -0,0 +1,582 @@ +/* + ISC License + + Copyright (c) 2025, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "orbitalMotion.hpp" +#include "astroConstants.h" + +constexpr double tolerance = 1e-8; + +/** + * @brief Computes the Hill frame direction cosine matrix from inertial position and velocity. + * @param rc_N Position vector in inertial frame. + * @param vc_N Velocity vector in inertial frame. + * @return 3x3 DCM that transforms Hill frame vectors to inertial frame. + */ +Eigen::Matrix3d OrbitalMotion::hillFrameDCM(const Eigen::Vector3d& rc_N, const Eigen::Vector3d& vc_N) { + Eigen::Vector3d ir_N = rc_N.normalized(); + Eigen::Vector3d hVec_N = rc_N.cross(vc_N); + Eigen::Vector3d ih_N = hVec_N.normalized(); + Eigen::Vector3d itheta_N = ih_N.cross(ir_N); + + Eigen::Matrix3d HN; + HN.row(0) = ir_N.transpose(); + HN.row(1) = itheta_N.transpose(); + HN.row(2) = ih_N.transpose(); + return HN; +} + +/** + * @brief Converts relative Hill frame position and velocity to inertial frame coordinates. + * @param rc_N Chief satellite inertial position. + * @param vc_N Chief satellite inertial velocity. + * @param rho_H Deputy satellite relative position in Hill frame. + * @param rhoPrime_H Deputy satellite relative velocity in Hill frame. + * @param rd_N Output: deputy inertial position. + * @param vd_N Output: deputy inertial velocity. + */ +void OrbitalMotion::hillToInertialState(const Eigen::Vector3d& rc_N, + const Eigen::Vector3d& vc_N, + const Eigen::Vector3d& rho_H, + const Eigen::Vector3d& rhoPrime_H, + Eigen::Vector3d& rd_N, + Eigen::Vector3d& vd_N) { + Eigen::Matrix3d HN = hillFrameDCM(rc_N, vc_N); + Eigen::Matrix3d NH = HN.transpose(); + Eigen::Vector3d hVec_N = rc_N.cross(vc_N); + double rc = rc_N.norm(); + double fDot = hVec_N.norm() / (rc * rc); + Eigen::Vector3d omega_HN_H(0, 0, fDot); + + Eigen::Vector3d rho_N = NH * rho_H; + rd_N = rc_N + rho_N; + + Eigen::Vector3d crossTerm = omega_HN_H.cross(rho_H); + Eigen::Vector3d velHill = crossTerm + rhoPrime_H; + vd_N = vc_N + NH * velHill; +} + +/** + * @brief Converts inertial position and velocity to Hill frame relative coordinates. + * @param rc_N Chief satellite inertial position. + * @param vc_N Chief satellite inertial velocity. + * @param rd_N Deputy satellite inertial position. + * @param vd_N Deputy satellite inertial velocity. + * @param rho_H Output: relative Hill frame position. + * @param rhoPrime_H Output: relative Hill frame velocity. + */ +void OrbitalMotion::inertialToHillState(const Eigen::Vector3d& rc_N, + const Eigen::Vector3d& vc_N, + const Eigen::Vector3d& rd_N, + const Eigen::Vector3d& vd_N, + Eigen::Vector3d& rho_H, + Eigen::Vector3d& rhoPrime_H) { + Eigen::Matrix3d HN = hillFrameDCM(rc_N, vc_N); + Eigen::Vector3d hVec_N = rc_N.cross(vc_N); + double rc = rc_N.norm(); + double fDot = hVec_N.norm() / (rc * rc); + Eigen::Vector3d omega_HN_H(0, 0, fDot); + + Eigen::Vector3d rho_N = rd_N - rc_N; + rho_H = HN * rho_N; + + Eigen::Vector3d rhoDot_N = vd_N - vc_N; + Eigen::Vector3d rhoDot_H = HN * rhoDot_N; + rhoPrime_H = rhoDot_H - omega_HN_H.cross(rho_H); +} + +/** + * @brief Converts eccentric anomaly to true anomaly. + * @param E Eccentric anomaly in radians. + * @param e Orbital eccentricity. + * @return True anomaly in radians. + */ +double OrbitalMotion::eccentricToTrueAnomaly(double E, double e) { + assert((e >= 0.0 || e < 1.0) && "Eccentricity out of bounds (0 <= e < 1)"); + return 2.0 * std::atan2(std::sqrt(1 + e) * std::sin(E / 2), std::sqrt(1 - e) * std::cos(E / 2)); +} + +/** + * @brief Converts eccentric anomaly to mean anomaly. + * @param E Eccentric anomaly in radians. + * @param e Orbital eccentricity. + * @return Mean anomaly in radians. + */ +double OrbitalMotion::eccentricToMeanAnomaly(double E, double e) { + assert((e >= 0.0 || e < 1.0) && "Eccentricity out of bounds (0 <= e < 1)"); + return E - e * std::sin(E); +} + +/** + * @brief Converts true anomaly to eccentric anomaly. + * @param f True anomaly in radians. + * @param e Orbital eccentricity. + * @return Eccentric anomaly in radians. + */ +double OrbitalMotion::trueToEccentricAnomaly(double f, double e) { + assert((e >= 0.0 || e < 1.0) && "Eccentricity out of bounds (0 <= e < 1)"); + return 2.0 * std::atan2(std::sqrt(1 - e) * std::sin(f / 2), std::sqrt(1 + e) * std::cos(f / 2)); +} + +/** + * @brief Convert true anomaly to mean anomaly + * @param f True anomaly in radians + * @param e Orbital eccentricity (0 <= e < 1). + * @return Mean anomaly in radians. + */ +double OrbitalMotion::trueToMeanAnomaly(double f, double e) { + assert((e >= 0.0 || e < 1.0) && "Eccentricity out of bounds (0 <= e < 1)"); + double eccentric = trueToEccentricAnomaly(f, e); + return eccentricToMeanAnomaly(eccentric, e); +} + +/** + * @brief Converts true anomaly to hyperbolic anomaly. + * @param f True anomaly in radians. + * @param e Orbital eccentricity (> 1). + * @return Hyperbolic anomaly in radians. + */ +double OrbitalMotion::trueToHyperbolicAnomaly(double f, double e) { + assert(e > 1.0 && "Eccentricity must be > 1 for hyperbolic orbits"); + return 2.0 * std::atanh(std::sqrt((e - 1) / (e + 1)) * std::tan(f / 2)); +} + +/** + * @brief Converts hyperbolic anomaly to true anomaly. + * @param H Hyperbolic anomaly in radians. + * @param e Orbital eccentricity (> 1). + * @return True anomaly in radians. + */ +double OrbitalMotion::hyperbolicToTrueAnomaly(double H, double e) { + assert(e > 1.0 && "Eccentricity must be > 1 for hyperbolic orbits"); + return 2.0 * std::atan(std::sqrt((e + 1) / (e - 1)) * std::tanh(H / 2)); +} + +/** + * @brief Converts hyperbolic anomaly to mean hyperbolic anomaly. + * @param H Hyperbolic anomaly in radians. + * @param e Orbital eccentricity (> 1). + * @return Mean hyperbolic anomaly in radians. + */ +double OrbitalMotion::hyperbolicToMeanAnomaly(double H, double e) { + assert(e > 1.0 && "Eccentricity must be > 1 for hyperbolic orbits"); + return e * std::sinh(H) - H; +} + +/** + * @brief Convert mean anomaly to eccentric anomaly + * @param M Mean anomaly in radians. + * @param e Orbital eccentricity (0 <= e < 1). + * @return Eccentric anomaly in radians. + */ +double OrbitalMotion::meanToEccentricAnomaly(double M, double e) { + assert((e >= 0.0 || e < 1.0) && "Eccentricity out of bounds (0 <= e < 1)"); + double E = M; + for (int i = 0; i < 200; ++i) { + double dE = (E - e * std::sin(E) - M) / (1 - e * std::cos(E)); + E -= dE; + if (std::abs(dE) < tolerance) break; + } + return E; +} + +/** + * @brief Convert mean anomaly to true anomaly + * @param M Mean anomaly in radians. + * @param e Orbital eccentricity (0 <= e < 1). + * @return True anomaly in radians. + */ +double OrbitalMotion::meanToTrueAnomaly(double M, double e) { + assert((e >= 0.0 || e < 1.0) && "Eccentricity out of bounds (0 <= e < 1)"); + double eccentric = meanToEccentricAnomaly(M, e); + return eccentricToTrueAnomaly(eccentric, e); +} + +/** + * @brief Mean hyperbolic anomaly to hyperbolic anomaly + * @param N Mean hyperbolic anomaly in radians. + * @param e Orbital eccentricity (> 1). + * @return Hyperbolic anomaly in radians. + */ +double OrbitalMotion::meanToHyperbolicAnomaly(double N, double e) { + assert(e > 1.0 && "Eccentricity must be > 1"); + double H = std::abs(N) > 7.0 ? 7.0 * (N > 0 ? 1 : -1) : N; + for (int i = 0; i < 200; ++i) { + double dH = (e * std::sinh(H) - H - N) / (e * std::cosh(H) - 1); + H -= dH; + if (std::abs(dH) < tolerance) break; + } + return H; +} + +/** + * @brief Converts orbital elements to position and velocity vectors. + * @param mu Gravitational parameter (km^3/s^2). + * @param elements Classical orbital elements (a, e, i, Omega, omega, f). + * @param rVec Output: position vector in km. + * @param vVec Output: velocity vector in km/s. + */ +CartesianState OrbitalMotion::elementsToCartesianState(double mu, const ClassicalElements& elements) { + double a = elements.semiMajorAxis; + double e = elements.eccentricity; + double i = elements.inclination; + double Omega = elements.rightAscensionAscendingNode; + double omega = elements.argPeriapsis; + double f = elements.trueAnomaly; + + double p = a * (1 - e * e); + double r = p / (1 + e * std::cos(f)); + double h = std::sqrt(mu * p); + + double cos_O = std::cos(Omega), sin_O = std::sin(Omega); + double cos_o = std::cos(omega), sin_o = std::sin(omega); + double cos_i = std::cos(i), sin_i = std::sin(i); + double cos_f = std::cos(f), sin_f = std::sin(f); + + double cos_theta = cos_o * cos_f - sin_o * sin_f; + double sin_theta = sin_o * cos_f + cos_o * sin_f; + + Eigen::Vector3d rVec{}; + rVec(0) = r * (cos_O * cos_theta - sin_O * sin_theta * cos_i); + rVec(1) = r * (sin_O * cos_theta + cos_O * sin_theta * cos_i); + rVec(2) = r * (sin_theta * sin_i); + + double vx = -mu / h * (cos_O * (sin_theta + e * sin_o) + sin_O * (cos_theta + e * cos_o) * cos_i); + double vy = -mu / h * (sin_O * (sin_theta + e * sin_o) - cos_O * (cos_theta + e * cos_o) * cos_i); + double vz = mu / h * (cos_theta + e * cos_o) * sin_i; + + Eigen::Vector3d vVec = Eigen::Vector3d(vx, vy, vz); + + CartesianState state{}; + state.position = rVec; + state.velocity = vVec; + + return state; +} + +/** + * @brief Converts position and velocity vectors to classical orbital elements. + * @param mu Gravitational parameter (km^3/s^2). + * @param rVec Position vector in km. + * @param vVec Velocity vector in km/s. + * @return elements : classical orbital elements (a, e, i, Omega, omega, f). + */ +ClassicalElements OrbitalMotion::cartesianStateToElements(double mu, + const Eigen::Vector3d& rVec, + const Eigen::Vector3d& vVec) { + double r = rVec.norm(); + double v = vVec.norm(); + Eigen::Vector3d hVec = rVec.cross(vVec); + double h = hVec.norm(); + Eigen::Vector3d nVec = Eigen::Vector3d::UnitZ().cross(hVec); + Eigen::Vector3d eVec = (v * v - mu / r) * rVec - (rVec.dot(vVec)) * vVec; + eVec /= mu; + + ClassicalElements elements{}; + + elements.radiusMagnitude = r; + elements.alpha = 2.0 / r - v * v / mu; + elements.semiMajorAxis = std::abs(elements.alpha) > 1e-10 ? 1.0 / elements.alpha : 0.0; + elements.eccentricity = eVec.norm(); + elements.inclination = std::acos(hVec(2) / h); + + double Omega = std::atan2(nVec(1), nVec(0)); + elements.rightAscensionAscendingNode = Omega < 0 ? Omega + 2 * M_PI : Omega; + + double omega = std::atan2(nVec.cross(eVec).dot(hVec.normalized()), nVec.dot(eVec)); + elements.argPeriapsis = omega < 0 ? omega + 2 * M_PI : omega; + + double f = std::atan2(eVec.cross(rVec).dot(hVec.normalized()), eVec.dot(rVec)); + elements.trueAnomaly = f < 0 ? f + 2 * M_PI : f; + + elements.radiusPeriapsis = h * h / mu / (1 + elements.eccentricity); + elements.radiusApoapsis = h * h / mu / (1 - elements.eccentricity); + + return elements; +} + +/** + * @brief Computes atmospheric density based on altitude using an empirical model. + * @param altitudeKm Altitude above Earth's surface in kilometers. + * @return Atmospheric density in kg/m^3. + */ +double OrbitalMotion::atmosphericDensity(double altitudeKm) { + if (altitudeKm > 1000.0) { + double logDensity = -7e-5 * altitudeKm - 14.464; + return std::pow(10.0, logDensity); + } + + double val = (altitudeKm - 526.8) / 292.8563; + double logDensity = 0.34047 * std::pow(val, 6) - 0.5889 * std::pow(val, 5) - 0.5269 * std::pow(val, 4) + + 1.0036 * std::pow(val, 3) + 0.60713 * std::pow(val, 2) - 2.3024 * val - 12.575; + + return std::pow(10.0, logDensity); +} + +/** + * @brief Computes acceleration due to atmospheric drag. + * @param Cd Drag coefficient. + * @param A Cross-sectional area in m^2. + * @param massKg Mass of the object in kilograms. + * @param positionKm Position vector in km (used to determine altitude). + * @param velocityKmPerSec Velocity vector in km/s. + * @return Drag acceleration vector in km/s^2. + */ +Eigen::Vector3d OrbitalMotion::atmosphericDragAccel(double Cd, + double A, + double massKg, + const Eigen::Vector3d& positionKm, + const Eigen::Vector3d& velocityKmPerSec) { + const double earthRadiusKm = 6378.137; + double r = positionKm.norm(); + double v = velocityKmPerSec.norm(); + double altitudeKm = r - earthRadiusKm; + + assert(altitudeKm > 0.0 && "Altitude must be above Earth's surface"); + + double rho = atmosphericDensity(altitudeKm); + double accelerationMag = -0.5 * rho * (Cd * A / massKg) * std::pow(v * 1000.0, 2) / 1000.0; + + return (accelerationMag / v) * velocityKmPerSec; +} + +/** + * @brief Computes acceleration due to solar radiation pressure. + * @param areaM2 Cross-sectional area in m^2. + * @param massKg Mass of the object in kilograms. + * @param sunVecAU Vector from object to Sun in AU. + * @return Acceleration due to radiation pressure in km/s^2. + */ +Eigen::Vector3d OrbitalMotion::solarRadiationPressureAccel(double areaM2, + double massKg, + const Eigen::Vector3d& sunVecAU) { + const double flux = 1372.5398; // W/m^2 + const double speedOfLight = 299792458.0; // m/s + const double Cr = 1.3; // Reflectivity coefficient + + double distanceAU = sunVecAU.norm(); + double scalingFactor = -Cr * areaM2 * flux / (massKg * speedOfLight * std::pow(distanceAU, 3)) / 1000.0; + return scalingFactor * sunVecAU; +} + +/** + * @brief Computes acceleration due to zonal harmonic perturbations (J2-J6). + * @param positionKm Position vector in km. + * @param maxJ Maximum J-term to include (between 2 and 6). + * @param body Celestial object enum (currently only Earth is implemented). + * @return Perturbing acceleration vector in km/s^2. + */ +Eigen::Vector3d OrbitalMotion::zonalHarmonicPerturbation(const Eigen::Vector3d& positionKm, + int maxJ, + CelestialObject body) { + assert((maxJ > 2 || maxJ < 6) && "maxJ must be in range [2,6]"); + double mu{}; + double req{}; + double J2{}, J3{}, J4{}, J5{}, J6{}; + if (body == CelestialObject::Sun) { + mu = MU_SUN; + req = REQ_SUN; + } else if (body == CelestialObject::Mercury) { + mu = MU_MERCURY; + req = REQ_MERCURY; + J2 = J2_MERCURY; + } else if (body == CelestialObject::Venus) { + mu = MU_VENUS; + req = REQ_VENUS; + J2 = J2_VENUS; + } else if (body == CelestialObject::Moon) { + mu = MU_MOON; + req = REQ_MOON; + } else if (body == CelestialObject::Mars) { + mu = MU_MARS; + req = REQ_MARS; + J2 = J2_MARS; + } else if (body == CelestialObject::Jupiter) { + mu = MU_JUPITER; + req = REQ_JUPITER; + J2 = J2_JUPITER; + } else if (body == CelestialObject::Saturn) { + mu = MU_SATURN; + req = REQ_SATURN; + J2 = J2_SATURN; + } else if (body == CelestialObject::Uranus) { + mu = MU_URANUS; + req = REQ_URANUS; + J2 = J2_URANUS; + } else if (body == CelestialObject::Neptune) { + mu = MU_NEPTUNE; + req = REQ_NEPTUNE; + J2 = J2_NEPTUNE; + } else { + mu = MU_EARTH; + req = REQ_EARTH; + J2 = J2_EARTH; + J3 = J3_EARTH; + J4 = J4_EARTH; + J5 = J5_EARTH; + J6 = J6_EARTH; + } + double x = positionKm(0), y = positionKm(1), z = positionKm(2); + double r = positionKm.norm(); + double zr = z / r; + + Eigen::Vector3d ajtot = Eigen::Vector3d::Zero(); + + if (maxJ >= 2) { + Eigen::Vector3d term = { + (1.0 - 5.0 * zr * zr) * x / r, (1.0 - 5.0 * zr * zr) * y / r, (3.0 - 5.0 * zr * zr) * z / r}; + ajtot += -1.5 * J2 * mu / std::pow(r, 2) * std::pow(req / r, 2) * term; + } + if (maxJ >= 3) { + Eigen::Vector3d term = {5.0 * (7.0 * std::pow(zr, 3) - 3.0 * zr) * x / r, + 5.0 * (7.0 * std::pow(zr, 3) - 3.0 * zr) * y / r, + -3.0 * (10.0 * zr * zr - (35.0 / 3.0) * std::pow(zr, 4) - 1.0)}; + ajtot += 0.5 * J3 * mu / std::pow(r, 2) * std::pow(req / r, 3) * term; + } + if (maxJ >= 4) { + Eigen::Vector3d term = {(3.0 - 42.0 * zr * zr + 63.0 * std::pow(zr, 4)) * x / r, + (3.0 - 42.0 * zr * zr + 63.0 * std::pow(zr, 4)) * y / r, + (15.0 - 70.0 * zr * zr + 63.0 * std::pow(zr, 4)) * z / r}; + ajtot += 0.625 * J4 * mu / std::pow(r, 2) * std::pow(req / r, 4) * term; + } + if (maxJ >= 5) { + Eigen::Vector3d term = {3.0 * (35.0 * zr - 210.0 * std::pow(zr, 3) + 231.0 * std::pow(zr, 5)) * x / r, + 3.0 * (35.0 * zr - 210.0 * std::pow(zr, 3) + 231.0 * std::pow(zr, 5)) * y / r, + -(15.0 - 315.0 * zr * zr + 945.0 * std::pow(zr, 4) - 693.0 * std::pow(zr, 6))}; + ajtot += 0.125 * J5 * mu / std::pow(r, 2) * std::pow(req / r, 5) * term; + } + if (maxJ >= 6) { + Eigen::Vector3d term = { + (35.0 - 945.0 * zr * zr + 3465.0 * std::pow(zr, 4) - 3003.0 * std::pow(zr, 6)) * x / r, + (35.0 - 945.0 * zr * zr + 3465.0 * std::pow(zr, 4) - 3003.0 * std::pow(zr, 6)) * y / r, + -(3003.0 * std::pow(zr, 6) - 4851.0 * std::pow(zr, 4) + 2205.0 * zr * zr - 245.0) * z / r}; + ajtot += -1.0 / 16.0 * J6 * mu / std::pow(r, 2) * std::pow(req / r, 6) * term; + } + + return ajtot; +} + +/** + * @brief Converts classical orbital elements to equinoctial elements. + * @param classical Input: Classical orbital elements. + * @param equinoctial Output: Converted equinoctial elements. + */ +EquinoctialElements OrbitalMotion::mapClassicalToEquinoctialElements(const ClassicalElements& classical) { + double e = classical.eccentricity; + double i = classical.inclination; + double Omega = classical.rightAscensionAscendingNode; + double omega = classical.argPeriapsis; + double f = classical.trueAnomaly; + + double E = trueToEccentricAnomaly(f, e); + double M = eccentricToMeanAnomaly(E, e); + + EquinoctialElements equinoctial{}; + equinoctial.a = classical.semiMajorAxis; + equinoctial.P1 = e * std::sin(Omega + omega); + equinoctial.P2 = e * std::cos(Omega + omega); + equinoctial.Q1 = std::tan(i / 2.0) * std::sin(Omega); + equinoctial.Q2 = std::tan(i / 2.0) * std::cos(Omega); + equinoctial.l = Omega + omega + M; + equinoctial.L = Omega + omega + f; + + return equinoctial; +} + +/** + * @brief Converts mean classical orbital elements to osculating elements using J2 perturbation theory. Equations + are found in Schaub/Junkins (F.1-20) + * @param req Equatorial radius of the central body (km). + * @param J2 J2 coefficient of the central body. + * @param meanEl Input: Mean classical elements. + * @param oscEl Output: Osculating classical elements. + * @param directionSign Direction sign for J2 application (+1 for direct, -1 for inverse mapping). + */ +void OrbitalMotion::mapMeanToOsculatingElements(double req, + double J2, + const ClassicalElements& meanEl, + ClassicalElements& oscEl, + double directionSign) { + double a = meanEl.semiMajorAxis; + double e = meanEl.eccentricity; + double i = meanEl.inclination; + double Omega = meanEl.rightAscensionAscendingNode; + double omega = meanEl.argPeriapsis; + double f = meanEl.trueAnomaly; + + double E = trueToEccentricAnomaly(f, e); + double M = eccentricToMeanAnomaly(E, e); + + double eta = std::sqrt(1.0 - e * e); + double gamma2 = directionSign * J2 / 2.0 * std::pow(req / a, 2); + double gamma2p = gamma2 / std::pow(eta, 4); + double a_r = (1.0 + e * std::cos(f)) / (eta * eta); + + double cosi = std::cos(i), cos2omega = std::cos(2.0 * omega), sin2omega = std::sin(2.0 * omega); + double cosf = std::cos(f); + + oscEl.semiMajorAxis = a + a * gamma2 * + ((3.0 * cosi * cosi - 1.0) * (std::pow(a_r, 3.0) - 1.0 / std::pow(eta, 3.0)) + + 3.0 * (1.0 - cosi * cosi) * std::pow(a_r, 3.0) * std::cos(2.0 * omega + 2.0 * f)); + + double de1 = gamma2p / 8.0 * e * eta * eta * + ((1.0 - 11.0 * cosi * cosi - 40.0 * cosi * cosi * cosi * cosi) / (1.0 - 5.0 * cosi * cosi)) * + cos2omega; + + double term = e * eta + e / (1.0 + eta) + 3.0 * cosf + 3.0 * e * cosf * cosf + e * e * std::pow(cosf, 3.0); + double term2 = e + 3.0 * cosf + 3.0 * e * cosf * cosf + e * e * std::pow(cosf, 3.0); + + double de = + de1 + eta * eta / 2.0 * + (gamma2 * ((3.0 * cosi * cosi - 1.0) / std::pow(eta, 6.0) * term + + 3.0 * (1.0 - cosi * cosi) / std::pow(eta, 6.0) * term2 * std::cos(2.0 * omega + 2.0 * f)) - + gamma2p * (1.0 - cosi * cosi) * (3.0 * std::cos(2.0 * omega + f) + std::cos(2.0 * omega + 3.0 * f))); + + double di = -e * de1 / (eta * eta) / std::tan(i) + + gamma2p / 2.0 * cosi * std::sqrt(1.0 - cosi * cosi) * + (3.0 * std::cos(2.0 * omega + 2.0 * f) + 3.0 * e * std::cos(2.0 * omega + f) + + e * std::cos(2.0 * omega + 3.0 * f)); + + double MpOp = M + omega + Omega + + gamma2p / 8.0 * eta * eta * eta * + ((1.0 - 11.0 * cosi * cosi - 40.0 * cosi * cosi * cosi * cosi) / (1.0 - 5.0 * cosi * cosi)) * + sin2omega; + + double d1 = (e + de) * std::sin(M); + double d2 = (e + de) * std::cos(M); + double Mp = std::atan2(d1, d2); + double ep = std::sqrt(d1 * d1 + d2 * d2); + + double d3 = + (std::sin(i / 2.0) + std::cos(i / 2.0) * di / 2.0) * std::sin(Omega) + std::sin(i / 2.0) * di * std::cos(Omega); + double d4 = + (std::sin(i / 2.0) + std::cos(i / 2.0) * di / 2.0) * std::cos(Omega) - std::sin(i / 2.0) * di * std::sin(Omega); + double Omegap = std::atan2(d3, d4); + double ip = 2.0 * std::asin(std::sqrt(d3 * d3 + d4 * d4)); + + double omegap = MpOp - Mp - Omegap; + double Ep = meanToEccentricAnomaly(Mp, ep); + double fp = eccentricToTrueAnomaly(Ep, ep); + + oscEl.semiMajorAxis = a + a * gamma2; + oscEl.eccentricity = ep; + oscEl.inclination = ip; + oscEl.rightAscensionAscendingNode = Omegap; + oscEl.argPeriapsis = omegap; + oscEl.trueAnomaly = fp; +} diff --git a/src/architecture/utilities/orbitalMotion.hpp b/src/architecture/utilities/orbitalMotion.hpp new file mode 100644 index 000000000..2b082659b --- /dev/null +++ b/src/architecture/utilities/orbitalMotion.hpp @@ -0,0 +1,91 @@ +/* + ISC License + + Copyright (c) 2025, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include +#include +#include + +struct CartesianState { + Eigen::Vector3d position; + Eigen::Vector3d velocity; +}; + +enum class CelestialObject { Sun, Mercury, Venus, Earth, Moon, Mars, Jupiter, Saturn, Uranus, Neptune, Pluto }; + +class ClassicalElements { + public: + double semiMajorAxis = 0, eccentricity = 0, inclination = 0, rightAscensionAscendingNode = 0, argPeriapsis = 0, + trueAnomaly = 0; + double radiusMagnitude = 0, alpha = 0, radiusPeriapsis = 0, radiusApoapsis = 0; +}; + +class EquinoctialElements { + public: + double a = 0, P1 = 0, P2 = 0, Q1 = 0, Q2 = 0, l = 0, L = 0; +}; + +class OrbitalMotion { + public: + static Eigen::Matrix3d hillFrameDCM(const Eigen::Vector3d& rc_N, const Eigen::Vector3d& vc_N); + static void hillToInertialState(const Eigen::Vector3d& rc_N, + const Eigen::Vector3d& vc_N, + const Eigen::Vector3d& rho_H, + const Eigen::Vector3d& rhoPrime_H, + Eigen::Vector3d& rd_N, + Eigen::Vector3d& vd_N); + static void inertialToHillState(const Eigen::Vector3d& rc_N, + const Eigen::Vector3d& vc_N, + const Eigen::Vector3d& rd_N, + const Eigen::Vector3d& vd_N, + Eigen::Vector3d& rho_H, + Eigen::Vector3d& rhoPrime_H); + + static double eccentricToTrueAnomaly(double E, double e); + static double eccentricToMeanAnomaly(double E, double e); + static double trueToEccentricAnomaly(double f, double e); + static double trueToHyperbolicAnomaly(double f, double e); + static double trueToMeanAnomaly(double f, double e); + static double hyperbolicToTrueAnomaly(double H, double e); + static double hyperbolicToMeanAnomaly(double H, double e); + static double meanToEccentricAnomaly(double M, double e); + static double meanToTrueAnomaly(double M, double e); + static double meanToHyperbolicAnomaly(double N, double e); + + static CartesianState elementsToCartesianState(double mu, const ClassicalElements& elements); + static ClassicalElements cartesianStateToElements(double mu, + const Eigen::Vector3d& rVec, + const Eigen::Vector3d& vVec); + + static double atmosphericDensity(double altitudeKm); + static Eigen::Vector3d atmosphericDragAccel(double Cd, + double A, + double massKg, + const Eigen::Vector3d& positionKm, + const Eigen::Vector3d& velocityKmPerSec); + + static Eigen::Vector3d solarRadiationPressureAccel(double areaM2, double massKg, const Eigen::Vector3d& sunVecAU); + static Eigen::Vector3d zonalHarmonicPerturbation(const Eigen::Vector3d& positionKm, int maxJ, CelestialObject body); + + static EquinoctialElements mapClassicalToEquinoctialElements(const ClassicalElements& classicals); + static void mapMeanToOsculatingElements(double req, + double J2, + const ClassicalElements& meanEl, + ClassicalElements& oscEl, + double directionSign); +}; From 33fc0e69bd8a02f7c5c59b63ca4f9eda4aadd0aa Mon Sep 17 00:00:00 2001 From: tteil Date: Sun, 11 May 2025 20:16:25 -0600 Subject: [PATCH 2/5] add unit test comparing to previous .c --- .../tests/test_compare_orbitalMotion.cpp | 266 ++++++++++++++++++ 1 file changed, 266 insertions(+) create mode 100644 src/architecture/utilities/tests/test_compare_orbitalMotion.cpp diff --git a/src/architecture/utilities/tests/test_compare_orbitalMotion.cpp b/src/architecture/utilities/tests/test_compare_orbitalMotion.cpp new file mode 100644 index 000000000..107098cdf --- /dev/null +++ b/src/architecture/utilities/tests/test_compare_orbitalMotion.cpp @@ -0,0 +1,266 @@ +/* + ISC License + + Copyright (c) 2025, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +#include "architecture/utilities/orbitalMotion.hpp" +#include +extern "C" { +#include "architecture/utilities/orbitalMotion.h" +} + +constexpr double tol = 1e-8; + +class OrbitalMotionTest : public ::testing::Test { + protected: + void SetUp() override { + // Test setup if needed + } +}; + +TEST_F(OrbitalMotionTest, Compare_atmosphericDensity) { + for (double alt = 100; alt <= 1000; alt += 100) { + double c_result = atmosphericDensity(alt); + double cpp_result = OrbitalMotion::atmosphericDensity(alt); + EXPECT_NEAR(c_result, cpp_result, tol); + } +} + +TEST_F(OrbitalMotionTest, Compare_eccentricToTrueAnomaly) { + for (double e = 0.01; e < 1.0; e += 0.1) { + double E = 1.0; + double c_result = E2f(E, e); + double cpp_result = OrbitalMotion::eccentricToTrueAnomaly(E, e); + EXPECT_NEAR(c_result, cpp_result, tol); + } +} + +TEST_F(OrbitalMotionTest, Compare_eccentricToMeanAnomaly) { + for (double e = 0.01; e < 1.0; e += 0.1) { + double E = 1.0; + double c_result = E2M(E, e); + double cpp_result = OrbitalMotion::eccentricToMeanAnomaly(E, e); + EXPECT_NEAR(c_result, cpp_result, tol); + } +} + +TEST_F(OrbitalMotionTest, Compare_trueToEccentricAnomaly) { + for (double e = 0.01; e < 1.0; e += 0.1) { + double f = 1.0; + double c_result = f2E(f, e); + double cpp_result = OrbitalMotion::trueToEccentricAnomaly(f, e); + EXPECT_NEAR(c_result, cpp_result, tol); + } +} + +TEST_F(OrbitalMotionTest, Compare_trueToHyperbolicAnomaly) { + for (double e = 1.1; e < 2.0; e += 0.2) { + double f = 1.0; + double c_result = f2H(f, e); + double cpp_result = OrbitalMotion::trueToHyperbolicAnomaly(f, e); + EXPECT_NEAR(c_result, cpp_result, tol); + } +} + +TEST_F(OrbitalMotionTest, Compare_hyperbolicToTrueAnomaly) { + for (double e = 1.1; e < 2.0; e += 0.2) { + double H = 1.0; + double c_result = H2f(H, e); + double cpp_result = OrbitalMotion::hyperbolicToTrueAnomaly(H, e); + EXPECT_NEAR(c_result, cpp_result, tol); + } +} + +TEST_F(OrbitalMotionTest, Compare_hyperbolicToMeanAnomaly) { + for (double e = 1.1; e < 2.0; e += 0.2) { + double H = 1.0; + double c_result = H2N(H, e); + double cpp_result = OrbitalMotion::hyperbolicToMeanAnomaly(H, e); + EXPECT_NEAR(c_result, cpp_result, tol); + } +} + +TEST_F(OrbitalMotionTest, Compare_solveKeplerEquationElliptic) { + for (double e = 0.01; e < 1.0; e += 0.1) { + double M = 1.0; + double c_result = M2E(M, e); + double cpp_result = OrbitalMotion::meanToEccentricAnomaly(M, e); + EXPECT_NEAR(c_result, cpp_result, tol); + } +} + +TEST_F(OrbitalMotionTest, Compare_solveKeplerEquationHyperbolic) { + for (double e = 1.1; e < 2.0; e += 0.2) { + double N = 1.0; + double c_result = N2H(N, e); + double cpp_result = OrbitalMotion::meanToHyperbolicAnomaly(N, e); + EXPECT_NEAR(c_result, cpp_result, tol); + } +} + +TEST_F(OrbitalMotionTest, Compare_hillToInertialState) { + double rc_N[3] = {7000, 0, 0}; + double vc_N[3] = {0, 7.5, 0}; + double rho_H[3] = {1, 0, 0}, rhoPrime_H[3] = {0, 0.1, 0}; + double rd_N_c[3], vd_N_c[3]; + hill2rv(rc_N, vc_N, rho_H, rhoPrime_H, rd_N_c, vd_N_c); + + Eigen::Vector3d rc(rc_N[0], rc_N[1], rc_N[2]); + Eigen::Vector3d vc(vc_N[0], vc_N[1], vc_N[2]); + Eigen::Vector3d rho(rho_H[0], rho_H[1], rho_H[2]); + Eigen::Vector3d rhoPrime(rhoPrime_H[0], rhoPrime_H[1], rhoPrime_H[2]); + Eigen::Vector3d rd_cpp, vd_cpp; + + OrbitalMotion::hillToInertialState(rc, vc, rho, rhoPrime, rd_cpp, vd_cpp); + + for (int i = 0; i < 3; ++i) { + EXPECT_NEAR(rd_N_c[i], rd_cpp(i), tol); + EXPECT_NEAR(vd_N_c[i], vd_cpp(i), tol); + } +} + +TEST_F(OrbitalMotionTest, Compare_inertialToHillState) { + double rc_N[3] = {7000, 0, 0}; + double vc_N[3] = {0, 7.5, 0}; + double rd_N[3] = {7001, 0, 0}; + double vd_N[3] = {0, 7.6, 0}; + double rho_H_c[3], rhoPrime_H_c[3]; + rv2hill(rc_N, vc_N, rd_N, vd_N, rho_H_c, rhoPrime_H_c); + + Eigen::Vector3d rc(rc_N[0], rc_N[1], rc_N[2]); + Eigen::Vector3d vc(vc_N[0], vc_N[1], vc_N[2]); + Eigen::Vector3d rd(rd_N[0], rd_N[1], rd_N[2]); + Eigen::Vector3d vd(vd_N[0], vd_N[1], vd_N[2]); + Eigen::Vector3d rho_H_cpp, rhoPrime_H_cpp; + + OrbitalMotion::inertialToHillState(rc, vc, rd, vd, rho_H_cpp, rhoPrime_H_cpp); + + for (int i = 0; i < 3; ++i) { + EXPECT_NEAR(rho_H_c[i], rho_H_cpp(i), tol); + EXPECT_NEAR(rhoPrime_H_c[i], rhoPrime_H_cpp(i), tol); + } +} + +TEST_F(OrbitalMotionTest, Compare_atmosphericDragAccel) { + double Cd = 2.2, A = 3.0, m = 1000.0; + double rvec[3] = {7000.0, 0, 0}; + double vvec[3] = {0, 7.5, 0}; + double advec[3]; + atmosphericDrag(Cd, A, m, rvec, vvec, advec); + + Eigen::Vector3d r(rvec[0], rvec[1], rvec[2]); + Eigen::Vector3d v(vvec[0], vvec[1], vvec[2]); + Eigen::Vector3d acc_cpp = OrbitalMotion::atmosphericDragAccel(Cd, A, m, r, v); + + for (int i = 0; i < 3; ++i) { + EXPECT_NEAR(advec[i], acc_cpp(i), tol); + } +} + +TEST_F(OrbitalMotionTest, Compare_solarRadiationPressureAccel) { + double A = 3.0, m = 1000.0; + double sunvec[3] = {1.0, 0.0, 0.0}; // in AU + double arvec[3]; + solarRad(A, m, sunvec, arvec); + + Eigen::Vector3d sun(sunvec[0], sunvec[1], sunvec[2]); + Eigen::Vector3d acc_cpp = OrbitalMotion::solarRadiationPressureAccel(A, m, sun); + + for (int i = 0; i < 3; ++i) { + EXPECT_NEAR(arvec[i], acc_cpp(i), tol); + } +} + +TEST_F(OrbitalMotionTest, Compare_elementsToCartesianState) { + ClassicalElements cpp_el; + cpp_el.semiMajorAxis = 7000; + cpp_el.eccentricity = 0.01; + cpp_el.inclination = 0.1; + cpp_el.rightAscensionAscendingNode = 0.5; + cpp_el.argPeriapsis = 0.4; + cpp_el.trueAnomaly = 1.0; + + ClassicElements c_el; + c_el.a = cpp_el.semiMajorAxis; + c_el.e = cpp_el.eccentricity; + c_el.i = cpp_el.inclination; + c_el.Omega = cpp_el.rightAscensionAscendingNode; + c_el.omega = cpp_el.argPeriapsis; + c_el.f = cpp_el.trueAnomaly; + + auto state_cpp = OrbitalMotion::elementsToCartesianState(398600.4418, cpp_el); + + double r_c[3], v_c[3]; + elem2rv(398600.4418, &c_el, r_c, v_c); + + for (int i = 0; i < 3; ++i) { + EXPECT_NEAR(r_c[i], state_cpp.position(i), tol); + EXPECT_NEAR(v_c[i], state_cpp.velocity(i), tol); + } +} + +TEST_F(OrbitalMotionTest, Compare_cartesianStateToElements) { + Eigen::Vector3d r_cpp(6524.834, 6862.875, 6448.296); + Eigen::Vector3d v_cpp(4.901327, 5.533756, -1.976341); + + double r_c[3] = {r_cpp(0), r_cpp(1), r_cpp(2)}; + double v_c[3] = {v_cpp(0), v_cpp(1), v_cpp(2)}; + + ClassicElements c_el; + ClassicalElements cpp_el = OrbitalMotion::cartesianStateToElements(398600.4418, r_cpp, v_cpp); + rv2elem(398600.4418, r_c, v_c, &c_el); + + EXPECT_NEAR(cpp_el.semiMajorAxis, c_el.a, tol); + EXPECT_NEAR(cpp_el.eccentricity, c_el.e, tol); + EXPECT_NEAR(cpp_el.inclination, c_el.i, tol); + EXPECT_NEAR(cpp_el.rightAscensionAscendingNode, c_el.Omega, tol); + EXPECT_NEAR(cpp_el.argPeriapsis, c_el.omega, tol); + EXPECT_NEAR(cpp_el.trueAnomaly, c_el.f, tol); + EXPECT_NEAR(cpp_el.radiusPeriapsis, c_el.rPeriap, tol); + EXPECT_NEAR(cpp_el.radiusApoapsis, c_el.rApoap, tol); +} + +TEST_F(OrbitalMotionTest, Compare_mapClassicalToEquinoctialElements) { + ClassicalElements cpp_el{}; + cpp_el.semiMajorAxis = 7000; + cpp_el.eccentricity = 0.05; + cpp_el.inclination = 0.1; + cpp_el.rightAscensionAscendingNode = 1.0; + cpp_el.argPeriapsis = 0.5; + cpp_el.trueAnomaly = 0.2; + + ClassicElements c_el{}; + c_el.a = cpp_el.semiMajorAxis; + c_el.e = cpp_el.eccentricity; + c_el.i = cpp_el.inclination; + c_el.Omega = cpp_el.rightAscensionAscendingNode; + c_el.omega = cpp_el.argPeriapsis; + c_el.f = cpp_el.trueAnomaly; + + equinoctialElements eq_c; + clElem2eqElem(&c_el, &eq_c); + + EquinoctialElements eq_cpp = OrbitalMotion::mapClassicalToEquinoctialElements(cpp_el); + + EXPECT_NEAR(eq_c.a, eq_cpp.a, tol); + EXPECT_NEAR(eq_c.P1, eq_cpp.P1, tol); + EXPECT_NEAR(eq_c.P2, eq_cpp.P2, tol); + EXPECT_NEAR(eq_c.Q1, eq_cpp.Q1, tol); + EXPECT_NEAR(eq_c.Q2, eq_cpp.Q2, tol); + EXPECT_NEAR(eq_c.l, eq_cpp.l, tol); + EXPECT_NEAR(eq_c.L, eq_cpp.L, tol); +} From 1d3bb72b8019750547f3f26031a863a7232117c5 Mon Sep 17 00:00:00 2001 From: tteil Date: Tue, 13 May 2025 20:20:49 -0600 Subject: [PATCH 3/5] add unit test to cmake list --- src/architecture/utilities/tests/CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/architecture/utilities/tests/CMakeLists.txt b/src/architecture/utilities/tests/CMakeLists.txt index 130c0f4c8..c10b42c4b 100644 --- a/src/architecture/utilities/tests/CMakeLists.txt +++ b/src/architecture/utilities/tests/CMakeLists.txt @@ -10,6 +10,10 @@ add_executable(test_orbitalMotion test_orbitalMotion.cpp) target_link_libraries(test_orbitalMotion GTest::gtest_main) target_link_libraries(test_orbitalMotion ArchitectureUtilities) +add_executable(test_compare_orbitalMotion test_compare_orbitalMotion.cpp) +target_link_libraries(test_compare_orbitalMotion GTest::gtest_main) +target_link_libraries(test_compare_orbitalMotion ArchitectureUtilities) + add_executable(test_saturate test_saturate.cpp) target_link_libraries(test_saturate GTest::gtest_main) target_link_libraries(test_saturate ArchitectureUtilities) @@ -41,6 +45,7 @@ endif() gtest_discover_tests(test_discretize) gtest_discover_tests(test_gaussMarkov) gtest_discover_tests(test_orbitalMotion) +gtest_discover_tests(test_compare_orbitalMotion) gtest_discover_tests(test_saturate) gtest_discover_tests(test_geodeticConversion) gtest_discover_tests(test_rigidBodyKinematics) From 01794f5d59dda776ec8e7729148e86f65e15bb7d Mon Sep 17 00:00:00 2001 From: tteil Date: Tue, 13 May 2025 20:20:55 -0600 Subject: [PATCH 4/5] wrap orbital motion cpp --- src/architecture/utilities/orbitalMotion.i | 39 ++++++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 src/architecture/utilities/orbitalMotion.i diff --git a/src/architecture/utilities/orbitalMotion.i b/src/architecture/utilities/orbitalMotion.i new file mode 100644 index 000000000..3a90faab5 --- /dev/null +++ b/src/architecture/utilities/orbitalMotion.i @@ -0,0 +1,39 @@ +/* + ISC License + + Copyright (c) 2025, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder + + Permission to use, copy, modify, and/or distribute this software for any + purpose with or without fee is hereby granted, provided that the above + copyright notice and this permission notice appear in all copies. + + THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR + ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN + ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF + OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + + */ + +%module orbitalMotion +%{ + #include "orbitalMotion.hpp" +%} + +%include "std_string.i" +%include "std_vector.i" +%include "swig_conly_data.i" +%include "swig_eigen.i" +%include "orbitalMotion.hpp" +EIGEN_MAT_WRAP(Vector6d, 170) + +%pythoncode %{ +from Basilisk.architecture.swig_common_model import * +%} + +%pythoncode %{ +import sys +protectAllClasses(sys.modules[__name__]) +%} From 23fe42464a6638ed314ac8d27bc659fbb96c504a Mon Sep 17 00:00:00 2001 From: tteil Date: Wed, 14 May 2025 22:25:55 -0600 Subject: [PATCH 5/5] add python unit test --- src/utilities/tests/test_orbital_motion.py | 111 +++++++++++++++++++++ 1 file changed, 111 insertions(+) create mode 100644 src/utilities/tests/test_orbital_motion.py diff --git a/src/utilities/tests/test_orbital_motion.py b/src/utilities/tests/test_orbital_motion.py new file mode 100644 index 000000000..579521f6f --- /dev/null +++ b/src/utilities/tests/test_orbital_motion.py @@ -0,0 +1,111 @@ +# ISC License +# +# Copyright (c) 2025, Laboratory for Atmospheric and Space Physics, University of Colorado at Boulder +# +# Permission to use, copy, modify, and/or distribute this software for any +# purpose with or without fee is hereby granted, provided that the above +# copyright notice and this permission notice appear in all copies. +# +# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES +# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF +# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR +# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES +# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN +# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF +# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + +from Basilisk.utilities import orbitalMotion as oe_py +from Basilisk.architecture.orbitalMotion import OrbitalMotion as oe_cpp +from Basilisk.architecture.orbitalMotion import ClassicalElements as elements_cpp +import numpy as np + + +def test_rv_oe(): + gravitational_parameter = 1e10 + position = np.array([1e5, 1e6, 1e2]) + velocity = np.array([10, 20, 30]) + + elements = oe_py.rv2elem(gravitational_parameter, position, velocity) + r, v = oe_py.elem2rv(gravitational_parameter, elements) + np.testing.assert_almost_equal(position, r, 8) + np.testing.assert_almost_equal(velocity, v, 8) + + gravitational_parameter = 1e10 + position = np.array([1e5, 1e6, 0]) + h = np.array([0.0, 0.0, 1.0]) + velocity = np.sqrt(gravitational_parameter / np.linalg.norm(position)) * np.cross( + h, position / np.linalg.norm(position) + ) + elements = oe_py.rv2elem(gravitational_parameter, position, velocity) + np.testing.assert_almost_equal(elements.a, np.linalg.norm(position), 8) + np.testing.assert_almost_equal(elements.e, 0, 8) + np.testing.assert_almost_equal(elements.i, 0, 8) + + +def test_compare_conversions(): + gravitational_parameter = 1e10 + position = np.array([1e5, 1e6, 1e2]) + velocity = np.array([10, 20, 30]) + + # state vector to orbital elements + elements_py = oe_py.rv2elem(gravitational_parameter, position, velocity) + elements_cpp = oe_cpp.cartesianStateToElements(gravitational_parameter, position, velocity) + np.testing.assert_almost_equal(elements_py.a, elements_cpp.semiMajorAxis, 8) + np.testing.assert_almost_equal(elements_py.e, elements_cpp.eccentricity, 8) + np.testing.assert_almost_equal(elements_py.i, elements_cpp.inclination, 8) + np.testing.assert_almost_equal(elements_py.Omega, elements_cpp.rightAscensionAscendingNode, 8) + np.testing.assert_almost_equal(elements_py.omega, elements_cpp.argPeriapsis, 8) + np.testing.assert_almost_equal(elements_py.f, elements_cpp.trueAnomaly, 8) + + # orbital elements to state vectors + r_py, v_py = oe_py.elem2rv(gravitational_parameter, elements_py) + state_cpp = oe_cpp.elementsToCartesianState(gravitational_parameter, elements_cpp) + np.testing.assert_almost_equal(r_py, np.array(state_cpp.position).flatten(), 8) + np.testing.assert_almost_equal(v_py, np.array(state_cpp.velocity).flatten(), 8) + + eccentric_anomaly = 0.5 + # eccentric to true anomaly and back + true_anomaly_py = oe_py.E2f(eccentric_anomaly, elements_py.e) + true_anomaly_cpp = oe_cpp.eccentricToTrueAnomaly(eccentric_anomaly, elements_py.e) + np.testing.assert_almost_equal(true_anomaly_py, true_anomaly_cpp, 8) + + eccentric_anomaly_py = oe_py.f2E(true_anomaly_py, elements_py.e) + eccentric_anomaly_cpp = oe_cpp.trueToEccentricAnomaly(true_anomaly_cpp, elements_py.e) + np.testing.assert_almost_equal(eccentric_anomaly, eccentric_anomaly_py, 8) + np.testing.assert_almost_equal(eccentric_anomaly, eccentric_anomaly_cpp, 8) + + # eccentric to mean anomaly + mean_anomaly_py = oe_py.E2M(eccentric_anomaly, elements_py.e) + mean_anomaly_cpp = oe_cpp.eccentricToMeanAnomaly(eccentric_anomaly, elements_py.e) + np.testing.assert_almost_equal(mean_anomaly_py, mean_anomaly_cpp, 8) + + eccentric_anomaly_py = oe_py.M2E(mean_anomaly_py, elements_py.e) + eccentric_anomaly_cpp = oe_cpp.meanToEccentricAnomaly(mean_anomaly_cpp, elements_py.e) + np.testing.assert_almost_equal(eccentric_anomaly_py, eccentric_anomaly_cpp, 8) + np.testing.assert_almost_equal(eccentric_anomaly_py, eccentric_anomaly, 8) + np.testing.assert_almost_equal(eccentric_anomaly_cpp, eccentric_anomaly, 8) + + # true to hyperbolic anomaly + e_hyperbolic = 1.2 + true_anomaly = 0.234 + hyper_anomaly_py = oe_py.f2H(true_anomaly, e_hyperbolic) + hyper_anomaly_cpp = oe_cpp.trueToHyperbolicAnomaly(true_anomaly, e_hyperbolic) + np.testing.assert_almost_equal(hyper_anomaly_py, hyper_anomaly_cpp, 8) + + true_anomaly_py = oe_py.H2f(hyper_anomaly_py, e_hyperbolic) + true_anomaly_cpp = oe_cpp.hyperbolicToTrueAnomaly(hyper_anomaly_cpp, e_hyperbolic) + np.testing.assert_almost_equal(true_anomaly_py, true_anomaly_cpp, 8) + np.testing.assert_almost_equal(true_anomaly_py, true_anomaly, 8) + np.testing.assert_almost_equal(true_anomaly_cpp, true_anomaly, 8) + + # true to mean anomaly + true_anomaly = 0.234 + mean_anomaly_py = oe_py.E2M(oe_py.f2E(true_anomaly, elements_py.e), elements_py.e) + mean_anomaly_cpp = oe_cpp.trueToMeanAnomaly(true_anomaly, elements_py.e) + np.testing.assert_almost_equal(mean_anomaly_py, mean_anomaly_cpp, 8) + + true_anomaly_py = oe_py.E2f(oe_py.M2E(mean_anomaly_py, elements_py.e), elements_py.e) + true_anomaly_cpp = oe_cpp.meanToTrueAnomaly(mean_anomaly_cpp, elements_py.e) + np.testing.assert_almost_equal(true_anomaly_py, true_anomaly_cpp, 8) + np.testing.assert_almost_equal(true_anomaly_py, true_anomaly, 8) + np.testing.assert_almost_equal(true_anomaly_cpp, true_anomaly, 8)