1use std::ops::ControlFlow;
38
39use hpr_atmos::AirState;
40use hpr_core::attitude::quaternion_derivative;
41use hpr_core::{DMat3, DVec3};
42use serde::{Deserialize, Serialize};
43
44use crate::dynamics::{MassState, free_motion};
45use crate::environment::Environment;
46use crate::error::SimError;
47use crate::integrator::{IntegrationError, Integrator, Method, OdeSystem, Step};
48use crate::state::{STATE_LEN, State};
49
50pub const FREE_BODY_MASS_REFUSED: &str = "free body's mass (kg), which must be finite and positive";
52
53pub const FREE_BODY_INERTIA_NOT_FINITE: &str = "free body's inertia entry (kg·m²)";
56
57pub const FREE_BODY_INERTIA_NOT_SYMMETRIC: &str =
60 "free body's inertia asymmetry (kg·m²), which must be zero";
61
62pub const FREE_BODY_INERTIA_NOT_POSITIVE: &str =
65 "free body's inertia leading minor, which must be positive";
66
67pub const FREE_BODY_TIME_REFUSED: &str =
70 "free body's sample time (s), which must be finite, not before the start and not descending";
71
72pub const FREE_BODY_LOAD_NOT_FINITE: &str =
75 "free body's load (N in the force, N·m in the moment), which must be finite";
76
77#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
80#[non_exhaustive]
81pub struct LoadInputs {
82 pub state: State,
84 pub height_msl_m: f64,
87 pub air: AirState,
89 pub wind_enu_m_s: DVec3,
91 pub air_velocity_body_m_s: DVec3,
94}
95
96#[derive(Debug, Clone, Copy, PartialEq, Default, Serialize, Deserialize)]
98pub struct Loads {
99 pub force_body_n: DVec3,
101 pub moment_body_n_m: DVec3,
103}
104
105pub trait FreeBodyLoads {
108 fn loads(&self, inputs: &LoadInputs) -> Result<Loads, SimError>;
114}
115
116impl<F: Fn(&LoadInputs) -> Result<Loads, SimError>> FreeBodyLoads for F {
117 fn loads(&self, inputs: &LoadInputs) -> Result<Loads, SimError> {
118 self(inputs)
119 }
120}
121
122#[derive(Debug, Clone, Copy, PartialEq, Eq, Default)]
124pub struct NoLoads;
125
126impl FreeBodyLoads for NoLoads {
127 fn loads(&self, _inputs: &LoadInputs) -> Result<Loads, SimError> {
128 Ok(Loads::default())
129 }
130}
131
132#[derive(Debug, Clone, PartialEq, Serialize, Deserialize)]
142#[serde(try_from = "FreeBodyData", into = "FreeBodyData")]
143pub struct FreeBody {
144 mass_kg: f64,
145 inertia_kg_m2: DMat3,
146}
147
148#[derive(Serialize, Deserialize)]
150struct FreeBodyData {
151 mass_kg: f64,
152 inertia_kg_m2: DMat3,
153}
154
155impl TryFrom<FreeBodyData> for FreeBody {
156 type Error = SimError;
157
158 fn try_from(data: FreeBodyData) -> Result<Self, SimError> {
159 Self::new(data.mass_kg, data.inertia_kg_m2)
160 }
161}
162
163impl From<FreeBody> for FreeBodyData {
164 fn from(body: FreeBody) -> Self {
165 Self {
166 mass_kg: body.mass_kg,
167 inertia_kg_m2: body.inertia_kg_m2,
168 }
169 }
170}
171
172impl FreeBody {
173 pub fn new(mass_kg: f64, inertia_kg_m2: DMat3) -> Result<Self, SimError> {
189 if !(mass_kg.is_finite() && mass_kg > 0.0) {
190 return Err(SimError::Domain {
191 what: FREE_BODY_MASS_REFUSED,
192 value: mass_kg,
193 });
194 }
195 let i = inertia_kg_m2;
196 if let Some(&value) = i.to_cols_array().iter().find(|entry| !entry.is_finite()) {
197 return Err(SimError::Domain {
198 what: FREE_BODY_INERTIA_NOT_FINITE,
199 value,
200 });
201 }
202 let asymmetry = (i.x_axis.y - i.y_axis.x)
203 .abs()
204 .max((i.x_axis.z - i.z_axis.x).abs())
205 .max((i.y_axis.z - i.z_axis.y).abs());
206 if asymmetry != 0.0 {
207 return Err(SimError::Domain {
208 what: FREE_BODY_INERTIA_NOT_SYMMETRIC,
209 value: asymmetry,
210 });
211 }
212 let minors = [
215 i.x_axis.x,
216 i.x_axis.x * i.y_axis.y - i.x_axis.y * i.y_axis.x,
217 i.determinant(),
218 ];
219 if let Some(&value) = minors.iter().find(|minor| **minor <= 0.0) {
220 return Err(SimError::Domain {
221 what: FREE_BODY_INERTIA_NOT_POSITIVE,
222 value,
223 });
224 }
225 Ok(Self {
226 mass_kg,
227 inertia_kg_m2,
228 })
229 }
230
231 #[must_use]
233 pub fn mass_kg(&self) -> f64 {
234 self.mass_kg
235 }
236
237 #[must_use]
239 pub fn inertia_kg_m2(&self) -> DMat3 {
240 self.inertia_kg_m2
241 }
242
243 fn mass_state(&self) -> MassState {
246 MassState {
247 mass_kg: self.mass_kg,
248 mass_rate_kg_s: 0.0,
249 cg_m: DVec3::ZERO,
250 cg_rate_m_s: DVec3::ZERO,
251 cg_accel_m_s2: DVec3::ZERO,
252 inertia_cg: self.inertia_kg_m2,
253 inertia_o: self.inertia_kg_m2,
254 inertia_o_rate: DMat3::ZERO,
255 relative_momentum: DVec3::ZERO,
256 relative_momentum_rate: DVec3::ZERO,
257 }
258 }
259
260 pub fn derivative(
272 &self,
273 environment: &Environment,
274 loads: &dyn FreeBodyLoads,
275 y: &[f64; STATE_LEN],
276 ) -> Result<[f64; STATE_LEN], SimError> {
277 let state = State::from_array(y);
278 let norm = state.attitude.length();
279 if !(norm.is_finite() && norm > 0.0) {
280 return Err(SimError::Domain {
281 what: "attitude quaternion norm",
282 value: norm,
283 });
284 }
285 let q = state.attitude / norm;
286 let omega = state.body_rate_rad_s;
287 let geodetic = environment
290 .earth
291 .frame()
292 .geodetic_from_enu(state.position_enu_m)?;
293 let height_msl_m = geodetic.height_m - environment.geoid_undulation_m;
294 let air = environment.air_at(height_msl_m)?;
295 let wind_enu_m_s = environment.wind_enu_m_s(height_msl_m)?;
296 let air_velocity_body_m_s = q
297 .conjugate()
298 .mul_vec3(state.velocity_enu_m_s - wind_enu_m_s);
299 let applied = loads.loads(&LoadInputs {
300 state: State {
301 attitude: q,
302 ..state
303 },
304 height_msl_m,
305 air,
306 wind_enu_m_s,
307 air_velocity_body_m_s,
308 })?;
309 if let Some(&value) = applied
310 .force_body_n
311 .to_array()
312 .iter()
313 .chain(&applied.moment_body_n_m.to_array())
314 .find(|component| !component.is_finite())
315 {
316 return Err(SimError::Domain {
317 what: FREE_BODY_LOAD_NOT_FINITE,
318 value,
319 });
320 }
321 let gravity_enu = environment.earth.gravity_enu_mps2(state.position_enu_m)?;
322 let coriolis_enu = environment
323 .earth
324 .rotation_acceleration_enu_mps2(state.velocity_enu_m_s);
325 let weight = q
326 .conjugate()
327 .mul_vec3((gravity_enu + coriolis_enu) * self.mass_kg);
328 let (a_body, omega_dot) = free_motion(
329 &self.mass_state(),
330 omega,
331 [DVec3::ZERO, DVec3::ZERO, weight + applied.force_body_n],
332 DMat3::ZERO,
333 [DVec3::ZERO, applied.moment_body_n_m, DVec3::ZERO],
334 )?;
335 let a = q.mul_vec3(a_body);
336 let v = state.velocity_enu_m_s;
337 let q_dot = quaternion_derivative(state.attitude, omega);
338 Ok([
339 v.x,
340 v.y,
341 v.z,
342 a.x,
343 a.y,
344 a.z,
345 q_dot.w,
346 q_dot.x,
347 q_dot.y,
348 q_dot.z,
349 omega_dot.x,
350 omega_dot.y,
351 omega_dot.z,
352 ])
353 }
354
355 pub fn fly(
367 &self,
368 environment: &Environment,
369 loads: &dyn FreeBodyLoads,
370 method: Method,
371 t0_s: f64,
372 state: State,
373 times_s: &[f64],
374 ) -> Result<Vec<State>, SimError> {
375 let mut integrator = Integrator::new(method, t0_s, state.to_array())?;
378 let mut last = t0_s;
379 for &time in times_s {
380 if !(time.is_finite() && time >= last) {
381 return Err(SimError::Domain {
382 what: FREE_BODY_TIME_REFUSED,
383 value: time,
384 });
385 }
386 last = time;
387 }
388 let mut system = Flight {
389 body: self,
390 environment,
391 loads,
392 times_s,
393 states: Vec::with_capacity(times_s.len()),
394 };
395 while system.next_time().is_some_and(|time| time <= t0_s) {
397 system.states.push(state);
398 }
399 match integrator.advance(&mut system, last) {
400 Ok(_) => {}
401 Err(IntegrationError::Derivative { source, .. }) => return Err(source),
402 Err(error) => return Err(SimError::Integration(Box::new(error))),
403 }
404 let end = State::from_array(integrator.state());
407 while system.next_time().is_some() {
408 system.states.push(end);
409 }
410 Ok(system.states)
411 }
412}
413
414struct Flight<'a> {
416 body: &'a FreeBody,
417 environment: &'a Environment,
418 loads: &'a dyn FreeBodyLoads,
419 times_s: &'a [f64],
420 states: Vec<State>,
421}
422
423impl Flight<'_> {
424 fn next_time(&self) -> Option<f64> {
426 self.times_s.get(self.states.len()).copied()
427 }
428}
429
430impl OdeSystem<STATE_LEN> for Flight<'_> {
431 type Error = SimError;
432
433 fn derivative(
434 &mut self,
435 _t_s: f64,
436 y: &[f64; STATE_LEN],
437 ) -> Result<[f64; STATE_LEN], SimError> {
438 self.body.derivative(self.environment, self.loads, y)
439 }
440
441 fn accept_step(&mut self, step: &Step<STATE_LEN>) -> ControlFlow<()> {
442 while let Some(time) = self.next_time() {
443 if time > step.end_s() {
444 break;
445 }
446 self.states.push(State::from_array(&step.state_at(time)));
447 }
448 ControlFlow::Continue(())
449 }
450}
451
452#[cfg(test)]
453mod tests {
454 use hpr_atmos::ConstantWind;
455 use hpr_core::DQuat;
456 use hpr_core::earth::Earth;
457
458 use super::*;
459 use crate::dynamics::{Conditions, Phase, Vehicle};
460 use crate::flight::{FlightSettings, Simulation};
461 use crate::rail::Rail;
462 use crate::testing::{UniformAir, analytic_environment, design, site};
463
464 fn brick() -> FreeBody {
467 const SLUG_FT2_KG_M2: f64 = 4.448_221_615_260_5 * 0.3048;
468 FreeBody::new(
469 0.155_404_754 * 4.448_221_615_260_5 / 0.3048,
470 DMat3::from_diagonal(DVec3::new(
471 0.001_894_220 * SLUG_FT2_KG_M2,
472 0.006_211_019 * SLUG_FT2_KG_M2,
473 0.007_194_665 * SLUG_FT2_KG_M2,
474 )),
475 )
476 .unwrap()
477 }
478
479 fn airless_earth() -> Environment {
481 Environment::new(
482 Earth::wgs84(site()).unwrap(),
483 UniformAir::vacuum(),
484 ConstantWind::calm(),
485 )
486 }
487
488 fn tumbling() -> State {
489 State {
490 position_enu_m: DVec3::new(40.0, -25.0, 900.0),
491 velocity_enu_m_s: DVec3::new(30.0, -20.0, 80.0),
492 attitude: DQuat::from_rotation_z(0.3)
493 * DQuat::from_rotation_y(0.4)
494 * DQuat::from_rotation_x(0.2),
495 body_rate_rad_s: DVec3::new(0.4, -0.3, 3.0),
496 }
497 }
498
499 #[test]
504 fn the_derivative_is_a_coasting_rockets() {
505 let environment = airless_earth();
506 let simulation = Simulation::new(
507 &design("rocketpy-valetudo"),
508 "example",
509 environment.clone(),
510 Rail::vertical(3.0),
511 FlightSettings::default(),
512 )
513 .unwrap();
514 let t = 10.0;
516 let vehicle =
517 Vehicle::new(simulation.assembly().clone(), simulation.aero().clone()).unwrap();
518 let nose = tumbling();
519 let evaluation = vehicle
520 .evaluate(
521 &environment,
522 0.0,
523 Conditions {
524 phase: Phase::Free,
525 window: (t, t + 1.0),
526 drag_area_m2: 0.0,
527 pivot: None,
528 },
529 t,
530 &nose.to_array(),
531 )
532 .unwrap();
533 let mass = evaluation.mass;
534 assert_eq!(mass.mass_rate_kg_s, 0.0);
535 assert!(mass.cg_m.length() > 1.0, "{:?}", mass.cg_m);
536 let (q, omega, r) = (nose.unit_attitude(), nose.body_rate_rad_s, mass.cg_m);
537 let omega_dot = DVec3::from_slice(&evaluation.derivative[10..13]);
538 let rocket_cg_acceleration = evaluation.acceleration_enu_m_s2
539 + q.mul_vec3(omega_dot.cross(r) + omega.cross(omega.cross(r)));
540
541 let body = FreeBody::new(mass.mass_kg, symmetric(mass.inertia_cg)).unwrap();
542 let at_cg = State {
543 position_enu_m: evaluation.cg_enu_m,
544 velocity_enu_m_s: evaluation.cg_velocity_enu_m_s,
545 ..nose
546 };
547 let derivative = body
548 .derivative(&environment, &NoLoads, &at_cg.to_array())
549 .unwrap();
550 let acceleration = DVec3::from_slice(&derivative[3..6]);
551 let turning = DVec3::from_slice(&derivative[10..13]);
552
553 let acceleration_gap = (acceleration - rocket_cg_acceleration).length();
557 assert!(acceleration_gap < 1e-12, "{acceleration_gap:e} m/s²");
558 let turning_gap = (turning - omega_dot).length();
559 assert!(turning_gap < 1e-12, "{turning_gap:e} rad/s²");
560 assert!(acceleration.length() > 9.7);
563 assert!(omega_dot.length() > 1e-3, "{omega_dot:?}");
564 let coriolis = environment
565 .earth
566 .rotation_acceleration_enu_mps2(at_cg.velocity_enu_m_s);
567 assert!(coriolis.length() > 1e-3, "{coriolis:?}");
568 }
569
570 fn symmetric(i: DMat3) -> DMat3 {
572 (i + i.transpose()) * 0.5
573 }
574
575 #[test]
579 fn a_tumbling_brick_keeps_its_angular_momentum_and_energy() {
580 let body = brick();
581 let inertia = body.inertia_kg_m2();
582 let start = State {
583 position_enu_m: DVec3::new(0.0, 0.0, 9144.0),
584 velocity_enu_m_s: DVec3::ZERO,
585 attitude: DQuat::IDENTITY,
586 body_rate_rad_s: DVec3::new(10.0, 20.0, 30.0).map(f64::to_radians),
587 };
588 let times: Vec<f64> = (0..=300).map(|i| 0.1 * f64::from(i)).collect();
589 let states = body
590 .fly(
591 &airless_earth(),
592 &NoLoads,
593 Method::default(),
594 0.0,
595 start,
596 ×,
597 )
598 .unwrap();
599 assert_eq!(states.len(), times.len());
600 assert_eq!(states[0], start);
601 let momentum = |s: &State| s.unit_attitude().mul_vec3(inertia * s.body_rate_rad_s);
602 let energy = |s: &State| 0.5 * s.body_rate_rad_s.dot(inertia * s.body_rate_rad_s);
603 let (h0, e0) = (momentum(&start), energy(&start));
604 let mut worst_momentum: f64 = 0.0;
605 let mut worst_energy: f64 = 0.0;
606 let mut largest_turn: f64 = 0.0;
607 for state in &states {
608 worst_momentum = worst_momentum.max((momentum(state) - h0).length() / h0.length());
609 worst_energy = worst_energy.max((energy(state) - e0).abs() / e0);
610 largest_turn =
611 largest_turn.max((state.body_rate_rad_s - start.body_rate_rad_s).length());
612 }
613 assert!(worst_momentum < 4e-7, "{worst_momentum:e}");
616 assert!(worst_energy < 4e-7, "{worst_energy:e}");
617 assert!(largest_turn > 0.5, "{largest_turn} rad/s");
620 }
621
622 #[test]
625 fn the_center_of_mass_follows_the_parabola() {
626 const G: f64 = 9.806_65;
627 let body = brick();
628 let start = tumbling();
629 let times = [0.0, 0.5, 3.0, 7.25, 12.0];
630 let states = body
631 .fly(
632 &analytic_environment(UniformAir::vacuum(), G),
633 &NoLoads,
634 Method::default(),
635 0.0,
636 start,
637 ×,
638 )
639 .unwrap();
640 for (time, state) in times.iter().zip(&states) {
641 let expected = start.position_enu_m + start.velocity_enu_m_s * *time
642 - DVec3::Z * (0.5 * G * time * time);
643 let gap = (state.position_enu_m - expected).length();
644 assert!(gap < 1e-8, "{gap:e} m at {time} s");
645 }
646 }
647
648 #[test]
655 fn the_coriolis_force_lifts_a_long_fall() {
656 use hpr_core::earth::{EarthRotation, GravityModel};
657 use hpr_core::geodesy::Geodetic;
658 use hpr_core::gravity::NormalGravity;
659 let equator = Geodetic::from_degrees(0.0, 0.0, 0.0).unwrap();
660 let environment = |rotation| {
661 Environment::new(
662 Earth::new(
663 NormalGravity::wgs84(),
664 equator,
665 GravityModel::default(),
666 rotation,
667 )
668 .unwrap(),
669 UniformAir::vacuum(),
670 ConstantWind::calm(),
671 )
672 };
673 let start = State {
674 position_enu_m: DVec3::new(0.0, 0.0, 9144.0),
675 velocity_enu_m_s: DVec3::ZERO,
676 attitude: DQuat::IDENTITY,
677 body_rate_rad_s: DVec3::ZERO,
678 };
679 let end = |rotation| {
680 let environment = environment(rotation);
681 let states = brick()
682 .fly(
683 &environment,
684 &NoLoads,
685 Method::default(),
686 0.0,
687 start,
688 &[30.0],
689 )
690 .unwrap();
691 let height = environment
692 .earth
693 .frame()
694 .geodetic_from_enu(states[0].position_enu_m)
695 .unwrap()
696 .height_m;
697 (height, states[0].velocity_enu_m_s.x)
698 };
699 let (turning, east_m_s) = end(EarthRotation::Coriolis);
700 let (still, no_drift_m_s) = end(EarthRotation::Ignore);
701 let omega: f64 = 7.292_115e-5;
702 let g = 2.0 * (9144.0 - still) / 30.0_f64.powi(2);
705 assert!((g - 9.754).abs() < 0.005, "{g}");
706 let lift_m = omega * omega * g * 30.0_f64.powi(4) / 6.0;
707 assert!((lift_m - 0.0070).abs() < 0.0001, "{lift_m}");
708 let lifted_m = turning - still;
709 assert!(
710 (lifted_m / lift_m - 1.0).abs() < 0.01,
711 "{lifted_m} {lift_m}"
712 );
713 let drift_m_s = omega * g * 30.0_f64.powi(2);
715 assert!(
716 (east_m_s / drift_m_s - 1.0).abs() < 0.01,
717 "{east_m_s} {drift_m_s}"
718 );
719 assert_eq!(no_drift_m_s, 0.0);
720 }
721
722 #[test]
726 fn samples_do_not_shape_the_flight() {
727 let body = brick();
728 let start = tumbling();
729 let environment = airless_earth();
730 let fly = |times: &[f64]| {
731 body.fly(&environment, &NoLoads, Method::default(), 2.0, start, times)
732 .unwrap()
733 };
734 let dense: Vec<f64> = (0..=100).map(|i| 2.0 + 0.05 * f64::from(i)).collect();
735 let many = fly(&dense);
736 let few = fly(&[2.0, 2.0, 4.5, 7.0]);
737 assert_eq!(few[0], start);
738 assert_eq!(few[1], start);
739 assert_eq!(few[3], many[100]);
740 let gap = (few[2].position_enu_m - many[50].position_enu_m).length();
741 assert!(gap < 1e-9, "{gap:e} m");
742 assert!(fly(&[]).is_empty());
743 }
744
745 #[test]
747 fn bad_bodies_and_times_are_refused_by_name() {
748 let good = DMat3::from_diagonal(DVec3::new(1.0, 2.0, 3.0));
749 let what = |result: Result<FreeBody, SimError>| match result {
750 Err(SimError::Domain { what, value }) => (what, value),
751 other => panic!("{other:?}"),
752 };
753 assert_eq!(what(FreeBody::new(0.0, good)).0, FREE_BODY_MASS_REFUSED);
754 assert_eq!(
755 what(FreeBody::new(-1.0, good)),
756 (FREE_BODY_MASS_REFUSED, -1.0)
757 );
758 assert_eq!(
759 what(FreeBody::new(f64::INFINITY, good)).0,
760 FREE_BODY_MASS_REFUSED
761 );
762 let mut not_finite = good;
763 not_finite.y_axis.z = f64::NAN;
764 assert_eq!(
765 what(FreeBody::new(1.0, not_finite)).0,
766 FREE_BODY_INERTIA_NOT_FINITE
767 );
768 let mut lopsided = good;
769 lopsided.x_axis.y = 0.25;
770 assert_eq!(
771 what(FreeBody::new(1.0, lopsided)),
772 (FREE_BODY_INERTIA_NOT_SYMMETRIC, 0.25)
773 );
774 let mut indefinite = good;
777 indefinite.x_axis.y = 2.0;
778 indefinite.y_axis.x = 2.0;
779 assert_eq!(
780 what(FreeBody::new(1.0, indefinite)),
781 (FREE_BODY_INERTIA_NOT_POSITIVE, -2.0)
782 );
783 assert_eq!(
784 what(FreeBody::new(1.0, DMat3::ZERO)),
785 (FREE_BODY_INERTIA_NOT_POSITIVE, 0.0)
786 );
787
788 let body = FreeBody::new(1.0, good).unwrap();
789 let environment = airless_earth();
790 let refused = |t0: f64, times: &[f64]| match body.fly(
791 &environment,
792 &NoLoads,
793 Method::default(),
794 t0,
795 tumbling(),
796 times,
797 ) {
798 Err(SimError::Domain { what, value }) => (what, value),
799 other => panic!("{other:?}"),
800 };
801 assert_eq!(refused(1.0, &[0.5]), (FREE_BODY_TIME_REFUSED, 0.5));
802 assert_eq!(refused(0.0, &[1.0, 0.75]), (FREE_BODY_TIME_REFUSED, 0.75));
803 assert_eq!(refused(0.0, &[f64::INFINITY]).0, FREE_BODY_TIME_REFUSED);
804 assert!(refused(0.0, &[f64::NAN]).1.is_nan());
805 let start = body.fly(
807 &environment,
808 &NoLoads,
809 Method::default(),
810 f64::NAN,
811 tumbling(),
812 &[1.0],
813 );
814 assert!(matches!(start, Err(SimError::Settings(_))), "{start:?}");
815 }
816
817 #[test]
822 fn a_body_read_from_json_is_checked_again() {
823 let read = |json: &str| serde_json::from_str::<FreeBody>(json).map_err(|e| e.to_string());
824 let refused = |json: &str, what: &str| {
825 let error = read(json).unwrap_err();
826 assert!(error.contains(what), "{error}");
827 };
828 refused(
829 r#"{"mass_kg":0.0,"inertia_kg_m2":[1,0,0,0,1,0,0,0,1]}"#,
830 FREE_BODY_MASS_REFUSED,
831 );
832 refused(
833 r#"{"mass_kg":-1.0,"inertia_kg_m2":[1,0,0,0,1,0,0,0,1]}"#,
834 FREE_BODY_MASS_REFUSED,
835 );
836 refused(
837 r#"{"mass_kg":1.0,"inertia_kg_m2":[-1,0,0,0,-2,0,0,0,3]}"#,
838 FREE_BODY_INERTIA_NOT_POSITIVE,
839 );
840 refused(
841 r#"{"mass_kg":1.0,"inertia_kg_m2":[1,0.25,0,0,2,0,0,0,3]}"#,
842 FREE_BODY_INERTIA_NOT_SYMMETRIC,
843 );
844 let body = brick();
845 let json = serde_json::to_string(&body).unwrap();
846 assert!(json.starts_with(r#"{"mass_kg":"#), "{json}");
847 assert_eq!(read(&json).unwrap(), body);
848 }
849
850 #[test]
855 fn loads_see_the_air_as_a_flight_does_and_enter_newtons_and_eulers_equations() {
856 use std::cell::Cell;
857
858 let undulation_m = 17.0;
862 let wind = DVec3::new(-6.0, 4.0, 0.0);
863 let environment = Environment::standard(site())
864 .unwrap()
865 .with_wind(
866 ConstantWind::new(wind.length(), wind.x.atan2(wind.y) + std::f64::consts::PI)
867 .unwrap(),
868 )
869 .with_geoid_undulation_m(undulation_m);
870 let body = brick();
871 let state = tumbling();
872 let force = DVec3::new(0.7, -1.1, 2.3);
873 let moment = DVec3::new(0.002, 0.003, -0.001);
874 let seen = Cell::new(None);
875 let loads = |inputs: &LoadInputs| {
876 seen.set(Some(*inputs));
877 Ok(Loads {
878 force_body_n: force,
879 moment_body_n_m: moment,
880 })
881 };
882 let y = state.to_array();
883 let loaded = body.derivative(&environment, &loads, &y).unwrap();
884 let free = body.derivative(&environment, &NoLoads, &y).unwrap();
885
886 let inputs = seen.get().unwrap();
887 let q = state.unit_attitude();
888 let height = environment
889 .earth
890 .frame()
891 .geodetic_from_enu(state.position_enu_m)
892 .unwrap()
893 .height_m;
894 assert_eq!(inputs.height_msl_m, height - undulation_m);
895 let density = |at_m: f64| environment.atmosphere.air(at_m).unwrap().air.density_kg_m3;
896 assert_eq!(inputs.air.density_kg_m3, density(height - undulation_m));
897 for wrong in [height, height + undulation_m] {
898 assert!((density(wrong) / density(height - undulation_m) - 1.0).abs() > 1e-4);
899 }
900 assert!(
901 (inputs.wind_enu_m_s - wind).length() < 1e-12,
902 "{:?}",
903 inputs.wind_enu_m_s
904 );
905 let through_air = q.conjugate().mul_vec3(state.velocity_enu_m_s - wind);
906 assert!((inputs.air_velocity_body_m_s - through_air).length() < 1e-12);
907 assert_eq!(inputs.state.body_rate_rad_s, state.body_rate_rad_s);
908
909 let gained = DVec3::from_slice(&loaded[3..6]) - DVec3::from_slice(&free[3..6]);
910 let expected = q.mul_vec3(force) / body.mass_kg();
911 assert!(
912 (gained - expected).length() < 1e-12,
913 "{gained:?} vs {expected:?}"
914 );
915 let turned = DVec3::from_slice(&loaded[10..13]) - DVec3::from_slice(&free[10..13]);
916 let expected = body.inertia_kg_m2().inverse() * moment;
917 assert!(
918 (turned - expected).length() < 1e-9 * expected.length(),
919 "{turned:?} vs {expected:?}"
920 );
921 assert_eq!(loaded[0..3], free[0..3]);
923 assert_eq!(loaded[6..10], free[6..10]);
924 }
925
926 #[test]
930 fn a_damped_spin_decays_as_the_exponential() {
931 let j = 0.05;
932 let c = 0.02;
933 let body = FreeBody::new(1.0, DMat3::from_diagonal(DVec3::splat(j))).unwrap();
934 let damping = |inputs: &LoadInputs| {
935 Ok(Loads {
936 force_body_n: DVec3::ZERO,
937 moment_body_n_m: inputs.state.body_rate_rad_s * -c,
938 })
939 };
940 let start = State {
941 body_rate_rad_s: DVec3::new(1.0, -2.0, 0.5),
942 ..tumbling()
943 };
944 let times = [1.0, 2.5, 5.0];
945 let states = body
946 .fly(
947 &analytic_environment(UniformAir::vacuum(), 9.806_65),
948 &damping,
949 Method::default(),
950 0.0,
951 start,
952 ×,
953 )
954 .unwrap();
955 for (time, state) in times.iter().zip(&states) {
956 let expected = start.body_rate_rad_s * (-c * time / j).exp();
957 let gap = (state.body_rate_rad_s - expected).length();
958 assert!(
959 gap < 1e-7 * start.body_rate_rad_s.length(),
960 "{time} s: {gap:e}"
961 );
962 }
963 assert!(states[2].body_rate_rad_s.length() < 0.14 * start.body_rate_rad_s.length());
965 }
966
967 #[test]
969 fn a_load_that_is_not_finite_is_refused() {
970 let body = brick();
971 let bad = |_: &LoadInputs| {
972 Ok(Loads {
973 force_body_n: DVec3::ZERO,
974 moment_body_n_m: DVec3::new(0.0, f64::NAN, 0.0),
975 })
976 };
977 let error = body
978 .derivative(&airless_earth(), &bad, &tumbling().to_array())
979 .unwrap_err();
980 assert!(
981 matches!(
982 error,
983 SimError::Domain { what, value }
984 if what == FREE_BODY_LOAD_NOT_FINITE && value.is_nan()
985 ),
986 "{error:?}"
987 );
988 }
989}