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

nyx_space/cosmic/
orbitdual.rs
