use astrodyn::{OrbitalError, Planet, RootInertial};
use bevy::prelude::*;
use crate::components::*;
use crate::frame_param::FrameOrigin;
use super::util::body_integ_origin_in_root;
pub fn orbital_elements_system<P: Planet>(
mut query: Query<(
Entity,
&TranslationalStateC<P>,
&OrbitalElementsConfigC,
&mut OrbitalElementsC<P>,
)>,
sources: Query<(&FrameUidC, &GravitySourceC)>,
) {
for (entity, state, config, mut elements) in &mut query {
let source = sources
.iter()
.find(|(uid, _)| uid.0 == config.gravity_source)
.map(|(_, s)| s)
.unwrap_or_else(|| {
panic!(
"{entity:?} orbital elements: \
OrbitalElementsConfigC.gravity_source = `{gravity_source}` \
does not resolve to a registered gravity source. Spawn the \
source (PlanetBundle / SunBundle / MoonBundle / populate_app) \
with that identity, or fix the config's identity.",
gravity_source = config.gravity_source
)
});
let mu_p = astrodyn::GravParam::<P>::from_si(source.mu);
let result =
astrodyn::compute_orbital_elements_typed::<P>(mu_p, state.position, state.velocity);
elements.0 = result.unwrap_or_else(|err| panic_for_orbital_error(entity, err));
}
}
fn panic_for_orbital_error(entity: Entity, err: OrbitalError) -> ! {
match err {
OrbitalError::InvalidMu(mu) => panic!(
"{entity:?} orbital elements: source gravity has μ <= 0 (got {mu}). \
Configure a positive μ on the source body before adding orbital-elements \
derived state."
),
OrbitalError::DegenerateOrbit => panic!(
"{entity:?} orbital elements: orbit degenerate (|h| ≈ 0, where h = r × v is \
the specific angular momentum). Common causes: zero velocity, purely radial \
trajectory, or position and velocity parallel. Either don't request orbital \
elements at this instant or initialize a non-degenerate orbit."
),
OrbitalError::KeplerConvergence(iters) => panic!(
"{entity:?} orbital elements: Kepler iteration failed to converge after \
{iters} iterations. Inspect the eccentric / hyperbolic orbit input."
),
}
}
pub fn euler_angles_system(
mut query: Query<(
Option<&RotationalStateC>,
&EulerAnglesConfigC,
&mut EulerAnglesC,
)>,
) {
for (rot_opt, config, mut angles) in &mut query {
if let Some(rot) = rot_opt {
let rot_untyped = astrodyn::typed_bridge::rot_typed_to_raw(&rot.0);
angles.0 = astrodyn::compute_body_euler_angles_typed(&rot_untyped, config.sequence);
} else {
angles.0 = Default::default();
}
}
}
pub fn lvlh_system<P: Planet>(mut query: Query<(&TranslationalStateC<P>, &mut LvlhFrameC)>) {
for (state, mut lvlh) in &mut query {
lvlh.0 = astrodyn::compute_body_lvlh_frame_typed::<P>(state.position, state.velocity);
}
}
pub fn geodetic_system<P: Planet>(
mut query: Query<(
Entity,
&TranslationalStateC<P>,
&GeodeticConfigC,
&mut GeodeticStateC,
)>,
planets: Query<(&FrameUidC, &PlanetFixedRotationC<P>)>,
) {
for (entity, state, config, mut geodetic) in &mut query {
let rot = planets
.iter()
.find(|(uid, _)| uid.0 == config.planet)
.map(|(_, r)| r)
.unwrap_or_else(|| {
panic!(
"{entity:?} geodetic state: \
GeodeticConfigC.planet = `{planet_uid}` does not resolve to \
an entity carrying PlanetFixedRotationC<{p}>. Spawn the planet \
source with that identity and a non-`None` `rotation_model` \
(or hand-insert `PlanetFixedRotationC<{p}>` on it), or fix \
the config's identity.",
planet_uid = config.planet,
p = std::any::type_name::<P>(),
)
});
use astrodyn::F64Ext;
geodetic.0 = astrodyn::compute_body_geodetic_typed::<P>(
state.position,
rot.0.matrix_ref(),
config.r_eq.m(),
config.r_pol.m(),
);
}
}
#[allow(clippy::type_complexity)]
pub fn solar_beta_system<P: Planet>(
frame_origin: FrameOrigin,
root_frame_entity: Res<crate::RootFrameEntityR>,
parents: Query<&ChildOf>,
mut query: Query<
(
Entity,
&TranslationalStateC<P>,
Option<&FrameEntityC>,
&mut SolarBetaC,
),
Without<SunMarker>,
>,
sun_query: Query<&TranslationalStateC<P>, With<SunMarker>>,
) {
let sun_state = match sun_query.single() {
Ok(s) => s,
Err(bevy::ecs::query::QuerySingleError::NoEntities(_)) => {
if let Some((entity, _, _, _)) = query.iter().next() {
panic!(
"{entity:?} solar beta: no SunMarker entity exists in the World, \
but {entity:?} carries SolarBetaC. Spawn a Sun source body via \
SunBundle (which inserts SunMarker + TranslationalStateC and \
registers the body in the source frame tree), or remove \
SolarBetaC from {entity:?}."
);
}
return;
}
Err(bevy::ecs::query::QuerySingleError::MultipleEntities(_)) => {
panic!(
"Multiple entities with SunMarker found in solar_beta_system. \
JEOD assumes exactly one Sun body; ensure exactly one SunMarker entity exists."
);
}
};
for (_entity, state, body_frame, mut beta) in &mut query {
let (integ_origin, integ_origin_vel) =
body_integ_origin_in_root(body_frame, &parents, root_frame_entity.0, &frame_origin);
let body_pos_rel = state.position.relabel_to::<RootInertial>();
let body_vel_rel = state.velocity.relabel_to::<RootInertial>();
let body_pos = body_pos_rel + integ_origin;
let body_vel = body_vel_rel + integ_origin_vel;
let sun_pos = sun_state.position.relabel_to::<RootInertial>();
beta.0 = astrodyn::compute_body_solar_beta_typed(body_pos, body_vel, sun_pos).value;
}
}
#[cfg(test)]
mod tests {
use super::*;
use crate::components::{
GeodeticConfigC, GeodeticStateC, GravitySourceC, OrbitalElementsC, OrbitalElementsConfigC,
SolarBetaC, TranslationalStateC,
};
use crate::test_utils::create_minimal_test_app;
use astrodyn::{Earth, GravityModel, GravitySource, TranslationalState};
use glam::DVec3;
fn spawn_vehicle_with_state(mu: f64, pos: DVec3, vel: DVec3) -> App {
let mut app = create_minimal_test_app();
app.world_mut().spawn((
crate::components::FrameUidC(
astrodyn::FrameUid::of::<astrodyn::PlanetInertial<Earth>>(),
),
GravitySourceC(GravitySource {
mu,
model: GravityModel::PointMass,
}),
));
app.world_mut().spawn((
TranslationalStateC::<Earth>::from_untyped(TranslationalState {
position: pos,
velocity: vel,
}),
OrbitalElementsConfigC {
gravity_source: astrodyn::FrameUid::of::<astrodyn::PlanetInertial<Earth>>(),
},
OrbitalElementsC::<Earth>::default(),
));
app.add_systems(Update, orbital_elements_system::<Earth>);
app
}
#[test]
#[should_panic(expected = "source gravity has \u{3bc} <= 0")]
fn invalid_mu_panics_with_caller_fix() {
let mut app = spawn_vehicle_with_state(
-1.0,
DVec3::new(7e6, 0.0, 0.0),
DVec3::new(0.0, 7500.0, 0.0),
);
app.update();
}
#[test]
#[should_panic(expected = "|h| \u{2248} 0")]
fn degenerate_orbit_panics_with_caller_fix() {
let mut app =
spawn_vehicle_with_state(3.986004418e14, DVec3::new(7e6, 0.0, 0.0), DVec3::ZERO);
app.update();
}
#[test]
#[should_panic(expected = "Kepler iteration failed to converge after 1234 iterations")]
fn kepler_convergence_panics_with_caller_fix() {
let mut app = create_minimal_test_app();
let entity = app.world_mut().spawn_empty().id();
panic_for_orbital_error(entity, OrbitalError::KeplerConvergence(1234));
}
#[test]
#[should_panic(expected = "does not resolve to a registered gravity source")]
fn orbital_gravity_source_lookup_miss_panics_with_caller_fix() {
let mut app = create_minimal_test_app();
app.world_mut().spawn((
TranslationalStateC::<Earth>::from_untyped(TranslationalState {
position: DVec3::new(7e6, 0.0, 0.0),
velocity: DVec3::new(0.0, 7500.0, 0.0),
}),
OrbitalElementsConfigC {
gravity_source: astrodyn::named_body_frame_uid("derived-state-no-such-source"),
},
OrbitalElementsC::<Earth>::default(),
));
app.add_systems(Update, orbital_elements_system::<Earth>);
app.update();
}
#[test]
#[should_panic(expected = "does not resolve to an entity carrying PlanetFixedRotationC")]
fn geodetic_planet_lookup_miss_panics_with_caller_fix() {
let mut app = create_minimal_test_app();
app.world_mut().spawn((
TranslationalStateC::<Earth>::from_untyped(TranslationalState {
position: DVec3::new(7e6, 0.0, 0.0),
velocity: DVec3::ZERO,
}),
GeodeticConfigC {
planet: astrodyn::named_body_frame_uid("derived-state-no-such-source"),
r_eq: astrodyn::EARTH.shape.r_eq(),
r_pol: astrodyn::EARTH.shape.r_pol(),
},
GeodeticStateC::default(),
));
app.add_systems(Update, geodetic_system::<Earth>);
app.update();
}
#[test]
#[should_panic(expected = "no SunMarker entity exists in the World")]
fn solar_beta_missing_sun_marker_panics_with_caller_fix() {
let mut app = create_minimal_test_app();
let root_frame_e = app.world_mut().spawn_empty().id();
app.insert_resource(crate::RootFrameEntityR(root_frame_e));
app.world_mut().spawn((
TranslationalStateC::<Earth>::from_untyped(TranslationalState {
position: DVec3::new(7e6, 0.0, 0.0),
velocity: DVec3::new(0.0, 7500.0, 0.0),
}),
SolarBetaC::default(),
));
app.add_systems(Update, solar_beta_system::<Earth>);
app.update();
}
#[test]
fn geodetic_system_runs_when_planet_entity_has_rotation_but_no_planet_shape() {
use crate::components::{GeodeticConfigC, GeodeticStateC, PlanetFixedRotationC};
use astrodyn::{FrameTransform, EARTH};
let mut app = create_minimal_test_app();
app.world_mut().spawn((
crate::components::FrameUidC(
astrodyn::FrameUid::of::<astrodyn::PlanetInertial<Earth>>(),
),
PlanetFixedRotationC::<Earth>(FrameTransform::from_matrix(glam::DMat3::IDENTITY)),
));
let vehicle = app
.world_mut()
.spawn((
TranslationalStateC::<Earth>::from_untyped(TranslationalState {
position: DVec3::new(EARTH.shape.r_eq() + 400_000.0, 0.0, 0.0),
velocity: DVec3::new(0.0, 7500.0, 0.0),
}),
GeodeticConfigC {
planet: astrodyn::FrameUid::of::<astrodyn::PlanetInertial<Earth>>(),
r_eq: EARTH.shape.r_eq(),
r_pol: EARTH.shape.r_pol(),
},
GeodeticStateC::default(),
))
.id();
app.add_systems(Update, geodetic_system::<Earth>);
app.update();
let geo = app
.world()
.get::<GeodeticStateC>(vehicle)
.expect("GeodeticStateC must remain after one tick")
.0;
assert!(
geo.altitude > 1.0e5,
"geodetic altitude = {} m — expected ~4e5 m, indicating the kernel ran",
geo.altitude,
);
}
}