hpr_sim/
state.rs

1//! The flight state: the body origin's position and velocity, the attitude and the body rates.
2
3use hpr_core::{DQuat, DVec3};
4use serde::{Deserialize, Serialize};
5
6/// The number of components in a [`State`] array.
7pub const STATE_LEN: usize = 13;
8
9/// A rigid body's state in the launch frame `L` (`docs/physics/frames.md`).
10///
11/// The reference point is the body origin, the nose tip (the decision record on the design tree,
12/// [ADR-007][adr-007]), which is fixed in the body; the center of mass moves relative to it as
13/// propellant burns. As an array the order is position, velocity, attitude `(w, x, y, z)` and body
14/// rates.
15///
16/// [adr-007]: https://github.com/nrdptel/hpr-sim/blob/main/docs/DECISIONS.md#adr-007-design-tree-stations-placement-automatic-radii-overrides-motors-and-checks-2026-09-17
17#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
18pub struct State {
19    /// The nose tip's position in `L`, m.
20    pub position_enu_m: DVec3,
21    /// The nose tip's velocity relative to `L`, m/s.
22    pub velocity_enu_m_s: DVec3,
23    /// The attitude, body to launch frame. The integrated quaternion's norm drifts slightly; every
24    /// use normalizes it.
25    pub attitude: DQuat,
26    /// The body's angular velocity relative to `L`, in body axes, rad/s.
27    pub body_rate_rad_s: DVec3,
28}
29
30impl State {
31    /// The state as the integrator's array.
32    #[must_use]
33    pub fn to_array(&self) -> [f64; STATE_LEN] {
34        let (p, v, q, w) = (
35            self.position_enu_m,
36            self.velocity_enu_m_s,
37            self.attitude,
38            self.body_rate_rad_s,
39        );
40        [
41            p.x, p.y, p.z, v.x, v.y, v.z, q.w, q.x, q.y, q.z, w.x, w.y, w.z,
42        ]
43    }
44
45    /// The state from the integrator's array.
46    #[must_use]
47    pub fn from_array(y: &[f64; STATE_LEN]) -> Self {
48        Self {
49            position_enu_m: DVec3::new(y[0], y[1], y[2]),
50            velocity_enu_m_s: DVec3::new(y[3], y[4], y[5]),
51            attitude: DQuat::from_xyzw(y[7], y[8], y[9], y[6]),
52            body_rate_rad_s: DVec3::new(y[10], y[11], y[12]),
53        }
54    }
55
56    /// The attitude scaled to unit length.
57    #[must_use]
58    pub fn unit_attitude(&self) -> DQuat {
59        self.attitude.normalize()
60    }
61
62    /// A point given in body axes (m from the nose tip), in `L`.
63    #[must_use]
64    pub fn point_enu_m(&self, body_m: DVec3) -> DVec3 {
65        self.position_enu_m + self.unit_attitude().mul_vec3(body_m)
66    }
67}
68
69#[cfg(test)]
70mod tests {
71    use super::*;
72
73    #[test]
74    fn arrays_round_trip() {
75        let state = State {
76            position_enu_m: DVec3::new(1.0, 2.0, 3.0),
77            velocity_enu_m_s: DVec3::new(4.0, 5.0, 6.0),
78            attitude: DQuat::from_xyzw(0.1, 0.2, 0.3, 0.9),
79            body_rate_rad_s: DVec3::new(7.0, 8.0, 9.0),
80        };
81        assert_eq!(State::from_array(&state.to_array()), state);
82        assert_eq!(state.to_array()[6], 0.9);
83    }
84}
hpr_sim/state.rs

hpr_sim/
state.rs
