From 3e9bad89290d4940dbe8a41fe5687b765e3bccc7 Mon Sep 17 00:00:00 2001
From: Paul Mathieu <paul@ponteilla.net>
Date: Tue, 18 Oct 2022 14:43:22 -0700
Subject: [PATCH] skycraft: slightly better orbit calculation

---
 skycraft/index.js | 73 ++++++++++++++++++++++++++++-------------------
 1 file changed, 44 insertions(+), 29 deletions(-)

diff --git a/skycraft/index.js b/skycraft/index.js
index f7c9198..bc71884 100644
--- a/skycraft/index.js
+++ b/skycraft/index.js
@@ -362,62 +362,77 @@ function normalizeAngle(theta) {
 }
 
 /** Let's be honest I should clean this up.
- * Right now it mostly only works with elliptical orbits (e < 1),
- * which is fine for planets & stuff, but not for my spacecraft (or comets)
- *
+ * 
  * This is the part that solves Kepler's equation using Newton's method.
  * For circular-ish orbits, one or two iterations are usually enough.
  * More excentric orbits can take more (6 or 7?).
+ * 
+ * For near-parabolic orbits (and some others?) it often fails to converge...
  */
-function getCartesianPosition(orbit, mu, time) {
+function getCartesianState(orbit, mu, time) {
     const {
         excentricity: e,
         semimajorAxis: a,
         inclination: i,
         ascendingNodeLongitude: Om,
         periapsisArgument: w,
+        t0,
     } = orbit;
-    const n = Math.sqrt(mu/(a**3));  // mean motion
-    const M = n * time;  // mean anomaly
-//    const nu = (
-//        M
-//        + (2 * e - e**3 / 4) * Math.sin(M)
-//        + 5/4 * e**2 * Math.sin(2*M)
-//        + 13/12 * e**3 * Math.sin(3*M)
-//    );  // true anomaly, doesn't work :(
+    let n = Math.sqrt(mu/(a**3));
+    if (a < 0) {
+        n = Math.sqrt(mu/-(a**3));  // mean motion
+    }
+    const M = n * (time - t0);  // mean anomaly
 
     // Newton's method
-    var E2;
-    var E = M;
-    for (var j = 1; j < 20; ++j) {
+    var E2 = 0;
+    var E = orbit.lastE || M;
+    let iterations = 0;
+    // a clever guess? https://link.springer.com/article/10.1023/A:1008200607490
+    // doesn't work at all.
+
+    while (Math.abs(E - E2) > 1e-10) {
         if (e < 0.001) {
             break;
         }
+        E = E2;
         if (e < 1) {
             E2 = E - (E - e * Math.sin(E) - M) / (1 - e * Math.cos(E));
         } else if (e > 1) {
-            E2 = E - (-E + e * sinh(E) - M) / (e * cosh(E) - 1);
+            E2 = E - (-E + e * Math.sinh(E) - M) / (e * Math.cosh(E) - 1);
         } else {
             E2 = E - (E + E*E*E/3 - M) / (1 + E*E);
         }
-        if (Math.abs(E - E2) < 1e-10) {
-            break;
+
+        iterations++;
+        if (iterations > 100) {
+            console.log('numerical instability');
+            return {};
         }
-        E = E2;
     }
-    const nu = 2 * Math.atan(Math.sqrt((1+e) / (1-e)) * Math.tan(E/2));
-    const r = a * (1 - e**2) / (1 + e * Math.cos(nu));
+    orbit.lastE = E;
+    let nu;
+    if (e > 1) {
+        nu = 2 * Math.atan(Math.sqrt((e+1) / (e-1)) * Math.tanh(E/2));
+    } else {
+        nu = 2 * Math.atan(Math.sqrt((1+e) / (1-e)) * Math.tan(E/2));
+    }
+    const p = a * (1 - e**2);
+    const r = p / (1 + e * Math.cos(nu));// * ((a < 0) ? -1 : 1);
+    const rd = e * Math.sqrt(mu / p) * Math.sin(nu);
 
-    const cOm = Math.cos(Om);
-    const sOm = Math.sin(Om);
-    const cwnu = Math.cos(w + nu);
-    const swnu = Math.sin(w + nu);
+    if (orbit.tf === undefined) {
+        orbit.tf = [se3.rotz(Om), se3.rotx(i), se3.rotz(w)].reduce(se3.product);
+    }
 
-    const x = r * Math.cos(i) * (cOm * cwnu - sOm * swnu);
-    const y = r * (sOm * cwnu + cOm * swnu);
-    const z = r * Math.sin(i) * cwnu;
+    const tf = se3.product(orbit.tf, se3.rotz(nu));
+    const pos = se3.apply(tf, [r, 0, 0, 1]);
+    const vel = se3.apply(tf, [rd, Math.sqrt(p * mu) / r, 0, 1]);
 
-    return [x, y, z];
+    return {
+        position: pos.slice(0, 3),
+        velocity: vel.slice(0, 3),
+    };
 }
 
 function getOrientation(body, time) {