1use 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
46const MASS_DERIVATIVE_STEP_S: f64 = 1e-4;
48
49const MIN_AIRSPEED_M_S: f64 = 1e-9;
51
52const MIN_DERIVATIVE_INTERVAL_S: f64 = 2e-5;
55
56#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash, serde::Serialize, serde::Deserialize)]
58#[serde(rename_all = "snake_case")]
59#[non_exhaustive]
60pub enum Phase {
61 Pad,
63 Rail,
65 TipOff,
69 Free,
71 Descent,
74}
75
76#[derive(Debug, Clone, Copy, PartialEq)]
78pub(crate) struct Conditions {
79 pub(crate) phase: Phase,
81 pub(crate) window: (f64, f64),
84 pub(crate) drag_area_m2: f64,
86 pub(crate) pivot: Option<crate::rail::Pivot>,
88}
89
90#[derive(Debug, Clone, Copy, Default)]
92struct BurningAreas {
93 airframe_m2: f64,
95 pods_m2: [f64; MOTOR_POD_SETS],
98}
99
100#[derive(Debug, Clone, Copy)]
102struct MotorTerms {
103 nozzle_m: DVec3,
105 exit_radius_m: f64,
107 area_m2: f64,
109 pod_set: Option<usize>,
113 ignition_s: f64,
115 burnout_s: f64,
117}
118
119#[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 pub(crate) relative_momentum: DVec3,
132 pub(crate) relative_momentum_rate: DVec3,
134}
135
136#[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 pub(crate) rail_force_n: f64,
153 pub(crate) acceleration_enu_m_s2: DVec3,
155 pub(crate) specific_force_enu_m_s2: DVec3,
158 pub(crate) recovery_drag_area_m2: f64,
160}
161
162#[derive(Debug, Clone)]
165pub(crate) struct Vehicle {
166 pub(crate) assembly: Assembly,
167 pub(crate) aero: AeroModel,
168 first_fin_index: usize,
170 motors: Vec<MotorTerms>,
171 ignition_s: Vec<Option<f64>>,
173 reference_area_m2: f64,
174 pub(crate) shifts: Shifts,
176 pub(crate) histories: Histories,
178}
179
180impl Vehicle {
181 #[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 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 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 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 pub(crate) fn ignition_s(&self) -> &[Option<f64>] {
251 &self.ignition_s
252 }
253
254 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 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 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 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 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 #[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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 flow.validate()?;
712 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 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 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 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 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#[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
802pub(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
834fn 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#[derive(Debug, Clone, Copy, PartialEq)]
864struct FlowAngles {
865 alpha_rad: f64,
867 roll_rad: f64,
870 cos_alpha: f64,
872 sin_alpha: f64,
876 cos_roll: f64,
878 sin_roll: f64,
880}
881
882const FLOW_RATIO_MIN_SPEED: f64 = 3.054_936_363_499_605e-151;
885
886fn flow_angles(v: DVec3) -> FlowAngles {
899 let crossflow = v.x.hypot(v.y);
900 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
928fn 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 #[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 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 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 #[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 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 #[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 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 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 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 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 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 #[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 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 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 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 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 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 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 let nose_rate = state.unit_attitude().mul_vec3(turning.cross(DVec3::Z));
1426 assert!(nose_rate.z < 0.0);
1427 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 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 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 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 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 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 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 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 let alpha = flow_angles(DVec3::Z).alpha_rad;
1583 assert_eq!(alpha, 0.0);
1584 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 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 let alpha = flow_angles(-DVec3::Z).alpha_rad;
1594 assert!((alpha - std::f64::consts::PI).abs() < 1e-15);
1595 }
1596
1597 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 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 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 #[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 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 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 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 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 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 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 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 #[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 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}