orbital_shipping/tests/orbit_test.cpp

#include <catch2/catch.hpp>
#define GLM_FORCE_XYZW_ONLY
#include <glm/glm.hpp>
using glm::dvec3;

#include "../src/orbits.cpp"

// NOTE: using WithinAbs instead of Approx because:
// 	https://github.com/catchorg/Catch2/issues/1962
// 	https://github.com/catchorg/Catch2/issues/1507
using Catch::Matchers::WithinAbs;


// NOTE: all examples from the book "Space Flight Dynamics" by Craig A. Kluever

const double MOLNIYA_SEMI_MAJOR_AXIS = 26564.5;				// km
const double MOLNIYA_ECCENTRICITY = 0.7411;
const double EARTH_GRAVITATIONAL_PARAMETER = 398601.68;		// km^3 / s^2
const double EARTH_RADIUS = 6378;							// km


TEST_CASE("orbit determination, example 2.1", "[orbits]")
{
	double r = 2124 + EARTH_RADIUS; // km
	double v = 7.58;				// km/s
	double gamma = 20 * M_PI / 180;	// flight path angle
	double mu = EARTH_GRAVITATIONAL_PARAMETER;

	double epsilon = orbitGetSpecificEnergyFromStateVectors(r, v, mu);
	REQUIRE_THAT(epsilon, WithinAbs(-18.1549, 2e-4));

	double h = orbitGetAngularMomentumFromStateVectors(r, v, gamma);
	REQUIRE_THAT(h, WithinAbs(60558.64, 1e-2));

	// NOTE: we're off by an order of magnitude here compared to the inputs...
	double a = orbitGetSemiMajorAxis(epsilon, mu);
	REQUIRE_THAT(a, WithinAbs(10977.76, 5e-1));

	double p = orbitGetSemiLatusRectum(h, mu);
	REQUIRE_THAT(p, WithinAbs(9200.57, 5e-2));

	double e = ellipseGetEccentricity(a, p);
	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));

	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]")
{
	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::dvec3 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::dvec3 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;
	double e = MOLNIYA_ECCENTRICITY;
	double mu = EARTH_GRAVITATIONAL_PARAMETER;
	double r = EARTH_RADIUS;
	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, 0, 0, 0, 0));

	double E1 = getEccAnomFromTrueAnom(sys.ep.e, initial_anom);
	REQUIRE_THAT(E1, WithinAbs(-0.8615, 1e-4));

	double M1 = getMeanAnomFromEccAnom(E1, sys.ep.e);
	REQUIRE_THAT(M1, WithinAbs(-0.2992, 1e-4));

	double n = getMeanMotion(sys.body.mu, sys.ep.a);
	REQUIRE_THAT(n, WithinAbs(0.00014582, 1e-8));

	double M2 = getPropagatedMeanAnom(M1, n, time_step);
	REQUIRE_THAT(M2, WithinAbs(0.1383, 1e-5));

	// TODO: could also test for the other trial values listed in table 4.1
	double E2_1 = getInitialTrialValue(M2, sys.ep.e);
	REQUIRE_THAT(E2_1, WithinAbs(0.315452, 1e-5));

	double ecc_anom = getPropagatedEccAnomaly(sys, initial_anom, time_step);
	REQUIRE_THAT(ecc_anom, WithinAbs(0.481518, 1e-5));

	double true_anom =
		orbitGetPropagatedTrueAnomaly(sys, initial_anom, time_step);
	REQUIRE_THAT(true_anom, WithinAbs(1.1339, 1e-4));

	double r2 = orbitGetRadialDistance(sys.ep.e, sys.ep.p, true_anom);
	REQUIRE_THAT(r2, WithinAbs(9116.1, 0.1));

	glm::vec2 pos = polarToRect(true_anom, r2);
	REQUIRE_THAT(pos.x, WithinAbs(3856.9, 0.1));
	REQUIRE_THAT(pos.y, WithinAbs(8259.9, 0.1));
}

TEST_CASE("orbital period, example 2.5c", "[orbits]")
{
	double a = 24371;		// semi-major axis in km
	double mu = EARTH_GRAVITATIONAL_PARAMETER;

	double T = orbitGetPeriod(a, mu);
	REQUIRE_THAT(T, WithinAbs(37863.5, 50e-3));
}

TEST_CASE("time of flight example 4.1a", "[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, 0, 0, 0, 0));

	// NOTE: get ToF from periapsis to true anomaly at 154.85 degrees
	double theta_0 = 0.0;
	double theta_1 = 154.85 * M_PI / 180;

	double n = getMeanMotion(sys.body.mu, sys.ep.a);
	REQUIRE_THAT(n, WithinAbs(0.00014582, 1e-8));

	double ecc_1 = getEccAnomFromTrueAnom(sys.ep.e, theta_1);
	REQUIRE_THAT(ecc_1, WithinAbs(2.0927, 1e-4));

	// NOTE: we really want < 33ms accuracy here, but either there's floating
	// 	point error, or the examples in the book are incorrect or just not
	// 	precise enough
	double tof = orbitGetTimeOfFlight(sys, theta_0, theta_1);
	REQUIRE_THAT(tof, WithinAbs(9945.2, 0.5));
}

TEST_CASE("time of flight example 4.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, 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;
	double theta_2 = 120 * M_PI / 180;

	double n = getMeanMotion(mu, a);
	REQUIRE_THAT(n, WithinAbs(0.00014582, 1e-8));

	double ecc_1 = getEccAnomFromTrueAnom(e, theta_1);
	REQUIRE_THAT(orbitClampAngle(ecc_1), WithinAbs(4.9012, 1e-4));

	double M1 = getMeanAnomFromEccAnom(ecc_1, e);
	REQUIRE_THAT(orbitClampAngle(M1), WithinAbs(5.6291, 1e-4));

	double ecc_2 = getEccAnomFromTrueAnom(e, theta_2);
	REQUIRE_THAT(orbitClampAngle(ecc_2), WithinAbs(1.1778, 1e-4));

	// NOTE: see note in example 4.1 above about accuracy
	double tof = orbitGetTimeOfFlight(sys, theta_1, theta_2);
	REQUIRE_THAT(tof, WithinAbs(7867.5, 2 * 0.5));
}