add function to convert orbital elements to state vectors

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

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)
 {

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

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]")