From 0a2e01909d1b07f89085ab11eeac5714dd6c516b Mon Sep 17 00:00:00 2001 From: cinnaboot Date: Tue, 21 Jun 2022 12:15:10 -0400 Subject: [PATCH] add functions to convert orbital elements to state vectors --- src/main.cpp | 12 ++++++---- src/orbits.cpp | 47 ++++++++++++++++++++++++++++++++++-- src/orbits.h | 14 ++++++++++- tests/orbit_test.cpp | 57 +++++++++++++++++++++++++++++++++++++++++--- 4 files changed, 120 insertions(+), 10 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index a306f9a..1f0acdd 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -202,18 +202,22 @@ loadScene(GameState* gs, RenderState* rs) RenderGroup* rg = getFreeRenderGroup(rs); initRenderGroup(rg, rs->rg_arena, shader, 256, "manual mesh group"); +#if 0 double a = 26564.5; // semi-major axis in km double e = 0.7411; // eccentricity GameOrbit* go_1 = loadOrbit(gs, rs, body, orbitInit(a, e), rg, "sat_01"); - addManeuver(go_1, ImpulseType::PROGRADE, M_PI / 4, 0.2); + addManeuver(go_1, ImpulseType::PROGRADE, -2.f, 0.2); +#endif - double a_2 = r + 10000; - double e_2 = 0; - loadOrbit(gs, rs, body, orbitInit(a_2, e_2), rg, "sat_02"); + OrbitalElements oe_2 = orbitInit(r + 100, 0, 0, 0, 0, 0); + GameOrbit* go_2 = loadOrbit(gs, rs, body, oe_2, rg, "sat_02"); + addManeuver(go_2, ImpulseType::PROGRADE, M_PI / 2, 2.2); +#if 0 double a_3 = r + 40000; double e_3 = 0.5; loadOrbit(gs, rs, body, orbitInit(a_3, e_3), rg, "sat_03"); +#endif return true; } diff --git a/src/orbits.cpp b/src/orbits.cpp index 05c7de0..ddfc8b9 100644 --- a/src/orbits.cpp +++ b/src/orbits.cpp @@ -51,16 +51,59 @@ ellipseInitAE(double a, double e) } OrbitalElements -orbitInit(double a, double e) +orbitInit(double a, double e, double iota, double ohm, double omega, double nu) { // TODO: remaining elements: iota, ohm, omega, nu OrbitalElements o = {0}; o.a = a; o.e = e; - o.nu = 0; + o.iota = iota; + o.ohm = ohm; + o.omega = omega; + o.nu = nu; return o; } +glm::dvec3 +orbitGetEccentricityVector(glm::dvec3 r, glm::dvec3 v, double mu) +{ + double v_mag = glm::dot(v, v); + double r_mag = glm::dot(r, r); + return 1 / mu * ((v_mag - mu / r_mag) * r - (glm::dot(r, v) * v)); +} + +// NOTE: returns position vector in perifocal plane +glm::dvec3 +orbitGetPositionVector(double r, double theta) +{ + return glm::dvec3(r * cos(theta), r * sin(theta), 0); +} + +// NOTE: returns velocity vector in perifocal plane +glm::dvec3 +orbitGetVelocityVector(double mu, double h, double e, double theta) +{ + return glm::dvec3(-1 * (mu / h) * sin(theta), mu / h * (e + cos(theta)), 0); +} + +// NOTE: return transform from perifocal to grav body (IJK) coordinate system +// in column major format +glm::dmat3 +orbitGetXForm(OrbitalElements elements) +{ + const OrbitalElements& el = elements; + glm::mat3 M(1.0); + M[0][0] = cos(el.ohm) * cos(el.omega) - sin(el.ohm) * sin(el.omega) * cos(el.iota); + M[1][0] = -cos(el.ohm) * sin(el.omega) - sin(el.ohm) * cos(el.omega) * cos(el.iota); + M[2][0] = sin(el.ohm) * sin(el.iota); + M[0][1] = sin(el.ohm) * cos(el.omega) + cos(el.ohm) * sin(el.omega) * cos(el.iota); + M[1][1] = -sin(el.ohm) * sin(el.omega) + cos(el.ohm) * cos(el.omega) * cos(el.iota); + M[2][1] = -cos(el.ohm) * sin(el.iota); + M[0][2] = sin(el.omega) * sin(el.iota); + M[1][2] = cos(el.omega) * sin(el.iota); + M[2][2] = cos(el.iota); + return M; +} // // NOTE: propagate anomaly functions: diff --git a/src/orbits.h b/src/orbits.h index 54c74e5..1adbb1c 100644 --- a/src/orbits.h +++ b/src/orbits.h @@ -3,11 +3,15 @@ #include +#define GLM_FORCE_XYZW_ONLY #include #include "util.h" +#define DEG2RAD(x) x * M_PI / 180 + + struct EllipseParameters { double a; // semi-major axis @@ -102,7 +106,15 @@ ellipsesEqual(EllipseParameters& e1, EllipseParameters& e2) } OrbitalElements -orbitInit(double a, double e); +orbitInit(double a, double e, double iota, double ohm, double omega, double nu); + +glm::dvec3 orbitGetEccentricityVector(glm::dvec3 r, glm::dvec3 v, double mu); + +glm::dvec3 orbitGetPositionVector(double r, double theta); + +glm::dvec3 orbitGetVelocityVector(double mu, double h, double e, double theta); + +glm::dmat3 orbitGetXForm(OrbitalElements elements); inline double orbitGetAngularMomentum(double p, double mu) diff --git a/tests/orbit_test.cpp b/tests/orbit_test.cpp index 01bdc69..bd99ccd 100644 --- a/tests/orbit_test.cpp +++ b/tests/orbit_test.cpp @@ -1,5 +1,6 @@ #include +#define GLM_FORCE_XYZW_ONLY #include #include "../src/orbits.cpp" @@ -42,6 +43,56 @@ TEST_CASE("orbit determination, example 2.1", "[orbits]") REQUIRE_THAT(e, WithinAbs(0.4024, 1e-4)); } +TEST_CASE("orbital elements to state vectors, example 3.2", "[orbits]") +{ + double a = MOLNIYA_SEMI_MAJOR_AXIS; + double e = MOLNIYA_ECCENTRICITY; + double mu = EARTH_GRAVITATIONAL_PARAMETER; + double r = EARTH_RADIUS; + TwoBodySystem sys = {0}; + systemInit(sys, gravBodyInit(mu, r), orbitInit(a, e, DEG2RAD(63.4), DEG2RAD(200), DEG2RAD(-90), DEG2RAD(30))); + // FIXME: should be initialized in systemInit() + sys.sat.theta = DEG2RAD(30); + + REQUIRE_THAT(sys.ep.p, WithinAbs(11974.3, 0.5)); + REQUIRE_THAT(sys.h, WithinAbs(69086.5, 1.0)); + + sys.sat.r = orbitGetRadialDistance(sys.ep.e, sys.ep.p, sys.sat.theta); + REQUIRE_THAT(sys.sat.r, WithinAbs(7293.3, 0.5)); + + // create state vectors in perifocal frame + glm::dvec3 pos = orbitGetPositionVector(sys.sat.r, sys.sat.theta); + REQUIRE_THAT(pos.x, WithinAbs(6316.21, 0.2)); + REQUIRE_THAT(pos.y, WithinAbs(3646.67, 0.2)); + + glm::dvec3 vel = orbitGetVelocityVector(sys.body.mu, sys.h, sys.ep.e, sys.sat.theta); + REQUIRE_THAT(vel.x, WithinAbs(-2.8848, 1e-4)); + REQUIRE_THAT(vel.y, WithinAbs( 9.2724, 1e-4)); + + // create rotation matrix + glm::dmat3 M = orbitGetXForm(sys.elements); + REQUIRE_THAT(M[0][0], WithinAbs(-0.1531, 1e-4)); + REQUIRE_THAT(M[1][0], WithinAbs(-0.9397, 1e-4)); + REQUIRE_THAT(M[2][0], WithinAbs(-0.3058, 1e-4)); + REQUIRE_THAT(M[0][1], WithinAbs( 0.4208, 1e-4)); + REQUIRE_THAT(M[1][1], WithinAbs(-0.3420, 1e-4)); + REQUIRE_THAT(M[2][1], WithinAbs( 0.8402, 1e-4)); + REQUIRE_THAT(M[0][2], WithinAbs(-0.8942, 1e-4)); + REQUIRE_THAT(M[1][2], WithinAbs( 0.0000, 1e-4)); + REQUIRE_THAT(M[2][2], WithinAbs( 0.4478, 1e-4)); + + // rotate perifocal state vectors to IJK coordinates + glm::vec3 r_pos = M * pos; + REQUIRE_THAT(r_pos.x, WithinAbs(-4394.0, 0.2)); + REQUIRE_THAT(r_pos.y, WithinAbs( 1410.3, 0.1)); + REQUIRE_THAT(r_pos.z, WithinAbs(-5647.7, 0.1)); + + glm::vec3 r_vel = M * vel; + REQUIRE_THAT(r_vel.x, WithinAbs(-8.2715, 0.1)); + REQUIRE_THAT(r_vel.y, WithinAbs(-4.3852, 0.1)); + REQUIRE_THAT(r_vel.z, WithinAbs( 2.5794, 0.1)); +} + TEST_CASE("orbit propagation, example 4.6", "[orbits]") { double a = MOLNIYA_SEMI_MAJOR_AXIS; @@ -51,7 +102,7 @@ TEST_CASE("orbit propagation, example 4.6", "[orbits]") double initial_anom = 260 * M_PI / 180; // NOTE: radians double time_step = 60 * 50; // NOTE: seconds TwoBodySystem sys = {0}; - systemInit(sys, gravBodyInit(mu, r), orbitInit(a, e)); + systemInit(sys, gravBodyInit(mu, r), orbitInit(a, e, 0, 0, 0, 0)); double E1 = getEccAnomFromTrueAnom(sys.ep.e, initial_anom); REQUIRE_THAT(E1, WithinAbs(-0.8615, 1e-4)); @@ -100,7 +151,7 @@ TEST_CASE("time of flight example 4.1a", "[orbits]") double mu = EARTH_GRAVITATIONAL_PARAMETER; double r = EARTH_RADIUS; TwoBodySystem sys = {0}; - systemInit(sys, gravBodyInit(mu, r), orbitInit(a, e)); + systemInit(sys, gravBodyInit(mu, r), orbitInit(a, e, 0, 0, 0, 0)); // NOTE: get ToF from periapsis to true anomaly at 154.85 degrees double theta_0 = 0.0; @@ -126,7 +177,7 @@ TEST_CASE("time of flight example 4.2", "[orbits]") double mu = EARTH_GRAVITATIONAL_PARAMETER; double r = EARTH_RADIUS; TwoBodySystem sys = {0}; - systemInit(sys, gravBodyInit(mu, r), orbitInit(a, e)); + systemInit(sys, gravBodyInit(mu, r), orbitInit(a, e, 0, 0, 0, 0)); // NOTE: get ToF from true anom 230 degrees to true anom at 120 degrees double theta_1 = 230 * M_PI / 180;