From afbdbb16de1da3ef21a8d3e27129079d653eaa4b Mon Sep 17 00:00:00 2001
From: cinnaboot <cinnaboot@protonmail.com>
Date: Tue, 19 Jul 2022 10:24:42 -0400
Subject: [PATCH] add orbitGetStateVectorsFromElements()

---
 src/orbits.cpp | 28 ++++++++++++++++++++++++++++
 src/orbits.h   |  9 +++++++++
 2 files changed, 37 insertions(+)

diff --git a/src/orbits.cpp b/src/orbits.cpp
index feb7401..5d55397 100644
--- a/src/orbits.cpp
+++ b/src/orbits.cpp
@@ -118,6 +118,34 @@ orbitGetElementsFromStateVectors(glm::dvec3 r, glm::dvec3 v, double mu)
 	return el;
 }
 
+StateVectors
+orbitGetStateVectorsFromElements(const OrbitalElements& el, double mu)
+{
+	double p = 0;
+
+	// FIXME: need a helper here
+	if (el.e < 1) { // ellipse or circle
+		p = el.a * (1 - pow(el.e, 2));
+	} else if (el.e == 1) { // parabola
+		assert(0);
+	} else { // hyperbola
+		assert(0);
+	}
+
+	// FIXME: we need to ensure that we update el.nu instead of sat.theta
+	double r = orbitGetRadialDistance(el.e, p, el.nu);
+	double h = orbitGetAngularMomentum(p, mu);
+	StateVectors sv;
+	// TODO: getPos/getVel don't need to be interface functions
+	sv.position = orbitGetPositionVector(r, el.nu);
+	sv.velocity = orbitGetVelocityVector(mu, h, el.e, el.nu);
+	glm::dmat3 M = orbitGetXForm(el);
+	sv.position = M * sv.position;
+	sv.velocity = M * sv.velocity;
+
+	return sv;
+}
+
 glm::dvec3
 orbitGetEccentricityVector(glm::dvec3 r, glm::dvec3 v, double mu)
 {
diff --git a/src/orbits.h b/src/orbits.h
index 5c12de8..15625b0 100644
--- a/src/orbits.h
+++ b/src/orbits.h
@@ -51,6 +51,12 @@ struct Satellite
 	double v;			// velocity magnitute
 };
 
+struct StateVectors
+{
+	glm::dvec3 position;
+	glm::dvec3 velocity;
+};
+
 // NOTE: top level composite for 2 body system
 struct TwoBodySystem
 {
@@ -112,6 +118,9 @@ orbitInit(double a, double e, double iota, double ohm, double omega, double nu);
 OrbitalElements
 orbitGetElementsFromStateVectors(glm::dvec3 r, glm::dvec3 v, double mu);
 
+StateVectors
+orbitGetStateVectorsFromElements(const OrbitalElements& el, double mu);
+
 glm::dvec3 orbitGetEccentricityVector(glm::dvec3 r, glm::dvec3 v, double mu);
 
 // NOTE: returns position vector in perifocal plane