Skip to main content

hpr_sim/
dynamics.rs

1//! The equations of motion: a rigid body of varying mass referred to a point fixed in the body.
2//!
3//! Source: the RocketPy technical documentation, "Equations of Motion" v0 (Kane's method with the
4//! Reynolds transport theorem) and v1 (the form solved), RocketPy 1.13.0, MIT; G. H. Ceotto,
5//! R. N. Schmitt, G. F. Alves, L. A. Pezente and B. S. Carmo, "RocketPy: Six Degree-of-Freedom
6//! Rocket Trajectory Simulator", *J. Aerosp. Eng.* 34(6), 2021. The nozzle gyration tensor is
7//! integrated here from the boxed rotational equation of v0 (a uniform jet over the exit disc).
8//! The derivation, the assumptions and every term are in `docs/physics/flight.md`.
9//!
10//! In body axes, with `O` the nose tip, `r` the center of mass from `O`, `m` the mass, `I` the
11//! inertia about `O` and `I_c` about the center of mass, primes body-frame time derivatives and
12//! `ṁ_k ≤ 0` each motor's mass rate with its nozzle exit at `n_k`:
13//!
14//! ```text
15//! T20 = −ω×(ω×m r) + ω×(2 Σ ṁ_k (n_k − r) − 2 m r′) + T − m r″ − 2 ṁ r′ + Σ m̈_k (n_k − r) + W + A
16//! T21 = −ω×(I ω) + (Σ ṁ_k S_k − I′) ω + r×W + M_A + M_T − ω×h − h′
17//! ω̇   = I_c⁻¹ (T21 − r × T20)
18//! a_O = T20/m − ω̇ × r
19//! ```
20//!
21//! `T` is the thrust, `W` the weight with the Coriolis force (both acting at the center of mass),
22//! `A` and `M_A` the aerodynamic force and its moment about `O`, `M_T` the thrust's moment about
23//! `O`, and `S_k = (r_e²/4) diag(1, 1, 2) + |n_k|² 1 − n_k n_kᵀ` the gyration tensor of motor
24//! `k`'s exit disc of radius `r_e` about `O`. `a_O` is `O`'s acceleration relative to the launch
25//! frame, whose rotation enters only through the Coriolis force (normal gravity already holds the
26//! centrifugal term) and not through the rotational equations (the Earth's rate, 7.3e-5 rad/s,
27//! left out: the decision record on rigid-body flight, [ADR-011][adr-011]; on a tumbling body
28//! of three unequal moments that moves a body rate by more than the Earth's rate, as NASA's
29//! check case 2 shows in `docs/physics/flight.md`). `h = Σ_j m_j ρ_j × ρ_j′` is the angular
30//! momentum about `O`, relative to the airframe, of the parts moving along it ([`crate::shifts`]),
31//! at their centers `ρ_j`: zero for a part on the axis, and zero with no part moving.
32//!
33//! [adr-011]: https://github.com/nrdptel/fusionspace-eridanus/blob/main/docs/DECISIONS.md#adr-011-rigid-body-flight-equations-of-motion-aerodynamic-coupling-rail-phases-and-termination-2026-09-17
34
35use hpr_aero::{AeroModel, DragConditions, Flow, MOTOR_POD_SETS};
36use hpr_core::attitude::quaternion_derivative;
37use hpr_core::{DMat3, DQuat, DVec3};
38use hpr_design::Assembly;
39
40use crate::environment::Environment;
41use crate::error::SimError;
42use crate::histories::Histories;
43use crate::shifts::Shifts;
44use crate::state::{STATE_LEN, State};
45
46/// The half-width of the central differences for the mass properties' time derivatives, s.
47const MASS_DERIVATIVE_STEP_S: f64 = 1e-4;
48
49/// Airspeeds below this (m/s) produce no aerodynamic force: the angles are undefined at rest.
50const MIN_AIRSPEED_M_S: f64 = 1e-9;
51
52/// Integration intervals shorter than this (s) take no mass-property rates: central differences
53/// over them would be rounding noise, and the motors can't change measurably within them.
54const MIN_DERIVATIVE_INTERVAL_S: f64 = 2e-5;
55
56/// Where the rocket is in its flight, which decides what it is free to do.
57#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, serde::Serialize, serde::Deserialize)]
58#[serde(rename_all = "snake_case")]
59#[non_exhaustive]
60pub enum Phase {
61    /// Held on the pad by gravity and friction: the state doesn't change.
62    Pad,
63    /// Guided along the rail: one degree of freedom, no rotation.
64    Rail,
65    /// Tip-off: sliding along the rail on the last guide alone, free to pitch and yaw about it,
66    /// roll held (`docs/physics/flight.md`). Only a flight that asks for it has this phase
67    /// ([`crate::Simulation::with_tip_off`]).
68    TipOff,
69    /// Free flight: six degrees of freedom.
70    Free,
71    /// Descent under recovery devices: a point mass, with the attitude frozen where it deployed
72    /// (`docs/physics/recovery.md`).
73    Descent,
74}
75
76/// What an evaluation needs besides the time and the state.
77#[derive(Debug, Clone, Copy, PartialEq)]
78pub(crate) struct Conditions {
79    /// Which phase the flight is in, which decides the degrees of freedom and the forces.
80    pub(crate) phase: Phase,
81    /// The integration interval: which motors burn through it, and the bounds the mass-property
82    /// difference stencils stay inside.
83    pub(crate) window: (f64, f64),
84    /// The open recovery devices' drag area, m² (descent phase only).
85    pub(crate) drag_area_m2: f64,
86    /// Where the rocket pivots and the rail's line (tip-off phase only).
87    pub(crate) pivot: Option<crate::rail::Pivot>,
88}
89
90/// The burning motors' cross-section, m², by the base it comes off.
91#[derive(Debug, Clone, Copy, Default)]
92struct BurningAreas {
93    /// In the airframe's aft base.
94    airframe_m2: f64,
95    /// In each pod set holding motor mounts, in [`hpr_design::Layout::motor_pod_sets`]' order,
96    /// over its pods.
97    pods_m2: [f64; MOTOR_POD_SETS],
98}
99
100/// A motor's fixed data for the equations.
101#[derive(Debug, Clone, Copy)]
102struct MotorTerms {
103    /// The nozzle exit in body axes, m.
104    nozzle_m: DVec3,
105    /// The nozzle exit radius, m (zero when the motor doesn't give one).
106    exit_radius_m: f64,
107    /// The motor's cross-section, for power-on base drag, m².
108    area_m2: f64,
109    /// Where its mount's pod set is among those holding motor mounts
110    /// ([`hpr_design::Layout::motor_pod_sets`]), if its mount is in a pod: its area then comes off
111    /// that set's pods' bases, not the airframe's.
112    pod_set: Option<usize>,
113    /// When it lights on the flight's clock, s: infinite for a motor that hasn't a known time.
114    ignition_s: f64,
115    /// The end of its thrust curve on the flight's clock, s: infinite for a motor unlit.
116    burnout_s: f64,
117}
118
119/// The mass properties at a time and their time derivatives.
120#[derive(Debug, Clone, Copy)]
121pub(crate) struct MassState {
122    pub(crate) mass_kg: f64,
123    pub(crate) mass_rate_kg_s: f64,
124    pub(crate) cg_m: DVec3,
125    pub(crate) cg_rate_m_s: DVec3,
126    pub(crate) cg_accel_m_s2: DVec3,
127    pub(crate) inertia_cg: DMat3,
128    pub(crate) inertia_o: DMat3,
129    pub(crate) inertia_o_rate: DMat3,
130    /// The moving parts' angular momentum about `O` relative to the airframe, `h`, kg·m²/s.
131    pub(crate) relative_momentum: DVec3,
132    /// Its body-frame rate, `h′`, N·m.
133    pub(crate) relative_momentum_rate: DVec3,
134}
135
136/// Everything the equations need from a flight at one instant, besides the derivative.
137#[derive(Debug, Clone, Copy)]
138pub(crate) struct Evaluation {
139    pub(crate) derivative: [f64; STATE_LEN],
140    pub(crate) mass: MassState,
141    pub(crate) cg_enu_m: DVec3,
142    pub(crate) cg_velocity_enu_m_s: DVec3,
143    pub(crate) height_above_ground_m: f64,
144    pub(crate) vertical_speed_m_s: f64,
145    pub(crate) airspeed_m_s: f64,
146    pub(crate) mach: f64,
147    pub(crate) angle_of_attack_rad: f64,
148    pub(crate) dynamic_pressure_pa: f64,
149    pub(crate) axial_coefficient: f64,
150    pub(crate) thrust_n: f64,
151    /// Force along the rail less friction, N (rail and pad phases; the liftoff condition).
152    pub(crate) rail_force_n: f64,
153    /// The body origin's acceleration relative to `L`, in `L`, m/s².
154    pub(crate) acceleration_enu_m_s2: DVec3,
155    /// The specific force there: the acceleration less gravity and the Coriolis term, in `L`,
156    /// m/s². It is the push of everything but gravity per unit mass, what an accelerometer reads.
157    pub(crate) specific_force_enu_m_s2: DVec3,
158    /// The drag area of the open recovery devices, m².
159    pub(crate) recovery_drag_area_m2: f64,
160}
161
162/// The rocket's models: the whole stack's for a flight, or after a powered separation the
163/// sustainer's.
164#[derive(Debug, Clone)]
165pub(crate) struct Vehicle {
166    pub(crate) assembly: Assembly,
167    pub(crate) aero: AeroModel,
168    /// The first fin set's index among the aerodynamic components.
169    first_fin_index: usize,
170    motors: Vec<MotorTerms>,
171    /// Each motor's ignition time on the flight's clock, `None` while it has no known time.
172    ignition_s: Vec<Option<f64>>,
173    reference_area_m2: f64,
174    /// The parts that move along the airframe.
175    pub(crate) shifts: Shifts,
176    /// The thrust and mass histories flown in place of its own, if any ([`crate::histories`]).
177    pub(crate) histories: Histories,
178}
179
180impl Vehicle {
181    /// The models of `assembly` flying on `aero`, every motor lit at launch.
182    #[cfg(test)]
183    pub(crate) fn new(assembly: Assembly, aero: AeroModel) -> Result<Self, SimError> {
184        let ignition_s = vec![Some(0.0); assembly.motors.len()];
185        Self::lit(assembly, aero, ignition_s)
186    }
187
188    /// The models of `assembly` flying on `aero`, its motors lit at `ignition_s` (one per motor,
189    /// `None` for one with no known time, which stays loaded and gives no thrust).
190    pub(crate) fn lit(
191        assembly: Assembly,
192        aero: AeroModel,
193        ignition_s: Vec<Option<f64>>,
194    ) -> Result<Self, SimError> {
195        if ignition_s.len() != assembly.motors.len() {
196            return Err(SimError::Domain {
197                what: "count of ignition times (one per motor)",
198                value: ignition_s.len() as f64,
199            });
200        }
201        // The aerodynamic model refuses more motor pod sets than the drag tells apart; this check
202        // keeps each motor's place in the list a valid index of [`BurningAreas::pods_m2`] whatever
203        // model it flies on.
204        let motor_pod_sets = assembly.layout.motor_pod_sets();
205        if motor_pod_sets.len() > MOTOR_POD_SETS {
206            return Err(SimError::Domain {
207                what: "count of pod sets holding motor mounts",
208                value: motor_pod_sets.len() as f64,
209            });
210        }
211        let motors = assembly
212            .motors
213            .iter()
214            .zip(&ignition_s)
215            .map(|(placed, ignition)| MotorTerms {
216                nozzle_m: placed.nozzle_m,
217                exit_radius_m: placed
218                    .mounted
219                    .motor
220                    .nozzle()
221                    .map_or(0.0, |nozzle| nozzle.exit_radius_m),
222                area_m2: std::f64::consts::PI * (0.5 * placed.mounted.diameter_m).powi(2),
223                // The assembly places a motor only in a mount it found, so `find` succeeds.
224                pod_set: assembly
225                    .layout
226                    .find(&placed.mount)
227                    .and_then(|(index, _)| assembly.layout.pod_set_of(index))
228                    .and_then(|set| motor_pod_sets.iter().position(|&s| s == set)),
229                ignition_s: ignition.unwrap_or(f64::INFINITY),
230                burnout_s: ignition.map_or(f64::INFINITY, |ignition_s| {
231                    ignition_s + placed.mounted.motor.burnout_time_s()
232                }),
233            })
234            .collect();
235        let reference_area_m2 = aero.reference_area_m2();
236        let first_fin_index = aero.fin_set_start();
237        Ok(Self {
238            assembly,
239            aero,
240            first_fin_index,
241            motors,
242            ignition_s,
243            reference_area_m2,
244            shifts: Shifts::default(),
245            histories: Histories::default(),
246        })
247    }
248
249    /// Each motor's ignition time on the flight's clock, `None` while it has no known time.
250    pub(crate) fn ignition_s(&self) -> &[Option<f64>] {
251        &self.ignition_s
252    }
253
254    /// The time the last motor with a known ignition burns out, s (`0` with none).
255    pub(crate) fn burnout_s(&self) -> f64 {
256        self.motors
257            .iter()
258            .map(|m| m.burnout_s)
259            .filter(|t| t.is_finite())
260            .fold(0.0, f64::max)
261    }
262
263    /// The mean of the nozzle exits of the motors lit by a known time, in body axes, m, or
264    /// `None` with no such motor: where a thrust history acts ([`crate::histories`]).
265    pub(crate) fn lit_nozzle_mean_m(&self) -> Option<DVec3> {
266        let lit: Vec<DVec3> = self
267            .motors
268            .iter()
269            .filter(|m| m.ignition_s.is_finite())
270            .map(|m| m.nozzle_m)
271            .collect();
272        (!lit.is_empty()).then(|| lit.iter().copied().sum::<DVec3>() / lit.len() as f64)
273    }
274
275    /// The times at which the thrust curves start, have knots or end on the flight's clock, and
276    /// the corners of the histories flown in their place ([`Histories::corners_s`]), sorted,
277    /// after `0`.
278    pub(crate) fn thrust_knots_s(&self) -> Vec<f64> {
279        let mut times: Vec<f64> = self
280            .assembly
281            .motors
282            .iter()
283            .zip(&self.ignition_s)
284            .filter_map(|(placed, ignition)| ignition.map(|ignition_s| (placed, ignition_s)))
285            .flat_map(|(placed, ignition_s)| {
286                std::iter::once(ignition_s).chain(
287                    placed
288                        .mounted
289                        .motor
290                        .curve()
291                        .times_s()
292                        .iter()
293                        .map(move |t| ignition_s + t),
294                )
295            })
296            .filter(|t| *t > 0.0)
297            .chain(self.histories.corners_s())
298            .collect();
299        times.sort_by(f64::total_cmp);
300        times.dedup();
301        times
302    }
303
304    /// The mass properties at `t` and their derivatives, from central differences of
305    /// `Assembly::mass_properties` kept inside `window` (the current integration interval), so
306    /// that no difference straddles a thrust-curve knot or a burnout. The parts that move along
307    /// the airframe are then put where they are, with their own rates in closed form
308    /// ([`Shifts::shift_state`]). A mass history ([`crate::histories::MassHistory`]) gives the
309    /// mass and the center of mass's axial place, the inertia about the center of mass staying
310    /// the assembly's, and is differenced the same way whenever it changes inside `window`.
311    pub(crate) fn mass_state(&self, t: f64, window: (f64, f64)) -> MassState {
312        let history = self.histories.mass.as_ref();
313        let props = |t: f64| {
314            let mut mp = self.assembly.mass_properties_lit(t, &self.ignition_s);
315            if let Some(history) = history {
316                mp.mass_kg = history.mass_kg(t);
317                mp.cg_m.z = -history.cg_aft_of_nose_m(t);
318            }
319            (
320                mp.mass_kg,
321                mp.cg_m,
322                mp.inertia_kg_m2,
323                mp.inertia_about(DVec3::ZERO),
324            )
325        };
326        let (mass_kg, cg_m, inertia_cg, inertia_o) = props(t);
327        let (a, b) = window;
328        let burning = self
329            .motors
330            .iter()
331            .any(|m| a < m.burnout_s && b > m.ignition_s)
332            || history.is_some_and(|history| history.varies_within(window));
333        let mut state = MassState {
334            mass_kg,
335            mass_rate_kg_s: 0.0,
336            cg_m,
337            cg_rate_m_s: DVec3::ZERO,
338            cg_accel_m_s2: DVec3::ZERO,
339            inertia_cg,
340            inertia_o,
341            inertia_o_rate: DMat3::ZERO,
342            relative_momentum: DVec3::ZERO,
343            relative_momentum_rate: DVec3::ZERO,
344        };
345        let mut mass_second_kg_s2 = 0.0;
346        if burning && b - a >= MIN_DERIVATIVE_INTERVAL_S {
347            let h = MASS_DERIVATIVE_STEP_S.min(0.5 * (b - a));
348            let c = t.max(a + h).min(b - h);
349            let (m_minus, r_minus, _, i_minus) = props(c - h);
350            let (m_plus, r_plus, _, i_plus) = props(c + h);
351            let (m_mid, r_mid) = if c == t {
352                (mass_kg, cg_m)
353            } else {
354                let (m, r, _, _) = props(c);
355                (m, r)
356            };
357            let offset = t - c;
358            let m_second = (m_plus - 2.0 * m_mid + m_minus) / (h * h);
359            state.mass_rate_kg_s = (m_plus - m_minus) / (2.0 * h) + offset * m_second;
360            let r_second = (r_plus - 2.0 * r_mid + r_minus) / (h * h);
361            state.cg_rate_m_s = (r_plus - r_minus) / (2.0 * h) + offset * r_second;
362            state.cg_accel_m_s2 = r_second;
363            state.inertia_o_rate = (i_plus - i_minus) * (0.5 / h);
364            mass_second_kg_s2 = m_second;
365        }
366        if !self.shifts.is_empty() {
367            self.shifts.shift_state(&mut state, mass_second_kg_s2, t);
368        }
369        state
370    }
371
372    /// `evaluate`, written over `out` (left as it was on an error), so a caller that keeps one
373    /// evaluation doesn't pass a 600-byte value back through two returns and a move.
374    pub(crate) fn evaluate_into(
375        &self,
376        environment: &Environment,
377        friction_coefficient: f64,
378        conditions: Conditions,
379        t: f64,
380        y: &[f64; STATE_LEN],
381        out: &mut Evaluation,
382    ) -> Result<(), SimError> {
383        *out = self.evaluate(environment, friction_coefficient, conditions, t, y)?;
384        Ok(())
385    }
386
387    /// Evaluates the equations of motion at `(t, y)` under `conditions`: the phase, the motors
388    /// burning as its integration interval decides, and the recovery devices open.
389    #[inline(always)]
390    pub(crate) fn evaluate(
391        &self,
392        environment: &Environment,
393        friction_coefficient: f64,
394        conditions: Conditions,
395        t: f64,
396        y: &[f64; STATE_LEN],
397    ) -> Result<Evaluation, SimError> {
398        let Conditions {
399            phase,
400            window,
401            drag_area_m2: recovery_drag_area_m2,
402            pivot,
403        } = conditions;
404        let state = State::from_array(y);
405        let norm = state.attitude.length();
406        if !(norm.is_finite() && norm > 0.0) {
407            return Err(SimError::Domain {
408                what: "attitude quaternion norm",
409                value: norm,
410            });
411        }
412        let q: DQuat = state.attitude / norm;
413        let to_body = q.conjugate();
414        let mass = self.mass_state(t, window);
415        let m = mass.mass_kg;
416        let r = mass.cg_m;
417        let omega = if matches!(phase, Phase::Free | Phase::TipOff) {
418            state.body_rate_rad_s
419        } else {
420            DVec3::ZERO
421        };
422
423        // Where the center of mass is, and the air there.
424        let v_o = state.velocity_enu_m_s;
425        let cg_enu_m = state.position_enu_m + q.mul_vec3(r);
426        let cg_velocity_enu_m_s = v_o + q.mul_vec3(omega.cross(r) + mass.cg_rate_m_s);
427        let frame = environment.earth.frame();
428        let geodetic = frame.geodetic_from_enu(cg_enu_m)?;
429        let height_above_ground_m = geodetic.height_m - frame.origin().height_m;
430        let up_ecef = DVec3::new(
431            geodetic.latitude_rad.cos() * geodetic.longitude_rad.cos(),
432            geodetic.latitude_rad.cos() * geodetic.longitude_rad.sin(),
433            geodetic.latitude_rad.sin(),
434        );
435        let up_enu = frame.ecef_from_enu_rotation().transpose() * up_ecef;
436        let vertical_speed_m_s = up_enu.dot(cg_velocity_enu_m_s);
437        let height_msl_m = geodetic.height_m - environment.geoid_undulation_m;
438        let air = environment.air_at(height_msl_m)?;
439        let wind_enu = environment.wind_enu_m_s(height_msl_m)?;
440
441        // Weight and the Coriolis force, at the center of mass.
442        let gravity_enu = environment.earth.gravity_enu_mps2(cg_enu_m)?;
443        let coriolis_enu = environment
444            .earth
445            .rotation_acceleration_enu_mps2(cg_velocity_enu_m_s);
446        let weight = to_body.mul_vec3((gravity_enu + coriolis_enu) * m);
447
448        // Thrust and the motors' mass terms. A motor burns during the interval if the interval
449        // ends by its burnout.
450        let (a, b) = window;
451        let pressure_pa = air.pressure_pa;
452        let mut thrust = DVec3::ZERO;
453        let mut thrust_moment = DVec3::ZERO;
454        let mut t03_jet = DVec3::ZERO;
455        let mut t04_jet = DVec3::ZERO;
456        let mut jet_gyration = DMat3::ZERO;
457        // The burning motors' cross-section in the airframe's base, and in each motor pod set's
458        // pods' bases.
459        let mut burning = BurningAreas::default();
460        for (terms, placed) in self.motors.iter().zip(&self.assembly.motors) {
461            if a >= terms.burnout_s || b <= terms.ignition_s {
462                continue;
463            }
464            let motor = &placed.mounted.motor;
465            // On the motor's own clock, from its ignition. Ignitions and burnouts are stop times,
466            // so the motor burns throughout this interval, and a stage evaluated on its ends
467            // (ignition or burnout, where the pressure correction switches) takes the one-sided
468            // limit inside the burn.
469            let (a, b) = (a - terms.ignition_s, b - terms.ignition_s);
470            let t = (t - terms.ignition_s)
471                .max(0.0_f64.next_up())
472                .min(motor.burnout_time_s().next_down());
473            let force = DVec3::Z * motor.thrust_at_pressure_n(t, pressure_pa);
474            thrust += force;
475            thrust_moment += terms.nozzle_m.cross(force);
476            match terms.pod_set {
477                None => burning.airframe_m2 += terms.area_m2,
478                Some(set) => burning.pods_m2[set] += terms.area_m2,
479            }
480            let mdot = -motor.mass_flow_kg_s(t);
481            let mddot = if b - a < MIN_DERIVATIVE_INTERVAL_S {
482                0.0
483            } else {
484                let h = MASS_DERIVATIVE_STEP_S.min(0.5 * (b - a));
485                let c = t.max(a + h).min(b - h);
486                -(motor.mass_flow_kg_s(c + h) - motor.mass_flow_kg_s(c - h)) / (2.0 * h)
487            };
488            let lever = terms.nozzle_m - r;
489            t03_jet += lever * (2.0 * mdot);
490            t04_jet += lever * mddot;
491            let n = terms.nozzle_m;
492            let disc = 0.25 * terms.exit_radius_m * terms.exit_radius_m;
493            let gyration = DMat3::from_diagonal(DVec3::new(disc, disc, 2.0 * disc))
494                + DMat3::from_diagonal(DVec3::splat(n.length_squared()))
495                - outer(n, n);
496            jet_gyration += gyration * mdot;
497        }
498
499        // A thrust history takes the motors' place, at the mean of their nozzle exits; their mass
500        // flow and the base area they free stay theirs ([`crate::histories`]).
501        if let Some(history) = &self.histories.thrust {
502            thrust = DVec3::Z * history.thrust_n(t);
503            thrust_moment = self.histories.nozzle_m.cross(thrust);
504        }
505
506        // Aerodynamics: the airframe in flight, the open canopies during the descent.
507        let aero = if phase == Phase::Descent {
508            canopy_drag(
509                &air,
510                to_body.mul_vec3(cg_velocity_enu_m_s - wind_enu),
511                recovery_drag_area_m2,
512            )
513        } else {
514            self.aerodynamics(&air, to_body.mul_vec3(v_o - wind_enu), omega, r, burning)?
515        };
516
517        // The equations.
518        let forces = aero.force + weight;
519        let t03 = t03_jet - mass.cg_rate_m_s * (2.0 * m);
520        let t04 = thrust - mass.cg_accel_m_s2 * m - mass.cg_rate_m_s * (2.0 * mass.mass_rate_kg_s)
521            + t04_jet;
522        let mut derivative = [0.0; STATE_LEN];
523        let mut acceleration_enu_m_s2 = DVec3::ZERO;
524        let mut rail_force_n = 0.0;
525        match phase {
526            Phase::Free => {
527                let (a_body, omega_dot) = free_motion(
528                    &mass,
529                    omega,
530                    [t03, t04, forces],
531                    jet_gyration,
532                    [r.cross(weight), aero.moment, thrust_moment],
533                )?;
534                acceleration_enu_m_s2 = q.mul_vec3(a_body);
535                let q_dot = quaternion_derivative(state.attitude, omega);
536                derivative = [
537                    v_o.x,
538                    v_o.y,
539                    v_o.z,
540                    acceleration_enu_m_s2.x,
541                    acceleration_enu_m_s2.y,
542                    acceleration_enu_m_s2.z,
543                    q_dot.w,
544                    q_dot.x,
545                    q_dot.y,
546                    q_dot.z,
547                    omega_dot.x,
548                    omega_dot.y,
549                    omega_dot.z,
550                ];
551            }
552            Phase::TipOff => {
553                // The free equations, and what the last guide adds to keep its pivot on the
554                // rail's line and the roll held.
555                let Some(pivot) = pivot else {
556                    return Err(SimError::Domain {
557                        what: "tip-off without a last guide to pivot about",
558                        value: f64::NAN,
559                    });
560                };
561                let (free_acceleration, free_angular) = free_motion(
562                    &mass,
563                    omega,
564                    [t03, t04, forces],
565                    jet_gyration,
566                    [r.cross(weight), aero.moment, thrust_moment],
567                )?;
568                let rail_body = to_body.mul_vec3(pivot.rail_enu);
569                // Which way the pivot slides: up the rail unless it has turned back.
570                let pivot_velocity = to_body.mul_vec3(v_o) + omega.cross(pivot.point_m);
571                let sliding = if pivot_velocity.dot(rail_body) < 0.0 {
572                    -1.0
573                } else {
574                    1.0
575                };
576                let (a_body, omega_dot) = crate::rail::pivot_motion(
577                    (m, r, mass.inertia_cg),
578                    omega,
579                    (free_acceleration, free_angular),
580                    (pivot.point_m, rail_body),
581                    (friction_coefficient, sliding),
582                )?;
583                // The force along the rail, as the rail phase gives it: the pivot's acceleration
584                // there times the mass.
585                let pivot_acceleration = a_body
586                    + omega_dot.cross(pivot.point_m)
587                    + omega.cross(omega.cross(pivot.point_m));
588                rail_force_n = m * pivot_acceleration.dot(rail_body);
589                acceleration_enu_m_s2 = q.mul_vec3(a_body);
590                let q_dot = quaternion_derivative(state.attitude, omega);
591                derivative = [
592                    v_o.x,
593                    v_o.y,
594                    v_o.z,
595                    acceleration_enu_m_s2.x,
596                    acceleration_enu_m_s2.y,
597                    acceleration_enu_m_s2.z,
598                    q_dot.w,
599                    q_dot.x,
600                    q_dot.y,
601                    q_dot.z,
602                    omega_dot.x,
603                    omega_dot.y,
604                    omega_dot.z,
605                ];
606            }
607            Phase::Descent => {
608                // A point mass: the canopies' drag and the weight, with no rotation. The mass
609                // terms of `T04` stay, so a device that opens while a motor burns still feels the
610                // thrust along the axis it froze at.
611                let t20 = t04 + forces;
612                acceleration_enu_m_s2 = q.mul_vec3(t20 / m);
613                derivative[..3].copy_from_slice(&v_o.to_array());
614                derivative[3..6].copy_from_slice(&acceleration_enu_m_s2.to_array());
615            }
616            Phase::Rail | Phase::Pad => {
617                // No rotation: the rail supplies the moments and the force across the axis.
618                let t20 = t04 + forces;
619                let across = (t20.x * t20.x + t20.y * t20.y).sqrt();
620                rail_force_n = t20.z - friction_coefficient * across;
621                if phase == Phase::Rail {
622                    let along = q.mul_vec3(DVec3::Z);
623                    acceleration_enu_m_s2 = along * (rail_force_n / m);
624                    derivative[..3].copy_from_slice(&v_o.to_array());
625                    derivative[3..6].copy_from_slice(&acceleration_enu_m_s2.to_array());
626                }
627            }
628        }
629
630        Ok(Evaluation {
631            derivative,
632            mass,
633            cg_enu_m,
634            cg_velocity_enu_m_s,
635            height_above_ground_m,
636            vertical_speed_m_s,
637            airspeed_m_s: aero.airspeed_m_s,
638            mach: aero.mach,
639            angle_of_attack_rad: aero.angle_of_attack_rad,
640            dynamic_pressure_pa: aero.dynamic_pressure_pa,
641            axial_coefficient: aero.axial_coefficient,
642            thrust_n: thrust.z,
643            rail_force_n,
644            acceleration_enu_m_s2,
645            specific_force_enu_m_s2: acceleration_enu_m_s2 - (gravity_enu + coriolis_enu),
646            recovery_drag_area_m2: if phase == Phase::Descent {
647                recovery_drag_area_m2
648            } else {
649                0.0
650            },
651        })
652    }
653
654    /// The aerodynamic force (body axes) and its moment about the nose tip.
655    ///
656    /// - The axial force is `−q A C_A z_B` from the whole rocket's drag at the center of mass's
657    ///   airspeed; it acts along the axis, so it has no moment about the nose tip.
658    /// - Each component's normal and side force comes from its own local flow: the nose tip's air
659    ///   velocity plus `ω × p` at the component's small-angle center of pressure `p` (at the
660    ///   center of mass's Mach number), which gives the aerodynamic damping in pitch and yaw.
661    ///   `C_N` acts along the crossing air `ŵ`, `C_Y` along `z_B × ŵ`, at the stations their
662    ///   moments give (`docs/physics/frames.md`).
663    /// - Fin sets use `sin α` in place of their model's `α`, so their force vanishes when the air
664    ///   comes from the tail as well as from the nose.
665    /// - With a normal-force table ([`hpr_aero::NormalForceTable`]), the table's normal force at
666    ///   the center of mass's airflow acts at its center of pressure, and each component adds only
667    ///   its force in its local flow less its force in the center of mass's: the damping, which
668    ///   stays HPR Sim's (ADR-032).
669    /// - The rolling moment about `z_B` is `q A d (C_l0 cos α + C_lp p d/2V)` from the fins' cant
670    ///   and the roll rate `p = ω_z` at the center of mass's Mach number
671    ///   ([`hpr_aero::AeroModel::roll`]); the cant's forcing follows the axial flow.
672    fn aerodynamics(
673        &self,
674        air: &hpr_atmos::AirState,
675        air_velocity_o_body: DVec3,
676        omega: DVec3,
677        cg_m: DVec3,
678        burning: BurningAreas,
679    ) -> Result<Aerodynamics, SimError> {
680        let rho = air.density_kg_m3;
681        let sound = air.speed_of_sound_m_s;
682        let area = self.reference_area_m2;
683        let v_cg = air_velocity_o_body + omega.cross(cg_m);
684        let speed = v_cg.length();
685        let mut out = Aerodynamics {
686            airspeed_m_s: speed,
687            mach: speed / sound,
688            ..Aerodynamics::default()
689        };
690        if rho <= 0.0 || speed < MIN_AIRSPEED_M_S {
691            return Ok(out);
692        }
693        let angles = flow_angles(v_cg);
694        let (alpha, roll) = (angles.alpha_rad, angles.roll_rad);
695        out.angle_of_attack_rad = alpha;
696        let q = 0.5 * rho * speed * speed;
697        out.dynamic_pressure_pa = q;
698        let reynolds_per_m = speed / air.kinematic_viscosity_m2_s();
699        let conditions = if burning.airframe_m2 > 0.0 || burning.pods_m2.iter().any(|&a| a > 0.0) {
700            DragConditions::thrusting(reynolds_per_m, burning.airframe_m2)
701                .with_pod_motors(burning.pods_m2)
702        } else {
703            DragConditions::coasting(reynolds_per_m)
704        };
705        let flow = Flow::new(out.mach, alpha, roll);
706        let drag = self.aero.drag(&flow, &conditions)?;
707        out.axial_coefficient = drag.axial_coefficient;
708        out.force = DVec3::new(0.0, 0.0, -q * area * drag.axial_coefficient);
709
710        // The normal force's range, checked once here, before the components' stations.
711        flow.validate()?;
712        // With a normal-force table, the table gives the static normal force at the center of
713        // mass's flow, and each component only the difference its rotation makes: its force in
714        // its own local flow less its force in the center of mass's. That difference is hpr's
715        // pitch and yaw damping, which a table doesn't carry (ADR-032).
716        let table = self.aero.normal_force_table().is_some();
717        if table {
718            let normal = self.aero.normal_force(&flow)?;
719            let across = DVec3::new(angles.cos_roll, angles.sin_roll, 0.0);
720            let side = DVec3::Z.cross(across);
721            out.force += across * (normal.coefficient * q * area);
722            out.moment += side * (-normal.moment_m * q * area);
723        }
724        for index in 0..self.aero.component_count() {
725            // A fin set's station moves with Mach, and so does a body's faster than sound. It is
726            // the component's center of pressure where one model carries it, and between the two
727            // models it is not (issue #106).
728            let station = self.aero.component_station_m(index, out.mach)?;
729            let p = DVec3::new(0.0, 0.0, -station);
730            let (force, moment) =
731                self.component_force(index, air_velocity_o_body + omega.cross(p), rho, sound)?;
732            out.force += force;
733            out.moment += moment;
734            if table {
735                let (force, moment) = self.component_force(index, v_cg, rho, sound)?;
736                out.force -= force;
737                out.moment -= moment;
738            }
739        }
740        // Roll about the axis, from the fins' cant and against the roll rate:
741        // `q A d (C_l0 cos α + C_lp p d/2V)`, the damping written as `ρ V A d² C_lp p/4`. The
742        // cant meets the air as it runs along the axis, so its forcing follows the axial flow,
743        // `cos α`: none broadside, reversed tail first, as the fins' normal force follows
744        // `sin α` (ADR-011).
745        let roll = self.aero.roll(out.mach)?;
746        let d = self.aero.reference_diameter_m();
747        out.moment.z += q * area * d * roll.forcing * angles.cos_alpha
748            + 0.25 * rho * speed * area * d * d * roll.damping * omega.z;
749        Ok(out)
750    }
751}
752
753impl Vehicle {
754    /// Component `index`'s normal and side force (body axes) and their moment about the nose tip,
755    /// in the local air velocity `local` (body axes); zero below [`MIN_AIRSPEED_M_S`].
756    fn component_force(
757        &self,
758        index: usize,
759        local: DVec3,
760        rho: f64,
761        sound: f64,
762    ) -> Result<(DVec3, DVec3), SimError> {
763        let local_speed = local.length();
764        if local_speed < MIN_AIRSPEED_M_S {
765            return Ok((DVec3::ZERO, DVec3::ZERO));
766        }
767        let angles = flow_angles(local);
768        let alpha_i = angles.alpha_rad;
769        let normal = self.aero.component_normal_force(
770            index,
771            &Flow::new(local_speed / sound, alpha_i, angles.roll_rad),
772        )?;
773        // Fin normal force follows the crossflow `V sin α`, as the body terms do: the small-angle
774        // slope times `sin α` rather than `α`, so it vanishes for axial flow either way (ADR-011).
775        let fin_scale = if index >= self.first_fin_index && alpha_i > 0.0 {
776            angles.sin_alpha / alpha_i
777        } else {
778            1.0
779        };
780        let q_i = 0.5 * rho * local_speed * local_speed * self.reference_area_m2 * fin_scale;
781        let across = DVec3::new(angles.cos_roll, angles.sin_roll, 0.0);
782        let side = DVec3::Z.cross(across);
783        Ok((
784            (across * normal.coefficient + side * normal.side_coefficient) * q_i,
785            (side * -normal.moment_m + across * normal.side_moment_m) * q_i,
786        ))
787    }
788}
789
790/// The aerodynamic results of one evaluation.
791#[derive(Debug, Clone, Copy, Default)]
792struct Aerodynamics {
793    force: DVec3,
794    moment: DVec3,
795    airspeed_m_s: f64,
796    mach: f64,
797    angle_of_attack_rad: f64,
798    dynamic_pressure_pa: f64,
799    axial_coefficient: f64,
800}
801
802/// The free-flight equations solved for the nose tip's acceleration `a_O` and the angular
803/// acceleration `ω̇`, both in body axes: `T20` from `[T03, T04, F]` (the Coriolis-like term's
804/// vector, the terms along the axis, and the external force), and `T21` from the jets' gyration
805/// and the moments about `O`, less the moving parts' `ω × h + h′`.
806pub(crate) fn free_motion(
807    mass: &MassState,
808    omega: DVec3,
809    [t03, t04, forces]: [DVec3; 3],
810    jet_gyration: DMat3,
811    [weight_moment, aero_moment, thrust_moment]: [DVec3; 3],
812) -> Result<(DVec3, DVec3), SimError> {
813    let (m, r) = (mass.mass_kg, mass.cg_m);
814    let t20 = -omega.cross(omega.cross(r * m)) + omega.cross(t03) + t04 + forces;
815    let mut t21 = -omega.cross(mass.inertia_o * omega)
816        + (jet_gyration - mass.inertia_o_rate) * omega
817        + weight_moment
818        + aero_moment
819        + thrust_moment;
820    if mass.relative_momentum != DVec3::ZERO || mass.relative_momentum_rate != DVec3::ZERO {
821        t21 -= omega.cross(mass.relative_momentum) + mass.relative_momentum_rate;
822    }
823    let determinant = mass.inertia_cg.determinant();
824    if !(determinant.is_finite() && determinant > 0.0) {
825        return Err(SimError::Domain {
826            what: "determinant of the inertia about the center of mass",
827            value: determinant,
828        });
829    }
830    let omega_dot = mass.inertia_cg.inverse() * (t21 - r.cross(t20));
831    Ok((t20 / m - omega_dot.cross(r), omega_dot))
832}
833
834/// The drag of the open recovery devices: `D = −½ ρ (C_D S) |v| v` on the center of mass's air
835/// velocity `air_velocity_cg_body` (body axes), with no moment about it.
836///
837/// Source: Knacke's steady drag on the drag area `C_D S` (`docs/physics/recovery.md`), the same
838/// form RocketPy's parachute phase uses (`flight.py:2770-2774`, MIT). The airframe's own drag is
839/// left out, as RocketPy leaves it out: the rocket's attitude under a canopy is not modeled.
840fn canopy_drag(
841    air: &hpr_atmos::AirState,
842    air_velocity_cg_body: DVec3,
843    drag_area_m2: f64,
844) -> Aerodynamics {
845    let speed = air_velocity_cg_body.length();
846    let rho = air.density_kg_m3;
847    let mut out = Aerodynamics {
848        airspeed_m_s: speed,
849        mach: speed / air.speed_of_sound_m_s,
850        ..Aerodynamics::default()
851    };
852    if rho <= 0.0 || speed < MIN_AIRSPEED_M_S || drag_area_m2 <= 0.0 {
853        return out;
854    }
855    out.angle_of_attack_rad = flow_angles(air_velocity_cg_body).alpha_rad;
856    out.dynamic_pressure_pa = 0.5 * rho * speed * speed;
857    out.force = air_velocity_cg_body * (-0.5 * rho * drag_area_m2 * speed);
858    out
859}
860
861/// The total angle of attack and the flow roll of a body moving at `v` (body axes) through still
862/// air, with their cosines and sines (`docs/physics/frames.md`).
863#[derive(Debug, Clone, Copy, PartialEq)]
864struct FlowAngles {
865    /// `α`, rad in `[0, π]`: the angle between `z_B` and `v`.
866    alpha_rad: f64,
867    /// `φ`, rad in `[−π, π]`: the direction the air crosses the body, from `x_B` toward `y_B`,
868    /// opposite the lateral velocity; zero when there is none.
869    roll_rad: f64,
870    /// `cos α = v_z/|v|`: within a few ulps of `cos(alpha_rad)`, as are the three below.
871    cos_alpha: f64,
872    /// `sin α = w/|v|`, `w` the crossflow. `w` comes from `hypot` and `|v|` from a square root of
873    /// the sum of squares, which round apart, so it can exceed 1 by an ulp near α = 90°: clamp it
874    /// before any `asin` or `sqrt(1 − sin²α)`.
875    sin_alpha: f64,
876    /// `cos φ = −v_x/w`.
877    cos_roll: f64,
878    /// `sin φ = −v_y/w`.
879    sin_roll: f64,
880}
881
882/// The speed below which [`flow_angles`] takes the cosines and sines from the angles, `2⁻⁵⁰⁰`:
883/// above it `|v|`'s squares lose nothing to underflow that a last bit would show.
884const FLOW_RATIO_MIN_SPEED: f64 = 3.054_936_363_499_605e-151;
885
886/// The flow angles of a body moving at `v` (body axes) through still air
887/// (`docs/physics/frames.md`): with the crossflow `w = √(v_x² + v_y²)`,
888///
889/// - `α = atan2(w, v_z)`, and `cos α = v_z / |v|`, `sin α = w / |v|`;
890/// - `φ = atan2(−v_y, −v_x)`, and `cos φ = −v_x / w`, `sin φ = −v_y / w`; with no crossflow
891///   (`v_x = v_y = 0`) `φ = 0`, so `cos φ = 1` and `sin φ = 0`.
892///
893/// The cosines and sines are the same numbers as `cos` and `sin` of the angles, taken from the
894/// components without the round trip through `atan2` (issue #285); they agree with that round trip
895/// to a few units in the last place of 1. When `|v|` or `w` is too small or too large for the
896/// quotients to hold every digit (zero, subnormal, overflowing or NaN), they come from the angles
897/// as before, so the degenerate cases (`atan2(±0, ±0)`) give what they always gave.
898fn flow_angles(v: DVec3) -> FlowAngles {
899    let crossflow = v.x.hypot(v.y);
900    // `atan2` keeps full precision near 0 and π, where `acos` loses half the digits.
901    let alpha_rad = crossflow.atan2(v.z);
902    let speed = v.length();
903    let (cos_alpha, sin_alpha) = if (FLOW_RATIO_MIN_SPEED..=f64::MAX).contains(&speed) {
904        (v.z / speed, crossflow / speed)
905    } else {
906        (alpha_rad.cos(), alpha_rad.sin())
907    };
908    let (roll_rad, cos_roll, sin_roll) = if v.x == 0.0 && v.y == 0.0 {
909        (0.0, 1.0, 0.0)
910    } else {
911        let roll = (-v.y).atan2(-v.x);
912        if (f64::MIN_POSITIVE..=f64::MAX).contains(&crossflow) {
913            (roll, -v.x / crossflow, -v.y / crossflow)
914        } else {
915            (roll, roll.cos(), roll.sin())
916        }
917    };
918    FlowAngles {
919        alpha_rad,
920        roll_rad,
921        cos_alpha,
922        sin_alpha,
923        cos_roll,
924        sin_roll,
925    }
926}
927
928/// `u vᵀ`.
929fn outer(u: DVec3, v: DVec3) -> DMat3 {
930    DMat3::from_cols(u * v.x, u * v.y, u * v.z)
931}
932
933#[cfg(test)]
934mod tests {
935    use super::*;
936    use crate::testing::{UniformAir, analytic_environment, analytic_wind_environment, design};
937    use hpr_atmos::ConstantWind;
938
939    fn valetudo() -> Vehicle {
940        let assembly = design("rocketpy-valetudo").assemble("example").unwrap();
941        let aero = AeroModel::new(&assembly.layout).unwrap();
942        Vehicle::new(assembly, aero).unwrap()
943    }
944
945    /// The engine's pitch damping is [`AeroModel::pitch_damping`]'s `C_mq` (M1.14b2): the Basic
946    /// Finner with no motor, pitching slowly about its center of mass with the air straight along
947    /// its axis there, feels `q̄ A d C_mq (q d/(2V))` about the center of mass, below and above
948    /// Mach 1. The read-out is what the comparison with DREV's range fits uses, so it must be the
949    /// engine's damping and not a model beside it.
950    #[expect(
951        clippy::approx_constant,
952        reason = "the range table's fifth Mach number is 2.718: a Mach number, not e"
953    )]
954    #[test]
955    fn the_engine_s_pitch_damping_is_the_model_s_read_out() {
956        let mut rocket = serde_json::to_value(design("range-basic-finner")).unwrap();
957        rocket["configurations"] = serde_json::json!([{ "id": "coast", "motors": [] }]);
958        let rocket: hpr_design::Rocket = serde_json::from_value(rocket).unwrap();
959        let assembly = rocket.assemble("coast").unwrap();
960        let aero = AeroModel::new(&assembly.layout).unwrap();
961        let vehicle = Vehicle::new(assembly, aero).unwrap();
962        let air = UniformAir::sea_level().0;
963        let cg = vehicle.assembly.mass_properties(0.0).cg_m;
964        let d = vehicle.aero.reference_diameter_m();
965        // A pitch rate of `q d/(2V)` = 1e-5. The body lift grows as the angle times its size, so
966        // the two part by a share that falls with the rate; at this one it is inside the 1e-4
967        // held below.
968        let rate = 1e-5;
969        for mach in [0.5, 1.077, 1.293, 1.832, 2.375, 2.718, 3.147] {
970            let speed = mach * air.speed_of_sound_m_s;
971            let omega = DVec3::new(2.0 * rate * speed / d, 0.0, 0.0);
972            let along = DVec3::new(0.0, 0.0, speed);
973            let out = vehicle
974                .aerodynamics(
975                    &air,
976                    along - omega.cross(cg),
977                    omega,
978                    cg,
979                    BurningAreas::default(),
980                )
981                .unwrap();
982            assert_eq!(out.angle_of_attack_rad, 0.0, "Mach {mach}");
983            let about_cg = out.moment - cg.cross(out.force);
984            let q_area_d = 0.5 * air.density_kg_m3 * speed * speed * vehicle.reference_area_m2 * d;
985            let engine = about_cg.x / (q_area_d * rate);
986            let read_out = vehicle.aero.pitch_damping(mach, -cg.z).unwrap();
987            assert!(read_out < 0.0, "Mach {mach}: {read_out}");
988            assert!(
989                (engine / read_out - 1.0).abs() < 1e-4,
990                "Mach {mach}: the engine's {engine}, the read-out's {read_out}"
991            );
992            // Nothing in yaw or roll from a pure pitch rate.
993            assert!(about_cg.y.abs() < 1e-9 * about_cg.x.abs(), "Mach {mach}");
994            assert!(about_cg.z.abs() < 1e-9 * about_cg.x.abs(), "Mach {mach}");
995        }
996    }
997
998    /// A motor in a pod burns into its pod's base, not the airframe's (ADR-092): Valetudo with
999    /// two pods, each its own motor tube, takes the pods' two motors apart from its own.
1000    #[test]
1001    fn a_pod_s_motors_burn_into_the_pod_s_base() {
1002        let mut rocket = serde_json::to_value(design("rocketpy-valetudo")).unwrap();
1003        let material =
1004            serde_json::json!({ "name": "test", "density": { "kind": "bulk", "kg_m3": 1000.0 } });
1005        let pods = serde_json::json!({
1006            "id": "pods",
1007            "part": { "pod_set": { "count": 2, "radial_offset_m": 0.1, "angle_rad": 0.0 } },
1008            "position": { "from": "top", "aft_offset_m": 0.8 },
1009            "children": [{
1010                "id": "pod-tube",
1011                "part": { "body_tube": {
1012                    "length_m": 0.9, "outer_radius_m": 0.02, "thickness_m": 0.002,
1013                    "material": material } },
1014                "motor_mount": { "overhang_m": 0.0 }
1015            }]
1016        });
1017        rocket["stages"][0]["components"][1]["children"]
1018            .as_array_mut()
1019            .unwrap()
1020            .push(pods);
1021        let mut pod_motor = rocket["configurations"][0]["motors"][0].clone();
1022        pod_motor["mount"] = serde_json::json!("pod-tube");
1023        rocket["configurations"][0]["motors"]
1024            .as_array_mut()
1025            .unwrap()
1026            .push(pod_motor);
1027        let rocket: hpr_design::Rocket = serde_json::from_value(rocket).unwrap();
1028        let assembly = rocket.assemble("example").unwrap();
1029        let aero = AeroModel::new(&assembly.layout).unwrap();
1030        let vehicle = Vehicle::new(assembly, aero).unwrap();
1031        let pod_sets: Vec<_> = vehicle.motors.iter().map(|m| m.pod_set).collect();
1032        assert_eq!(pod_sets, [None, Some(0), Some(0)]);
1033        let area = vehicle.motors[1].area_m2;
1034        assert!(area > 0.0 && vehicle.motors[2].area_m2 == area);
1035
1036        // The engine hands the pods' area to the pods' bases: its axial coefficient is the drag
1037        // model's with the two pod motors, and not with them on the airframe's base. A base's drag
1038        // is linear in its area until the base runs out, so the pods are narrower than their
1039        // motors here, for the two to differ.
1040        let air = UniformAir::sea_level().0;
1041        let speed = 50.0;
1042        let v = DVec3::new(0.0, 0.0, speed);
1043        let cg = vehicle.assembly.mass_properties(0.0).cg_m;
1044        let axial = |[airframe_m2, pods_m2]: [f64; 2]| {
1045            let areas = BurningAreas {
1046                airframe_m2,
1047                pods_m2: [pods_m2, 0.0, 0.0, 0.0],
1048            };
1049            vehicle
1050                .aerodynamics(&air, v, DVec3::ZERO, cg, areas)
1051                .unwrap()
1052                .axial_coefficient
1053        };
1054        let flow = Flow::new(speed / air.speed_of_sound_m_s, 0.0, 0.0);
1055        let reynolds_per_m = speed / air.kinematic_viscosity_m2_s();
1056        let pods = DragConditions::thrusting(reynolds_per_m, 0.0).with_pod_motors([
1057            2.0 * area,
1058            0.0,
1059            0.0,
1060            0.0,
1061        ]);
1062        let expected = vehicle.aero.drag(&flow, &pods).unwrap().axial_coefficient;
1063        assert_eq!(axial([0.0, 2.0 * area]), expected);
1064        assert!(std::f64::consts::PI * 0.02_f64.powi(2) < area);
1065        assert!((axial([2.0 * area, 0.0]) - expected).abs() > 1e-3);
1066    }
1067
1068    /// Motors in two pod sets burn into their own sets' bases (ADR-168): Valetudo with a pair of
1069    /// pods narrower than their motors and a single pod wider than its. Each set's area comes off
1070    /// its own pods, so moving area between the sets moves the drag; past [`MOTOR_POD_SETS`] sets
1071    /// the aerodynamic model refuses the rocket by name.
1072    #[test]
1073    fn two_pod_sets_motors_burn_into_their_own_bases() {
1074        let material =
1075            serde_json::json!({ "name": "test", "density": { "kind": "bulk", "kg_m3": 1000.0 } });
1076        let pod_set = |id: &str, count: u32, radius_m: f64, angle_rad: f64| {
1077            serde_json::json!({
1078                "id": id,
1079                "part": { "pod_set": {
1080                    "count": count, "radial_offset_m": 0.15, "angle_rad": angle_rad } },
1081                "position": { "from": "top", "aft_offset_m": 0.8 },
1082                "children": [{
1083                    "id": format!("{id}-tube"),
1084                    "part": { "body_tube": {
1085                        "length_m": 0.9, "outer_radius_m": radius_m, "thickness_m": 0.002,
1086                        "material": material } },
1087                    "motor_mount": { "overhang_m": 0.0 }
1088                }]
1089            })
1090        };
1091        let with_sets = |sets: &[(&str, u32, f64)]| {
1092            let mut rocket = serde_json::to_value(design("rocketpy-valetudo")).unwrap();
1093            for (k, &(id, count, radius_m)) in sets.iter().enumerate() {
1094                rocket["stages"][0]["components"][1]["children"]
1095                    .as_array_mut()
1096                    .unwrap()
1097                    .push(pod_set(id, count, radius_m, 0.3 * k as f64));
1098                let mut motor = rocket["configurations"][0]["motors"][0].clone();
1099                motor["mount"] = serde_json::json!(format!("{id}-tube"));
1100                rocket["configurations"][0]["motors"]
1101                    .as_array_mut()
1102                    .unwrap()
1103                    .push(motor);
1104            }
1105            serde_json::from_value::<hpr_design::Rocket>(rocket).unwrap()
1106        };
1107        let rocket = with_sets(&[("pair", 2, 0.02), ("single", 1, 0.06)]);
1108        let assembly = rocket.assemble("example").unwrap();
1109        let aero = AeroModel::new(&assembly.layout).unwrap();
1110        let vehicle = Vehicle::new(assembly, aero).unwrap();
1111        let pod_sets: Vec<_> = vehicle.motors.iter().map(|m| m.pod_set).collect();
1112        assert_eq!(pod_sets, [None, Some(0), Some(0), Some(1)]);
1113        let area = vehicle.motors[1].area_m2;
1114        assert!(std::f64::consts::PI * 0.02_f64.powi(2) < area);
1115        assert!(area < std::f64::consts::PI * 0.06_f64.powi(2) - area);
1116
1117        let air = UniformAir::sea_level().0;
1118        let speed = 50.0;
1119        let v = DVec3::new(0.0, 0.0, speed);
1120        let cg = vehicle.assembly.mass_properties(0.0).cg_m;
1121        let axial = |pods_m2: [f64; MOTOR_POD_SETS]| {
1122            let areas = BurningAreas {
1123                airframe_m2: 0.0,
1124                pods_m2,
1125            };
1126            vehicle
1127                .aerodynamics(&air, v, DVec3::ZERO, cg, areas)
1128                .unwrap()
1129                .axial_coefficient
1130        };
1131        let flow = Flow::new(speed / air.speed_of_sound_m_s, 0.0, 0.0);
1132        let reynolds_per_m = speed / air.kinematic_viscosity_m2_s();
1133        let split = [2.0 * area, area, 0.0, 0.0];
1134        let conditions = DragConditions::thrusting(reynolds_per_m, 0.0).with_pod_motors(split);
1135        let expected = vehicle
1136            .aero
1137            .drag(&flow, &conditions)
1138            .unwrap()
1139            .axial_coefficient;
1140        assert_eq!(axial(split), expected);
1141        // All of it in the pair, whose bases are already used up, leaves the single pod's base
1142        // whole; all of it in the single pod overfills that base.
1143        assert!(axial([3.0 * area, 0.0, 0.0, 0.0]) - expected > 1e-4);
1144        assert!((axial([0.0, 3.0 * area, 0.0, 0.0]) - expected).abs() > 1e-4);
1145        // The single pod's own area moves its base; more on the pair, already used up, moves
1146        // nothing.
1147        assert!(axial([2.0 * area, 0.0, 0.0, 0.0]) > expected);
1148        assert_eq!(axial([5.0 * area, area, 0.0, 0.0]), expected);
1149
1150        let five: Vec<(String, u32, f64)> = (0..5).map(|k| (format!("set-{k}"), 1, 0.06)).collect();
1151        let five: Vec<(&str, u32, f64)> = five
1152            .iter()
1153            .map(|(id, n, r)| (id.as_str(), *n, *r))
1154            .collect();
1155        let layout = with_sets(&five).assemble("example").unwrap().layout;
1156        match AeroModel::new(&layout) {
1157            Err(hpr_aero::AeroError::Unsupported(why)) => {
1158                assert!(why.contains("motor mounts in 5 pod sets"), "{why}");
1159            }
1160            other => panic!("not refused: {other:?}"),
1161        }
1162    }
1163
1164    #[test]
1165    fn a_supersonic_flow_takes_the_body_s_terms_at_its_mach_number() {
1166        // Faster than sound the nose and the cylinder behind it take the shock-expansion shares at
1167        // the flow's Mach number (ADR-034). With no rotation every component sees the same flow,
1168        // so the normal force is `q A Σ C_N,i` and its moment `q A Σ C_N,i X_i`, fins at
1169        // `sin α/α` (ADR-011).
1170        let vehicle = valetudo();
1171        let aero = &vehicle.aero;
1172        let covered = aero
1173            .supersonic_body()
1174            .expect("Valetudo's ogive nose is pointed")
1175            .covered;
1176        let air = UniformAir::sea_level().0;
1177        let alpha: f64 = 0.02;
1178        let cg = vehicle.assembly.mass_properties(10.0).cg_m;
1179        let normal = |mach: f64| {
1180            let speed = mach * air.speed_of_sound_m_s;
1181            let v = DVec3::new(alpha.sin(), 0.0, alpha.cos()) * speed;
1182            let out = vehicle
1183                .aerodynamics(&air, v, DVec3::ZERO, cg, BurningAreas::default())
1184                .unwrap();
1185            let q_area = 0.5 * air.density_kg_m3 * speed * speed * vehicle.reference_area_m2;
1186            let angles = flow_angles(v);
1187            let (a, roll) = (angles.alpha_rad, angles.roll_rad);
1188            let flow = Flow::new(mach, a, roll);
1189            let (mut force, mut moment, mut body) = (0.0, 0.0, 0.0);
1190            for index in 0..aero.component_count() {
1191                let n = aero.component_normal_force(index, &flow).unwrap();
1192                let scale = if index >= vehicle.first_fin_index {
1193                    a.sin() / a
1194                } else {
1195                    body += n.coefficient * f64::from(u8::from(index < covered));
1196                    1.0
1197                };
1198                force += n.coefficient * scale;
1199                moment += n.moment_m * scale;
1200            }
1201            let got = (
1202                out.force.truncate().length() / q_area,
1203                out.moment.truncate().length() / q_area,
1204            );
1205            (got, (force, moment), body)
1206        };
1207        for mach in [1.0, 2.0, 3.0] {
1208            let (got, want, _) = normal(mach);
1209            assert!(
1210                (got.0 - want.0).abs() <= 1e-12 * want.0,
1211                "Mach {mach}: {got:?} {want:?}"
1212            );
1213            assert!(
1214                (got.1 - want.1).abs() <= 1e-12 * want.1,
1215                "Mach {mach}: {got:?} {want:?}"
1216            );
1217        }
1218        // The nose and cylinder lift more at Mach 2 than slender-body theory's Mach-free terms.
1219        let (_, _, slender) = normal(1.0);
1220        let (_, _, supersonic) = normal(2.0);
1221        assert!(supersonic > 1.1 * slender, "{supersonic} vs {slender}");
1222    }
1223
1224    #[test]
1225    fn mass_rates_match_the_motor_and_stay_inside_the_interval() {
1226        // The mass rate from the differences is the motor's `−F/c`; a window ending just after `t`
1227        // keeps the stencil inside it without losing accuracy.
1228        let vehicle = valetudo();
1229        let motor = &vehicle.assembly.motors[0].mounted.motor;
1230        for (t, window) in [
1231            (1.5, (1.4, 1.6)),
1232            (1.5, (1.0, 1.50001)),
1233            (0.00002, (0.0, 0.1)),
1234        ] {
1235            let state = vehicle.mass_state(t, window);
1236            let expected = -motor.state(t).mass_flow_kg_s;
1237            assert!(
1238                (state.mass_rate_kg_s - expected).abs() <= 1e-6 * expected.abs(),
1239                "t {t}: {} vs {expected}",
1240                state.mass_rate_kg_s
1241            );
1242        }
1243        let coasting = vehicle.mass_state(10.0, (9.0, 11.0));
1244        assert_eq!(coasting.mass_rate_kg_s, 0.0);
1245        assert_eq!(coasting.inertia_o_rate, DMat3::ZERO);
1246    }
1247
1248    /// Under a mass history the inertia about the nose tip is the assembly's own about its
1249    /// center of mass, moved there by the parallel-axis theorem with the history's mass and
1250    /// center, `I_O = I_c + m (|r|² E − r rᵀ)`, and its rate is that expression's derivative:
1251    /// `ṁ (|r|² E − r rᵀ) + m (2 (r·r′) E − r′ rᵀ − r r′ᵀ)` with `I_c` fixed after burnout.
1252    #[test]
1253    fn a_mass_history_moves_the_assembly_s_inertia_by_the_parallel_axis_theorem() {
1254        let mut vehicle = valetudo();
1255        let own = vehicle
1256            .assembly
1257            .mass_properties_lit(10.0, &vehicle.ignition_s);
1258        // 20 kg falling to 16 kg and a center moving 1.0 m to 1.2 m aft over 20 s: at 10 s,
1259        // 18 kg at 1.1 m, ṁ = −0.2 kg/s and r′_z = −0.01 m/s (the center moves aft, −z_B).
1260        let history =
1261            crate::histories::MassHistory::new(vec![0.0, 20.0], vec![20.0, 16.0], vec![1.0, 1.2])
1262                .unwrap();
1263        vehicle.histories.mass = Some(history);
1264        let state = vehicle.mass_state(10.0, (9.0, 11.0));
1265
1266        let (m, m_rate) = (18.0, -0.2);
1267        let r = DVec3::new(own.cg_m.x, own.cg_m.y, -1.1);
1268        let r_rate = DVec3::new(0.0, 0.0, -0.01);
1269        let outer = |a: DVec3, b: DVec3| DMat3::from_cols(a * b.x, a * b.y, a * b.z);
1270        let transfer = DMat3::IDENTITY * r.length_squared() - outer(r, r);
1271        let expected = own.inertia_kg_m2 + transfer * m;
1272        let expected_rate = transfer * m_rate
1273            + (DMat3::IDENTITY * (2.0 * r.dot(r_rate)) - outer(r_rate, r) - outer(r, r_rate)) * m;
1274
1275        assert_eq!(state.inertia_cg, own.inertia_kg_m2);
1276        let close = |a: DMat3, b: DMat3, tolerance: f64| {
1277            (0..3).all(|i| (a.col(i) - b.col(i)).abs().max_element() <= tolerance)
1278        };
1279        assert!(
1280            close(state.inertia_o, expected, 1e-12 * expected.col(0).x.abs()),
1281            "{:?} vs {expected:?}",
1282            state.inertia_o
1283        );
1284        assert!(
1285            close(
1286                state.inertia_o_rate,
1287                expected_rate,
1288                1e-6 * expected_rate.col(0).x.abs()
1289            ),
1290            "{:?} vs {expected_rate:?}",
1291            state.inertia_o_rate
1292        );
1293        assert!((state.mass_rate_kg_s - m_rate).abs() < 1e-9);
1294        assert!((state.cg_rate_m_s - r_rate).length() < 1e-9);
1295    }
1296
1297    #[test]
1298    fn rail_friction_is_coulomb_on_the_force_across_the_rail() {
1299        // In a vacuum on a rail at 60°, friction removes exactly μ g cos E from the acceleration
1300        // along the rail: the weight's share across the rail is the only normal force, and the
1301        // mass terms act along the axis.
1302        let vehicle = valetudo();
1303        let environment = analytic_environment(UniformAir::vacuum(), 9.806_65);
1304        let elevation = std::f64::consts::FRAC_PI_3;
1305        let state = State {
1306            position_enu_m: DVec3::new(0.0, 0.0, 10.0),
1307            velocity_enu_m_s: DVec3::ZERO,
1308            attitude: crate::rail::Rail {
1309                elevation_rad: elevation,
1310                ..crate::rail::Rail::vertical(3.0)
1311            }
1312            .attitude(),
1313            body_rate_rad_s: DVec3::ZERO,
1314        };
1315        let along = |mu: f64| {
1316            vehicle
1317                .evaluate(
1318                    &environment,
1319                    mu,
1320                    Conditions {
1321                        phase: Phase::Rail,
1322                        window: (1.0, 2.0),
1323                        drag_area_m2: 0.0,
1324                        pivot: None,
1325                    },
1326                    1.5,
1327                    &state.to_array(),
1328                )
1329                .unwrap()
1330        };
1331        let (free, rubbing) = (along(0.0), along(0.3));
1332        let mass = vehicle.mass_state(1.5, (1.0, 2.0)).mass_kg;
1333        let expected = 0.3 * mass * 9.806_65 * elevation.cos();
1334        assert!((free.rail_force_n - rubbing.rail_force_n - expected).abs() < 1e-9 * expected);
1335        let direction = state.unit_attitude().mul_vec3(DVec3::Z);
1336        let difference = free.acceleration_enu_m_s2 - rubbing.acceleration_enu_m_s2;
1337        assert!((difference - direction * (expected / mass)).length() < 1e-12);
1338    }
1339
1340    /// The tip-off phase's state for [`valetudo`] on a rail at `elevation_rad` heading north,
1341    /// sliding at `speed_m_s` with its axis along the rail and no turning rate, and the pivot:
1342    /// the middle of its last guide, on the rail's line.
1343    fn tipping(elevation_rad: f64, speed_m_s: f64) -> (Vehicle, State, crate::rail::Pivot) {
1344        let vehicle = valetudo();
1345        let rail = crate::rail::Rail {
1346            elevation_rad,
1347            ..crate::rail::Rail::vertical(5.0)
1348        };
1349        let guides = crate::rail::Guides::of(&vehicle.assembly);
1350        let pivot = crate::rail::Pivot {
1351            point_m: DVec3::new(0.0, 0.0, -guides.pivot_station_m.unwrap()),
1352            rail_enu: rail.direction_enu(),
1353        };
1354        let state = State {
1355            position_enu_m: DVec3::new(0.0, 0.0, 10.0),
1356            velocity_enu_m_s: rail.direction_enu() * speed_m_s,
1357            attitude: rail.attitude(),
1358            body_rate_rad_s: DVec3::ZERO,
1359        };
1360        (vehicle, state, pivot)
1361    }
1362
1363    fn tip_off_at(
1364        vehicle: &Vehicle,
1365        environment: &Environment,
1366        (state, pivot): (&State, crate::rail::Pivot),
1367        (friction, t): (f64, f64),
1368    ) -> Evaluation {
1369        vehicle
1370            .evaluate(
1371                environment,
1372                friction,
1373                Conditions {
1374                    phase: Phase::TipOff,
1375                    window: (t - 0.5, t + 0.5),
1376                    drag_area_m2: 0.0,
1377                    pivot: Some(pivot),
1378                },
1379                t,
1380                &state.to_array(),
1381            )
1382            .unwrap()
1383    }
1384
1385    #[test]
1386    fn tip_off_turns_a_fixed_mass_as_a_pendulum_about_its_last_guide() {
1387        // ADR-357 decision 4, M1.14b9: at the instant the forward guide leaves, a body of fixed
1388        // mass with no thrust and no air turns nose-down at m g r cos θ / (I + m r²), with r the
1389        // distance along the axis from the last guide to the center of mass, I the inertia about
1390        // the center of mass for a turn in the rail's vertical plane and θ the rail's angle above
1391        // the horizontal. Long after burnout the mass is fixed and nothing pushes; the rail leans
1392        // 5° to 60° off the vertical, and the rocket slides up it or sits still.
1393        let g = 9.806_65;
1394        let environment = analytic_environment(UniformAir::vacuum(), g);
1395        for (elevation_deg, speed_m_s) in
1396            [(85.0_f64, 25.0), (84.0, 0.0), (60.0, 12.0), (30.0, 40.0)]
1397        {
1398            let theta = elevation_deg.to_radians();
1399            let (vehicle, state, pivot) = tipping(theta, speed_m_s);
1400            let t = vehicle.burnout_s() + 100.0;
1401            let evaluation = tip_off_at(&vehicle, &environment, (&state, pivot), (0.0, t));
1402            let mass = evaluation.mass;
1403            assert_eq!(mass.mass_rate_kg_s, 0.0);
1404            assert_eq!(evaluation.thrust_n, 0.0);
1405            assert_eq!(evaluation.dynamic_pressure_pa, 0.0);
1406            // The center of mass is on the axis, so r is a distance along it.
1407            assert!(mass.cg_m.x.abs() < 1e-12 && mass.cg_m.y.abs() < 1e-12);
1408            let r = mass.cg_m.z - pivot.point_m.z;
1409            assert!(
1410                r > 0.3,
1411                "the center of mass is ahead of the last guide by {r} m"
1412            );
1413            // The turn's axis: horizontal, square to the rail's vertical plane, nose-down by
1414            // the right-hand rule.
1415            let to_body = state.unit_attitude().conjugate();
1416            let axis = to_body.mul_vec3(DVec3::Z.cross(pivot.rail_enu).normalize());
1417            let inertia = axis.dot(mass.inertia_cg * axis);
1418            let expected = mass.mass_kg * g * r * theta.cos() / (inertia + mass.mass_kg * r * r);
1419            let turning = DVec3::from_slice(&evaluation.derivative[10..13]);
1420            assert!(
1421                (turning - axis * expected).length() <= 1e-9 * expected,
1422                "rail at {elevation_deg} deg: {turning:?} against {expected} rad/s² about {axis:?}"
1423            );
1424            // Nose-down: the nose's direction gains a downward rate.
1425            let nose_rate = state.unit_attitude().mul_vec3(turning.cross(DVec3::Z));
1426            assert!(nose_rate.z < 0.0);
1427            // And the pivot keeps to the rail's line: its acceleration has no part across it.
1428            let rail_body = to_body.mul_vec3(pivot.rail_enu);
1429            let pivot_acceleration =
1430                to_body.mul_vec3(evaluation.acceleration_enu_m_s2) + turning.cross(pivot.point_m);
1431            let across = pivot_acceleration - rail_body * pivot_acceleration.dot(rail_body);
1432            assert!(across.length() < 1e-12, "{across:?}");
1433            // Along it, gravity alone: −g sin θ.
1434            assert!((pivot_acceleration.dot(rail_body) + g * theta.sin()).abs() < 1e-12);
1435        }
1436    }
1437
1438    #[test]
1439    fn tip_off_holds_the_pivot_and_the_roll_under_thrust_air_and_a_turning_rate() {
1440        // The three conditions, checked from the outside on a burning rocket in a crosswind
1441        // that already turns and rolls: the pivot's acceleration has no part across the rail,
1442        // the roll acceleration is zero, and what the guide adds is a force through the pivot
1443        // (its moment about the pivot is along the long axis alone).
1444        let environment = analytic_wind_environment(
1445            UniformAir::sea_level(),
1446            9.806_65,
1447            ConstantWind::new(8.0, 1.2).unwrap(),
1448        );
1449        let (vehicle, mut state, pivot) = tipping(84.0_f64.to_radians(), 22.0);
1450        state.body_rate_rad_s = DVec3::new(0.05, -0.03, 0.0);
1451        // Turned a little off the rail already.
1452        state.attitude =
1453            (state.attitude * DQuat::from_scaled_axis(DVec3::new(0.002, -0.001, 0.0))).normalize();
1454        let t = 0.4;
1455        let window = (0.0, 1.0);
1456        let at = |phase: Phase, friction: f64| {
1457            vehicle
1458                .evaluate(
1459                    &environment,
1460                    friction,
1461                    Conditions {
1462                        phase,
1463                        window,
1464                        drag_area_m2: 0.0,
1465                        pivot: (phase == Phase::TipOff).then_some(pivot),
1466                    },
1467                    t,
1468                    &state.to_array(),
1469                )
1470                .unwrap()
1471        };
1472        let free = at(Phase::Free, 0.0);
1473        assert!(free.thrust_n > 100.0 && free.dynamic_pressure_pa > 100.0);
1474        let to_body = state.unit_attitude().conjugate();
1475        let rail_body = to_body.mul_vec3(pivot.rail_enu);
1476        let omega = state.body_rate_rad_s;
1477        let p = pivot.point_m;
1478        for friction in [0.0, 0.3] {
1479            let held = at(Phase::TipOff, friction);
1480            let turning = DVec3::from_slice(&held.derivative[10..13]);
1481            assert_eq!(turning.z, 0.0);
1482            let acceleration = to_body.mul_vec3(held.acceleration_enu_m_s2);
1483            let pivot_acceleration = acceleration + turning.cross(p) + omega.cross(omega.cross(p));
1484            let across = pivot_acceleration - rail_body * pivot_acceleration.dot(rail_body);
1485            assert!(across.length() < 1e-10, "μ = {friction}: {across:?}");
1486            // What the guide adds, from the two evaluations: a force (the center of mass's
1487            // acceleration changes by F/m) and a moment about the center of mass.
1488            let r = held.mass.cg_m;
1489            let free_turning = DVec3::from_slice(&free.derivative[10..13]);
1490            let center = |a: DVec3, w: DVec3| a + w.cross(r) + omega.cross(omega.cross(r));
1491            let force = (center(acceleration, turning)
1492                - center(to_body.mul_vec3(free.acceleration_enu_m_s2), free_turning))
1493                * held.mass.mass_kg;
1494            let moment = held.mass.inertia_cg * (turning - free_turning);
1495            // About the pivot, only the roll moment is left.
1496            let about_pivot = moment - (p - r).cross(force);
1497            assert!(
1498                about_pivot.truncate().length() < 1e-9 * force.length().max(1.0),
1499                "μ = {friction}: {about_pivot:?}"
1500            );
1501            // Friction: μ times the force across the rail, against the sliding.
1502            let along = force.dot(rail_body);
1503            let normal = (force - rail_body * along).length();
1504            assert!(normal > 1.0, "the guide carries {normal} N");
1505            assert!(
1506                (along + friction * normal).abs() < 1e-9 * normal,
1507                "μ = {friction}: {along} N along the rail against {normal} N across"
1508            );
1509        }
1510    }
1511
1512    #[test]
1513    fn jet_damping_matches_the_classical_form() {
1514        // For an axisymmetric rocket turning slowly about a transverse axis in a vacuum, the
1515        // equations about the nose tip reduce to the classical jet-damping result about the center
1516        // of mass: I_c ω̇ = [ṁ (r_e²/4 + l²) − İ_c] ω, with l from the center of mass to the nozzle
1517        // exit and ṁ < 0. Every term on the right is computed here independently of the equations.
1518        let vehicle = valetudo();
1519        let environment = analytic_environment(UniformAir::vacuum(), 0.0);
1520        let placed = &vehicle.assembly.motors[0];
1521        let motor = &placed.mounted.motor;
1522        let exit_radius = motor.nozzle().map_or(0.0, |n| n.exit_radius_m);
1523        assert!(exit_radius > 0.0);
1524        let t = 1.5;
1525        let window = (1.0, 2.0);
1526        let rate = 0.2;
1527        let state = State {
1528            position_enu_m: DVec3::new(0.0, 0.0, 1000.0),
1529            velocity_enu_m_s: DVec3::new(0.0, 0.0, 50.0),
1530            attitude: DQuat::IDENTITY,
1531            body_rate_rad_s: DVec3::new(0.0, rate, 0.0),
1532        };
1533        let evaluation = vehicle
1534            .evaluate(
1535                &environment,
1536                0.0,
1537                Conditions {
1538                    phase: Phase::Free,
1539                    window,
1540                    drag_area_m2: 0.0,
1541                    pivot: None,
1542                },
1543                t,
1544                &state.to_array(),
1545            )
1546            .unwrap();
1547        let omega_dot = DVec3::from_slice(&evaluation.derivative[10..13]);
1548
1549        let props = vehicle.assembly.mass_properties(t);
1550        let h = 1e-4;
1551        let inertia_rate = (vehicle
1552            .assembly
1553            .mass_properties(t + h)
1554            .inertia_kg_m2
1555            .y_axis
1556            .y
1557            - vehicle
1558                .assembly
1559                .mass_properties(t - h)
1560                .inertia_kg_m2
1561                .y_axis
1562                .y)
1563            / (2.0 * h);
1564        let mdot = -motor.state(t).mass_flow_kg_s;
1565        let lever = placed.nozzle_m.z - props.cg_m.z;
1566        let inertia = props.inertia_kg_m2.y_axis.y;
1567        let expected = (mdot * (0.25 * exit_radius * exit_radius + lever * lever) - inertia_rate)
1568            * rate
1569            / inertia;
1570        assert!(expected < 0.0, "jet damping must damp: {expected}");
1571        assert!(
1572            (omega_dot.y - expected).abs() <= 1e-6 * expected.abs(),
1573            "{} vs {expected}",
1574            omega_dot.y
1575        );
1576        assert!(omega_dot.x.abs() + omega_dot.z.abs() < 1e-12);
1577    }
1578
1579    #[test]
1580    fn flow_angles_follow_the_frames_conventions() {
1581        // Moving along +z_B: no angle of attack.
1582        let alpha = flow_angles(DVec3::Z).alpha_rad;
1583        assert_eq!(alpha, 0.0);
1584        // Moving along +z_B and +x_B: the air crosses the body toward −x_B (φ = π).
1585        let angles = flow_angles(DVec3::new(1.0, 0.0, 1.0));
1586        assert!((angles.alpha_rad - std::f64::consts::FRAC_PI_4).abs() < 1e-15);
1587        assert!((angles.roll_rad.abs() - std::f64::consts::PI).abs() < 1e-15);
1588        // Moving along −y_B only: the air crosses toward +y_B (φ = π/2), α = 90°.
1589        let angles = flow_angles(-DVec3::Y);
1590        assert!((angles.alpha_rad - std::f64::consts::FRAC_PI_2).abs() < 1e-15);
1591        assert!((angles.roll_rad - std::f64::consts::FRAC_PI_2).abs() < 1e-15);
1592        // Tail first.
1593        let alpha = flow_angles(-DVec3::Z).alpha_rad;
1594        assert!((alpha - std::f64::consts::PI).abs() < 1e-15);
1595    }
1596
1597    /// The flow angles as they were taken before issue #285: `atan2`, then `cos` and `sin` of the
1598    /// angles.
1599    fn flow_angles_by_atan2(v: DVec3) -> FlowAngles {
1600        let alpha_rad = v.x.hypot(v.y).atan2(v.z);
1601        let roll_rad = if v.x == 0.0 && v.y == 0.0 {
1602            0.0
1603        } else {
1604            (-v.y).atan2(-v.x)
1605        };
1606        FlowAngles {
1607            alpha_rad,
1608            roll_rad,
1609            cos_alpha: alpha_rad.cos(),
1610            sin_alpha: alpha_rad.sin(),
1611            cos_roll: roll_rad.cos(),
1612            sin_roll: roll_rad.sin(),
1613        }
1614    }
1615
1616    /// The fields' bits, so that `+0` and `−0` differ.
1617    fn flow_bits(a: FlowAngles) -> [u64; 6] {
1618        [
1619            a.alpha_rad,
1620            a.roll_rad,
1621            a.cos_alpha,
1622            a.sin_alpha,
1623            a.cos_roll,
1624            a.sin_roll,
1625        ]
1626        .map(f64::to_bits)
1627    }
1628
1629    /// How far the cosines and sines taken from the components may sit from `cos` and `sin` of
1630    /// the `atan2` angles, absolute (they are at most 1): `atan2`'s error of up to one unit in the
1631    /// last place of an angle up to π (`2ε`), the `cos` or `sin` rounding (`ε/2`), and the
1632    /// quotient's own (`|v|` or `w` and the division, under `2ε`).
1633    const FLOW_TRIG_TOLERANCE: f64 = 5.0 * f64::EPSILON;
1634
1635    #[test]
1636    fn flow_angles_cut_off_where_v_s_squares_stop_underflowing() {
1637        assert_eq!(FLOW_RATIO_MIN_SPEED, 2f64.powi(-500));
1638    }
1639
1640    /// Zero velocity, with every sign of zero, and the purely axial flows give what the `atan2`
1641    /// path gave, bit for bit, except where that path's `sin(fl(π))` and `cos(fl(π/2))` leave
1642    /// `1.2e-16` and `6.1e-17` in place of zero: there the quotients give the exact zero.
1643    #[test]
1644    fn flow_angles_keep_the_degenerate_cases() {
1645        let zeros = [0.0, -0.0];
1646        for x in zeros {
1647            for y in zeros {
1648                // No velocity: `atan2(0, ±0)` gives `α = 0` or `π`, and `φ = 0`, as before.
1649                for z in zeros {
1650                    let v = DVec3::new(x, y, z);
1651                    assert_eq!(
1652                        flow_bits(flow_angles(v)),
1653                        flow_bits(flow_angles_by_atan2(v))
1654                    );
1655                }
1656                // Nose first along the axis: α = 0, φ = 0, exactly as before.
1657                for z in [1e-300, 1.0, 340.0, 1e300] {
1658                    let v = DVec3::new(x, y, z);
1659                    assert_eq!(
1660                        flow_bits(flow_angles(v)),
1661                        flow_bits(flow_angles_by_atan2(v))
1662                    );
1663                }
1664                // Tail first along the axis: α = π and φ = 0 as before, `cos α = −1` and
1665                // `sin α = +0`, where the `atan2` path gave `sin(fl(π)) = 1.2e-16`.
1666                for z in [-1.0, -340.0] {
1667                    let v = DVec3::new(x, y, z);
1668                    let (got, was) = (flow_angles(v), flow_angles_by_atan2(v));
1669                    assert_eq!(got.alpha_rad.to_bits(), was.alpha_rad.to_bits());
1670                    assert_eq!(got.roll_rad.to_bits(), was.roll_rad.to_bits());
1671                    assert_eq!(got.alpha_rad, std::f64::consts::PI);
1672                    assert_eq!(got.roll_rad.to_bits(), 0.0f64.to_bits());
1673                    assert_eq!(got.cos_alpha, -1.0);
1674                    assert_eq!(got.sin_alpha.to_bits(), 0.0f64.to_bits());
1675                    assert_eq!((got.cos_roll, got.sin_roll.to_bits()), (1.0, 0));
1676                    assert_eq!(was.sin_alpha, std::f64::consts::PI.sin());
1677                }
1678            }
1679        }
1680        // Crossflow along ±x_B only, with either zero along y_B: φ keeps `atan2`'s ±π or ±0,
1681        // and `sin φ` its sign, now an exact zero where `sin(±fl(π))` was ∓1.2e-16.
1682        for (x, y) in [(1.0, 0.0), (1.0, -0.0), (-1.0, 0.0), (-1.0, -0.0)] {
1683            let v = DVec3::new(x, y, 0.0);
1684            let (got, was) = (flow_angles(v), flow_angles_by_atan2(v));
1685            assert_eq!(got.alpha_rad.to_bits(), was.alpha_rad.to_bits());
1686            assert_eq!(got.roll_rad.to_bits(), was.roll_rad.to_bits());
1687            assert_eq!(got.cos_roll, -x);
1688            assert_eq!(got.sin_roll, 0.0);
1689            assert_eq!(
1690                got.sin_roll.is_sign_negative(),
1691                was.sin_roll.is_sign_negative()
1692            );
1693            // Broadside: `cos α` is the exact zero, where `cos(fl(π/2))` was 6.1e-17.
1694            assert_eq!(got.cos_alpha.to_bits(), 0.0f64.to_bits());
1695            assert_eq!(got.sin_alpha, 1.0);
1696            assert_eq!(was.cos_alpha, std::f64::consts::FRAC_PI_2.cos());
1697        }
1698        // A subnormal crossflow, or a speed whose squares underflow, would cost the quotients
1699        // their digits (`hypot(5e-324, 5e-324)` rounds to `5e-324`): there `cos φ` and `sin φ`,
1700        // or `cos α` and `sin α`, come from the angles, bit for bit as before.
1701        let tiny = f64::from_bits(1);
1702        for v in [
1703            DVec3::new(tiny, tiny, 1.0),
1704            DVec3::new(-tiny, 3.0 * tiny, -1.0),
1705            DVec3::new(tiny, -tiny, 0.0),
1706        ] {
1707            let (got, was) = (
1708                flow_bits(flow_angles(v)),
1709                flow_bits(flow_angles_by_atan2(v)),
1710            );
1711            assert_eq!(
1712                [got[0], got[1], got[4], got[5]],
1713                [was[0], was[1], was[4], was[5]]
1714            );
1715        }
1716        let v = DVec3::new(1e-160, -1e-160, 1e-160);
1717        let (got, was) = (flow_angles(v), flow_angles_by_atan2(v));
1718        assert_eq!(flow_bits(got)[..4], flow_bits(was)[..4]);
1719    }
1720
1721    /// One velocity component for [`flow_angles_match_the_atan2_path`]: zeros of both signs,
1722    /// flight-sized values, tiny ones, subnormals and the full range of normal numbers.
1723    fn flow_component() -> impl proptest::strategy::Strategy<Value = f64> {
1724        use proptest::prelude::*;
1725        prop_oneof![
1726            Just(0.0),
1727            Just(-0.0),
1728            -1e3..1e3,
1729            -1e-6..1e-6,
1730            -1e-150..1e-150,
1731            proptest::num::f64::SUBNORMAL
1732                | proptest::num::f64::POSITIVE
1733                | proptest::num::f64::NEGATIVE,
1734            proptest::num::f64::NORMAL
1735                | proptest::num::f64::SUBNORMAL
1736                | proptest::num::f64::POSITIVE
1737                | proptest::num::f64::NEGATIVE,
1738        ]
1739    }
1740
1741    proptest::proptest! {
1742        #![proptest_config(proptest::prelude::ProptestConfig::with_cases(20_000))]
1743
1744        /// The angles are the `atan2` path's, bit for bit, and the cosines and sines are within
1745        /// [`FLOW_TRIG_TOLERANCE`] of its `cos` and `sin`, on the unit circle, and of the sign the
1746        /// components give.
1747        #[test]
1748        fn flow_angles_match_the_atan2_path(
1749            x in flow_component(),
1750            y in flow_component(),
1751            z in flow_component(),
1752        ) {
1753            let v = DVec3::new(x, y, z);
1754            let (got, was) = (flow_angles(v), flow_angles_by_atan2(v));
1755            proptest::prop_assert_eq!(got.alpha_rad.to_bits(), was.alpha_rad.to_bits());
1756            proptest::prop_assert_eq!(got.roll_rad.to_bits(), was.roll_rad.to_bits());
1757            let pairs = [
1758                (got.cos_alpha, was.cos_alpha),
1759                (got.sin_alpha, was.sin_alpha),
1760                (got.cos_roll, was.cos_roll),
1761                (got.sin_roll, was.sin_roll),
1762            ];
1763            for (new, old) in pairs {
1764                proptest::prop_assert!(
1765                    (new - old).abs() <= FLOW_TRIG_TOLERANCE,
1766                    "{v:?}: {new:e} against {old:e}"
1767                );
1768            }
1769            for (c, s) in [(got.cos_alpha, got.sin_alpha), (got.cos_roll, got.sin_roll)] {
1770                proptest::prop_assert!((c.hypot(s) - 1.0).abs() <= 2.0 * f64::EPSILON);
1771            }
1772            // The signs the components give: `sin α ≥ 0`; `cos α` goes with `v_z`, `cos φ` with
1773            // `−v_x` and `sin φ` with `−v_y` wherever the quotients are taken.
1774            proptest::prop_assert!(got.sin_alpha.is_sign_positive());
1775            let speed = v.length();
1776            if (FLOW_RATIO_MIN_SPEED..=f64::MAX).contains(&speed) && z != 0.0 {
1777                proptest::prop_assert_eq!(got.cos_alpha.is_sign_negative(), z < 0.0);
1778            }
1779            let crossflow = x.hypot(y);
1780            if (f64::MIN_POSITIVE..=f64::MAX).contains(&crossflow) {
1781                proptest::prop_assert_eq!(got.cos_roll.is_sign_negative(), x.is_sign_positive());
1782                proptest::prop_assert_eq!(got.sin_roll.is_sign_negative(), y.is_sign_positive());
1783            }
1784        }
1785    }
1786
1787    #[test]
1788    fn outer_product_is_u_v_transpose() {
1789        let m = outer(DVec3::new(1.0, 2.0, 3.0), DVec3::new(4.0, 5.0, 6.0));
1790        assert_eq!(m.row(1), DVec3::new(8.0, 10.0, 12.0));
1791        assert_eq!(m.col(2), DVec3::new(6.0, 12.0, 18.0));
1792    }
1793}