add test case for vectors to orbital elements

tests/orbit_test.cpp

@@ -2,6 +2,7 @@
 #include <catch2/catch.hpp>
 #define GLM_FORCE_XYZW_ONLY
 #include <glm/glm.hpp>
+using glm::dvec3;
 
 #include "../src/orbits.cpp"
 
@@ -43,6 +44,80 @@ TEST_CASE("orbit determination, example 2.1", "[orbits]")
 	REQUIRE_THAT(e, WithinAbs(0.4024, 1e-4));
 }
 
+TEST_CASE("state vectors to orbital elements, example 3.1", "[orbits]")
+{
+	double mu = EARTH_GRAVITATIONAL_PARAMETER;
+	dvec3 r = dvec3(9031.5, -5316.9, -1647.2);
+	dvec3 v = dvec3(-2.8640, 5.1112, -5.0805);
+
+	double r_mag = orbitGetVectorMagnitude(r);
+	REQUIRE_THAT(r_mag, WithinAbs(10609, 1));
+
+	double v_mag = orbitGetVectorMagnitude(v);
+	REQUIRE_THAT(v_mag, WithinAbs(7.7549, 1e-4));
+
+	double epsilon = orbitGetSpecificEnergyFromStateVectors(r_mag, v_mag, mu);
+	REQUIRE_THAT(epsilon, WithinAbs(-7.5027, 5e-4));
+
+	double a = orbitGetSemiMajorAxis(epsilon, mu);
+	REQUIRE_THAT(a, WithinAbs(26563.6, 0.5));
+
+	dvec3 ecc_v = orbitGetEccentricityVector(r, v, mu);
+	REQUIRE_THAT(ecc_v.x, WithinAbs(0.1903, 1e-4));
+	REQUIRE_THAT(ecc_v.y, WithinAbs(0.2718, 1e-4));
+	REQUIRE_THAT(ecc_v.z, WithinAbs(-0.6627, 1e-4));
+
+	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));
+	//=========================================================================
+}
+
 TEST_CASE("orbital elements to state vectors, example 3.2", "[orbits]")
 {
 	double a = MOLNIYA_SEMI_MAJOR_AXIS;