add functions to convert orbital elements to state vectors

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;
 }

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:

src/orbits.h

@@ -3,11 +3,15 @@
 
 #include <cmath>
 
+#define GLM_FORCE_XYZW_ONLY
 #include <glm/glm.hpp>
 
 #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)

tests/orbit_test.cpp

@@ -1,5 +1,6 @@
 
 #include <catch2/catch.hpp>
+#define GLM_FORCE_XYZW_ONLY
 #include <glm/glm.hpp>
 
 #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;