Implement maneuver representation enum

Author: ChristopherRabotin

src/dynamics/guidance/mnvr.rs

@@ -44,10 +44,8 @@ pub struct Maneuver {
     /// Thrust level, if 1.0 use all thruster available at full power
     /// TODO: Convert this to a common polynomial as well to optimize throttle, throttle rate (and accel?)
     pub thrust_prct: f64,
-    /// The interpolation polynomial for the azimuth (in-plane) angle
-    pub alpha_inplane_radians: CommonPolynomial,
-    /// The interpolation polynomial for the elevation (out-of-plane) angle
-    pub delta_outofplane_radians: CommonPolynomial,
+    /// The representation of this maneuver.
+    pub representation: MnvrRepr,
     /// The frame in which the maneuvers are defined.
     pub frame: LocalFrame,
 }
@@ -67,11 +65,7 @@ impl fmt::Display for Maneuver {
                 self.end - self.start,
                 self.end,
             )?;
-            write!(
-                f,
-                "\n\tin-plane angle α: {}\n\tout-of-plane angle β: {}",
-                self.alpha_inplane_radians, self.delta_outofplane_radians
-            )?;
+            write!(f, "\n{}", self.representation)?;
             write!(
                 f,
                 "\n\tinitial dir: [{:.6}, {:.6}, {:.6}]\n\tfinal dir  : [{:.6}, {:.6}, {:.6}]",
@@ -80,13 +74,40 @@ impl fmt::Display for Maneuver {
         } else {
             write!(
                 f,
-                "Impulsive maneuver @ {}\n\tin-plane angle α: {}\n\tout-of-plane angle β: {}",
-                self.start, self.alpha_inplane_radians, self.delta_outofplane_radians
+                "Impulsive maneuver @ {}\n{}",
+                self.start, self.representation
             )
         }
     }
 }
 
