From a3dfae36c4b5868212ba17db9886f4b1fda32648 Mon Sep 17 00:00:00 2001 From: cinnaboot Date: Tue, 12 Jul 2022 09:08:57 -0400 Subject: [PATCH] add function to convert orbital elements to state vectors --- src/game.cpp | 46 ++++++++++---------------------------- src/orbits.cpp | 49 ++++++++++++++++++++++++++++++++++++++++ src/orbits.h | 7 ++++-- tests/orbit_test.cpp | 53 ++++++-------------------------------------- 4 files changed, 73 insertions(+), 82 deletions(-) diff --git a/src/game.cpp b/src/game.cpp index 90974c6..fbd2292 100644 --- a/src/game.cpp +++ b/src/game.cpp @@ -223,16 +223,6 @@ applyManeuver(GameOrbit* orbit, double previous_true_anomaly) const ManeuverNode& node = orbit->maneuver; TwoBodySystem& sys = orbit->system; - LOGF(Debug, "applying maneuver,\n" - "theta: %f\n," - "dv: %f,\n" - "sat.theta: %f,\n" - "previous theta: %f\n", - node.true_anomaly, - node.impulse_delta_v, - sys.sat.theta, - previous_true_anomaly); - // FIXME: assuming prograde impulse vector assert(node.impulse_type == ImpulseType::PROGRADE); @@ -240,35 +230,22 @@ applyManeuver(GameOrbit* orbit, double previous_true_anomaly) double theta = node.true_anomaly; double mu = sys.body.mu; double r = orbitGetRadialDistance(sys.ep.e, sys.ep.p, theta); - double v = orbitGetVelocity(sys.epsilon, mu, r); - double gamma = orbitGetFlightPathAngle(sys.ep.e, theta); - - // calculate new satellite state vectors - v += node.impulse_delta_v; // prograde only - - // get new angular momentum and specific orbital energy - double h = orbitGetAngularMomentumFromStateVectors(r, v, gamma); - double epsilon = orbitGetSpecificEnergyFromStateVectors(r, v, mu); - - // update orbit pararmeters - sys.elements.a = orbitGetSemiMajorAxis(epsilon, mu); - double p = orbitGetSemiLatusRectum(h, mu); - sys.elements.e = ellipseGetEccentricity(sys.elements.a, p); dvec3 pos = orbitGetPositionVector(r, theta); - dvec3 vel = orbitGetVelocityVector(mu, h, sys.elements.e, theta); - - // FIXME: need to count rotation from the 'I' axis on equatorial orbits - // WIP - // also need to get the updated eccentricity vector before we can calculate - // the other angular orbital elements - //sys.elements.omega = sys.elements.nu; - // WIP + dvec3 vel = orbitGetVelocityVector(mu, sys.h, sys.elements.e, theta); sys.rotation = orbitGetXForm(sys.elements); - sys.sat.position = sys.rotation * pos; - sys.sat.velocity = sys.rotation * vel; + pos = sys.rotation * pos; + vel = sys.rotation * vel; + + // apply impulse along velocity vector + dvec3 impulse = glm::normalize(vel) * node.impulse_delta_v; + vel = vel + impulse; + sys.elements = orbitGetElementsFromStateVectors(pos, vel, mu); systemInit(orbit->system, sys.body, sys.elements); + // update satellite true anamoly + sys.sat.theta = sys.elements.nu; + // update ellipse3D & GLBuffer vertices ellipse3DUpdate(sys.rotation, sys.ep, orbit->e3d); GLBuffer* buf = &orbit->ellipse_entity->meshes[0].vertex_attrib_buffers[0]; @@ -277,6 +254,7 @@ applyManeuver(GameOrbit* orbit, double previous_true_anomaly) updateGLBuffer(buf, orbit->e3d.vertices); } + // internal void diff --git a/src/orbits.cpp b/src/orbits.cpp index c3703b5..8f3e307 100644 --- a/src/orbits.cpp +++ b/src/orbits.cpp @@ -65,6 +65,55 @@ orbitInit(double a, double e, double iota, double ohm, double omega, double nu) return o; } +OrbitalElements +orbitGetElementsFromStateVectors(glm::dvec3 r, glm::dvec3 v, double mu) +{ + OrbitalElements el = {0}; + const glm::dvec3 I = glm::dvec3(1, 0, 0); + const glm::dvec3 J = glm::dvec3(0, 1, 0); + const glm::dvec3 K = glm::dvec3(0, 0, 1); + double r_mag = orbitGetVectorMagnitude(r); + double v_mag = orbitGetVectorMagnitude(v); + double epsilon = orbitGetSpecificEnergyFromStateVectors(r_mag, v_mag, mu); + glm::dvec3 ecc_v = orbitGetEccentricityVector(r, v, mu); + + el.a = orbitGetSemiMajorAxis(epsilon, mu); + el.e = fabs(orbitGetVectorMagnitude(ecc_v)); + + glm::dvec3 h = glm::cross(r, v); + double cosi = glm::dot(K, h) / orbitGetVectorMagnitude(h); + el.iota = acos(cosi); + + if (el.iota == 0) { // prograde equatorial orbit + el.ohm = 0; + double i_dot_e = glm::dot(I, ecc_v); + el.omega = acos(i_dot_e / el.e); + if (ecc_v.y < 0) el.omega *= -1; // quadrant check + } else if (el.iota == M_PI) { // retrograde equatorial orbit + // FIXME: retrograde equatorial orbit case + assert(0); + } else { + glm::dvec3 n = glm::cross(K, h); // ascending node vector + double n_mag = orbitGetVectorMagnitude(n); + double cos_ohm = glm::dot(I, n) / n_mag; + double sin_ohm = glm::dot(J, n) / n_mag; + el.ohm = atan2(sin_ohm, cos_ohm); + + double cos_omega = glm::dot(n, ecc_v) / (n_mag * el.e); + el.omega = acos(cos_omega); + if (ecc_v.z < 0) el.omega = 2 * M_PI - el.omega; // quadrant check + } + + // FIXME: el.e == 0 would be undefined here + double cos_theta = glm::dot(r, ecc_v) / (el.e * r_mag); + el.nu= acos(cos_theta); + // FIXME: breaks test case + //if (glm::dot(r, v) < 0) el.nu *= -1; //quadrant check + if (glm::dot(r, v) < 0) el.nu = 2 * M_PI - el.nu; //quadrant check + + return el; +} + glm::dvec3 orbitGetEccentricityVector(glm::dvec3 r, glm::dvec3 v, double mu) { diff --git a/src/orbits.h b/src/orbits.h index fe63ba2..5c12de8 100644 --- a/src/orbits.h +++ b/src/orbits.h @@ -43,8 +43,8 @@ struct GravBody struct Satellite { - glm::vec3 position; - glm::vec3 velocity; + glm::dvec3 position; + glm::dvec3 velocity; double theta; // true anomaly double r; // radius magnitude at theta double gamma; // (γ) flight path angle @@ -109,6 +109,9 @@ ellipsesEqual(EllipseParameters& e1, EllipseParameters& e2) OrbitalElements orbitInit(double a, double e, double iota, double ohm, double omega, double nu); +OrbitalElements +orbitGetElementsFromStateVectors(glm::dvec3 r, glm::dvec3 v, double mu); + glm::dvec3 orbitGetEccentricityVector(glm::dvec3 r, glm::dvec3 v, double mu); // NOTE: returns position vector in perifocal plane diff --git a/tests/orbit_test.cpp b/tests/orbit_test.cpp index f38c969..5207ed0 100644 --- a/tests/orbit_test.cpp +++ b/tests/orbit_test.cpp @@ -70,52 +70,13 @@ TEST_CASE("state vectors to orbital elements, example 3.1", "[orbits]") double e = fabs(orbitGetVectorMagnitude(ecc_v)); REQUIRE_THAT(e, WithinAbs(0.7411, 1e-4)); - //========================================================================= - // FIXME: need interface functions for these equations - dvec3 h = glm::cross(r, v); - REQUIRE_THAT(h.x, WithinAbs(35432, 1)); - REQUIRE_THAT(h.y, WithinAbs(50602, 1)); - REQUIRE_THAT(h.z, WithinAbs(30934, 1)); - - dvec3 I = dvec3(1, 0, 0); - dvec3 J = dvec3(0, 1, 0); - dvec3 K = dvec3(0, 0, 1); - double cosi = glm::dot(K, h) / orbitGetVectorMagnitude(h); - REQUIRE_THAT(cosi, WithinAbs(0.4478, 1e-4)); - double i = acos(cosi); - REQUIRE_THAT(i, WithinAbs(DEG2RAD(63.4), 1e-4)); - - dvec3 n = glm::cross(K, h); // NOTE: ascending node vector - REQUIRE_THAT(n.x, WithinAbs(-50602, 1)); - REQUIRE_THAT(n.y, WithinAbs( 35432, 1)); - REQUIRE_THAT(n.z, WithinAbs( 0, 1)); - - double n_mag = orbitGetVectorMagnitude(n); - REQUIRE_THAT(n_mag, WithinAbs(61774, 1)); - - double cos_ohm = glm::dot(I, n) / n_mag; - REQUIRE_THAT(cos_ohm, WithinAbs(-0.8192, 1e-4)); - - double sin_ohm = glm::dot(J, n) / n_mag; - REQUIRE_THAT(sin_ohm, WithinAbs(0.5736, 1e-4)); - - double ohm = atan2(sin_ohm, cos_ohm); - REQUIRE_THAT(ohm, WithinAbs(DEG2RAD(145), 1e-4)); - - double cos_omega = glm::dot(n, ecc_v) / (n_mag * e); - REQUIRE_THAT(cos_omega, WithinAbs(10e-5, 5e-4)); - - double omega = acos(cos_omega); - if (ecc_v.z < 0) omega = 2 * M_PI - omega; // quadrant check - REQUIRE_THAT(omega, WithinAbs(DEG2RAD(270), 1e-4)); - - double cos_theta = glm::dot(r, ecc_v) / (e * r_mag); - REQUIRE_THAT(cos_theta, WithinAbs(0.1736, 1e-4)); - - double theta = acos(cos_theta); - if (glm::dot(r, v) < 0) theta = 2 * M_PI - theta; // quadrant check - REQUIRE_THAT(theta, WithinAbs(DEG2RAD(280), 1)); - //========================================================================= + OrbitalElements el = orbitGetElementsFromStateVectors(r, v, mu); + REQUIRE_THAT(el.a, WithinAbs(26563.6, 0.5)); + REQUIRE_THAT(el.e, WithinAbs(0.7411, 1e-4)); + REQUIRE_THAT(el.iota, WithinAbs(DEG2RAD(63.4), 1e-4)); + REQUIRE_THAT(el.ohm, WithinAbs(DEG2RAD(145), 1e-4)); + REQUIRE_THAT(el.omega, WithinAbs(DEG2RAD(270), 1e-4)); + REQUIRE_THAT(el.nu, WithinAbs(orbitClampAngle(DEG2RAD(280)), 1)); } TEST_CASE("orbital elements to state vectors, example 3.2", "[orbits]")