Skip to content

Instantly share code, notes, and snippets.

@triffid
Created May 28, 2021 07:42
Show Gist options
  • Save triffid/166b8f5597ce52bcafd15a18218b066f to your computer and use it in GitHub Desktop.
Save triffid/166b8f5597ce52bcafd15a18218b066f to your computer and use it in GitHub Desktop.
orbelems
/*
* This is some code that converts cartesian coordinate state vectors to
* classical orbital elements, and back again.
*/
#include <stdio.h>
#include <math.h>
double Em11 = 0.00000000001;
double G = 6.67300 * Em11;
double E24 = 1000000000000000000000000.0;
double E24m11 = 10000000000000.0;
double km = 1000.0;
class Body
{
protected:
double GM;
double r;
public:
Body(double GM, double r) : GM(GM), r(r) { }
double mass() { return GM / G; }
double gm() { return GM; }
double radius() { return r; }
};
class Vector3
{
public:
double x, y, z;
Vector3() : x(0), y(0), z(0) { }
Vector3(double x, double y, double z) : x(x), y(y), z(z) { }
double sqlen() { return x*x + y*y + z*z; }
double len() { return sqrt(sqlen()); }
double dot(Vector3 o)
{
return x*o.x + y*o.y + z*o.z;
}
Vector3 operator*(Vector3 o)
{
return Vector3(y*o.z - z*o.y, z*o.x - x*o.z, x*o.y - y*o.x);
}
Vector3 operator*(double s)
{
return Vector3(s*x, s*y, s*z);
}
Vector3 operator-(double s)
{
return Vector3(x-s, y-s, z-s);
}
Vector3 operator+(Vector3 o)
{
return Vector3(x+o.x, y+o.y, z+o.z);
}
Vector3 operator-(Vector3 o)
{
return Vector3(x-o.x, y-o.y, z-o.z);
}
};
class Orbit
{
protected:
double a, e, i, l, w, t;
Vector3 epoch;
Body *ref;
public:
Orbit() { }
Orbit(Body *ref, double a, double e, double i, double l, double w, double t) : ref(ref), a(a), e(e), i(i), l(l), w(w), t(t) { }
Orbit(Body *ref, Vector3 r, Vector3 v) : ref(ref) { calcFromPosVel(r, v); }
Body *reference() { return ref; }
double semiMajorAxis() { return a; }
double eccentricity() { return e; }
double inclination() { return i; }
double longitudeOfAscendingNode() { return l; }
double argumentOfPeriapsis() { return w; }
double trueAnomaly() { return t; }
double semiMinorAxis()
{
return sqrt(a*a * (1 - e*e));
}
double period()
{
double u = ref->gm();
return 2 * M_PI * sqrt(a*a*a / u);
}
double eccentricAnomaly()
{
double E = acos((e + cos(t)) / (1 + e * cos(t)));
if (t > M_PI && E < M_PI)
E = 2*M_PI - E;
return E;
}
double meanAnomaly()
{
double E = eccentricAnomaly();
double M = E - e * sin(E);
if (E > M_PI && M < M_PI)
M = 2*M_PI - M;
return M;
}
double meanMotion()
{
double u = ref->gm();
return sqrt(u / (a*a*a));
}
double timeSincePeriapsis()
{
return meanAnomaly() / meanMotion();
}
double semiparameter()
{
return a * (1 - e*e);
}
Vector3 position()
{
double p = semiparameter();
Vector3 r;
r.x = p * (cos(l) * cos(w + t) - sin(l) * cos(i) * sin(w + t));
r.y = p * (sin(l) * cos(w + t) + cos(l) * cos(i) * sin(w + t));
r.z = p * sin(i) * sin(w + t);
return r - epoch;
}
Vector3 velocity()
{
double p = semiparameter();
double u = ref->gm();
Vector3 v;
double g = -sqrt(u/p);
v.x = g * (cos(l) * (sin(w + t) + e * sin(w)) +
sin(l) * cos(i) * (cos(w + t) + e * cos(w)));
v.y = g * (sin(l) * (sin(w + t) + e * sin(w)) -
cos(l) * cos(i) * (cos(w + t) + e * cos(w)));
v.z = g * (sin(i) * (cos(w + t) + e * cos(w)));
return v;
}
void print()
{
printf("semi-major axis: %f\n", a);
printf("eccentricity: %f\n", e);
printf("inclination: %f\n", i);
printf("longitude of ascending node: %f\n", l);
printf("argument of periapsis: %f\n", w);
printf("true anomaly: %f\n", t);
printf("orbit period: %f\n", period());
printf("eccentric anomaly: %f\n", eccentricAnomaly());
printf("mean anomaly: %f\n", meanAnomaly());
printf("mean motion: %f\n", meanMotion());
printf("time since periapsis: %f\n", timeSincePeriapsis());
printf("epoch: %f %f %f\n", epoch.x, epoch.y, epoch.z);
Vector3 pos = position();
printf("position: %f %f %f, alt=%fkm\n", pos.x, pos.y, pos.z, (pos.len() - ref->radius()) / km);
Vector3 vel = velocity();
printf("velocity: %f %f %f, len=%f\n", vel.x, vel.y, vel.z, vel.len());
printf("--\n");
}
// vectors in geocentric equatorial inertial coordinates
void calcFromPosVel(Vector3 r, Vector3 v)
{
// calculate specific relative angular momement
Vector3 h = r * v;
// calculate vector to the ascending node
Vector3 n(-h.y, h.x, 0);
// standard gravity
double u = ref->gm();
// calculate eccentricity vector and scalar
Vector3 e = ((v * h) * (1.0 / u)) - (r * (1.0 / r.len()));
this->e = e.len();
// calculate specific orbital energy and semi-major axis
double E = v.sqlen() * 0.5 - u / r.len();
this->a = -u / (E * 2);
// calculate inclination
this->i = acos(h.z / h.len());
// calculate longitude of ascending node
if (this->i == 0.0)
this->l = 0;
else if (n.y >= 0.0)
this->l = acos(n.x / n.len());
else
this->l = 2 * M_PI - acos(n.x / n.len());
// calculate argument of periapsis
if (this->i == 0.0)
this->w = acos(e.x / e.len());
else if (e.z >= 0.0)
this->w = acos(n.dot(e) / (n.len() * e.len()));
else
this->w = 2 * M_PI - acos(n.dot(e) / (n.len() * e.len()));
// calculate true anomaly
if (r.dot(v) >= 0.0)
this->t = acos(e.dot(r) / (e.len() * r.len()));
else
this->t = 2 * M_PI - acos(e.dot(r) / (e.len() * r.len()));
// calculate epoch
this->epoch = Vector3(0,0,0);
this->epoch = position() - r;
}
// For small eccentricities a good approximation of true anomaly can be
// obtained by the following formula (the error is of the order e^3)
double estimateTrueAnomaly(double meanAnomaly)
{
double M = meanAnomaly;
return M + 2 * e * sin(M) + 1.25 * e * e * sin(2 * M);
}
double calcEccentricAnomaly(double meanAnomaly)
{
double t = estimateTrueAnomaly(meanAnomaly);
double E = acos((e + cos(t)) / (1 + e * cos(t)));
double M = E - e * sin(E);
// iterate to get M closer to meanAnomaly
double rate = 0.01;
bool lastDec = false;
while(1)
{
//printf(" using approx %f to %f\n", M, meanAnomaly);
if (fabs(M - meanAnomaly) < 0.0000000000001)
break;
if (M > meanAnomaly)
{
E -= rate;
lastDec = true;
}
else
{
E += rate;
if (lastDec)
rate *= 0.1;
}
M = E - e * sin(E);
}
if (meanAnomaly > M_PI && E < M_PI)
E = 2*M_PI - E;
return E;
}
void calcTrueAnomaly(double eccentricAnomaly)
{
double E = eccentricAnomaly;
this->t = acos((cos(E) - e) / (1 - e * cos(E)));
//this->t = 2 * atan2(sqrt(1+e)*sin(E/2), sqrt(1-e)*cos(E/2));
if (eccentricAnomaly > M_PI && this->t < M_PI)
this->t = 2*M_PI - this->t;
}
void step(double time)
{
double M = meanAnomaly();
M += meanMotion() * time;
while (M < -2*M_PI)
M = M + 2*M_PI;
if (M < 0)
M = 2*M_PI + M;
while (M > 2*M_PI)
M = M - 2*M_PI;
double E = calcEccentricAnomaly(M);
calcTrueAnomaly(E);
//printf("since M: %f, E=%f, t=%f\n", M, E, t);
}
};
Body earth(6.67300 * 5.9742 * E24m11, 6378.1 * km);
Orbit moon(&earth, 384399 * km, 0.0549, 18.29, 0, 0, 0);
int main()
{
Vector3 pos(0, earth.radius() + 300 * km, 0);
Vector3 vel(-7000, 0, 0);
Orbit o(&earth, pos, vel);
o.print();
for (int i = 0; i < o.period(); i++)
{
o.step(10);
o.print();
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment