nyx_space/cosmic/
orbitdual.rs

1/*
2    Nyx, blazing fast astrodynamics
3    Copyright (C) 2018-onwards Christopher Rabotin <christopher.rabotin@gmail.com>
4
5    This program is free software: you can redistribute it and/or modify
6    it under the terms of the GNU Affero General Public License as published
7    by the Free Software Foundation, either version 3 of the License, or
8    (at your option) any later version.
9
10    This program is distributed in the hope that it will be useful,
11    but WITHOUT ANY WARRANTY; without even the implied warranty of
12    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13    GNU Affero General Public License for more details.
14
15    You should have received a copy of the GNU Affero General Public License
16    along with this program.  If not, see <https://www.gnu.org/licenses/>.
17*/
18
19use anise::astro::orbit::ECC_EPSILON;
20use anise::astro::PhysicsResult;
21use anise::prelude::{Frame, Orbit};
22use snafu::ResultExt;
23
24use super::AstroError;
25use crate::cosmic::AstroPhysicsSnafu;
26use crate::linalg::{Vector3, U7};
27use crate::md::StateParameter;
28use crate::time::Epoch;
29use crate::TimeTagged;
30use hyperdual::linalg::norm;
31use hyperdual::{Float, OHyperdual};
32use std::f64::consts::PI;
33use std::fmt;
34
35/// Orbit defines an orbital state
36///
37/// Unless noted otherwise, algorithms are from GMAT 2016a [StateConversionUtil.cpp](https://github.com/ChristopherRabotin/GMAT/blob/37201a6290e7f7b941bc98ee973a527a5857104b/src/base/util/StateConversionUtil.cpp).
38/// Regardless of the constructor used, this struct stores all the state information in Cartesian coordinates
39/// as these are always non singular.
40/// _Note:_ although not yet supported, this struct may change once True of Date or other nutation frames
41/// are added to the toolkit.
42#[derive(Copy, Clone, Debug)]
43pub struct OrbitDual {
44    /// in km
45    pub x: OHyperdual<f64, U7>,
46    /// in km
47    pub y: OHyperdual<f64, U7>,
48    /// in km
49    pub z: OHyperdual<f64, U7>,
50    /// in km/s
51    pub vx: OHyperdual<f64, U7>,
52    /// in km/s
53    pub vy: OHyperdual<f64, U7>,
54    /// in km/s
55    pub vz: OHyperdual<f64, U7>,
56    pub dt: Epoch,
57    /// Frame contains everything we need to compute state information
58    pub frame: Frame,
59}
60
61impl From<Orbit> for OrbitDual {
62    /// Initialize a new OrbitDual from an orbit, no other initializers
63    fn from(orbit: Orbit) -> Self {
64        Self {
65            x: OHyperdual::from_slice(&[orbit.radius_km.x, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
66            y: OHyperdual::from_slice(&[orbit.radius_km.y, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0]),
67            z: OHyperdual::from_slice(&[orbit.radius_km.z, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]),
68            vx: OHyperdual::from_slice(&[orbit.velocity_km_s.x, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0]),
69            vy: OHyperdual::from_slice(&[orbit.velocity_km_s.y, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0]),
70            vz: OHyperdual::from_slice(&[orbit.velocity_km_s.z, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]),
71            dt: orbit.epoch,
72            frame: orbit.frame,
73        }
74    }
75}
76
77/// A type which stores the partial of an element
78#[derive(Copy, Clone, Debug)]
79pub struct OrbitPartial {
80    pub param: StateParameter,
81    pub dual: OHyperdual<f64, U7>,
82}
83
84impl OrbitPartial {
85    /// Returns the real value of this parameter
86    pub fn real(&self) -> f64 {
87        self.dual[0]
88    }
89    /// The partial of this parameter with respect to X
90    pub fn wtr_x(&self) -> f64 {
91        self.dual[1]
92    }
93    /// The partial of this parameter with respect to Y
94    pub fn wtr_y(&self) -> f64 {
95        self.dual[2]
96    }
97    /// The partial of this parameter with respect to Z
98    pub fn wtr_z(&self) -> f64 {
99        self.dual[3]
100    }
101    /// The partial of this parameter with respect to VX
102    pub fn wtr_vx(&self) -> f64 {
103        self.dual[4]
104    }
105    /// The partial of this parameter with respect to VY
106    pub fn wtr_vy(&self) -> f64 {
107        self.dual[5]
108    }
109    /// The partial of this parameter with respect to VZ
110    pub fn wtr_vz(&self) -> f64 {
111        self.dual[6]
112    }
113}
114
115impl fmt::Display for OrbitPartial {
116    fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
117        write!(f, "{:?} {}", self.param, self.dual)
118    }
119}
120
121impl OrbitDual {
122    pub fn partial_for(&self, param: StateParameter) -> Result<OrbitPartial, AstroError> {
123        match param {
124            StateParameter::X => Ok(OrbitPartial {
125                dual: self.x,
126                param: StateParameter::X,
127            }),
128            StateParameter::Y => Ok(OrbitPartial {
129                dual: self.y,
130                param: StateParameter::Y,
131            }),
132            StateParameter::Z => Ok(OrbitPartial {
133                dual: self.z,
134                param: StateParameter::Z,
135            }),
136            StateParameter::VX => Ok(OrbitPartial {
137                dual: self.vx,
138                param: StateParameter::VX,
139            }),
140            StateParameter::VY => Ok(OrbitPartial {
141                dual: self.vy,
142                param: StateParameter::VY,
143            }),
144            StateParameter::VZ => Ok(OrbitPartial {
145                dual: self.vz,
146                param: StateParameter::VZ,
147            }),
148            StateParameter::Rmag => Ok(self.rmag_km()),
149            StateParameter::Vmag => Ok(self.vmag_km_s()),
150            StateParameter::HX => Ok(self.hx()),
151            StateParameter::HY => Ok(self.hy()),
152            StateParameter::HZ => Ok(self.hz()),
153            StateParameter::Hmag => Ok(self.hmag()),
154            StateParameter::Energy => Ok(self.energy_km2_s2().context(AstroPhysicsSnafu)?),
155            StateParameter::SMA => Ok(self.sma_km().context(AstroPhysicsSnafu)?),
156            StateParameter::Eccentricity => Ok(self.ecc().context(AstroPhysicsSnafu)?),
157            StateParameter::Inclination => Ok(self.inc_deg()),
158            StateParameter::AoP => Ok(self.aop_deg().context(AstroPhysicsSnafu)?),
159            StateParameter::AoL => Ok(self.aol_deg().context(AstroPhysicsSnafu)?),
160            StateParameter::RAAN => Ok(self.raan_deg()),
161            StateParameter::Periapsis => Ok(self.periapsis_km().context(AstroPhysicsSnafu)?),
162            StateParameter::Apoapsis => Ok(self.apoapsis_km().context(AstroPhysicsSnafu)?),
163            StateParameter::TrueLongitude => Ok(self.tlong_deg().context(AstroPhysicsSnafu)?),
164            StateParameter::FlightPathAngle => Ok(self.fpa_deg().context(AstroPhysicsSnafu)?),
165            StateParameter::MeanAnomaly => Ok(self.ma_deg().context(AstroPhysicsSnafu)?),
166            StateParameter::EccentricAnomaly => Ok(self.ea_deg().context(AstroPhysicsSnafu)?),
167            StateParameter::Height => Ok(self.height_km().context(AstroPhysicsSnafu)?),
168            StateParameter::Latitude => Ok(self.latitude_deg().context(AstroPhysicsSnafu)?),
169            StateParameter::Longitude => Ok(self.longitude_deg()),
170            StateParameter::C3 => Ok(self.c3().context(AstroPhysicsSnafu)?),
171            StateParameter::RightAscension => Ok(self.right_ascension_deg()),
172            StateParameter::Declination => Ok(self.declination_deg()),
173            StateParameter::HyperbolicAnomaly => self.hyperbolic_anomaly_deg(),
174            StateParameter::SemiParameter => {
175                Ok(self.semi_parameter_km().context(AstroPhysicsSnafu)?)
176            }
177            StateParameter::SemiMinorAxis => {
178                Ok(self.semi_minor_axis_km().context(AstroPhysicsSnafu)?)
179            }
180            StateParameter::TrueAnomaly => Ok(self.ta_deg().context(AstroPhysicsSnafu)?),
181            _ => Err(AstroError::PartialsUndefined),
182        }
183    }
184
185    /// Returns the magnitude of the radius vector in km
186    pub fn rmag_km(&self) -> OrbitPartial {
187        OrbitPartial {
188            param: StateParameter::Rmag,
189            dual: (self.x.powi(2) + self.y.powi(2) + self.z.powi(2)).sqrt(),
190        }
191    }
192
193    /// Returns the magnitude of the velocity vector in km/s
194    pub fn vmag_km_s(&self) -> OrbitPartial {
195        OrbitPartial {
196            param: StateParameter::Vmag,
197            dual: (self.vx.powi(2) + self.vy.powi(2) + self.vz.powi(2)).sqrt(),
198        }
199    }
200
201    /// Returns the radius vector of this Orbit in [km, km, km]
202    pub(crate) fn radius(&self) -> Vector3<OHyperdual<f64, U7>> {
203        Vector3::new(self.x, self.y, self.z)
204    }
205
206    /// Returns the velocity vector of this Orbit in [km/s, km/s, km/s]
207    pub(crate) fn velocity(&self) -> Vector3<OHyperdual<f64, U7>> {
208        Vector3::new(self.vx, self.vy, self.vz)
209    }
210
211    /// Returns the orbital momentum vector
212    pub(crate) fn hvec(&self) -> Vector3<OHyperdual<f64, U7>> {
213        self.radius().cross(&self.velocity())
214    }
215
216    /// Returns the orbital momentum value on the X axis
217    pub fn hx(&self) -> OrbitPartial {
218        OrbitPartial {
219            dual: self.hvec()[0],
220            param: StateParameter::HX,
221        }
222    }
223
224    /// Returns the orbital momentum value on the Y axis
225    pub fn hy(&self) -> OrbitPartial {
226        OrbitPartial {
227            dual: self.hvec()[1],
228            param: StateParameter::HY,
229        }
230    }
231
232    /// Returns the orbital momentum value on the Z axis
233    pub fn hz(&self) -> OrbitPartial {
234        OrbitPartial {
235            dual: self.hvec()[2],
236            param: StateParameter::HZ,
237        }
238    }
239
240    /// Returns the norm of the orbital momentum
241    pub fn hmag(&self) -> OrbitPartial {
242        OrbitPartial {
243            dual: norm(&self.hvec()),
244            param: StateParameter::Hmag,
245        }
246    }
247
248    /// Returns the specific mechanical energy
249    pub fn energy_km2_s2(&self) -> PhysicsResult<OrbitPartial> {
250        Ok(OrbitPartial {
251            dual: self.vmag_km_s().dual.powi(2) / OHyperdual::from(2.0)
252                - OHyperdual::from(self.frame.mu_km3_s2()?) / self.rmag_km().dual,
253            param: StateParameter::Energy,
254        })
255    }
256
257    /// Returns the semi-major axis in km
258    pub fn sma_km(&self) -> PhysicsResult<OrbitPartial> {
259        Ok(OrbitPartial {
260            dual: -OHyperdual::from(self.frame.mu_km3_s2()?)
261                / (OHyperdual::from(2.0) * self.energy_km2_s2()?.dual),
262            param: StateParameter::SMA,
263        })
264    }
265
266    /// Returns the eccentricity vector (no unit)
267    pub(crate) fn evec(&self) -> PhysicsResult<Vector3<OHyperdual<f64, U7>>> {
268        let r = self.radius();
269        let v = self.velocity();
270        let hgm = OHyperdual::from(self.frame.mu_km3_s2()?);
271        // Split up this operation because it doesn't seem to be implemented
272        // ((norm(&v).powi(2) - hgm / norm(&r)) * r - (r.dot(&v)) * v) / hgm
273        Ok(Vector3::new(
274            ((norm(&v).powi(2) - hgm / norm(&r)) * r[0] - (r.dot(&v)) * v[0]) / hgm,
275            ((norm(&v).powi(2) - hgm / norm(&r)) * r[1] - (r.dot(&v)) * v[1]) / hgm,
276            ((norm(&v).powi(2) - hgm / norm(&r)) * r[2] - (r.dot(&v)) * v[2]) / hgm,
277        ))
278    }
279
280    /// Returns the eccentricity (no unit)
281    pub fn ecc(&self) -> PhysicsResult<OrbitPartial> {
282        Ok(OrbitPartial {
283            dual: norm(&self.evec()?),
284            param: StateParameter::Eccentricity,
285        })
286    }
287
288    /// Returns the inclination in degrees
289    pub fn inc_deg(&self) -> OrbitPartial {
290        OrbitPartial {
291            dual: (self.hvec()[(2, 0)] / self.hmag().dual).acos().to_degrees(),
292            param: StateParameter::Inclination,
293        }
294    }
295
296    /// Returns the argument of periapsis in degrees
297    pub fn aop_deg(&self) -> PhysicsResult<OrbitPartial> {
298        let n = Vector3::new(
299            OHyperdual::from(0.0),
300            OHyperdual::from(0.0),
301            OHyperdual::from(1.0),
302        )
303        .cross(&self.hvec());
304        let aop = (n.dot(&self.evec()?) / (norm(&n) * self.ecc()?.dual)).acos();
305        if aop.is_nan() {
306            warn!("AoP is NaN");
307            Ok(OrbitPartial {
308                dual: OHyperdual::from(0.0),
309                param: StateParameter::AoP,
310            })
311        } else if self.evec()?[2].real() < 0.0 {
312            Ok(OrbitPartial {
313                dual: (OHyperdual::from(2.0 * PI) - aop).to_degrees(),
314                param: StateParameter::AoP,
315            })
316        } else {
317            Ok(OrbitPartial {
318                dual: aop.to_degrees(),
319                param: StateParameter::AoP,
320            })
321        }
322    }
323
324    /// Returns the right ascension of ther ascending node in degrees
325    pub fn raan_deg(&self) -> OrbitPartial {
326        let n = Vector3::new(
327            OHyperdual::from(0.0),
328            OHyperdual::from(0.0),
329            OHyperdual::from(1.0),
330        )
331        .cross(&self.hvec());
332        let raan = (n[(0, 0)] / norm(&n)).acos();
333        if raan.is_nan() {
334            warn!("RAAN is NaN");
335            OrbitPartial {
336                dual: OHyperdual::from(0.0),
337                param: StateParameter::RAAN,
338            }
339        } else if n[(1, 0)] < 0.0 {
340            OrbitPartial {
341                dual: (OHyperdual::from(2.0 * PI) - raan).to_degrees(),
342                param: StateParameter::RAAN,
343            }
344        } else {
345            OrbitPartial {
346                dual: raan.to_degrees(),
347                param: StateParameter::RAAN,
348            }
349        }
350    }
351
352    /// Returns the true anomaly in degrees between 0 and 360.0
353    pub fn ta_deg(&self) -> PhysicsResult<OrbitPartial> {
354        if self.ecc()?.real() < ECC_EPSILON {
355            warn!(
356                "true anomaly ill-defined for circular orbit (e = {})",
357                self.ecc()?
358            );
359        }
360        let cos_nu = self.evec()?.dot(&self.radius()) / (self.ecc()?.dual * self.rmag_km().dual);
361        if (cos_nu.real().abs() - 1.0).abs() < f64::EPSILON {
362            // This bug drove me crazy when writing SMD in Go in 2017.
363            if cos_nu > 1.0 {
364                Ok(OrbitPartial {
365                    dual: OHyperdual::from(180.0),
366                    param: StateParameter::TrueAnomaly,
367                })
368            } else {
369                Ok(OrbitPartial {
370                    dual: OHyperdual::from(0.0),
371                    param: StateParameter::TrueAnomaly,
372                })
373            }
374        } else {
375            let ta = cos_nu.acos();
376            if ta.is_nan() {
377                warn!("TA is NaN");
378                Ok(OrbitPartial {
379                    dual: OHyperdual::from(0.0),
380                    param: StateParameter::TrueAnomaly,
381                })
382            } else if self.radius().dot(&self.velocity()) < 0.0 {
383                Ok(OrbitPartial {
384                    dual: (OHyperdual::from(2.0 * PI) - ta).to_degrees(),
385                    param: StateParameter::TrueAnomaly,
386                })
387            } else {
388                Ok(OrbitPartial {
389                    dual: ta.to_degrees(),
390                    param: StateParameter::TrueAnomaly,
391                })
392            }
393        }
394    }
395
396    /// Returns the true longitude in degrees
397    pub fn tlong_deg(&self) -> PhysicsResult<OrbitPartial> {
398        // Angles already in degrees
399        Ok(OrbitPartial {
400            dual: self.aop_deg()?.dual + self.raan_deg().dual + self.ta_deg()?.dual,
401            param: StateParameter::TrueLongitude,
402        })
403    }
404
405    /// Returns the argument of latitude in degrees
406    ///
407    /// NOTE: If the orbit is near circular, the AoL will be computed from the true longitude
408    /// instead of relying on the ill-defined true anomaly.
409    pub fn aol_deg(&self) -> PhysicsResult<OrbitPartial> {
410        if self.ecc()?.real() < ECC_EPSILON {
411            Ok(OrbitPartial {
412                dual: self.tlong_deg()?.dual - self.raan_deg().dual,
413                param: StateParameter::AoL,
414            })
415        } else {
416            Ok(OrbitPartial {
417                dual: self.aop_deg()?.dual + self.ta_deg()?.dual,
418                param: StateParameter::AoL,
419            })
420        }
421    }
422
423    /// Returns the radius of periapsis (or perigee around Earth), in kilometers.
424    pub fn periapsis_km(&self) -> PhysicsResult<OrbitPartial> {
425        Ok(OrbitPartial {
426            dual: self.sma_km()?.dual * (OHyperdual::from(1.0) - self.ecc()?.dual),
427            param: StateParameter::Periapsis,
428        })
429    }
430
431    /// Returns the radius of apoapsis (or apogee around Earth), in kilometers.
432    pub fn apoapsis_km(&self) -> PhysicsResult<OrbitPartial> {
433        Ok(OrbitPartial {
434            dual: self.sma_km()?.dual * (OHyperdual::from(1.0) + self.ecc()?.dual),
435            param: StateParameter::Apoapsis,
436        })
437    }
438
439    /// Returns the eccentric anomaly in degrees
440    ///
441    /// This is a conversion from GMAT's StateConversionUtil::TrueToEccentricAnomaly
442    pub fn ea_deg(&self) -> PhysicsResult<OrbitPartial> {
443        let (sin_ta, cos_ta) = self.ta_deg()?.dual.to_radians().sin_cos();
444        let ecc_cos_ta = self.ecc()?.dual * cos_ta;
445        let sin_ea = ((OHyperdual::from(1.0) - self.ecc()?.dual.powi(2)).sqrt() * sin_ta)
446            / (OHyperdual::from(1.0) + ecc_cos_ta);
447        let cos_ea = (self.ecc()?.dual + cos_ta) / (OHyperdual::from(1.0) + ecc_cos_ta);
448        // The atan2 function is a bit confusing: https://doc.rust-lang.org/std/primitive.f64.html#method.atan2 .
449        Ok(OrbitPartial {
450            dual: sin_ea.atan2(cos_ea).to_degrees(),
451            param: StateParameter::EccentricAnomaly,
452        })
453    }
454
455    /// Returns the flight path angle in degrees
456    pub fn fpa_deg(&self) -> PhysicsResult<OrbitPartial> {
457        let nu = self.ta_deg()?.dual.to_radians();
458        let ecc = self.ecc()?.dual;
459        let denom =
460            (OHyperdual::from(1.0) + OHyperdual::from(2.0) * ecc * nu.cos() + ecc.powi(2)).sqrt();
461        let sin_fpa = ecc * nu.sin() / denom;
462        let cos_fpa = OHyperdual::from(1.0) + ecc * nu.cos() / denom;
463        Ok(OrbitPartial {
464            dual: sin_fpa.atan2(cos_fpa).to_degrees(),
465            param: StateParameter::FlightPathAngle,
466        })
467    }
468
469    /// Returns the mean anomaly in degrees
470    pub fn ma_deg(&self) -> PhysicsResult<OrbitPartial> {
471        if self.ecc()?.real() < 1.0 {
472            Ok(OrbitPartial {
473                dual: (self.ea_deg()?.dual.to_radians()
474                    - self.ecc()?.dual * self.ea_deg()?.dual.to_radians().sin())
475                .to_degrees(),
476                param: StateParameter::MeanAnomaly,
477            })
478        } else if self.ecc()?.real() > 1.0 {
479            debug!("computing the hyperbolic anomaly");
480            // From GMAT's TrueToHyperbolicAnomaly
481            Ok(OrbitPartial {
482                dual: ((self.ta_deg()?.dual.to_radians().sin()
483                    * (self.ecc()?.dual.powi(2) - OHyperdual::from(1.0)))
484                .sqrt()
485                    / (OHyperdual::from(1.0)
486                        + self.ecc()?.dual * self.ta_deg()?.dual.to_radians().cos()))
487                .asinh()
488                .to_degrees(),
489                param: StateParameter::MeanAnomaly,
490            })
491        } else {
492            error!("parabolic orbit: setting mean anomaly to 0.0");
493            Ok(OrbitPartial {
494                dual: OHyperdual::from(0.0),
495                param: StateParameter::MeanAnomaly,
496            })
497        }
498    }
499
500    /// Returns the semi parameter (or semilatus rectum)
501    pub fn semi_parameter_km(&self) -> PhysicsResult<OrbitPartial> {
502        Ok(OrbitPartial {
503            dual: self.sma_km()?.dual * (OHyperdual::from(1.0) - self.ecc()?.dual.powi(2)),
504            param: StateParameter::SemiParameter,
505        })
506    }
507
508    /// Returns the geodetic longitude (λ) in degrees. Value is between 0 and 360 degrees.
509    ///
510    /// Although the reference is not Vallado, the math from Vallado proves to be equivalent.
511    /// Reference: G. Xu and Y. Xu, "GPS", DOI 10.1007/978-3-662-50367-6_2, 2016
512    pub fn longitude_deg(&self) -> OrbitPartial {
513        OrbitPartial {
514            dual: self.y.atan2(self.x).to_degrees(),
515            param: StateParameter::Longitude,
516        }
517    }
518
519    /// Returns the geodetic latitude (φ) in degrees. Value is between -180 and +180 degrees.
520    ///
521    /// Reference: Vallado, 4th Ed., Algorithm 12 page 172.
522    pub fn latitude_deg(&self) -> PhysicsResult<OrbitPartial> {
523        let flattening = self.frame.flattening()?;
524        let eps = 1e-12;
525        let max_attempts = 20;
526        let mut attempt_no = 0;
527        let r_delta = (self.x.powi(2) + self.y.powi(2)).sqrt();
528        let mut latitude = (self.z / self.rmag_km().dual).asin();
529        let e2 = OHyperdual::from(flattening * (2.0 - flattening));
530        loop {
531            attempt_no += 1;
532            let c_earth = OHyperdual::from(self.frame.semi_major_radius_km()?)
533                / ((OHyperdual::from(1.0) - e2 * (latitude).sin().powi(2)).sqrt());
534            let new_latitude = (self.z + c_earth * e2 * (latitude).sin()).atan2(r_delta);
535            if (latitude - new_latitude).abs() < eps {
536                return Ok(OrbitPartial {
537                    dual: new_latitude.to_degrees(),
538                    param: StateParameter::Latitude,
539                });
540            } else if attempt_no >= max_attempts {
541                error!(
542                    "geodetic latitude failed to converge -- error = {}",
543                    (latitude - new_latitude).abs()
544                );
545                return Ok(OrbitPartial {
546                    dual: new_latitude.to_degrees(),
547                    param: StateParameter::Latitude,
548                });
549            }
550            latitude = new_latitude;
551        }
552    }
553
554    /// Returns the geodetic height in km.
555    ///
556    /// Reference: Vallado, 4th Ed., Algorithm 12 page 172.
557    pub fn height_km(&self) -> PhysicsResult<OrbitPartial> {
558        let flattening = self.frame.flattening()?;
559        let semi_major_radius = self.frame.semi_major_radius_km()?;
560        let e2 = OHyperdual::from(flattening * (2.0 - flattening));
561        let latitude = self.latitude_deg()?.dual.to_radians();
562        let sin_lat = latitude.sin();
563        if (latitude - OHyperdual::from(1.0)).abs() < 0.1 {
564            // We are near poles, let's use another formulation.
565            let s_earth0: f64 = semi_major_radius * (1.0 - flattening).powi(2);
566            let s_earth = OHyperdual::from(s_earth0)
567                / ((OHyperdual::from(1.0) - e2 * sin_lat.powi(2)).sqrt());
568            Ok(OrbitPartial {
569                dual: self.z / latitude.sin() - s_earth,
570                param: StateParameter::Height,
571            })
572        } else {
573            let c_earth = OHyperdual::from(semi_major_radius)
574                / ((OHyperdual::from(1.0) - e2 * sin_lat.powi(2)).sqrt());
575            let r_delta = (self.x.powi(2) + self.y.powi(2)).sqrt();
576            Ok(OrbitPartial {
577                dual: r_delta / latitude.cos() - c_earth,
578                param: StateParameter::Height,
579            })
580        }
581    }
582
583    /// Returns the right ascension of this orbit in degrees
584    #[allow(clippy::eq_op)]
585    pub fn right_ascension_deg(&self) -> OrbitPartial {
586        OrbitPartial {
587            dual: (self.y.atan2(self.x)).to_degrees(),
588            param: StateParameter::RightAscension,
589        }
590    }
591
592    /// Returns the declination of this orbit in degrees
593    #[allow(clippy::eq_op)]
594    pub fn declination_deg(&self) -> OrbitPartial {
595        OrbitPartial {
596            dual: (self.z / self.rmag_km().dual).asin().to_degrees(),
597            param: StateParameter::Declination,
598        }
599    }
600
601    /// Returns the semi minor axis in km, includes code for a hyperbolic orbit
602    pub fn semi_minor_axis_km(&self) -> PhysicsResult<OrbitPartial> {
603        if self.ecc()?.real() <= 1.0 {
604            Ok(OrbitPartial {
605                dual: ((self.sma_km()?.dual * self.ecc()?.dual).powi(2)
606                    - self.sma_km()?.dual.powi(2))
607                .sqrt(),
608                param: StateParameter::SemiMinorAxis,
609            })
610        } else {
611            Ok(OrbitPartial {
612                dual: self.hmag().dual.powi(2)
613                    / (OHyperdual::from(self.frame.mu_km3_s2()?)
614                        * (self.ecc()?.dual.powi(2) - OHyperdual::from(1.0)).sqrt()),
615                param: StateParameter::SemiMinorAxis,
616            })
617        }
618    }
619
620    /// Returns the velocity declination of this orbit in degrees
621    pub fn velocity_declination(&self) -> OrbitPartial {
622        OrbitPartial {
623            dual: (self.vz / self.vmag_km_s().dual).asin().to_degrees(),
624            param: StateParameter::VelocityDeclination,
625        }
626    }
627
628    /// Returns the $C_3$ of this orbit
629    pub fn c3(&self) -> PhysicsResult<OrbitPartial> {
630        Ok(OrbitPartial {
631            dual: -OHyperdual::from(self.frame.mu_km3_s2()?) / self.sma_km()?.dual,
632            param: StateParameter::C3,
633        })
634    }
635
636    /// Returns the hyperbolic anomaly in degrees between 0 and 360.0
637    pub fn hyperbolic_anomaly_deg(&self) -> Result<OrbitPartial, AstroError> {
638        let ecc = self.ecc().context(AstroPhysicsSnafu)?;
639        if ecc.real() <= 1.0 {
640            Err(AstroError::NotHyperbolic)
641        } else {
642            let (sin_ta, cos_ta) = self
643                .ta_deg()
644                .context(AstroPhysicsSnafu)?
645                .dual
646                .to_radians()
647                .sin_cos();
648            let sinh_h = (sin_ta * (ecc.dual.powi(2) - OHyperdual::from(1.0)).sqrt())
649                / (OHyperdual::from(1.0) + ecc.dual * cos_ta);
650            Ok(OrbitPartial {
651                dual: sinh_h.asinh().to_degrees(),
652                param: StateParameter::HyperbolicAnomaly,
653            })
654        }
655    }
656}
657
658impl TimeTagged for OrbitDual {
659    fn epoch(&self) -> Epoch {
660        self.dt
661    }
662
663    fn set_epoch(&mut self, epoch: Epoch) {
664        self.dt = epoch
665    }
666}
nyx_space/cosmic/orbitdual.rs

nyx_space/cosmic/
orbitdual.rs
