hpr_core/
frames.rs

1//! The launch frame, the body frame and launch attitude angles.
2//!
3//! `docs/physics/frames.md` is the specification; this module implements it.
4//!
5//! - **Launch frame `L`:** East-North-Up at the launch site. Origin at the pad on the ground;
6//!   `x_L` east, `y_L` north, `z_L` up along the ellipsoid normal. See [`LaunchFrame`].
7//! - **Body frame `B`:** `z_B` along the rocket's axis of symmetry, positive toward the nose;
8//!   `x_B` along the design's reference radial direction; `y_B = z_B × x_B`.
9//! - **Attitude:** the unit quaternion `q` with `v_L = q ⊗ v_B ⊗ q*` ([`crate::attitude`]).
10//! - **Launch angles:** azimuth `A` (clockwise from true north), elevation `E` (above the
11//!   horizon) and roll `φ` (about `z_B`), with `q = R_z(−A) R_x(E − π/2) R_z(φ)`. See
12//!   [`LaunchAngles`].
13
14use std::f64::consts::{FRAC_PI_2, TAU};
15
16use glam::{DMat3, DQuat, DVec3};
17use serde::{Deserialize, Serialize};
18
19use crate::error::CoreError;
20use crate::geodesy::{Ellipsoid, Geodetic, ecef_from_enu_rotation};
21
22/// Below this horizontal length of the body axis (in the launch frame, for a unit axis) the
23/// azimuth is undefined: the axis is within about 1e-12 rad of vertical.
24const VERTICAL_TOLERANCE: f64 = 1e-12;
25
26/// An East-North-Up frame tied to a point on an ellipsoid: the launch frame `L`.
27#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
28#[serde(try_from = "LaunchFrameData", into = "LaunchFrameData")]
29pub struct LaunchFrame {
30    ellipsoid: Ellipsoid,
31    origin: Geodetic,
32    origin_ecef_m: DVec3,
33    ecef_from_enu: DMat3,
34}
35
36#[derive(Serialize, Deserialize)]
37#[serde(deny_unknown_fields)]
38struct LaunchFrameData {
39    ellipsoid: Ellipsoid,
40    origin: Geodetic,
41}
42
43impl TryFrom<LaunchFrameData> for LaunchFrame {
44    type Error = CoreError;
45
46    fn try_from(data: LaunchFrameData) -> Result<Self, CoreError> {
47        LaunchFrame::new(data.ellipsoid, data.origin)
48    }
49}
50
51impl From<LaunchFrame> for LaunchFrameData {
52    fn from(frame: LaunchFrame) -> Self {
53        LaunchFrameData {
54            ellipsoid: frame.ellipsoid,
55            origin: frame.origin,
56        }
57    }
58}
59
60impl LaunchFrame {
61    /// The ENU frame with its origin at `origin` on `ellipsoid`.
62    ///
63    /// # Errors
64    ///
65    /// [`CoreError::Domain`] if `origin` fails [`Geodetic::new`]'s checks (for example degrees
66    /// written into the radian fields).
67    pub fn new(ellipsoid: Ellipsoid, origin: Geodetic) -> Result<Self, CoreError> {
68        let origin = origin.validated()?;
69        Ok(Self {
70            ellipsoid,
71            origin,
72            origin_ecef_m: ellipsoid.ecef_from_geodetic(origin),
73            ecef_from_enu: ecef_from_enu_rotation(origin),
74        })
75    }
76
77    /// The ENU frame at `origin` on the WGS 84 ellipsoid.
78    ///
79    /// # Errors
80    ///
81    /// As [`LaunchFrame::new`].
82    pub fn wgs84(origin: Geodetic) -> Result<Self, CoreError> {
83        Self::new(Ellipsoid::WGS84, origin)
84    }
85
86    /// The ellipsoid the frame is tied to.
87    #[must_use]
88    pub fn ellipsoid(&self) -> Ellipsoid {
89        self.ellipsoid
90    }
91
92    /// The origin's geodetic position.
93    #[must_use]
94    pub fn origin(&self) -> Geodetic {
95        self.origin
96    }
97
98    /// The origin's ECEF position, m.
99    #[must_use]
100    pub fn origin_ecef_m(&self) -> DVec3 {
101        self.origin_ecef_m
102    }
103
104    /// The rotation mapping launch-frame components to ECEF components; its columns are `ê`,
105    /// `n̂` and `û` at the origin.
106    #[must_use]
107    pub fn ecef_from_enu_rotation(&self) -> DMat3 {
108        self.ecef_from_enu
109    }
110
111    /// ECEF position of a point given in the launch frame: `r_ECEF = r_0 + R r_L`.
112    #[must_use]
113    pub fn ecef_from_enu(&self, position_enu_m: DVec3) -> DVec3 {
114        self.origin_ecef_m + self.ecef_from_enu * position_enu_m
115    }
116
117    /// Launch-frame position of a point given in ECEF: `r_L = Rᵀ (r_ECEF − r_0)`.
118    #[must_use]
119    pub fn enu_from_ecef(&self, position_ecef_m: DVec3) -> DVec3 {
120        self.ecef_from_enu.transpose() * (position_ecef_m - self.origin_ecef_m)
121    }
122
123    /// Geodetic position of a point given in the launch frame.
124    ///
125    /// # Errors
126    ///
127    /// As [`Ellipsoid::geodetic_from_ecef`]: only for non-finite input or points deep inside the
128    /// Earth.
129    pub fn geodetic_from_enu(&self, position_enu_m: DVec3) -> Result<Geodetic, CoreError> {
130        self.ellipsoid
131            .geodetic_from_ecef(self.ecef_from_enu(position_enu_m))
132    }
133
134    /// Launch-frame position of a geodetic point.
135    #[must_use]
136    pub fn enu_from_geodetic(&self, point: Geodetic) -> DVec3 {
137        self.enu_from_ecef(self.ellipsoid.ecef_from_geodetic(point))
138    }
139
140    /// The Earth's rotation vector `Ω = ω (0, cos φ₀, sin φ₀)` resolved in the launch frame,
141    /// rad/s, for a rotation rate `omega_rad_s` (WGS 84: 7.292115e-5 rad/s).
142    #[must_use]
143    pub fn earth_rotation_enu_rad_s(&self, omega_rad_s: f64) -> DVec3 {
144        let (sin_lat, cos_lat) = self.origin.latitude_rad.sin_cos();
145        DVec3::new(0.0, omega_rad_s * cos_lat, omega_rad_s * sin_lat)
146    }
147}
148
149/// The attitude of the body axis relative to the launch frame, as launch-rail style angles.
150///
151/// `q = R_z(−A) ⊗ R_x(E − π/2) ⊗ R_z(φ)`, so the body axis `z_B` points along
152/// `(sin A cos E, cos A cos E, sin E)` in the launch frame. At `E = ±90°` azimuth and roll turn
153/// about the same axis; [`LaunchAngles::from_quaternion`] then reports `A = 0` and puts the whole
154/// turn into `φ`.
155#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
156pub struct LaunchAngles {
157    /// Azimuth `A` of the body axis, rad, clockwise from true north (`π/2` points east).
158    pub azimuth_rad: f64,
159    /// Elevation `E` of the body axis above the horizon, rad (`π/2` is vertical).
160    pub elevation_rad: f64,
161    /// Roll `φ` about the body axis, rad, right-handed about `z_B`.
162    pub roll_rad: f64,
163}
164
165impl LaunchAngles {
166    /// The attitude quaternion `q = R_z(−A) ⊗ R_x(E − π/2) ⊗ R_z(φ)`.
167    #[must_use]
168    pub fn to_quaternion(self) -> DQuat {
169        DQuat::from_rotation_z(-self.azimuth_rad)
170            * DQuat::from_rotation_x(self.elevation_rad - FRAC_PI_2)
171            * DQuat::from_rotation_z(self.roll_rad)
172    }
173
174    /// The launch angles of a unit attitude quaternion, with `A ∈ [0, 2π)`, `E ∈ [−π/2, π/2]`
175    /// and `φ ∈ (−π, π]`.
176    #[must_use]
177    pub fn from_quaternion(q: DQuat) -> Self {
178        let axis = q.mul_vec3(DVec3::Z);
179        let horizontal = axis.x.hypot(axis.y);
180        let elevation_rad = axis.z.atan2(horizontal);
181        let azimuth_rad = if horizontal < VERTICAL_TOLERANCE {
182            0.0
183        } else {
184            // `rem_euclid` can round a tiny negative angle up to exactly 2π.
185            let a = axis.x.atan2(axis.y).rem_euclid(TAU);
186            if a < TAU { a } else { 0.0 }
187        };
188        // The zero-roll radial axes for this azimuth and elevation, and the body's actual x axis.
189        let reference = DQuat::from_rotation_z(-azimuth_rad)
190            * DQuat::from_rotation_x(elevation_rad - FRAC_PI_2);
191        let x0 = reference.mul_vec3(DVec3::X);
192        let y0 = reference.mul_vec3(DVec3::Y);
193        let x_body = q.mul_vec3(DVec3::X);
194        // atan2 returns −π for (−0, negative); the documented range is (−π, π].
195        let roll = x_body.dot(y0).atan2(x_body.dot(x0));
196        let roll_rad = if roll <= -std::f64::consts::PI {
197            std::f64::consts::PI
198        } else {
199            roll
200        };
201        Self {
202            azimuth_rad,
203            elevation_rad,
204            roll_rad,
205        }
206    }
207}
208
209#[cfg(test)]
210mod tests {
211    use std::f64::consts::{FRAC_PI_2, PI};
212
213    use proptest::prelude::*;
214
215    use super::*;
216
217    /// The rotation angle between two attitudes, rad (`q` and `−q` are the same attitude).
218    fn angle_between(a: DQuat, b: DQuat) -> f64 {
219        let d = a.inverse() * b;
220        2.0 * d.xyz().length().atan2(d.w.abs())
221    }
222
223    fn unit_quaternion() -> impl Strategy<Value = DQuat> {
224        prop::array::uniform4(-1.0..1.0f64)
225            .prop_filter("non-degenerate", |c| {
226                c.iter().map(|x| x * x).sum::<f64>() > 1e-6
227            })
228            .prop_map(|c| DQuat::from_array(c).normalize())
229    }
230
231    #[test]
232    fn named_attitudes() {
233        let vertical = LaunchAngles {
234            azimuth_rad: 0.0,
235            elevation_rad: FRAC_PI_2,
236            roll_rad: 0.0,
237        };
238        assert!(angle_between(vertical.to_quaternion(), DQuat::IDENTITY) < 1e-15);
239
240        // Horizontal, pointing east: the nose axis is +x_L; zero roll keeps x_B horizontal and to
241        // the right of the heading (south), so y_B = z_B × x_B points down.
242        let east = LaunchAngles {
243            azimuth_rad: FRAC_PI_2,
244            elevation_rad: 0.0,
245            roll_rad: 0.0,
246        }
247        .to_quaternion();
248        assert!((east.mul_vec3(DVec3::Z) - DVec3::X).length() < 1e-15);
249        assert!((east.mul_vec3(DVec3::X) - DVec3::NEG_Y).length() < 1e-15);
250        assert!((east.mul_vec3(DVec3::Y) - DVec3::NEG_Z).length() < 1e-15);
251
252        // A rail tilted 5° off vertical toward the north-east.
253        let rail = LaunchAngles {
254            azimuth_rad: 45f64.to_radians(),
255            elevation_rad: 85f64.to_radians(),
256            roll_rad: 0.3,
257        }
258        .to_quaternion();
259        let axis = rail.mul_vec3(DVec3::Z);
260        let (s5, c5) = 5f64.to_radians().sin_cos();
261        let expected = DVec3::new(s5 * 0.5f64.sqrt(), s5 * 0.5f64.sqrt(), c5);
262        assert!((axis - expected).length() < 1e-14);
263    }
264
265    #[test]
266    fn vertical_attitudes_report_zero_azimuth_and_the_whole_turn_as_roll() {
267        let q = DQuat::from_rotation_z(-1.1);
268        let angles = LaunchAngles::from_quaternion(q);
269        assert_eq!(angles.azimuth_rad, 0.0);
270        assert!((angles.elevation_rad - FRAC_PI_2).abs() < 1e-15);
271        assert!((angles.roll_rad + 1.1).abs() < 1e-15);
272
273        // Nose straight down by a half turn about x_L: exactly the zero-roll reference.
274        let down = LaunchAngles::from_quaternion(DQuat::from_xyzw(1.0, 0.0, 0.0, 0.0));
275        assert_eq!(down.azimuth_rad, 0.0);
276        assert!((down.elevation_rad + FRAC_PI_2).abs() < 1e-15);
277        assert_eq!(down.roll_rad, 0.0);
278        // By a half turn about y_L, x_B ends up at −x_L and atan2 meets (−0, −1): roll lands on
279        // +π, not −π.
280        for sign in [1.0, -1.0] {
281            let down = LaunchAngles::from_quaternion(DQuat::from_xyzw(0.0, sign, 0.0, 0.0));
282            assert!((down.elevation_rad + FRAC_PI_2).abs() < 1e-15);
283            assert_eq!(down.roll_rad, PI);
284        }
285    }
286
287    /// ADR-003 adopts RocketPy's launch attitude convention: heading = azimuth, inclination =
288    /// elevation, and roll = the rail-button angular position for a `tail_to_nose` rocket or
289    /// 2π minus it for `nose_to_tail`. The oracle builds real RocketPy flights and reads the
290    /// initial Euler parameters (`validation/oracles/rocketpy/attitude.py`).
291    #[test]
292    fn launch_angles_match_the_rocketpy_oracle() {
293        #[derive(serde::Deserialize)]
294        struct Oracle {
295            cases: Vec<Case>,
296        }
297        #[derive(serde::Deserialize)]
298        struct Case {
299            heading_deg: f64,
300            inclination_deg: f64,
301            rail_button_angle_deg: f64,
302            coordinate_system_orientation: String,
303            e0: f64,
304            e1: f64,
305            e2: f64,
306            e3: f64,
307        }
308        let oracle: Oracle = serde_json::from_str(include_str!(
309            "../../../validation/fixtures/earth/rocketpy-attitude.json"
310        ))
311        .unwrap();
312        assert!(oracle.cases.len() >= 6);
313        let mut orientations = std::collections::BTreeSet::new();
314        for case in &oracle.cases {
315            let button = case.rail_button_angle_deg.to_radians();
316            let roll_rad = match case.coordinate_system_orientation.as_str() {
317                "tail_to_nose" => button,
318                "nose_to_tail" => 2.0 * PI - button,
319                other => panic!("unknown orientation {other}"),
320            };
321            orientations.insert(case.coordinate_system_orientation.as_str());
322            let q = LaunchAngles {
323                azimuth_rad: case.heading_deg.to_radians(),
324                elevation_rad: case.inclination_deg.to_radians(),
325                roll_rad,
326            }
327            .to_quaternion();
328            let rocketpy = DQuat::from_xyzw(case.e1, case.e2, case.e3, case.e0);
329            assert!(
330                angle_between(q, rocketpy) < 1e-12,
331                "heading {} inclination {} buttons {}: {q} vs {rocketpy}",
332                case.heading_deg,
333                case.inclination_deg,
334                case.rail_button_angle_deg
335            );
336        }
337        assert_eq!(
338            orientations.len(),
339            2,
340            "both rocket orientations are covered"
341        );
342    }
343
344    #[test]
345    fn launch_frame_rejects_unchecked_sites() {
346        let degrees = Geodetic {
347            latitude_rad: 32.99,
348            longitude_rad: -106.97,
349            height_m: 1400.0,
350        };
351        assert!(LaunchFrame::wgs84(degrees).is_err());
352        let json = r#"{"ellipsoid": {"semi_major_axis_m": 6378137.0, "flattening": 0.0033528106647474805},
353            "origin": {"latitude_rad": 1.0, "longitude_rad": 0.0, "height_m": "NaN"}}"#;
354        assert!(serde_json::from_str::<LaunchFrame>(json).is_err());
355    }
356
357    #[test]
358    fn launch_frame_axes_and_origin() {
359        let site = Geodetic::from_degrees(32.99, -106.97, 1400.0).unwrap();
360        let frame = LaunchFrame::wgs84(site).unwrap();
361        assert_eq!(frame.enu_from_ecef(frame.origin_ecef_m()), DVec3::ZERO);
362        let back = frame.geodetic_from_enu(DVec3::ZERO).unwrap();
363        assert!((back.latitude_rad - site.latitude_rad).abs() < 1e-15);
364        assert!((back.height_m - site.height_m).abs() < 1e-8);
365        // 1 km straight up is 1 km higher on the ellipsoid, same latitude and longitude.
366        let up = frame
367            .geodetic_from_enu(DVec3::new(0.0, 0.0, 1000.0))
368            .unwrap();
369        assert!((up.height_m - 2400.0).abs() < 1e-8);
370        assert!((up.latitude_rad - site.latitude_rad).abs() < 1e-15);
371        assert!((up.longitude_rad - site.longitude_rad).abs() < 1e-15);
372        // Moving north raises latitude; moving east raises longitude.
373        let north = frame
374            .geodetic_from_enu(DVec3::new(0.0, 1000.0, 0.0))
375            .unwrap();
376        assert!(north.latitude_rad > site.latitude_rad);
377        let east = frame
378            .geodetic_from_enu(DVec3::new(1000.0, 0.0, 0.0))
379            .unwrap();
380        assert!(east.longitude_rad > site.longitude_rad);
381        // A tangent-plane point 10 km away sits above the curving ellipsoid by about d²/(2N).
382        let n = frame.ellipsoid().prime_vertical_radius_m(site.latitude_rad);
383        let far = frame
384            .geodetic_from_enu(DVec3::new(1.0e4, 0.0, 0.0))
385            .unwrap();
386        let drop = far.height_m - site.height_m;
387        assert!((drop - 1.0e8 / (2.0 * (n + 1400.0))).abs() < 1e-3, "{drop}");
388
389        let json = serde_json::to_string(&frame).unwrap();
390        assert_eq!(serde_json::from_str::<LaunchFrame>(&json).unwrap(), frame);
391    }
392
393    proptest! {
394        /// Angles → quaternion → angles, away from the vertical singularity.
395        #[test]
396        fn launch_angles_round_trip(
397            azimuth in 0.0..2.0 * PI,
398            elevation in -FRAC_PI_2 + 1e-3..FRAC_PI_2 - 1e-3,
399            roll in -PI + 1e-9..PI,
400        ) {
401            let angles = LaunchAngles { azimuth_rad: azimuth, elevation_rad: elevation, roll_rad: roll };
402            let back = LaunchAngles::from_quaternion(angles.to_quaternion());
403            let wrap = |x: f64| (x + PI).rem_euclid(2.0 * PI) - PI;
404            prop_assert!(wrap(back.azimuth_rad - azimuth).abs() < 1e-11, "azimuth {} vs {}", back.azimuth_rad, azimuth);
405            prop_assert!((back.elevation_rad - elevation).abs() < 1e-11, "elevation {} vs {}", back.elevation_rad, elevation);
406            prop_assert!(wrap(back.roll_rad - roll).abs() < 1e-11, "roll {} vs {}", back.roll_rad, roll);
407            prop_assert!((0.0..2.0 * PI).contains(&back.azimuth_rad));
408        }
409
410        /// Quaternion → angles → quaternion is the same rotation everywhere, the vertical
411        /// singularity included, and the angles land in their documented ranges.
412        #[test]
413        fn quaternion_round_trips_through_launch_angles(q in unit_quaternion(), case in 0u8..3) {
414            // A third of the cases point exactly up and a third exactly down.
415            let turn = DQuat::from_rotation_z(2.0 * q.w.atan2(q.z));
416            let q = match case {
417                0 => q,
418                1 => turn,
419                _ => turn * DQuat::from_rotation_x(PI),
420            };
421            let angles = LaunchAngles::from_quaternion(q);
422            prop_assert!(angle_between(angles.to_quaternion(), q) < 1e-12);
423            prop_assert!((0.0..2.0 * PI).contains(&angles.azimuth_rad));
424            prop_assert!(angles.elevation_rad.abs() <= FRAC_PI_2);
425            prop_assert!(angles.roll_rad > -PI && angles.roll_rad <= PI);
426            // The body axis agrees with the azimuth and elevation formula.
427            let (sa, ca) = angles.azimuth_rad.sin_cos();
428            let (se, ce) = angles.elevation_rad.sin_cos();
429            prop_assert!((q.mul_vec3(DVec3::Z) - DVec3::new(sa * ce, ca * ce, se)).length() < 1e-12);
430        }
431
432        /// ENU ↔ ECEF and ENU ↔ geodetic round trips for points within 2000 km of the site and up
433        /// to 1000 km high, at any site.
434        #[test]
435        fn launch_frame_round_trips(
436            lat in -FRAC_PI_2..=FRAC_PI_2,
437            lon in -PI..PI,
438            h0 in -500.0..5000.0f64,
439            offset in prop::array::uniform3(-2.0e6..2.0e6f64),
440            up in 0.0..1.0e6f64,
441        ) {
442            let frame = LaunchFrame::wgs84(Geodetic::new(lat, lon, h0).unwrap()).unwrap();
443            let p = DVec3::new(offset[0], offset[1], up);
444            let ecef = frame.ecef_from_enu(p);
445            prop_assert!((frame.enu_from_ecef(ecef) - p).length() < 1e-7);
446            let g = frame.geodetic_from_enu(p).unwrap();
447            prop_assert!((frame.enu_from_geodetic(g) - p).length() < 1e-7);
448            // The frame rotation is orthonormal and right-handed.
449            let r = frame.ecef_from_enu_rotation();
450            prop_assert!((r.transpose() * r).abs_diff_eq(DMat3::IDENTITY, 1e-15));
451            prop_assert!((r.determinant() - 1.0).abs() < 1e-15);
452        }
453    }
454}
hpr_core/frames.rs

hpr_core/
frames.rs
