fix label rendering over large bodies

1 files changed, 57 insertions, 1 deletions

diff --git a/src/solar_system/orbit.rs b/src/solar_system/orbit.rs
index fe55e4f..b042bd3 100644
--- a/src/solar_system/orbit.rs
+++ b/src/solar_system/orbit.rs
@@ -20,10 +20,66 @@ impl StaticOrbit
         this_body: &OrbitalBody)
         -> Second
     {
-        ((self.semi_major_axis.powf(3.0) / this_body.sgp).sqrt() * std::f64::consts::TAU) as u64 
+        ((self.semi_major_axis.powf(3.0) / this_body.sgp).sqrt() * std::f64::consts::TAU) as Second 
     }
 
     pub fn parent(&self) -> Option<BodyId> {
         self.parent
     }
+
+    pub fn calculate_position_at(
+        &self,
+        this_body: &OrbitalBody,
+        time: Second)
+    -> cgmath::Vector3<f64>
+    {
+        if self.parent.is_none() {
+            return cgmath::Vector3::<f64>::new(0.0, 0.0, 0.0);
+        }
+
+        let eccentricity = self.eccentricity;
+
+        let long_asc_node = self.long_asc_node;
+        let arg_periaps = self.long_periapsis - long_asc_node;
+
+        let sma_cubed = self.semi_major_axis.powf(3.0);   
+        let mean_motion = (this_body.sgp / sma_cubed).sqrt();
+
+        let mean_anomaly_epoch = self.mean_long - self.long_periapsis;
+        let mean_anomaly = mean_anomaly_epoch + (mean_motion * time as f64);
+
+        //Find the eccentric anomaly via newton's method
+        let mut eccentric_anomaly = mean_anomaly;
+        for _ in 0..100 {
+            let (e_sin, e_cos) = eccentric_anomaly.sin_cos();
+            let new_anomaly = eccentric_anomaly -
+                (
+                    (eccentric_anomaly - eccentricity * e_sin - mean_anomaly) /
+                    (1.0 - eccentricity * e_cos)
+                );
+
+            let diff = eccentric_anomaly - new_anomaly;
+            eccentric_anomaly = new_anomaly;
+            if diff.abs() < 1e-6 {
+                break;
+            }
+        }
+
+        let true_anomaly = 2.0 * (
+            ((1.0 + eccentricity) / (1.0 - eccentricity)).sqrt() 
+            * (eccentric_anomaly / 2.0).tan()
+        ).atan();
+
+        let vw = true_anomaly + arg_periaps;
+        let (vw_sin, vw_cos) = vw.sin_cos();
+        let (omega_sin, omega_cos) = long_asc_node.sin_cos();
+        let (i_sin, i_cos) = self.inclination.sin_cos();
+
+        let r = self.semi_major_axis * (1.0 - eccentricity * eccentric_anomaly.cos());
+        let x = r * (omega_cos * vw_cos - omega_sin * vw_sin * i_cos);
+        let y = r * i_sin * vw_sin;
+        let z = r * (omega_sin * vw_cos + omega_cos * vw_sin * i_cos);
+
+        cgmath::Vector3::<f64>::new(x, y, z)
+    }
 }