work on orbits (not right yet), orbit camera

1 files changed, 80 insertions, 56 deletions

diff --git a/src/solar_system.rs b/src/solar_system.rs
index e8137f0..6730719 100644
--- a/src/solar_system.rs
+++ b/src/solar_system.rs
@@ -1,6 +1,7 @@
+use cgmath::Point3;
 use serde::{Deserialize};
-use crate::{known_stars::{KNOWN_STARS, StarNotFoundError}, timeman::Second};
-use std::error::Error;
+use crate::{GameState, known_stars::{KNOWN_STARS, StarNotFoundError}, timeman::Second};
+use std::{cell::RefCell, error::Error};
 
 const GRAVITATIONAL_CONSTANT: f64 = 6.67408e-11;
 
@@ -16,7 +17,7 @@ pub type SystemId = usize;
 pub struct SerialOrbitalBody
 {
     name: String,
-    orbits: BodyId,
+    orbits: Option<BodyId>,
     mass: Kilograms,
     radius: Kilometers,
 
@@ -33,13 +34,14 @@ pub struct SerialOrbitalBody
 pub struct OrbitalBody
 {
     body: SerialOrbitalBody,
-    position: Option<(Second, cgmath::Point3<Kilometers>)>
+    position: Option<cgmath::Point3<Kilometers>>
 }
 
 pub struct SolarSystem
 {
     name: String,
     bodies: Vec<OrbitalBody>,
+    time: Option<Second>
 }
 
 impl SolarSystem
@@ -53,8 +55,16 @@ impl SolarSystem
         let mut bodies = Vec::<OrbitalBody>::new();
 
         for result in body_reader.deserialize() {
-            let record: SerialOrbitalBody = result?;
+            let mut record: SerialOrbitalBody = result?;
 
+            match record.orbits {
+                Some(orbits) => {
+                    if record.sgp == 0.0 {
+                        record.sgp = bodies[orbits].body.mass + record.mass * GRAVITATIONAL_CONSTANT;
+                    }
+                }
+                None => {}
+            }
             println!("New body: {:?}", record);
 
             bodies.push(OrbitalBody { body: record, position: None });
@@ -63,6 +73,7 @@ impl SolarSystem
         Ok(Self {
             name: bodies[0].name().clone(),
             bodies: bodies,
+            time: None
         })
     }
 
@@ -83,79 +94,92 @@ impl SolarSystem
     {
         self.bodies.as_slice()
     }
+
+    fn update_bodies(
+        &mut self,
+        time: Second)
+    {
+        self.bodies.iter_mut().for_each(|body| {
+            body.position = Some(body.calculate_orbit_at(time));
+        });
+        self.time = Some(time);
+    }
+
+    pub fn update(
+        &mut self,
+        time: Second)
+    {
+        match self.time {
+            Some(cache_time) => {
+                if cache_time == time { return; }
+                self.update_bodies(time);
+            }
+            None => {
+                self.update_bodies(time);
+            }
+        }
+    }
 }
 
 impl OrbitalBody
 {
     pub fn name(&self) -> &String { &self.body.name }
     pub fn radius(&self) -> f32 { self.body.radius as f32 }
+    pub fn position(&self) -> Point3<f64> { self.position.unwrap() }
 
-    pub fn position(&self, time: Second)
-        -> Option<cgmath::Point3<Kilometers>>
+    fn calculate_orbit_at(
+        &self,
+        time: Second)
+    -> Point3<f64>
     {
-        match self.position {
-            Some((cache_time, pos)) => {
-                if time == cache_time {
-                    return Some(pos);
-                }
-                return None;
-            },
-            None => None
+        if self.body.orbits.is_none() {
+            return Point3::<f64>::new(0.0, 0.0, 0.0);
         }
-    }
 
-    fn calculate_orbit(
-        &self,
-        time: i64)
-    -> cgmath::Point3<Kilometers> {
-        cgmath::Point3 { x: 0.0, y: 0.0, z: 0.0 }
-        /*let arg_periapsis = self.long_periapsis - self.long_asc_node;
+        let eccentricity = self.body.eccentricity;
 
+        let long_asc_node = self.body.long_asc_node;
+        let arg_periaps = self.body.long_periapsis - long_asc_node;
 
-        let mean_angular_motion: f64 = (
-            self.sgp as f64 / (self.semi_major_axis as f64).powf(3.0)
-        ).sqrt();
+        let sma_cubed = self.body.semi_major_axis.powf(3.0);
+        let mean_motion = (self.body.sgp / sma_cubed.abs()).sqrt();
 
-        let mean_anomaly = (self.mean_long - self.long_periapsis) + (mean_angular_motion * time as f64) as f32;
+        let mean_anomaly_epoch = self.body.mean_long - self.body.long_periapsis;
+        let mean_anomaly = mean_anomaly_epoch + (mean_motion * time as f64);
 
-        let mut eccentric_anomaly = mean_anomaly + self.eccentricity * mean_anomaly.sin();
+        //Find the eccentric anomaly via newton's method
+        let mut eccentric_anomaly = mean_anomaly;
         for _ in 0..100 {
-            let new_eccentric = eccentric_anomaly + 
-                (mean_anomaly - eccentric_anomaly + self.eccentricity * eccentric_anomaly.sin()) /
-                (1.0 - self.eccentricity * eccentric_anomaly.cos());
-            if (new_eccentric -  eccentric_anomaly).abs() < 1e-6 {
-                eccentric_anomaly = new_eccentric;
+            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;
             }
-            eccentric_anomaly = new_eccentric;
         }
 
-        let beta = self.eccentricity / 1.0 + (1.0 - self.eccentricity * self.eccentricity).sqrt();
-        let true_anomaly = eccentric_anomaly + 2.0 * 
-            ((beta * eccentric_anomaly.sin()) / (1.0 - beta * eccentric_anomaly.cos())).atan();
-
-        let radius: f64 = self.semi_major_axis * (1.0 - self.eccentricity * eccentric_anomaly.cos()) as f64;
-
-        let ohm_sin = self.long_asc_node.sin();
-        let ohm_cos = self.long_asc_node.cos();
-
-        let inc_sin = self.inclination.sin();
-        let inc_cos = self.inclination.cos();
+        let true_anomaly = 2.0 * (
+            ((1.0 + eccentricity) / (1.0 - eccentricity)).sqrt() 
+            * (eccentric_anomaly / 2.0).tan()
+        ).atan();
 
-        let per_anom_sin = (arg_periapsis + true_anomaly).sin();
-        let per_anom_cos = (arg_periapsis + true_anomaly).cos();
+        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.body.inclination.sin_cos();
 
-        let x: f32 = (radius as f32) * (
-            (ohm_cos * per_anom_cos) -
-            (ohm_sin * per_anom_sin * inc_cos));
-        let y: f32 = (radius as f32) * (
-            (ohm_sin * per_anom_cos) +
-            (ohm_cos * per_anom_sin * inc_cos));
-        let z: f32 = (radius as f32) * (inc_sin * per_anom_sin);
+        let r = self.body.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);
 
-        OrbitState {
-            position: cgmath::Vector3::new(x, y, z)
-        }*/
+        Point3::<f64>::new(x, y, z)
     }
 }