Skip to main content

hpr_sim/
free_body.rs

1//! A free rigid body of fixed mass and inertia, with no design, motor or aerodynamics: the
2//! flight's gravity, Earth rotation, rigid-body equations and integrator on a body a test gives.
3//!
4//! It exists to fly published check cases whose bodies are not rockets, such as NASA's dragless
5//! sphere and tumbling brick (NASA/TM-2015-218675, cases 1 to 3 and 6 to 10), through the code a
6//! rocket flies on. Nothing here is a second model: the weight and the Coriolis force come from
7//! the [`Environment`]'s Earth, as [`crate::dynamics`] takes them, the air and the wind from the
8//! environment at the height a rocket's flight reads them at, the two accelerations from that
9//! module's free-flight equations, and the steps from [`crate::integrator`]. A test holds the
10//! derivative to the one a coasting rocket's flight evaluates at the same state.
11//!
12//! The caller may add loads of its own ([`FreeBodyLoads`]): a force `F` through the center of
13//! mass and a moment `M` about it, in body axes, formed from the state, the air there and the
14//! velocity through it ([`LoadInputs`]). A check case's constant drag or damping coefficient is
15//! such a load; the body has no aerodynamics of its own.
16//!
17//! With the reference point at the center of mass, a constant mass `m` and a constant inertia `I`
18//! about it, and no load but gravity `g`, the Coriolis force and the caller's, the equations of
19//! [`crate::dynamics`] reduce to Newton's and Euler's:
20//!
21//! ```text
22//! a  = g(x) − 2 Ω × v + q F / m
23//! ω̇ = I⁻¹ (M − ω × I ω)
24//! q̇ = ½ q ⊗ ω
25//! ```
26//!
27//! `x` and `v` are the center of mass's position and velocity in the launch frame `L`, `Ω` the
28//! Earth's rate, `q` the attitude (body to `L`) and `ω` the angular velocity relative to `L` in
29//! body axes. As in a rocket's flight, the Earth's rate enters the translation only: the
30//! rotational equations treat `L` as inertial (the decision record on rigid-body flight,
31//! [ADR-011][adr-011]). Source: the equations of motion on the page
32//! [Rigid-body flight][guide-rigid-body], which cites RocketPy's derivation.
33//!
34//! [adr-011]: https://github.com/nrdptel/fusionspace-eridanus/blob/main/docs/decisions/0011-rigid-body-flight-equations-of-motion.md
35//! [guide-rigid-body]: https://hpr.fusionspace.co/physics/flight.html
36
37use std::ops::ControlFlow;
38
39use hpr_atmos::AirState;
40use hpr_core::attitude::quaternion_derivative;
41use hpr_core::{DMat3, DVec3};
42use serde::{Deserialize, Serialize};
43
44use crate::dynamics::{MassState, free_motion};
45use crate::environment::Environment;
46use crate::error::SimError;
47use crate::integrator::{IntegrationError, Integrator, Method, OdeSystem, Step};
48use crate::state::{STATE_LEN, State};
49
50/// [`SimError::Domain`]'s `what` for a mass that is not finite and positive.
51pub const FREE_BODY_MASS_REFUSED: &str = "free body's mass (kg), which must be finite and positive";
52
53/// [`SimError::Domain`]'s `what` for an inertia tensor with an entry that is not finite; the
54/// value is the entry.
55pub const FREE_BODY_INERTIA_NOT_FINITE: &str = "free body's inertia entry (kg·m²)";
56
57/// [`SimError::Domain`]'s `what` for an inertia tensor that is not symmetric; the value is the
58/// largest difference between an entry and its mirror, kg·m².
59pub const FREE_BODY_INERTIA_NOT_SYMMETRIC: &str =
60    "free body's inertia asymmetry (kg·m²), which must be zero";
61
62/// [`SimError::Domain`]'s `what` for an inertia tensor that is not positive definite; the value
63/// is the leading principal minor that is not positive.
64pub const FREE_BODY_INERTIA_NOT_POSITIVE: &str =
65    "free body's inertia leading minor, which must be positive";
66
67/// [`SimError::Domain`]'s `what` for a sample time that is not finite, comes before the start or
68/// comes before the sample ahead of it; the value is the time, s.
69pub const FREE_BODY_TIME_REFUSED: &str =
70    "free body's sample time (s), which must be finite, not before the start and not descending";
71
72/// [`SimError::Domain`]'s `what` for a caller's load with a component that is not finite; the
73/// value is the component.
74pub const FREE_BODY_LOAD_NOT_FINITE: &str =
75    "free body's load (N in the force, N·m in the moment), which must be finite";
76
77/// What a free body's loads are formed from at one state, read as a rocket's flight reads them:
78/// the air and the wind at the center of mass's height above mean sea level.
79#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
80#[non_exhaustive]
81pub struct LoadInputs {
82    /// The state, its attitude normalized.
83    pub state: State,
84    /// The height above mean sea level, m: the ellipsoidal height less the environment's geoid
85    /// undulation.
86    pub height_msl_m: f64,
87    /// The air there.
88    pub air: AirState,
89    /// The wind there, in the launch frame's East-North-Up axes, m/s.
90    pub wind_enu_m_s: DVec3,
91    /// The velocity through the air in body axes, m/s: the velocity less the wind, turned into
92    /// the body.
93    pub air_velocity_body_m_s: DVec3,
94}
95
96/// A force through the center of mass and a moment about it, in body axes.
97#[derive(Debug, Clone, Copy, PartialEq, Default, Serialize, Deserialize)]
98pub struct Loads {
99    /// The force, N.
100    pub force_body_n: DVec3,
101    /// The moment about the center of mass, N·m.
102    pub moment_body_n_m: DVec3,
103}
104
105/// The loads a caller puts on a free body, formed at each state the integrator evaluates. Any
106/// `Fn(&LoadInputs) -> Result<Loads, SimError>` is one; [`NoLoads`] is none.
107pub trait FreeBodyLoads {
108    /// The loads at `inputs`.
109    ///
110    /// # Errors
111    ///
112    /// Whatever the caller's loads refuse; the flight stops with it.
113    fn loads(&self, inputs: &LoadInputs) -> Result<Loads, SimError>;
114}
115
116impl<F: Fn(&LoadInputs) -> Result<Loads, SimError>> FreeBodyLoads for F {
117    fn loads(&self, inputs: &LoadInputs) -> Result<Loads, SimError> {
118        self(inputs)
119    }
120}
121
122/// No load but gravity and the Coriolis force.
123#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
124pub struct NoLoads;
125
126impl FreeBodyLoads for NoLoads {
127    fn loads(&self, _inputs: &LoadInputs) -> Result<Loads, SimError> {
128        Ok(Loads::default())
129    }
130}
131
132/// A rigid body of fixed mass and inertia.
133///
134/// Its [`State`] is the center of mass's, where a rocket's is its nose tip's: the position and
135/// velocity are the center of mass's in the launch frame, the attitude takes the body's axes to
136/// the launch frame's, and the body rates are relative to the launch frame, in body axes. The
137/// body's axes are whatever the inertia is given in; no axis is a nose.
138///
139/// It serializes as its `mass_kg` and `inertia_kg_m2` and is checked again when read, so a body
140/// that [`FreeBody::new`] would refuse can't be read from a file either.
141#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
142#[serde(try_from = "FreeBodyData", into = "FreeBodyData")]
143pub struct FreeBody {
144    mass_kg: f64,
145    inertia_kg_m2: DMat3,
146}
147
148/// [`FreeBody`] as it is written: the two values [`FreeBody::new`] takes.
149#[derive(Serialize, Deserialize)]
150struct FreeBodyData {
151    mass_kg: f64,
152    inertia_kg_m2: DMat3,
153}
154
155impl TryFrom<FreeBodyData> for FreeBody {
156    type Error = SimError;
157
158    fn try_from(data: FreeBodyData) -> Result<Self, SimError> {
159        Self::new(data.mass_kg, data.inertia_kg_m2)
160    }
161}
162
163impl From<FreeBody> for FreeBodyData {
164    fn from(body: FreeBody) -> Self {
165        Self {
166            mass_kg: body.mass_kg,
167            inertia_kg_m2: body.inertia_kg_m2,
168        }
169    }
170}
171
172impl FreeBody {
173    /// A body of mass `mass_kg` and inertia tensor `inertia_kg_m2` about its center of mass, in
174    /// its own axes, kg·m².
175    ///
176    /// The tensor must be symmetric exactly, to the last bit. One computed by parts, such as a
177    /// rocket's inertia moved to its center of mass, is often off by rounding; the caller
178    /// averages it with its transpose first, `(I + Iᵀ)/2`, so that what is flown is what the
179    /// caller chose, not a repair made here.
180    ///
181    /// # Errors
182    ///
183    /// [`SimError::Domain`] naming what was refused: a mass that is not finite and positive
184    /// ([`FREE_BODY_MASS_REFUSED`]), or an inertia with an entry that is not finite
185    /// ([`FREE_BODY_INERTIA_NOT_FINITE`]), that is not symmetric
186    /// ([`FREE_BODY_INERTIA_NOT_SYMMETRIC`]) or that is not positive definite
187    /// ([`FREE_BODY_INERTIA_NOT_POSITIVE`]).
188    pub fn new(mass_kg: f64, inertia_kg_m2: DMat3) -> Result<Self, SimError> {
189        if !(mass_kg.is_finite() && mass_kg > 0.0) {
190            return Err(SimError::Domain {
191                what: FREE_BODY_MASS_REFUSED,
192                value: mass_kg,
193            });
194        }
195        let i = inertia_kg_m2;
196        if let Some(&value) = i.to_cols_array().iter().find(|entry| !entry.is_finite()) {
197            return Err(SimError::Domain {
198                what: FREE_BODY_INERTIA_NOT_FINITE,
199                value,
200            });
201        }
202        let asymmetry = (i.x_axis.y - i.y_axis.x)
203            .abs()
204            .max((i.x_axis.z - i.z_axis.x).abs())
205            .max((i.y_axis.z - i.z_axis.y).abs());
206        if asymmetry != 0.0 {
207            return Err(SimError::Domain {
208                what: FREE_BODY_INERTIA_NOT_SYMMETRIC,
209                value: asymmetry,
210            });
211        }
212        // Sylvester's criterion: a symmetric matrix is positive definite exactly when its three
213        // leading principal minors are positive.
214        let minors = [
215            i.x_axis.x,
216            i.x_axis.x * i.y_axis.y - i.x_axis.y * i.y_axis.x,
217            i.determinant(),
218        ];
219        if let Some(&value) = minors.iter().find(|minor| **minor <= 0.0) {
220            return Err(SimError::Domain {
221                what: FREE_BODY_INERTIA_NOT_POSITIVE,
222                value,
223            });
224        }
225        Ok(Self {
226            mass_kg,
227            inertia_kg_m2,
228        })
229    }
230
231    /// The mass, kg.
232    #[must_use]
233    pub fn mass_kg(&self) -> f64 {
234        self.mass_kg
235    }
236
237    /// The inertia tensor about the center of mass, in body axes, kg·m².
238    #[must_use]
239    pub fn inertia_kg_m2(&self) -> DMat3 {
240        self.inertia_kg_m2
241    }
242
243    /// The mass properties as the flight's equations take them: the center of mass at the
244    /// reference point, and nothing changing.
245    fn mass_state(&self) -> MassState {
246        MassState {
247            mass_kg: self.mass_kg,
248            mass_rate_kg_s: 0.0,
249            cg_m: DVec3::ZERO,
250            cg_rate_m_s: DVec3::ZERO,
251            cg_accel_m_s2: DVec3::ZERO,
252            inertia_cg: self.inertia_kg_m2,
253            inertia_o: self.inertia_kg_m2,
254            inertia_o_rate: DMat3::ZERO,
255            relative_momentum: DVec3::ZERO,
256            relative_momentum_rate: DVec3::ZERO,
257        }
258    }
259
260    /// The state's time derivative in `environment` under the caller's `loads`, in
261    /// [`State::to_array`]'s order: the weight and the Coriolis force at the center of mass, as a
262    /// rocket's flight takes them, and the caller's force and moment, through the flight's
263    /// free-motion equations.
264    ///
265    /// # Errors
266    ///
267    /// [`SimError::Domain`] for an attitude quaternion whose norm is zero or not finite, or with
268    /// [`FREE_BODY_LOAD_NOT_FINITE`] for a load that is not finite; [`SimError::Core`] if the
269    /// Earth model refuses the position; whatever the environment refuses of the air or the wind
270    /// there; and whatever `loads` refuses.
271    pub fn derivative(
272        &self,
273        environment: &Environment,
274        loads: &dyn FreeBodyLoads,
275        y: &[f64; STATE_LEN],
276    ) -> Result<[f64; STATE_LEN], SimError> {
277        let state = State::from_array(y);
278        let norm = state.attitude.length();
279        if !(norm.is_finite() && norm > 0.0) {
280            return Err(SimError::Domain {
281                what: "attitude quaternion norm",
282                value: norm,
283            });
284        }
285        let q = state.attitude / norm;
286        let omega = state.body_rate_rad_s;
287        // The air and the wind where the center of mass is, at the height a rocket's flight
288        // reads them at (`crate::dynamics`).
289        let geodetic = environment
290            .earth
291            .frame()
292            .geodetic_from_enu(state.position_enu_m)?;
293        let height_msl_m = geodetic.height_m - environment.geoid_undulation_m;
294        let air = environment.air_at(height_msl_m)?;
295        let wind_enu_m_s = environment.wind_enu_m_s(height_msl_m)?;
296        let air_velocity_body_m_s = q
297            .conjugate()
298            .mul_vec3(state.velocity_enu_m_s - wind_enu_m_s);
299        let applied = loads.loads(&LoadInputs {
300            state: State {
301                attitude: q,
302                ..state
303            },
304            height_msl_m,
305            air,
306            wind_enu_m_s,
307            air_velocity_body_m_s,
308        })?;
309        if let Some(&value) = applied
310            .force_body_n
311            .to_array()
312            .iter()
313            .chain(&applied.moment_body_n_m.to_array())
314            .find(|component| !component.is_finite())
315        {
316            return Err(SimError::Domain {
317                what: FREE_BODY_LOAD_NOT_FINITE,
318                value,
319            });
320        }
321        let gravity_enu = environment.earth.gravity_enu_mps2(state.position_enu_m)?;
322        let coriolis_enu = environment
323            .earth
324            .rotation_acceleration_enu_mps2(state.velocity_enu_m_s);
325        let weight = q
326            .conjugate()
327            .mul_vec3((gravity_enu + coriolis_enu) * self.mass_kg);
328        let (a_body, omega_dot) = free_motion(
329            &self.mass_state(),
330            omega,
331            [DVec3::ZERO, DVec3::ZERO, weight + applied.force_body_n],
332            DMat3::ZERO,
333            [DVec3::ZERO, applied.moment_body_n_m, DVec3::ZERO],
334        )?;
335        let a = q.mul_vec3(a_body);
336        let v = state.velocity_enu_m_s;
337        let q_dot = quaternion_derivative(state.attitude, omega);
338        Ok([
339            v.x,
340            v.y,
341            v.z,
342            a.x,
343            a.y,
344            a.z,
345            q_dot.w,
346            q_dot.x,
347            q_dot.y,
348            q_dot.z,
349            omega_dot.x,
350            omega_dot.y,
351            omega_dot.z,
352        ])
353    }
354
355    /// Flies the body in `environment` under the caller's `loads` from `state` at `t0_s` and
356    /// gives its state at each of `times_s`, read from the steps' dense output as a flight's
357    /// recorder reads its samples. The times don't shape the steps, but for the last, where the
358    /// integration stops. Nothing ends the flight early: the ground is not looked for.
359    ///
360    /// # Errors
361    ///
362    /// [`SimError::Domain`] with [`FREE_BODY_TIME_REFUSED`] for a sample time that is not finite,
363    /// is before `t0_s` or is before the one ahead of it; [`SimError::Settings`] for a bad method
364    /// or a start that is not finite; whatever [`Self::derivative`] refuses; and
365    /// [`SimError::Integration`] if the integration fails, the step limit included.
366    pub fn fly(
367        &self,
368        environment: &Environment,
369        loads: &dyn FreeBodyLoads,
370        method: Method,
371        t0_s: f64,
372        state: State,
373        times_s: &[f64],
374    ) -> Result<Vec<State>, SimError> {
375        // The integrator refuses a start that is not finite; checked first, so a sample time
376        // is not blamed for it.
377        let mut integrator = Integrator::new(method, t0_s, state.to_array())?;
378        let mut last = t0_s;
379        for &time in times_s {
380            if !(time.is_finite() && time >= last) {
381                return Err(SimError::Domain {
382                    what: FREE_BODY_TIME_REFUSED,
383                    value: time,
384                });
385            }
386            last = time;
387        }
388        let mut system = Flight {
389            body: self,
390            environment,
391            loads,
392            times_s,
393            states: Vec::with_capacity(times_s.len()),
394        };
395        // The samples at the start itself, which no step covers.
396        while system.next_time().is_some_and(|time| time <= t0_s) {
397            system.states.push(state);
398        }
399        match integrator.advance(&mut system, last) {
400            Ok(_) => {}
401            Err(IntegrationError::Derivative { source, .. }) => return Err(source),
402            Err(error) => return Err(SimError::Integration(Box::new(error))),
403        }
404        // A last sample within the rounding of the stop, which the integrator reaches without a
405        // step.
406        let end = State::from_array(integrator.state());
407        while system.next_time().is_some() {
408            system.states.push(end);
409        }
410        Ok(system.states)
411    }
412}
413
414/// A free body's flight as the integrator sees it, collecting the samples.
415struct Flight<'a> {
416    body: &'a FreeBody,
417    environment: &'a Environment,
418    loads: &'a dyn FreeBodyLoads,
419    times_s: &'a [f64],
420    states: Vec<State>,
421}
422
423impl Flight<'_> {
424    /// The next sample time not yet given a state.
425    fn next_time(&self) -> Option<f64> {
426        self.times_s.get(self.states.len()).copied()
427    }
428}
429
430impl OdeSystem<STATE_LEN> for Flight<'_> {
431    type Error = SimError;
432
433    fn derivative(
434        &mut self,
435        _t_s: f64,
436        y: &[f64; STATE_LEN],
437    ) -> Result<[f64; STATE_LEN], SimError> {
438        self.body.derivative(self.environment, self.loads, y)
439    }
440
441    fn accept_step(&mut self, step: &Step<STATE_LEN>) -> ControlFlow<()> {
442        while let Some(time) = self.next_time() {
443            if time > step.end_s() {
444                break;
445            }
446            self.states.push(State::from_array(&step.state_at(time)));
447        }
448        ControlFlow::Continue(())
449    }
450}
451
452#[cfg(test)]
453mod tests {
454    use hpr_atmos::ConstantWind;
455    use hpr_core::DQuat;
456    use hpr_core::earth::Earth;
457
458    use super::*;
459    use crate::dynamics::{Conditions, Phase, Vehicle};
460    use crate::flight::{FlightSettings, Simulation};
461    use crate::rail::Rail;
462    use crate::testing::{UniformAir, analytic_environment, design, site};
463
464    /// The tumbling brick of NASA/TM-2015-218675 (Volume II, Table 4), in SI: three unequal
465    /// moments about its own axes.
466    fn brick() -> FreeBody {
467        const SLUG_FT2_KG_M2: f64 = 4.448_221_615_260_5 * 0.3048;
468        FreeBody::new(
469            0.155_404_754 * 4.448_221_615_260_5 / 0.3048,
470            DMat3::from_diagonal(DVec3::new(
471                0.001_894_220 * SLUG_FT2_KG_M2,
472                0.006_211_019 * SLUG_FT2_KG_M2,
473                0.007_194_665 * SLUG_FT2_KG_M2,
474            )),
475        )
476        .unwrap()
477    }
478
479    /// WGS 84's gravity at the body's own place and the Coriolis force, with no air.
480    fn airless_earth() -> Environment {
481        Environment::new(
482            Earth::wgs84(site()).unwrap(),
483            UniformAir::vacuum(),
484            ConstantWind::calm(),
485        )
486    }
487
488    fn tumbling() -> State {
489        State {
490            position_enu_m: DVec3::new(40.0, -25.0, 900.0),
491            velocity_enu_m_s: DVec3::new(30.0, -20.0, 80.0),
492            attitude: DQuat::from_rotation_z(0.3)
493                * DQuat::from_rotation_y(0.4)
494                * DQuat::from_rotation_x(0.2),
495            body_rate_rad_s: DVec3::new(0.4, -0.3, 3.0),
496        }
497    }
498
499    /// The free body's derivative is the flight's: a coasting rocket in no air, evaluated by the
500    /// equations a flight integrates, accelerates and turns as a free body of its mass and
501    /// inertia does at the same state. The rocket's state is its nose tip's, so its center of
502    /// mass's acceleration is `a_O + q (ω̇ × r + ω × (ω × r))`.
503    #[test]
504    fn the_derivative_is_a_coasting_rockets() {
505        let environment = airless_earth();
506        let simulation = Simulation::new(
507            &design("rocketpy-valetudo"),
508            "example",
509            environment.clone(),
510            Rail::vertical(3.0),
511            FlightSettings::default(),
512        )
513        .unwrap();
514        // Valetudo's motor burns out at 3.26 s; at 10 s nothing about its mass changes.
515        let t = 10.0;
516        let vehicle =
517            Vehicle::new(simulation.assembly().clone(), simulation.aero().clone()).unwrap();
518        let nose = tumbling();
519        let evaluation = vehicle
520            .evaluate(
521                &environment,
522                0.0,
523                Conditions {
524                    phase: Phase::Free,
525                    window: (t, t + 1.0),
526                    drag_area_m2: 0.0,
527                    pivot: None,
528                },
529                t,
530                &nose.to_array(),
531            )
532            .unwrap();
533        let mass = evaluation.mass;
534        assert_eq!(mass.mass_rate_kg_s, 0.0);
535        assert!(mass.cg_m.length() > 1.0, "{:?}", mass.cg_m);
536        let (q, omega, r) = (nose.unit_attitude(), nose.body_rate_rad_s, mass.cg_m);
537        let omega_dot = DVec3::from_slice(&evaluation.derivative[10..13]);
538        let rocket_cg_acceleration = evaluation.acceleration_enu_m_s2
539            + q.mul_vec3(omega_dot.cross(r) + omega.cross(omega.cross(r)));
540
541        let body = FreeBody::new(mass.mass_kg, symmetric(mass.inertia_cg)).unwrap();
542        let at_cg = State {
543            position_enu_m: evaluation.cg_enu_m,
544            velocity_enu_m_s: evaluation.cg_velocity_enu_m_s,
545            ..nose
546        };
547        let derivative = body
548            .derivative(&environment, &NoLoads, &at_cg.to_array())
549            .unwrap();
550        let acceleration = DVec3::from_slice(&derivative[3..6]);
551        let turning = DVec3::from_slice(&derivative[10..13]);
552
553        // The two differ by rounding only, measured 3.7e-15 m/s² and 2.4e-14 rad/s²: the rocket's
554        // side adds and takes away centripetal terms of about 20 m/s², for a nose tip 2 m from
555        // the center of mass turning at 3 rad/s. The bound leaves forty times that.
556        let acceleration_gap = (acceleration - rocket_cg_acceleration).length();
557        assert!(acceleration_gap < 1e-12, "{acceleration_gap:e} m/s²");
558        let turning_gap = (turning - omega_dot).length();
559        assert!(turning_gap < 1e-12, "{turning_gap:e} rad/s²");
560        // The comparison has something to hold: gravity, the Coriolis force and the gyroscopic
561        // term are each far above the bound.
562        assert!(acceleration.length() > 9.7);
563        assert!(omega_dot.length() > 1e-3, "{omega_dot:?}");
564        let coriolis = environment
565            .earth
566            .rotation_acceleration_enu_mps2(at_cg.velocity_enu_m_s);
567        assert!(coriolis.length() > 1e-3, "{coriolis:?}");
568    }
569
570    /// A tensor with its rounding asymmetry averaged out, as [`FreeBody::new`] asks.
571    fn symmetric(i: DMat3) -> DMat3 {
572        (i + i.transpose()) * 0.5
573    }
574
575    /// With no torque, a body of three unequal moments keeps its angular momentum, seen from the
576    /// launch frame, and its rotational energy: Euler's equations' two integrals. The brick
577    /// tumbles for 30 s on the default method.
578    #[test]
579    fn a_tumbling_brick_keeps_its_angular_momentum_and_energy() {
580        let body = brick();
581        let inertia = body.inertia_kg_m2();
582        let start = State {
583            position_enu_m: DVec3::new(0.0, 0.0, 9144.0),
584            velocity_enu_m_s: DVec3::ZERO,
585            attitude: DQuat::IDENTITY,
586            body_rate_rad_s: DVec3::new(10.0, 20.0, 30.0).map(f64::to_radians),
587        };
588        let times: Vec<f64> = (0..=300).map(|i| 0.1 * f64::from(i)).collect();
589        let states = body
590            .fly(
591                &airless_earth(),
592                &NoLoads,
593                Method::default(),
594                0.0,
595                start,
596                &times,
597            )
598            .unwrap();
599        assert_eq!(states.len(), times.len());
600        assert_eq!(states[0], start);
601        let momentum = |s: &State| s.unit_attitude().mul_vec3(inertia * s.body_rate_rad_s);
602        let energy = |s: &State| 0.5 * s.body_rate_rad_s.dot(inertia * s.body_rate_rad_s);
603        let (h0, e0) = (momentum(&start), energy(&start));
604        let mut worst_momentum: f64 = 0.0;
605        let mut worst_energy: f64 = 0.0;
606        let mut largest_turn: f64 = 0.0;
607        for state in &states {
608            worst_momentum = worst_momentum.max((momentum(state) - h0).length() / h0.length());
609            worst_energy = worst_energy.max((energy(state) - e0).abs() / e0);
610            largest_turn =
611                largest_turn.max((state.body_rate_rad_s - start.body_rate_rad_s).length());
612        }
613        // Measured 3.4e-8 and 2.7e-8 at the default tolerances (1e-8 a step); about ten times
614        // that is allowed.
615        assert!(worst_momentum < 4e-7, "{worst_momentum:e}");
616        assert!(worst_energy < 4e-7, "{worst_energy:e}");
617        // The rates are far from constant (they move by up to 0.79 rad/s), so the integrals are
618        // not held trivially.
619        assert!(largest_turn > 0.5, "{largest_turn} rad/s");
620    }
621
622    /// Under uniform gravity with the Earth's rotation off, the center of mass follows the
623    /// parabola whatever the body does.
624    #[test]
625    fn the_center_of_mass_follows_the_parabola() {
626        const G: f64 = 9.806_65;
627        let body = brick();
628        let start = tumbling();
629        let times = [0.0, 0.5, 3.0, 7.25, 12.0];
630        let states = body
631            .fly(
632                &analytic_environment(UniformAir::vacuum(), G),
633                &NoLoads,
634                Method::default(),
635                0.0,
636                start,
637                &times,
638            )
639            .unwrap();
640        for (time, state) in times.iter().zip(&states) {
641            let expected = start.position_enu_m + start.velocity_enu_m_s * *time
642                - DVec3::Z * (0.5 * G * time * time);
643            let gap = (state.position_enu_m - expected).length();
644            assert!(gap < 1e-8, "{gap:e} m at {time} s");
645        }
646    }
647
648    /// A body dropped at rest over the equator is carried east by the Coriolis force, at
649    /// `Ω g t²` after `t` seconds, and that eastward speed is in turn pushed up, at `2 Ω² g t²`:
650    /// with the Earth's rotation on, the fall ends `Ω² g t⁴/6` higher than with it off, 7.0 mm
651    /// after 30 s. The lift is even in the sign of `Ω`: a Coriolis force of the wrong sign
652    /// would drift the body west and lift it the same, so a fall's height holds the term's
653    /// size and not its sign, which the eastward drift holds.
654    #[test]
655    fn the_coriolis_force_lifts_a_long_fall() {
656        use hpr_core::earth::{EarthRotation, GravityModel};
657        use hpr_core::geodesy::Geodetic;
658        use hpr_core::gravity::NormalGravity;
659        let equator = Geodetic::from_degrees(0.0, 0.0, 0.0).unwrap();
660        let environment = |rotation| {
661            Environment::new(
662                Earth::new(
663                    NormalGravity::wgs84(),
664                    equator,
665                    GravityModel::default(),
666                    rotation,
667                )
668                .unwrap(),
669                UniformAir::vacuum(),
670                ConstantWind::calm(),
671            )
672        };
673        let start = State {
674            position_enu_m: DVec3::new(0.0, 0.0, 9144.0),
675            velocity_enu_m_s: DVec3::ZERO,
676            attitude: DQuat::IDENTITY,
677            body_rate_rad_s: DVec3::ZERO,
678        };
679        let end = |rotation| {
680            let environment = environment(rotation);
681            let states = brick()
682                .fly(
683                    &environment,
684                    &NoLoads,
685                    Method::default(),
686                    0.0,
687                    start,
688                    &[30.0],
689                )
690                .unwrap();
691            let height = environment
692                .earth
693                .frame()
694                .geodetic_from_enu(states[0].position_enu_m)
695                .unwrap()
696                .height_m;
697            (height, states[0].velocity_enu_m_s.x)
698        };
699        let (turning, east_m_s) = end(EarthRotation::Coriolis);
700        let (still, no_drift_m_s) = end(EarthRotation::Ignore);
701        let omega: f64 = 7.292_115e-5;
702        // Gravity over the fall, from the fall itself, 4.4 km in 30 s: the equator's 9.780 m/s²
703        // less 3.086e-6 m/s² a meter over heights of 9.1 to 4.8 km.
704        let g = 2.0 * (9144.0 - still) / 30.0_f64.powi(2);
705        assert!((g - 9.754).abs() < 0.005, "{g}");
706        let lift_m = omega * omega * g * 30.0_f64.powi(4) / 6.0;
707        assert!((lift_m - 0.0070).abs() < 0.0001, "{lift_m}");
708        let lifted_m = turning - still;
709        assert!(
710            (lifted_m / lift_m - 1.0).abs() < 0.01,
711            "{lifted_m} {lift_m}"
712        );
713        // East, at Ω g t²: 0.64 m/s. With the rotation off there is no drift at all.
714        let drift_m_s = omega * g * 30.0_f64.powi(2);
715        assert!(
716            (east_m_s / drift_m_s - 1.0).abs() < 0.01,
717            "{east_m_s} {drift_m_s}"
718        );
719        assert_eq!(no_drift_m_s, 0.0);
720    }
721
722    /// The samples are the integration's own states, whatever times are asked for: a flight
723    /// sampled at two times gives the states a flight sampled at many gives there, to the dense
724    /// output's accuracy, and a repeated time or the start itself gives its state each time.
725    #[test]
726    fn samples_do_not_shape_the_flight() {
727        let body = brick();
728        let start = tumbling();
729        let environment = airless_earth();
730        let fly = |times: &[f64]| {
731            body.fly(&environment, &NoLoads, Method::default(), 2.0, start, times)
732                .unwrap()
733        };
734        let dense: Vec<f64> = (0..=100).map(|i| 2.0 + 0.05 * f64::from(i)).collect();
735        let many = fly(&dense);
736        let few = fly(&[2.0, 2.0, 4.5, 7.0]);
737        assert_eq!(few[0], start);
738        assert_eq!(few[1], start);
739        assert_eq!(few[3], many[100]);
740        let gap = (few[2].position_enu_m - many[50].position_enu_m).length();
741        assert!(gap < 1e-9, "{gap:e} m");
742        assert!(fly(&[]).is_empty());
743    }
744
745    /// Each refusal names what it refused.
746    #[test]
747    fn bad_bodies_and_times_are_refused_by_name() {
748        let good = DMat3::from_diagonal(DVec3::new(1.0, 2.0, 3.0));
749        let what = |result: Result<FreeBody, SimError>| match result {
750            Err(SimError::Domain { what, value }) => (what, value),
751            other => panic!("{other:?}"),
752        };
753        assert_eq!(what(FreeBody::new(0.0, good)).0, FREE_BODY_MASS_REFUSED);
754        assert_eq!(
755            what(FreeBody::new(-1.0, good)),
756            (FREE_BODY_MASS_REFUSED, -1.0)
757        );
758        assert_eq!(
759            what(FreeBody::new(f64::INFINITY, good)).0,
760            FREE_BODY_MASS_REFUSED
761        );
762        let mut not_finite = good;
763        not_finite.y_axis.z = f64::NAN;
764        assert_eq!(
765            what(FreeBody::new(1.0, not_finite)).0,
766            FREE_BODY_INERTIA_NOT_FINITE
767        );
768        let mut lopsided = good;
769        lopsided.x_axis.y = 0.25;
770        assert_eq!(
771            what(FreeBody::new(1.0, lopsided)),
772            (FREE_BODY_INERTIA_NOT_SYMMETRIC, 0.25)
773        );
774        // Symmetric, with a product of inertia too large for its moments: the second minor is
775        // 1·2 − 2² = −2.
776        let mut indefinite = good;
777        indefinite.x_axis.y = 2.0;
778        indefinite.y_axis.x = 2.0;
779        assert_eq!(
780            what(FreeBody::new(1.0, indefinite)),
781            (FREE_BODY_INERTIA_NOT_POSITIVE, -2.0)
782        );
783        assert_eq!(
784            what(FreeBody::new(1.0, DMat3::ZERO)),
785            (FREE_BODY_INERTIA_NOT_POSITIVE, 0.0)
786        );
787
788        let body = FreeBody::new(1.0, good).unwrap();
789        let environment = airless_earth();
790        let refused = |t0: f64, times: &[f64]| match body.fly(
791            &environment,
792            &NoLoads,
793            Method::default(),
794            t0,
795            tumbling(),
796            times,
797        ) {
798            Err(SimError::Domain { what, value }) => (what, value),
799            other => panic!("{other:?}"),
800        };
801        assert_eq!(refused(1.0, &[0.5]), (FREE_BODY_TIME_REFUSED, 0.5));
802        assert_eq!(refused(0.0, &[1.0, 0.75]), (FREE_BODY_TIME_REFUSED, 0.75));
803        assert_eq!(refused(0.0, &[f64::INFINITY]).0, FREE_BODY_TIME_REFUSED);
804        assert!(refused(0.0, &[f64::NAN]).1.is_nan());
805        // A start that is not finite is the integrator's to refuse, not a sample time's.
806        let start = body.fly(
807            &environment,
808            &NoLoads,
809            Method::default(),
810            f64::NAN,
811            tumbling(),
812            &[1.0],
813        );
814        assert!(matches!(start, Err(SimError::Settings(_))), "{start:?}");
815    }
816
817    /// A body read from JSON goes through [`FreeBody::new`] again: a mass of zero, two negative
818    /// moments (whose determinant is positive, so the equations of motion would take them) and
819    /// a tensor that is not symmetric are each refused by name, and a good body comes back as
820    /// it was written.
821    #[test]
822    fn a_body_read_from_json_is_checked_again() {
823        let read = |json: &str| serde_json::from_str::<FreeBody>(json).map_err(|e| e.to_string());
824        let refused = |json: &str, what: &str| {
825            let error = read(json).unwrap_err();
826            assert!(error.contains(what), "{error}");
827        };
828        refused(
829            r#"{"mass_kg":0.0,"inertia_kg_m2":[1,0,0,0,1,0,0,0,1]}"#,
830            FREE_BODY_MASS_REFUSED,
831        );
832        refused(
833            r#"{"mass_kg":-1.0,"inertia_kg_m2":[1,0,0,0,1,0,0,0,1]}"#,
834            FREE_BODY_MASS_REFUSED,
835        );
836        refused(
837            r#"{"mass_kg":1.0,"inertia_kg_m2":[-1,0,0,0,-2,0,0,0,3]}"#,
838            FREE_BODY_INERTIA_NOT_POSITIVE,
839        );
840        refused(
841            r#"{"mass_kg":1.0,"inertia_kg_m2":[1,0.25,0,0,2,0,0,0,3]}"#,
842            FREE_BODY_INERTIA_NOT_SYMMETRIC,
843        );
844        let body = brick();
845        let json = serde_json::to_string(&body).unwrap();
846        assert!(json.starts_with(r#"{"mass_kg":"#), "{json}");
847        assert_eq!(read(&json).unwrap(), body);
848    }
849
850    /// A caller's loads are formed from the air and the wind where a rocket's flight reads them,
851    /// and enter as a force through the center of mass and a moment about it: the acceleration
852    /// gains `q F / m` and the turning `I⁻¹ M`, each to rounding, at a state that tumbles in a
853    /// wind.
854    #[test]
855    fn loads_see_the_air_as_a_flight_does_and_enter_newtons_and_eulers_equations() {
856        use std::cell::Cell;
857
858        // The 1976 atmosphere, which changes with height, and a geoid 17 m above the
859        // ellipsoid: air read at the ellipsoidal height, or with the undulation's sign turned,
860        // is other air.
861        let undulation_m = 17.0;
862        let wind = DVec3::new(-6.0, 4.0, 0.0);
863        let environment = Environment::standard(site())
864            .unwrap()
865            .with_wind(
866                ConstantWind::new(wind.length(), wind.x.atan2(wind.y) + std::f64::consts::PI)
867                    .unwrap(),
868            )
869            .with_geoid_undulation_m(undulation_m);
870        let body = brick();
871        let state = tumbling();
872        let force = DVec3::new(0.7, -1.1, 2.3);
873        let moment = DVec3::new(0.002, 0.003, -0.001);
874        let seen = Cell::new(None);
875        let loads = |inputs: &LoadInputs| {
876            seen.set(Some(*inputs));
877            Ok(Loads {
878                force_body_n: force,
879                moment_body_n_m: moment,
880            })
881        };
882        let y = state.to_array();
883        let loaded = body.derivative(&environment, &loads, &y).unwrap();
884        let free = body.derivative(&environment, &NoLoads, &y).unwrap();
885
886        let inputs = seen.get().unwrap();
887        let q = state.unit_attitude();
888        let height = environment
889            .earth
890            .frame()
891            .geodetic_from_enu(state.position_enu_m)
892            .unwrap()
893            .height_m;
894        assert_eq!(inputs.height_msl_m, height - undulation_m);
895        let density = |at_m: f64| environment.atmosphere.air(at_m).unwrap().air.density_kg_m3;
896        assert_eq!(inputs.air.density_kg_m3, density(height - undulation_m));
897        for wrong in [height, height + undulation_m] {
898            assert!((density(wrong) / density(height - undulation_m) - 1.0).abs() > 1e-4);
899        }
900        assert!(
901            (inputs.wind_enu_m_s - wind).length() < 1e-12,
902            "{:?}",
903            inputs.wind_enu_m_s
904        );
905        let through_air = q.conjugate().mul_vec3(state.velocity_enu_m_s - wind);
906        assert!((inputs.air_velocity_body_m_s - through_air).length() < 1e-12);
907        assert_eq!(inputs.state.body_rate_rad_s, state.body_rate_rad_s);
908
909        let gained = DVec3::from_slice(&loaded[3..6]) - DVec3::from_slice(&free[3..6]);
910        let expected = q.mul_vec3(force) / body.mass_kg();
911        assert!(
912            (gained - expected).length() < 1e-12,
913            "{gained:?} vs {expected:?}"
914        );
915        let turned = DVec3::from_slice(&loaded[10..13]) - DVec3::from_slice(&free[10..13]);
916        let expected = body.inertia_kg_m2().inverse() * moment;
917        assert!(
918            (turned - expected).length() < 1e-9 * expected.length(),
919            "{turned:?} vs {expected:?}"
920        );
921        // The position and attitude rates don't depend on the loads.
922        assert_eq!(loaded[0..3], free[0..3]);
923        assert_eq!(loaded[6..10], free[6..10]);
924    }
925
926    /// A moment `M = −c ω` on a body of equal moments `J` damps its spin as `ω₀ e^(−c t / J)`,
927    /// with no gyroscopic term to couple the axes: the integrator carries the caller's moment
928    /// through a 5 s decay to 1e-7 of the start.
929    #[test]
930    fn a_damped_spin_decays_as_the_exponential() {
931        let j = 0.05;
932        let c = 0.02;
933        let body = FreeBody::new(1.0, DMat3::from_diagonal(DVec3::splat(j))).unwrap();
934        let damping = |inputs: &LoadInputs| {
935            Ok(Loads {
936                force_body_n: DVec3::ZERO,
937                moment_body_n_m: inputs.state.body_rate_rad_s * -c,
938            })
939        };
940        let start = State {
941            body_rate_rad_s: DVec3::new(1.0, -2.0, 0.5),
942            ..tumbling()
943        };
944        let times = [1.0, 2.5, 5.0];
945        let states = body
946            .fly(
947                &analytic_environment(UniformAir::vacuum(), 9.806_65),
948                &damping,
949                Method::default(),
950                0.0,
951                start,
952                &times,
953            )
954            .unwrap();
955        for (time, state) in times.iter().zip(&states) {
956            let expected = start.body_rate_rad_s * (-c * time / j).exp();
957            let gap = (state.body_rate_rad_s - expected).length();
958            assert!(
959                gap < 1e-7 * start.body_rate_rad_s.length(),
960                "{time} s: {gap:e}"
961            );
962        }
963        // The decay is real: the spin fell to e^(−2) of its start.
964        assert!(states[2].body_rate_rad_s.length() < 0.14 * start.body_rate_rad_s.length());
965    }
966
967    /// A load that is not finite stops the derivative, naming the component.
968    #[test]
969    fn a_load_that_is_not_finite_is_refused() {
970        let body = brick();
971        let bad = |_: &LoadInputs| {
972            Ok(Loads {
973                force_body_n: DVec3::ZERO,
974                moment_body_n_m: DVec3::new(0.0, f64::NAN, 0.0),
975            })
976        };
977        let error = body
978            .derivative(&airless_earth(), &bad, &tumbling().to_array())
979            .unwrap_err();
980        assert!(
981            matches!(
982                error,
983                SimError::Domain { what, value }
984                    if what == FREE_BODY_LOAD_NOT_FINITE && value.is_nan()
985            ),
986            "{error:?}"
987        );
988    }
989}