use std::collections::HashSet;
use bevy::ecs::message::MessageReader;
use bevy::ecs::system::ParamSet;
use bevy::prelude::*;
use astrodyn::{MassPointState, Planet};
use crate::components::{
Abm4StateC, FrameAngVelC, FrameAttachEvent, FrameAttachedC, FrameDetachEvent, FrameEntityC,
FrameRotC, FrameTransC, GaussJacksonStateC, MassChildOf, MassPropertiesC, RotationalStateC,
TranslationalStateC,
};
use crate::frame_param::RelativeFrameState;
use crate::RootFrameEntityR;
use glam::DVec3;
#[allow(clippy::type_complexity, clippy::too_many_arguments)]
pub fn frame_attach_system<P: Planet>(
mut commands: Commands,
mut attach_events: MessageReader<FrameAttachEvent>,
mut detach_events: MessageReader<FrameDetachEvent>,
already_frame_attached: Query<Entity, With<FrameAttachedC>>,
has_mass_parent: Query<&MassChildOf>,
parent_frame_components: Query<(
bevy::ecs::query::Has<FrameTransC>,
bevy::ecs::query::Has<FrameRotC>,
bevy::ecs::query::Has<FrameAngVelC>,
)>,
body_components: Query<(
bevy::ecs::query::Has<TranslationalStateC<P>>,
bevy::ecs::query::Has<FrameTransC>,
)>,
mut integrators: Query<(Option<&mut GaussJacksonStateC>, Option<&mut Abm4StateC>)>,
) {
let mut attached_this_tick: HashSet<Entity> = HashSet::new();
let mut detached_this_tick: HashSet<Entity> = HashSet::new();
for evt in attach_events.read() {
let (body_has_trans, body_has_frame_trans) =
body_components.get(evt.body).unwrap_or((false, false));
assert!(
body_has_trans && !body_has_frame_trans,
"FrameAttachEvent: body {:?} is not a valid body entity \
(TranslationalStateC present: {}, FrameTransC present: {}). \
A frame-attach target must carry `TranslationalStateC` so the \
per-tick propagation can overwrite its state, and must NOT carry \
`FrameTransC` (the propagation query is filtered \
`Without<FrameTransC>` to keep body and frame state disjoint). \
Pass the entity id of a body spawned via the typestate \
`VehicleBuilder` (or a manually-spawned body that includes \
`TranslationalStateC` / `RotationalStateC`) — not a frame entity, \
a source entity, or an arbitrary placeholder. Parent frame {:?}.",
evt.body,
body_has_trans,
body_has_frame_trans,
evt.parent_frame,
);
assert!(
!attached_this_tick.contains(&evt.body),
"FrameAttachEvent: body {:?} already had a FrameAttachEvent processed \
earlier in this tick. Two simultaneous attach events on the same \
body would silently overwrite the first event's offset and \
`t_parent_body` (the queued `commands.insert` is invisible to the \
component query until the next apply boundary). Coalesce duplicate \
events in mission code, or send a FrameDetachEvent on the \
intervening tick before re-attaching.",
evt.body
);
assert!(
already_frame_attached.get(evt.body).is_err(),
"FrameAttachEvent: body {:?} is already frame-attached. Send a \
FrameDetachEvent before re-attaching to a different parent frame; \
silent overwrite would lose the original frame-tree relationship \
and leave the captured offset desynchronized from the body's \
actual position.",
evt.body
);
assert!(
has_mass_parent.get(evt.body).is_err(),
"FrameAttachEvent: body {:?} is a mass-tree child (carries \
MassChildOf). Send a DetachEvent first — frame attachment and \
mass-tree attachment are mutually exclusive (JEOD's \
`attach_to_frame` writes the attachment on the integrated tree \
root, not on a child body). Mixing both would let the \
frame-attached propagation overwrite the parent-derived state \
every tick.",
evt.body
);
let (has_trans, has_rot, has_angvel) = parent_frame_components
.get(evt.parent_frame)
.unwrap_or((false, false, false));
assert!(
has_trans && has_rot && has_angvel,
"FrameAttachEvent: parent_frame {:?} is not a frame entity \
(missing{}{}{}). Frame-tree consumers walk every parent_frame \
via `RelativeFrameState`, which requires the full \
FrameTransC / FrameRotC / FrameAngVelC triplet on each node. \
Spawn the parent via `PlanetBundle` (for planet-inertial / \
planet-fixed frames) or by inserting the triplet directly \
(e.g., for a custom joint frame), and pass that frame's \
entity id — not a body, source, or arbitrary placeholder \
entity. Body {:?}.",
evt.parent_frame,
if has_trans { "" } else { " FrameTransC" },
if has_rot { "" } else { " FrameRotC" },
if has_angvel { "" } else { " FrameAngVelC" },
evt.body,
);
commands.entity(evt.body).insert(FrameAttachedC {
parent_frame: evt.parent_frame,
offset: evt.offset,
t_parent_body: evt.t_parent_body,
});
attached_this_tick.insert(evt.body);
if let Ok((gj, abm4)) = integrators.get_mut(evt.body) {
if let Some(mut state) = gj {
state.0.reset();
}
if let Some(mut state) = abm4 {
state.0.reset();
}
}
}
for evt in detach_events.read() {
assert!(
!detached_this_tick.contains(&evt.body),
"FrameDetachEvent: body {:?} already had a FrameDetachEvent processed \
earlier in this tick. Two simultaneous detach events would silently \
no-op the second one (the queued `commands.remove` is invisible to \
the component query until the next apply boundary). Coalesce \
duplicate events in mission code.",
evt.body
);
assert!(
already_frame_attached.get(evt.body).is_ok(),
"FrameDetachEvent: body {:?} is not currently frame-attached. \
Send a FrameAttachEvent first, or remove the duplicate detach \
to avoid masking caller bugs.",
evt.body
);
commands.entity(evt.body).remove::<FrameAttachedC>();
detached_this_tick.insert(evt.body);
if let Ok((gj, abm4)) = integrators.get_mut(evt.body) {
if let Some(mut state) = gj {
state.0.reset();
}
if let Some(mut state) = abm4 {
state.0.reset();
}
}
}
}
#[allow(clippy::type_complexity, clippy::too_many_arguments)]
pub fn propagate_frame_attached_state_system<P: Planet>(
attached: Query<(Entity, &FrameAttachedC)>,
mut state_q: Query<
(
&mut TranslationalStateC<P>,
Option<&mut RotationalStateC>,
Option<&FrameEntityC>,
),
Without<crate::components::FrameTransC>,
>,
mut frame_qs: ParamSet<(
RelativeFrameState,
Query<
(
&'static mut crate::components::FrameTransC,
Option<&'static mut crate::components::FrameRotC>,
Option<&'static mut crate::components::FrameAngVelC>,
),
Without<TranslationalStateC<P>>,
>,
)>,
body_frames: Query<&FrameEntityC>,
parents: Query<&ChildOf>,
body_mass: Query<&MassPropertiesC>,
root_frame: Res<RootFrameEntityR>,
) {
if attached.is_empty() {
return;
}
let root = root_frame.0;
struct AttachWork {
body_entity: Entity,
derived: astrodyn::RefFrameState,
integ_origin_pos: DVec3,
integ_origin_vel: DVec3,
}
let mut work: Vec<AttachWork> = Vec::with_capacity(attached.iter().len());
{
let rel = frame_qs.p0();
for (body_entity, attach) in attached.iter() {
let parent_state = rel.relative_state(root, attach.parent_frame);
let composite_offset = body_mass
.get(body_entity)
.ok()
.map(|mp| {
let untyped = astrodyn::typed_bridge::mass_typed_to_raw(&mp.0);
MassPointState {
position: untyped.position,
t_parent_this: untyped.t_parent_this,
}
})
.unwrap_or_default();
let derived = astrodyn::derive_frame_attached_state(astrodyn::FrameAttachInputs {
parent_frame: parent_state,
attach_offset: MassPointState {
position: attach.offset,
t_parent_this: attach.t_parent_body,
},
composite_offset,
});
let integ_frame_entity = body_frames
.get(body_entity)
.ok()
.and_then(|fe| parents.get(fe.0).ok().map(|child_of| child_of.parent()));
let (integ_origin_pos, integ_origin_vel) = match integ_frame_entity {
Some(integ_e) if integ_e != root => rel.position_velocity(root, integ_e),
_ => (DVec3::ZERO, DVec3::ZERO),
};
work.push(AttachWork {
body_entity,
derived,
integ_origin_pos,
integ_origin_vel,
});
}
}
let mut frame_writeback_q = frame_qs.p1();
for AttachWork {
body_entity,
derived,
integ_origin_pos,
integ_origin_vel,
} in &work
{
let (mut trans, rot_opt, frame_opt) = state_q.get_mut(*body_entity).unwrap_or_else(|err| {
panic!(
"propagate_frame_attached_state_system: body {body_entity:?} \
carries FrameAttachedC but the body-state query returned \
{err:?}. The attach-time validation in `frame_attach_system` \
rejects bodies that lack TranslationalStateC, so this means \
either the marker was inserted manually (forbidden — use \
FrameAttachEvent / FrameDetachEvent) or the body was \
despawned without a paired FrameDetachEvent. Send a \
FrameDetachEvent before despawning a frame-attached body."
)
});
let derived_trans = astrodyn::TranslationalState {
position: derived.trans.position - *integ_origin_pos,
velocity: derived.trans.velocity - *integ_origin_vel,
};
trans.0 = astrodyn::typed_bridge::trans_raw_to_planet::<P>(&derived_trans);
if let Some(mut rot) = rot_opt {
let derived_rot = astrodyn::RotationalState {
quaternion: derived.rot.q_parent_this,
ang_vel_body: derived.rot.ang_vel_this,
};
rot.0 = astrodyn::typed_bridge::rot_raw_to_self_ref(&derived_rot);
}
if let Some(frame_entity) = frame_opt.map(|f| f.0) {
let (mut frame_trans, frame_rot, frame_angvel) = frame_writeback_q
.get_mut(frame_entity)
.unwrap_or_else(|err| {
panic!(
"propagate_frame_attached_state_system: body {body_entity:?} \
carries FrameEntityC({frame_entity:?}) but the frame-state \
writeback query returned {err:?}. The frame entity must \
carry FrameTransC (and the optional FrameRotC / \
FrameAngVelC) — either the entity has been despawned \
without clearing FrameEntityC on the body, or \
FrameEntityC was set to an entity that was never \
spawned as a frame. Spawn frame entities through \
`register_*_frames_system` (which inserts the full \
FrameTrans/Rot/AngVel triple) and clear FrameEntityC \
before despawning the frame entity."
)
});
frame_trans.position = derived_trans.position;
frame_trans.velocity = derived_trans.velocity;
if let Some(mut rot) = frame_rot {
rot.q_parent_this = derived.rot.q_parent_this;
rot.t_parent_this = derived.rot.t_parent_this;
}
if let Some(mut av) = frame_angvel {
av.0 = derived.rot.ang_vel_this;
}
}
}
}
#[allow(clippy::type_complexity, clippy::too_many_arguments)]
pub fn propagate_frame_attached_state_post_integration_system<P: Planet>(
attached: Query<(Entity, &FrameAttachedC)>,
state_q: Query<
(
&mut TranslationalStateC<P>,
Option<&mut RotationalStateC>,
Option<&FrameEntityC>,
),
Without<crate::components::FrameTransC>,
>,
frame_qs: ParamSet<(
RelativeFrameState,
Query<
(
&'static mut crate::components::FrameTransC,
Option<&'static mut crate::components::FrameRotC>,
Option<&'static mut crate::components::FrameAngVelC>,
),
Without<TranslationalStateC<P>>,
>,
)>,
body_frames: Query<&FrameEntityC>,
parents: Query<&ChildOf>,
body_mass: Query<&MassPropertiesC>,
root_frame: Res<RootFrameEntityR>,
) {
propagate_frame_attached_state_system::<P>(
attached,
state_q,
frame_qs,
body_frames,
parents,
body_mass,
root_frame,
);
}
#[cfg(test)]
mod tests {
use super::*;
use crate::components::{
DynamicsConfigC, ExternalForceC, ExternalTorqueC, FrameDerivativesC, KinematicChildC,
MassChildOf, MassPropertiesC, RotationalStateC, TotalForceC, TranslationalStateC,
};
use crate::{AstrodynPlugin, IntegrationDtR};
use astrodyn::{MassProperties, RotationalState, TranslationalState};
use bevy::prelude::FixedUpdate;
use bevy::time::{Fixed, Time};
use glam::DVec3;
use std::time::Duration;
fn step_bevy(app: &mut App, n: usize, dt: f64) {
app.insert_resource(IntegrationDtR(dt));
for _ in 0..n {
app.world_mut()
.resource_mut::<Time<Fixed>>()
.advance_by(Duration::from_secs_f64(dt));
app.world_mut().run_schedule(FixedUpdate);
}
}
fn spawn_test_body(app: &mut App) -> Entity {
app.world_mut()
.spawn((
MassPropertiesC::from(astrodyn::typed_bridge::mass_raw_to_self_ref(
&(MassProperties::new(1.0)),
)),
RotationalStateC::from(astrodyn::typed_bridge::rot_raw_to_self_ref(
&(RotationalState::default()),
)),
TranslationalStateC::<astrodyn::Earth>::from_untyped(TranslationalState::default()),
TotalForceC::default(),
FrameDerivativesC::default(),
DynamicsConfigC::default(),
ExternalForceC::default(),
ExternalTorqueC::default(),
))
.id()
}
#[test]
fn attach_event_inserts_marker() {
let mut app = App::new();
app.add_plugins((MinimalPlugins, AstrodynPlugin));
let body = spawn_test_body(&mut app);
let parent_frame = **app.world().resource::<RootFrameEntityR>();
app.world_mut()
.resource_mut::<bevy::ecs::message::Messages<FrameAttachEvent>>()
.write(FrameAttachEvent {
body,
parent_frame,
offset: DVec3::ZERO,
t_parent_body: glam::DMat3::IDENTITY,
});
step_bevy(&mut app, 1, 0.1);
assert!(
app.world().entity(body).contains::<FrameAttachedC>(),
"FrameAttachedC should be present after FrameAttachEvent processed"
);
}
#[test]
fn detach_event_removes_marker() {
let mut app = App::new();
app.add_plugins((MinimalPlugins, AstrodynPlugin));
let body = spawn_test_body(&mut app);
let parent_frame = **app.world().resource::<RootFrameEntityR>();
app.world_mut()
.resource_mut::<bevy::ecs::message::Messages<FrameAttachEvent>>()
.write(FrameAttachEvent {
body,
parent_frame,
offset: DVec3::ZERO,
t_parent_body: glam::DMat3::IDENTITY,
});
step_bevy(&mut app, 1, 0.1);
assert!(app.world().entity(body).contains::<FrameAttachedC>());
app.world_mut()
.resource_mut::<bevy::ecs::message::Messages<FrameDetachEvent>>()
.write(FrameDetachEvent { body });
step_bevy(&mut app, 1, 0.1);
assert!(
!app.world().entity(body).contains::<FrameAttachedC>(),
"FrameAttachedC should have been removed after FrameDetachEvent"
);
}
#[test]
#[should_panic(expected = "already had a FrameAttachEvent processed earlier in this tick")]
fn duplicate_attach_event_in_same_tick_panics() {
let mut app = App::new();
app.add_plugins((MinimalPlugins, AstrodynPlugin));
let body = spawn_test_body(&mut app);
let parent_frame = **app.world().resource::<RootFrameEntityR>();
let mut messages = app
.world_mut()
.resource_mut::<bevy::ecs::message::Messages<FrameAttachEvent>>();
messages.write(FrameAttachEvent {
body,
parent_frame,
offset: DVec3::new(1.0, 0.0, 0.0),
t_parent_body: glam::DMat3::IDENTITY,
});
messages.write(FrameAttachEvent {
body,
parent_frame,
offset: DVec3::new(2.0, 0.0, 0.0),
t_parent_body: glam::DMat3::IDENTITY,
});
step_bevy(&mut app, 1, 0.1);
}
#[test]
#[should_panic(expected = "already had a FrameDetachEvent processed earlier in this tick")]
fn duplicate_detach_event_in_same_tick_panics() {
let mut app = App::new();
app.add_plugins((MinimalPlugins, AstrodynPlugin));
let body = spawn_test_body(&mut app);
let parent_frame = **app.world().resource::<RootFrameEntityR>();
app.world_mut()
.resource_mut::<bevy::ecs::message::Messages<FrameAttachEvent>>()
.write(FrameAttachEvent {
body,
parent_frame,
offset: DVec3::ZERO,
t_parent_body: glam::DMat3::IDENTITY,
});
step_bevy(&mut app, 1, 0.1);
assert!(app.world().entity(body).contains::<FrameAttachedC>());
let mut messages = app
.world_mut()
.resource_mut::<bevy::ecs::message::Messages<FrameDetachEvent>>();
messages.write(FrameDetachEvent { body });
messages.write(FrameDetachEvent { body });
step_bevy(&mut app, 1, 0.1);
}
#[test]
#[should_panic(expected = "is not a frame entity")]
fn attach_event_with_non_frame_parent_panics() {
let mut app = App::new();
app.add_plugins((MinimalPlugins, AstrodynPlugin));
let body = spawn_test_body(&mut app);
let bogus_parent = app.world_mut().spawn_empty().id();
app.world_mut()
.resource_mut::<bevy::ecs::message::Messages<FrameAttachEvent>>()
.write(FrameAttachEvent {
body,
parent_frame: bogus_parent,
offset: DVec3::ZERO,
t_parent_body: glam::DMat3::IDENTITY,
});
step_bevy(&mut app, 1, 0.1);
}
#[test]
#[should_panic(expected = "is not a valid body entity")]
fn attach_event_with_non_body_target_panics() {
let mut app = App::new();
app.add_plugins((MinimalPlugins, AstrodynPlugin));
let bogus_body = app.world_mut().spawn_empty().id();
let parent_frame = **app.world().resource::<RootFrameEntityR>();
app.world_mut()
.resource_mut::<bevy::ecs::message::Messages<FrameAttachEvent>>()
.write(FrameAttachEvent {
body: bogus_body,
parent_frame,
offset: DVec3::ZERO,
t_parent_body: glam::DMat3::IDENTITY,
});
step_bevy(&mut app, 1, 0.1);
}
#[test]
#[should_panic(expected = "is not a valid body entity")]
fn attach_event_with_frame_entity_target_panics() {
let mut app = App::new();
app.add_plugins((MinimalPlugins, AstrodynPlugin));
let bogus_body = **app.world().resource::<RootFrameEntityR>();
let parent_frame = bogus_body;
app.world_mut()
.resource_mut::<bevy::ecs::message::Messages<FrameAttachEvent>>()
.write(FrameAttachEvent {
body: bogus_body,
parent_frame,
offset: DVec3::ZERO,
t_parent_body: glam::DMat3::IDENTITY,
});
step_bevy(&mut app, 1, 0.1);
}
#[test]
fn frame_attached_parent_propagates_before_kinematic_child() {
let mut app = App::new();
app.add_plugins((MinimalPlugins, AstrodynPlugin));
let parent_frame = **app.world().resource::<RootFrameEntityR>();
let parent_offset = DVec3::new(1234.5, -678.9, 42.0);
let parent_body = app
.world_mut()
.spawn((
Name::new("frame_attached_root"),
MassPropertiesC::from(astrodyn::typed_bridge::mass_raw_to_self_ref(
&(MassProperties::new(10.0)),
)),
RotationalStateC::from(astrodyn::typed_bridge::rot_raw_to_self_ref(
&(RotationalState::default()),
)),
TranslationalStateC::<astrodyn::Earth>::from_untyped(TranslationalState::default()),
TotalForceC::default(),
FrameDerivativesC::default(),
DynamicsConfigC::default(),
ExternalForceC::default(),
ExternalTorqueC::default(),
))
.id();
let child_link_offset = DVec3::new(0.0, 100.0, 0.0);
let child_body = app
.world_mut()
.spawn((
Name::new("kinematic_child"),
MassPropertiesC::from(astrodyn::typed_bridge::mass_raw_to_self_ref(
&(MassProperties::new(5.0)),
)),
MassChildOf::with_rotation(parent_body, child_link_offset, glam::DMat3::IDENTITY),
KinematicChildC,
RotationalStateC::from(astrodyn::typed_bridge::rot_raw_to_self_ref(
&(RotationalState::default()),
)),
TranslationalStateC::<astrodyn::Earth>::from_untyped(TranslationalState::default()),
TotalForceC::default(),
FrameDerivativesC::default(),
DynamicsConfigC::default(),
ExternalForceC::default(),
ExternalTorqueC::default(),
))
.id();
app.world_mut()
.resource_mut::<bevy::ecs::message::Messages<FrameAttachEvent>>()
.write(FrameAttachEvent {
body: parent_body,
parent_frame,
offset: parent_offset,
t_parent_body: glam::DMat3::IDENTITY,
});
step_bevy(&mut app, 1, 0.1);
let parent_state = app
.world()
.get::<TranslationalStateC<astrodyn::Earth>>(parent_body)
.expect("parent body should still have TranslationalStateC");
let parent_pos = parent_state.0.position.raw_si();
let parent_mass = 10.0;
let child_mass = 5.0;
let parent_composite_cm = child_link_offset * (child_mass / (parent_mass + child_mass));
let expected_parent_pos = parent_offset + parent_composite_cm;
assert!(
(parent_pos - expected_parent_pos).length() < 1e-9,
"frame-attached parent must end the tick at its struct → composite \
derived state. Expected {expected_parent_pos:?} (= captured offset \
{parent_offset:?} + composite CoM {parent_composite_cm:?}); \
got {parent_pos:?}",
);
let child_state = app
.world()
.get::<TranslationalStateC<astrodyn::Earth>>(child_body)
.expect("kinematic child should still have TranslationalStateC");
let child_pos = child_state.0.position.raw_si();
let expected_child_pos = parent_offset + child_link_offset;
assert!(
(child_pos - expected_child_pos).length() < 1e-9,
"kinematic child of a frame-attached root must derive its state from \
the freshly-propagated parent. Expected {expected_child_pos:?} \
(= parent_offset + child_link_offset), got {child_pos:?}. \
If the schedule order regressed (kinematic walk before frame-attach \
propagation), the child would read the default-zero parent state \
and miss the parent_offset contribution.",
);
}
}