+/// Defines the available maneuver representations.
+#[derive(Copy, Clone, Debug, PartialEq, Serialize, Deserialize)]
+pub enum MnvrRepr {
+    /// Represents the maneuver as a fixed vector in the local frame.
+    Vector(Vector3<f64>),
+    /// Represents the maneuver as a polynominal of azimuth (right ascension / in-plane) and elevation (declination / out of plane)
+    Angles {
+        azimuth: CommonPolynomial,
+        elevation: CommonPolynomial,
+    },
+}
+
+impl fmt::Display for MnvrRepr {
+    /// Prints the polynomial with the least significant coefficients first
+    #[allow(clippy::identity_op)]
+    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
+        match self {
+            MnvrRepr::Vector(vector) => write!(f, "{vector}"),
+            MnvrRepr::Angles { azimuth, elevation } => write!(
+                f,
+                "\tazimuth (in-plane) α: {}\n\televation (out-of-plane) β: {}",
+                azimuth, elevation
+            ),
+        }
+    }
+}
+
 impl Maneuver {
     /// Creates an impulsive maneuver whose vector is the deltaV.
     /// TODO: This should use William's algorithm
@@ -102,24 +123,26 @@ impl Maneuver {
         vector: Vector3<f64>,
         frame: LocalFrame,
     ) -> Self {
-        // Convert to angles
-        let (alpha, delta) = ra_dec_from_unit_vector(vector);
         Self {
             start,
             end,
             thrust_prct: thrust_lvl,
-            alpha_inplane_radians: CommonPolynomial::Constant(alpha),
-            delta_outofplane_radians: CommonPolynomial::Constant(delta),
+            representation: MnvrRepr::Vector(vector),
             frame,
         }
     }
 
     /// Return the thrust vector computed at the provided epoch
     pub fn vector(&self, epoch: Epoch) -> Vector3<f64> {
-        let t = (epoch - self.start).to_seconds();
-        let alpha = self.alpha_inplane_radians.eval(t);
-        let delta = self.delta_outofplane_radians.eval(t);
-        unit_vector_from_ra_dec(alpha, delta)
+        match self.representation {
+            MnvrRepr::Vector(vector) => vector,
+            MnvrRepr::Angles { azimuth, elevation } => {
+                let t = (epoch - self.start).to_seconds();
+                let alpha = azimuth.eval(t);
+                let delta = elevation.eval(t);
+                unit_vector_from_ra_dec(alpha, delta)
+            }
+        }
     }
 
     /// Return the duration of this maneuver
@@ -134,9 +157,14 @@ impl Maneuver {
 
     /// Returns the direction of the burn at the start of the burn, useful for setting new angles
     pub fn direction(&self) -> Vector3<f64> {
-        let alpha = self.alpha_inplane_radians.coeff_in_order(0).unwrap();
-        let delta = self.delta_outofplane_radians.coeff_in_order(0).unwrap();
-        unit_vector_from_ra_dec(alpha, delta)
+        match self.representation {
+            MnvrRepr::Vector(vector) => vector / vector.norm(),
+            MnvrRepr::Angles { azimuth, elevation } => {
+                let alpha = azimuth.coeff_in_order(0).unwrap();
+                let delta = elevation.coeff_in_order(0).unwrap();
+                unit_vector_from_ra_dec(alpha, delta)
+            }
+        }
     }
 
     /// Set the time-invariant direction for this finite burn while keeping the other components as they are
@@ -146,12 +174,15 @@ impl Maneuver {
 
     /// Returns the rate of direction of the burn at the start of the burn, useful for setting new angles
     pub fn rate(&self) -> Vector3<f64> {
-        match self.alpha_inplane_radians.coeff_in_order(1) {
-            Ok(alpha) => {
-                let delta = self.delta_outofplane_radians.coeff_in_order(1).unwrap();
-                unit_vector_from_ra_dec(alpha, delta)
-            }
-            Err(_) => Vector3::zeros(),
+        match self.representation {
+            MnvrRepr::Vector(_) => Vector3::zeros(),
+            MnvrRepr::Angles { azimuth, elevation } => match azimuth.coeff_in_order(1) {
+                Ok(alpha) => {
+                    let delta = elevation.coeff_in_order(1).unwrap();
+                    unit_vector_from_ra_dec(alpha, delta)
+                }
+                Err(_) => Vector3::zeros(),
+            },
         }
     }
 
@@ -162,12 +193,15 @@ impl Maneuver {
 
     /// Returns the acceleration of the burn at the start of the burn, useful for setting new angles
     pub fn accel(&self) -> Vector3<f64> {
-        match self.alpha_inplane_radians.coeff_in_order(2) {
-            Ok(alpha) => {
-                let delta = self.delta_outofplane_radians.coeff_in_order(2).unwrap();
-                unit_vector_from_ra_dec(alpha, delta)
-            }
-            Err(_) => Vector3::zeros(),
+        match self.representation {
+            MnvrRepr::Vector(_) => Vector3::zeros(),
+            MnvrRepr::Angles { azimuth, elevation } => match azimuth.coeff_in_order(2) {
+                Ok(alpha) => {
+                    let delta = elevation.coeff_in_order(2).unwrap();
+                    unit_vector_from_ra_dec(alpha, delta)
+                }
+                Err(_) => Vector3::zeros(),
+            },
         }
     }
 
@@ -183,48 +217,58 @@ impl Maneuver {
         rate: Vector3<f64>,
         accel: Vector3<f64>,
     ) -> Result<(), GuidanceError> {
-        let (alpha, delta) = ra_dec_from_unit_vector(dir);
-        if alpha.is_nan() || delta.is_nan() {
-            return Err(GuidanceError::InvalidDirection {
-                x: dir[0],
-                y: dir[1],
-                z: dir[2],
-                in_plane_deg: alpha.to_degrees(),
-                out_of_plane_deg: delta.to_degrees(),
-            });
-        }
-        if rate.norm() < 2e-16 && accel.norm() < 2e-16 {
-            self.alpha_inplane_radians = CommonPolynomial::Constant(alpha);
-            self.delta_outofplane_radians = CommonPolynomial::Constant(delta);
+        if rate.norm() < f64::EPSILON && accel.norm() < f64::EPSILON {
+            // Set as a vector
+            self.representation = MnvrRepr::Vector(dir)
         } else {
-            let (alpha_dt, delta_dt) = ra_dec_from_unit_vector(rate);
-            if alpha_dt.is_nan() || delta_dt.is_nan() {
-                return Err(GuidanceError::InvalidRate {
-                    x: rate[0],
-                    y: rate[1],
-                    z: rate[2],
-                    in_plane_deg_s: alpha_dt.to_degrees(),
-                    out_of_plane_deg_s: delta_dt.to_degrees(),
+            let (alpha, delta) = ra_dec_from_unit_vector(dir);
+            if alpha.is_nan() || delta.is_nan() {
+                return Err(GuidanceError::InvalidDirection {
+                    x: dir[0],
+                    y: dir[1],
+                    z: dir[2],
+                    in_plane_deg: alpha.to_degrees(),
+                    out_of_plane_deg: delta.to_degrees(),
                 });
             }
-            if accel.norm() < 2e-16 {
-                self.alpha_inplane_radians = CommonPolynomial::Linear(alpha_dt, alpha);
-                self.delta_outofplane_radians = CommonPolynomial::Linear(delta_dt, delta);
+            if rate.norm() < f64::EPSILON && accel.norm() < f64::EPSILON {
+                self.representation = MnvrRepr::Angles {
+                    azimuth: CommonPolynomial::Constant(alpha),
+                    elevation: CommonPolynomial::Constant(delta),
+                };
             } else {
-                let (alpha_ddt, delta_ddt) = ra_dec_from_unit_vector(accel);
-                if alpha_ddt.is_nan() || delta_ddt.is_nan() {
-                    return Err(GuidanceError::InvalidAcceleration {
-                        x: accel[0],
-                        y: accel[1],
-                        z: accel[2],
-                        in_plane_deg_s2: alpha_ddt.to_degrees(),
-                        out_of_plane_deg_s2: delta_ddt.to_degrees(),
+                let (alpha_dt, delta_dt) = ra_dec_from_unit_vector(rate);
+                if alpha_dt.is_nan() || delta_dt.is_nan() {
+                    return Err(GuidanceError::InvalidRate {
+                        x: rate[0],
+                        y: rate[1],
+                        z: rate[2],
+                        in_plane_deg_s: alpha_dt.to_degrees(),
+                        out_of_plane_deg_s: delta_dt.to_degrees(),
                     });
                 }
-                self.alpha_inplane_radians =
-                    CommonPolynomial::Quadratic(alpha_ddt, alpha_dt, alpha);
-                self.delta_outofplane_radians =
-                    CommonPolynomial::Quadratic(delta_ddt, delta_dt, delta);
+                if accel.norm() < f64::EPSILON {
+                    self.representation = MnvrRepr::Angles {
+                        azimuth: CommonPolynomial::Linear(alpha_dt, alpha),
+                        elevation: CommonPolynomial::Linear(delta_dt, delta),
+                    };
+                } else {
+                    let (alpha_ddt, delta_ddt) = ra_dec_from_unit_vector(accel);
+                    if alpha_ddt.is_nan() || delta_ddt.is_nan() {
+                        return Err(GuidanceError::InvalidAcceleration {
+                            x: accel[0],
+                            y: accel[1],
+                            z: accel[2],
+                            in_plane_deg_s2: alpha_ddt.to_degrees(),
+                            out_of_plane_deg_s2: delta_ddt.to_degrees(),
+                        });
+                    }
+
+                    self.representation = MnvrRepr::Angles {
+                        azimuth: CommonPolynomial::Quadratic(alpha_ddt, alpha_dt, alpha),
+                        elevation: CommonPolynomial::Quadratic(delta_ddt, delta_dt, delta),
+                    };
+                }
             }
         }
         Ok(())

src/dynamics/guidance/mod.rs

@@ -29,7 +29,7 @@ mod finiteburns;
 pub use finiteburns::FiniteBurns;
 
 mod mnvr;
-pub use mnvr::Maneuver;
+pub use mnvr::{Maneuver, MnvrRepr};
 
 mod ruggiero;
 pub use ruggiero::{Objective, Ruggiero, StateParameter};
@@ -182,8 +182,8 @@ fn ra_dec_from_vec() {
         loop {
             let unit_v = unit_vector_from_ra_dec(alpha, delta);
             let (alpha2, delta2) = ra_dec_from_unit_vector(unit_v);
-            assert!((alpha - alpha2).abs() < 2e-16);
-            assert!((delta - delta2).abs() < 2e-16);
+            assert!((alpha - alpha2).abs() < f64::EPSILON);
+            assert!((delta - delta2).abs() < f64::EPSILON);
             alpha += TAU * 0.1; // Increment right ascension by one tenth of a circle
             if alpha > PI {
                 alpha = 0.0;

src/md/opti/multipleshooting/equidistant_heuristic.rs

@@ -41,7 +41,7 @@ impl<'a> MultipleShooting<'a, Node, 3, 3> {
 
         // Compute the direction of the objective
         let mut direction = xf.radius_km - x0.orbit.radius_km;
-        if direction.norm() < 2e-16 {
+        if direction.norm() < f64::EPSILON {
             return Err(TargetingError::TargetsTooClose);
         }
         let distance_increment = direction.norm() / (node_count as f64);