use astrodyn::{
Acceleration, AngularAcceleration, BodyFrame, Force, Planet, Position, RootInertial,
RotationalState, SelfRef, Torque, TranslationalState, Velocity,
};
use bevy::prelude::*;
use glam::DVec3;
use crate::components::*;
use crate::frame_param::{FrameOrigin, RelativeFrameState};
use crate::{IntegrationDtR, SimulationTimeR};
use astrodyn::typed_bridge::{
mass_raw_to_self_ref, mass_typed_to_raw, rot_raw_to_self_ref, rot_typed_to_raw,
trans_raw_to_planet, trans_typed_to_raw,
};
use super::util::body_integ_origin_in_root_lazy;
#[allow(clippy::type_complexity)]
pub fn frame_switch_system<P: Planet>(
mut commands: Commands,
root_frame_entity: Res<crate::RootFrameEntityR>,
sources: Query<&FrameEntityC, With<GravitySourceC>>,
parents: Query<&ChildOf>,
rel: RelativeFrameState,
mut bodies: Query<(
Entity,
&mut TranslationalStateC<P>,
&FrameEntityC,
&mut FrameSwitchesC,
&mut GravityControlsC,
)>,
) {
let known_source_frames: std::collections::HashSet<Entity> =
sources.iter().map(|fe| fe.0).collect();
for (body_entity, mut trans, body_frame_entity, mut switches, mut gravity_controls) in
&mut bodies
{
if switches.0.is_empty() {
continue;
}
let current_integ_frame_entity = parents
.get(body_frame_entity.0)
.unwrap_or_else(|err| {
panic!(
"frame_switch_system: body {body_entity:?} frame entity {fe:?} \
has no ChildOf parent ({err:?}). The body's frame entity must \
be parented under its integration frame entity (set by \
register_body_frames_system).",
fe = body_frame_entity.0,
)
})
.parent();
let current_is_known = current_integ_frame_entity == root_frame_entity.0
|| known_source_frames.contains(¤t_integ_frame_entity);
assert!(
current_is_known,
"frame_switch_system: body {body_entity:?} frame entity \
{fe:?} has parent {parent:?} which is neither the root \
frame entity ({root_e:?}) nor a registered source's \
frame entity. The integration frame entity must be one \
of those — register the source via PlanetBundle before \
spawning the body, or attach the body under the root.",
fe = body_frame_entity.0,
parent = current_integ_frame_entity,
root_e = root_frame_entity.0,
);
let mut trigger_idx = None;
for (idx, sw) in switches.0.iter().enumerate() {
if !sw.active {
continue;
}
let target_frame_entity =
sources
.get(sw.target_source)
.map(|fe| fe.0)
.unwrap_or_else(|err| {
panic!(
"frame_switch_system: body {body_entity:?} switch evaluation failed: \
target source {target:?} is not a registered gravity source — \
it is missing GravitySourceC and/or FrameEntityC. Spawn it via \
PlanetBundle (which inserts both) before referencing it from a \
FrameSwitchConfig. Underlying error: {err:?}",
target = sw.target_source,
)
});
let threshold_sq = sw.switch_distance * sw.switch_distance;
let triggered = match sw.switch_sense {
astrodyn::SwitchSense::OnApproach => {
let pos_in_target = rel.position(target_frame_entity, body_frame_entity.0);
pos_in_target.length_squared() < threshold_sq
}
astrodyn::SwitchSense::OnDeparture => {
trans.0.position.raw_si().length_squared() > threshold_sq
}
};
if triggered {
trigger_idx = Some(idx);
break;
}
}
let Some(idx) = trigger_idx else {
continue;
};
let target_source = switches.0[idx].target_source;
switches.0[idx].active = false;
let new_parent_frame_entity = sources.get(target_source).map(|fe| fe.0).expect(
"frame_switch_system: target source resolved during evaluation \
but failed during application — caller-side mutation between lookups",
);
let new_state = rel.relative_state(new_parent_frame_entity, body_frame_entity.0);
commands
.entity(body_frame_entity.0)
.insert(ChildOf(new_parent_frame_entity))
.insert(FrameTransC {
position: new_state.trans.position,
velocity: new_state.trans.velocity,
});
let pos_typed = astrodyn::Position::<astrodyn::PlanetInertial<P>>::from_raw_si(
new_state.trans.position,
);
let vel_typed = astrodyn::Velocity::<astrodyn::PlanetInertial<P>>::from_raw_si(
new_state.trans.velocity,
);
trans.0.position = pos_typed;
trans.0.velocity = vel_typed;
for ctrl in &mut gravity_controls.0.controls {
ctrl.differential = ctrl.source_name != target_source;
}
}
}
#[allow(clippy::type_complexity, clippy::too_many_arguments)]
pub fn integration_system<P: Planet>(
frame_origin: FrameOrigin,
root_frame_entity: Res<crate::RootFrameEntityR>,
parents: Query<&ChildOf>,
mut bodies: Query<
(
Entity,
&DynamicsConfigC,
&mut TranslationalStateC<P>,
Option<&mut RotationalStateC>,
Option<&MassPropertiesC>,
&GravityControlsC,
&mut TotalForceC,
Option<&IntegratorTypeC>,
Option<&mut GaussJacksonStateC>,
Option<&mut Abm4StateC>,
Option<&mut FlatPlateConfigC>,
Option<&StructuralTransformC>,
Option<&mut RadiationForceC>,
Option<&mut FrameDerivativesC>,
Option<&FrameEntityC>,
),
(
Without<KinematicChildC>,
Without<crate::DetachedSubtreeStateC>,
Without<crate::components::FrameAttachedC>,
),
>,
sources: Query<
(
&GravitySourceC,
Option<&PlanetFixedRotationC<P>>,
&SourceInertialPositionC,
Option<&SourceInertialVelocityC>,
Option<&TidalDeltaC20C>,
Option<&TidalConfigC>,
),
Without<DynamicsConfigC>,
>,
dt: Res<IntegrationDtR>,
sim_time: Res<SimulationTimeR>,
) {
let dt = dt.0;
if dt == 0.0 {
return;
}
let integ_dt = dt * sim_time.0.time_scale_factor;
let eval_gravity = |entity: Entity,
controls: &GravityControlsC,
pos: DVec3,
vel: DVec3,
integ_origin_pos: DVec3,
integ_origin_vel: DVec3,
time_frac: f64|
-> DVec3 {
let stage_dt = time_frac * integ_dt;
let stage_origin_pos = integ_origin_pos + integ_origin_vel * stage_dt;
let sub_dt = if integ_origin_vel != DVec3::ZERO {
stage_dt
} else {
0.0
};
let typed_abs_pos = Position::<RootInertial>::from_raw_si(pos + stage_origin_pos); let typed_abs_vel = Velocity::<RootInertial>::from_raw_si(vel + integ_origin_vel); let typed_origin = Position::<RootInertial>::from_raw_si(stage_origin_pos);
let source_vel = |v: Option<&SourceInertialVelocityC>| -> DVec3 {
v.map(|v| v.0.raw_si()).unwrap_or(DVec3::ZERO)
};
let typed_accel = astrodyn::accumulate_gravity_typed(
typed_abs_pos,
&controls.0,
typed_origin,
|source_entity| match sources.get(source_entity) {
Ok((s, r, p, v, tidal, tidal_config)) => {
let base_pos = p.0.raw_si();
let stage_pos = if sub_dt != 0.0 {
base_pos + source_vel(v) * sub_dt
} else {
base_pos
};
Some(astrodyn::ResolvedSource {
source: &s.0,
rotation: r.map(|r| r.0.matrix_ref()),
position: stage_pos,
delta_c20: tidal.map_or(0.0, |t| t.0.value),
has_delta_coeffs: tidal_config.is_some(),
})
}
Err(_) => {
panic!(
"Entity {entity:?}: GravityControl references source \
{source_entity:?} which does not exist or lacks \
GravitySourceC + SourceInertialPositionC."
);
}
},
);
let mut accel = typed_accel.grav_accel.raw_si();
let rel = astrodyn::accumulate_relativistic_corrections_typed(
typed_abs_pos,
typed_abs_vel,
&controls.0,
|source_entity| {
sources.get(source_entity).ok().map(|(s, _, p, v, _, _)| {
astrodyn::ResolvedRelativisticSource {
mu: s.mu,
position: p.0.raw_si(),
velocity: source_vel(v),
}
})
},
);
accel += rel.raw_si();
accel
};
for (
entity,
config,
mut state,
mut rot_state,
mass,
controls,
mut total_force,
integrator,
mut gj_state,
mut abm4_state,
mut flat_config,
struct_xform,
mut srp_force,
mut frame_derivs,
body_frame_entity,
) in &mut bodies
{
let integ_frame_entity = body_frame_entity
.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_frame_entity.0 => {
frame_origin.origin_in(root_frame_entity.0, integ_e)
}
_ => (DVec3::ZERO, DVec3::ZERO),
};
let integrator_type = integrator.map_or(astrodyn::IntegratorType::Rk4, |c| c.0);
if matches!(integrator_type, astrodyn::IntegratorType::GaussJackson(..)) {
assert!(
gj_state.is_some(),
"Entity {entity:?}: IntegratorTypeC is GaussJackson but \
GaussJacksonStateC component is missing. Create the state \
from the same config used in IntegratorTypeC, e.g.: \
GaussJacksonStateC(GaussJacksonState::new(config))"
);
}
if matches!(integrator_type, astrodyn::IntegratorType::Abm4) {
assert!(
abm4_state.is_some(),
"Entity {entity:?}: IntegratorTypeC is Abm4 but \
Abm4StateC component is missing. Add \
Abm4StateC(Abm4State::new()) to the entity."
);
}
let stage_inputs_and_order = flat_config
.as_ref()
.and_then(|fc| fc.stage_inputs.map(|si| (si, fc.integration_order)));
if let Some((srp_inputs, thermal_order)) = stage_inputs_and_order {
assert!(
matches!(integrator_type, astrodyn::IntegratorType::Rk4),
"Entity {entity:?}: derivative-class ThermalIntegrationOrder \
requires RK4 integrator; use Scheduled or switch integrator.",
);
let t_struct_body = struct_xform.map_or(glam::DMat3::IDENTITY, |s| *s.0.matrix_ref());
let non_grav_non_srp_force = total_force.force.raw_si();
let constant_torque = total_force.torque.raw_si();
let mut final_srp_inertial_force = DVec3::ZERO;
let mut final_srp_torque = DVec3::ZERO;
let mut k1_temp_dots: Option<Vec<f64>> = None;
let mass_copy_untyped = mass.map(|m| mass_typed_to_raw(&m.0));
let mut state_untyped = trans_typed_to_raw(&state.0);
let mut rot_state_untyped = rot_state.as_ref().map(|r| rot_typed_to_raw(&r.0));
let thermal = flat_config
.as_mut()
.expect("stage_inputs_and_order => flat_config present");
astrodyn::integrate_body_coupled(
config,
&mut state_untyped,
rot_state_untyped.as_mut(),
mass_copy_untyped.as_ref(),
|stage_trans, stage_rot, stage_thermal, time_frac| {
let gravity_accel = eval_gravity(
entity,
controls,
stage_trans.position,
stage_trans.velocity,
integ_origin_pos,
integ_origin_vel,
time_frac,
);
let t_inertial_body = stage_rot.map_or(glam::DMat3::IDENTITY, |r| {
r.quaternion.left_quat_to_transformation()
});
let t_inertial_struct =
astrodyn::compute_t_inertial_struct(&t_struct_body, &t_inertial_body);
use astrodyn::{Position, RootInertial};
let stage_dt = time_frac * integ_dt;
let stage_origin = if integ_origin_vel != DVec3::ZERO {
integ_origin_pos + integ_origin_vel * stage_dt
} else {
integ_origin_pos
};
let stage_pos_root: Position<RootInertial> =
Position::<RootInertial>::from_raw_si(stage_trans.position + stage_origin);
let sun_to_vehicle: Position<RootInertial> =
stage_pos_root - srp_inputs.sun_position;
let sun_to_vehicle = sun_to_vehicle.raw_si();
let distance = sun_to_vehicle.length().max(1.0);
let stage_flux_inertial_hat = sun_to_vehicle / distance;
let stage_flux_mag = astrodyn::solar_flux_at_distance(distance);
let flux_struct_hat = t_inertial_struct * stage_flux_inertial_hat;
let srp_result = astrodyn::compute_flat_plate_srp_thermal(
&stage_thermal.plates,
&stage_thermal.t_pow4_cached,
flux_struct_hat,
stage_flux_mag,
srp_inputs.center_grav.raw_si(),
srp_inputs.illum_factor,
);
let srp_force_inertial = t_inertial_struct.transpose() * srp_result.force;
final_srp_inertial_force = srp_force_inertial;
final_srp_torque = srp_result.torque;
let temp_dots = match thermal_order {
astrodyn::ThermalIntegrationOrder::DerivativeRk4 => {
srp_result.temp_dots
}
astrodyn::ThermalIntegrationOrder::DerivativeFirstOrder => {
if time_frac == 0.0 {
k1_temp_dots = Some(srp_result.temp_dots.clone());
srp_result.temp_dots
} else {
k1_temp_dots
.as_ref()
.expect("stage 1 runs before stages 2-4")
.clone()
}
}
astrodyn::ThermalIntegrationOrder::Scheduled => {
unreachable!("Scheduled bodies do not enter the coupled path")
}
};
let srp_torque_body = t_struct_body * srp_result.torque;
astrodyn::CoupledStageEval {
gravity_accel,
non_grav_force: non_grav_non_srp_force + srp_force_inertial,
torque: constant_torque + srp_torque_body,
temp_dots,
}
},
&mut thermal.0,
dt,
sim_time.0.time_scale_factor,
);
state.0 = trans_raw_to_planet::<P>(&state_untyped);
if let (Some(rs), Some(ru)) = (rot_state.as_mut(), rot_state_untyped) {
rs.0 = rot_raw_to_self_ref(&ru);
}
if let Some(ref mut srp_force) = srp_force {
srp_force.force = final_srp_inertial_force;
srp_force.torque = final_srp_torque;
}
total_force.force += Force::<RootInertial>::from_raw_si(final_srp_inertial_force);
let final_srp_torque_body = t_struct_body * final_srp_torque;
total_force.torque += Torque::<BodyFrame<SelfRef>>::from_raw_si(final_srp_torque_body);
if let (Some(ref mut fd), Some(mass_p)) = (frame_derivs.as_mut(), mass_copy_untyped) {
fd.trans_accel += Acceleration::<RootInertial>::from_raw_si(
final_srp_inertial_force * mass_p.inverse_mass,
);
fd.rot_accel += AngularAcceleration::<BodyFrame<SelfRef>>::from_raw_si(
mass_p.inverse_inertia * final_srp_torque_body,
);
}
continue;
}
let mut state_untyped = trans_typed_to_raw(&state.0);
let mut rot_state_untyped = rot_state.as_ref().map(|r| rot_typed_to_raw(&r.0));
let mass_untyped = mass.map(|m| mass_typed_to_raw(&m.0));
astrodyn::integrate_body(
config,
&mut state_untyped,
rot_state_untyped.as_mut(),
mass_untyped.as_ref(),
|pos, vel, time_frac| {
eval_gravity(
entity,
controls,
pos,
vel,
integ_origin_pos,
integ_origin_vel,
time_frac,
)
},
total_force.force.raw_si(),
total_force.torque.raw_si(),
dt,
sim_time.0.time_scale_factor,
integrator_type,
gj_state.as_mut().map(|g| g.0.inner_mut()),
abm4_state.as_mut().map(|a| a.0.inner_mut()),
);
state.0 = trans_raw_to_planet::<P>(&state_untyped);
if let (Some(rs), Some(ru)) = (rot_state.as_mut(), rot_state_untyped) {
rs.0 = rot_raw_to_self_ref(&ru);
}
}
}
struct CrossIntegFrameAttachWork {
parent_integ_origin_pos: glam::DVec3,
parent_integ_origin_vel: glam::DVec3,
new_parent_frame_entity: Entity,
reparent_entries: Vec<CrossIntegReparentEntry>,
}
struct CrossIntegReparentEntry {
body_entity: Entity,
body_frame_entity: Entity,
old_integ_origin_pos: glam::DVec3,
old_integ_origin_vel: glam::DVec3,
}
#[allow(clippy::type_complexity)]
fn apply_cross_integ_frame_attach<P: Planet>(
work: &CrossIntegFrameAttachWork,
commands: &mut Commands,
bodies: &mut Query<(
Entity,
&crate::MassBodyIdC,
&mut MassPropertiesC,
Option<&mut TranslationalStateC<P>>,
Option<&mut RotationalStateC>,
)>,
frame_states: &Query<(&FrameTransC, &FrameRotC, &FrameAngVelC)>,
parent_entity_skip: Entity,
) {
for entry in &work.reparent_entries {
let shift = astrodyn::CrossIntegFrameStateShift::between_integ_origins(
entry.old_integ_origin_pos,
entry.old_integ_origin_vel,
work.parent_integ_origin_pos,
work.parent_integ_origin_vel,
);
let (frame_trans, _, _) = frame_states
.get(entry.body_frame_entity)
.unwrap_or_else(|err| {
panic!(
"staging_system: cross-integ-frame attach: body-frame entity \
{fe:?} has no FrameTransC ({err:?}). Every body-frame entity \
must be alive with FrameTransC attached (spawned by \
register_body_frames_system).",
fe = entry.body_frame_entity,
)
});
let (new_pos, new_vel) = shift.apply(frame_trans.position, frame_trans.velocity);
commands
.entity(entry.body_frame_entity)
.insert(ChildOf(work.new_parent_frame_entity))
.insert(FrameTransC {
position: new_pos,
velocity: new_vel,
});
if entry.body_entity == parent_entity_skip {
continue;
}
if let Ok((_, _, _, Some(mut t), _)) = bodies.get_mut(entry.body_entity) {
let old = trans_typed_to_raw(&t.0);
let (new_pos, new_vel) = shift.apply(old.position, old.velocity);
t.0 = trans_raw_to_planet::<P>(&TranslationalState {
position: new_pos,
velocity: new_vel,
});
}
}
}
#[allow(clippy::type_complexity, clippy::too_many_arguments)]
pub fn staging_system<P: Planet>(
mut commands: Commands,
tree: Option<ResMut<crate::MassTreeR>>,
mut attach_events: bevy::ecs::message::MessageReader<
crate::AttachEvent<astrodyn::SelfRef, astrodyn::SelfRef>,
>,
mut detach_events: bevy::ecs::message::MessageReader<crate::DetachEvent>,
mut bodies: Query<(
Entity,
&crate::MassBodyIdC,
&mut MassPropertiesC,
Option<&mut TranslationalStateC<P>>,
Option<&mut RotationalStateC>,
)>,
body_frames: Query<&FrameEntityC>,
parents: Query<&ChildOf>,
detached_q: Query<Entity, With<crate::DetachedSubtreeStateC>>,
eligibility: Query<(
Has<DynamicsConfigC>,
Has<TranslationalStateC<P>>,
Has<RotationalStateC>,
)>,
frame_states: Query<(&FrameTransC, &FrameRotC, &FrameAngVelC)>,
source_frames: Query<&FrameEntityC, With<GravitySourceC>>,
mut integrators: Query<(
&crate::MassBodyIdC,
Option<&mut GaussJacksonStateC>,
Option<&mut Abm4StateC>,
)>,
frame_origin: FrameOrigin,
root_frame_entity: Option<Res<crate::RootFrameEntityR>>,
) {
let attach_pending = !attach_events.is_empty();
let detach_pending = !detach_events.is_empty();
let Some(mut tree) = tree else {
assert!(
!attach_pending,
"AttachEvent received but `MassTreeR` is not registered in the world. \
Mass-tree attach events require an arena to write into; without \
`MassTreeR` the event would silently drop and the body would propagate \
unattached. Fix: either (a) insert `MassTreeR(MassTree::new())` as a \
resource before sending the event, or (b) use \
`SimulationBuilder::register_in_mass_tree(idx, name)` + \
`populate_app::<P>()` which pre-allocates the arena + `MassBodyId` for \
each registered body. See `crates/astrodyn_bevy/src/scenario.rs` \
for the canonical flow."
);
assert!(
!detach_pending,
"DetachEvent received but `MassTreeR` is not registered in the world. \
Mass-tree detach events require an arena to mutate; without \
`MassTreeR` the event would silently drop and the targeted subtree \
would never be split off (no `DetachedSubtreeStateC` would be \
inserted, so the detached body never advances ballistically). \
Fix: either (a) insert `MassTreeR(MassTree::new())` as a resource \
before sending the event, or (b) use \
`SimulationBuilder::register_in_mass_tree(idx, name)` + \
`populate_app::<P>()` which pre-allocates the arena + `MassBodyId` \
for each registered body. See `crates/astrodyn_bevy/src/scenario.rs` \
for the canonical flow."
);
return;
};
let mut affected_ids: Vec<astrodyn::MassBodyId> = Vec::new();
struct AttachWork {
parent_entity: Entity,
subject_root_entity: Entity,
subject_root_id: astrodyn::MassBodyId,
is_chained_attach: bool,
parent_id: astrodyn::MassBodyId,
parent_position: glam::DVec3,
parent_velocity: glam::DVec3,
parent_quaternion: astrodyn::JeodQuat,
parent_ang_vel_body: glam::DVec3,
parent_mass: astrodyn::MassProperties,
orig_parent_cm_struct: glam::DVec3,
parent_t_inertial_struct: glam::DMat3,
child_position: glam::DVec3,
child_velocity: glam::DVec3,
child_quaternion: astrodyn::JeodQuat,
child_ang_vel_body: glam::DVec3,
child_mass: astrodyn::MassProperties,
parent_integ_origin_pos: glam::DVec3,
parent_integ_origin_vel: glam::DVec3,
child_was_detached: bool,
parent_was_detached: bool,
cross_integ: Option<CrossIntegFrameAttachWork>,
}
let mut attach_work: Vec<AttachWork> = Vec::new();
let mut detach_work: Vec<(Entity, astrodyn::DetachedSubtreeState)> = Vec::new();
let known_source_frames: std::collections::HashSet<Entity> =
source_frames.iter().map(|fe| fe.0).collect();
let id_to_entity: std::collections::HashMap<astrodyn::MassBodyId, Entity> = bodies
.iter()
.map(|(e, body_id, _, _, _)| (body_id.0, e))
.collect();
for evt in attach_events.read() {
let (_, child_body_id, _child_mass_c, _child_trans, _child_rot) =
bodies.get(evt.child).unwrap_or_else(|_| {
panic!(
"AttachEvent.child = {:?} is not a mass body — entity is missing MassBodyIdC \
and/or MassPropertiesC. Spawn the body via the mass-tree API before attaching.",
evt.child
)
});
let child_id = child_body_id.0;
let subject_root_id = tree.root_of(child_id);
let is_chained_attach = subject_root_id != child_id;
let subject_root_entity = if is_chained_attach {
*id_to_entity.get(&subject_root_id).unwrap_or_else(|| {
panic!(
"AttachEvent.child = {:?} (mass id {:?}): subject root \
{subject_root_id:?} for the rerooted subtree has no entity in the bodies \
query — every mass-tree node must be spawned with `MassBodyIdC` before \
any AttachEvent references it. The chained-attach reroot path needs to \
read the subject root's composite-body state for the momentum-conservation \
combine and reparent its `MassChildOf` Relations under the new parent; \
missing the entity here would silently break either step.",
evt.child, child_id,
)
})
} else {
evt.child
};
let (_, subject_root_body_id, subject_root_mass_c, subject_root_trans, subject_root_rot) =
bodies.get(subject_root_entity).unwrap_or_else(|_| {
panic!(
"AttachEvent.child = {:?}: subject root entity {subject_root_entity:?} \
is not a mass body — entity is missing MassBodyIdC and/or \
MassPropertiesC. The mass tree's `root_of(child_id)` returned a node \
whose backing entity does not satisfy the bodies query.",
evt.child,
)
});
assert_eq!(
subject_root_body_id.0, subject_root_id,
"subject root entity {:?} carries MassBodyIdC({:?}) but the mass tree's \
root_of(child_id={:?}) returned {:?}. The id_to_entity map and the arena \
tree are out of sync — every entity carrying MassBodyIdC must be reachable \
via id_to_entity at the same id.",
subject_root_entity, subject_root_body_id.0, child_id, subject_root_id,
);
let child_mass: astrodyn::MassProperties = mass_typed_to_raw(&subject_root_mass_c.0);
let (child_position_integ, child_velocity_integ) = subject_root_trans
.as_ref()
.map(|t| (t.0.position.raw_si(), t.0.velocity.raw_si()))
.unwrap_or((glam::DVec3::ZERO, glam::DVec3::ZERO));
let (child_quaternion, child_ang_vel_body) = subject_root_rot
.as_ref()
.map(|r| {
let untyped = rot_typed_to_raw(&r.0);
(untyped.quaternion, untyped.ang_vel_body)
})
.unwrap_or((astrodyn::JeodQuat::identity(), glam::DVec3::ZERO));
let (_, parent_body_id, parent_mass_c, parent_trans, parent_rot) =
bodies.get(evt.parent).unwrap_or_else(|_| {
panic!(
"AttachEvent.parent = {:?} is not a mass body — entity is missing MassBodyIdC \
and/or MassPropertiesC. Spawn the parent via the mass-tree API before attaching.",
evt.parent
)
});
let parent_id = parent_body_id.0;
let parent_mass: astrodyn::MassProperties = mass_typed_to_raw(&parent_mass_c.0);
let (parent_position_integ, parent_velocity_integ) = parent_trans
.as_ref()
.map(|t| (t.0.position.raw_si(), t.0.velocity.raw_si()))
.unwrap_or((glam::DVec3::ZERO, glam::DVec3::ZERO));
let (parent_quaternion, parent_ang_vel_body) = parent_rot
.as_ref()
.map(|r| {
let untyped = rot_typed_to_raw(&r.0);
(untyped.quaternion, untyped.ang_vel_body)
})
.unwrap_or((astrodyn::JeodQuat::identity(), glam::DVec3::ZERO));
let resolve_original_parent = |body: Entity, role: &str| -> Option<Entity> {
match body_frames.get(body) {
Ok(frame_handle) => {
let child_of = parents.get(frame_handle.0).unwrap_or_else(|err| {
panic!(
"AttachEvent.{role} = {body:?}: body-frame entity \
{fe:?} has no ChildOf parent. The body-frame entity \
must be parented under its integration-frame entity \
(root frame entity, or a registered source's frame \
entity). `register_body_frames_system` inserts that \
ChildOf when it runs in the AstrodynPlugin schedules \
(Startup, PreUpdate, FixedUpdate); a missing parent \
here means the frame tree is corrupt. Underlying \
query error: {err:?}",
fe = frame_handle.0,
)
});
Some(child_of.parent())
}
Err(_) => {
let (has_dyn_cfg, has_trans, _has_rot) =
eligibility.get(body).unwrap_or((false, false, false));
if has_dyn_cfg && has_trans {
panic!(
"AttachEvent.{role} = {body:?}: entity carries \
DynamicsConfigC + TranslationalStateC (the \
eligibility filter for register_body_frames_system) \
but does not yet carry FrameEntityC. This is a \
registration race — the body was spawned mid-tick \
after register_body_frames_system already ran in \
PreUpdate / FixedUpdate (before \
AstrodynSet::EphemerisUpdate), so its frame-tree node \
has not been spawned yet by the time staging_system \
runs. Spawn the body before the first FixedUpdate \
step (e.g. in Startup or PreUpdate ahead of \
register_body_frames_system), or defer the \
AttachEvent until the next tick so the registration \
pass has had a chance to run."
);
}
None
}
}
};
let parent_orig = resolve_original_parent(evt.parent, "parent");
let child_orig = resolve_original_parent(evt.child, "child");
if parent_orig.is_none() && child_orig.is_some() {
panic!(
"AttachEvent: dynamic child {child:?} (carries FrameEntityC) \
cannot be attached to mass-only parent {parent:?} (no \
FrameEntityC). JEOD's dyn_body_attach.cc::attach_validate_parent \
rejects this with \"Dynamic attachments can only be made to \
valid DynBodies\" (Modified_data parents need both \
DynamicsConfigC and TranslationalStateC). The combine-back-write \
in this function only writes the merged composite into the \
parent's TranslationalStateC / RotationalStateC, which a \
mass-only parent does not carry — the merged state would be \
silently lost. Either promote the parent to a dynamic body \
(add DynamicsConfigC + TranslationalStateC + RotationalStateC) \
before the attach, or attach the parent to its own dynamic \
ancestor first so the composite has a free-flying root.",
child = evt.child,
parent = evt.parent,
);
}
let parent_has_state =
parent_orig.map(|_| eligibility.get(evt.parent).unwrap_or((false, false, false)));
let child_has_state =
child_orig.map(|_| eligibility.get(evt.child).unwrap_or((false, false, false)));
let rotational_asymmetry = match (parent_has_state, child_has_state) {
(Some((_, _, parent_rot)), Some((_, _, child_rot))) => parent_rot != child_rot,
_ => false,
};
for (entity, orig, role) in [
(evt.parent, parent_orig, "parent"),
(evt.child, child_orig, "child"),
] {
if orig.is_none() {
continue;
}
let (has_dyn_cfg, has_trans, has_rot) =
eligibility.get(entity).unwrap_or((false, false, false));
let mut missing: Vec<&'static str> = Vec::new();
if !has_dyn_cfg {
missing.push("DynamicsConfigC");
}
if !has_trans {
missing.push("TranslationalStateC");
}
if rotational_asymmetry && !has_rot {
missing.push("RotationalStateC");
}
if !missing.is_empty() {
let missing = missing.join(", ");
panic!(
"AttachEvent.{role} = {entity:?}: dynamic body carries \
FrameEntityC (registration ran) but is missing required \
state component(s): {missing}. The stage_attach_combine \
kernel reads TranslationalStateC / RotationalStateC for \
pre-attach pose + velocity, and the merged composite is \
written back only into those same components — without \
them the kernel silently substitutes zero / identity for \
the missing input and the merged result is silently \
dropped. JEOD's dyn_body_attach.cc::attach_validate_child \
rejects this same case with \"Child body has an \
incomplete state\" / \"Root body has an incomplete state\". \
Re-insert the missing component(s) on the entity before \
firing the AttachEvent, or remove the body from the \
mass-tree before stripping its state. (Note: \
RotationalStateC is required only when the attach \
partner carries it — pure 3-DOF attach between two \
bodies that both lack RotationalStateC is allowed.)"
);
}
}
let mut cross_integ: Option<CrossIntegFrameAttachWork> = None;
if let (Some(parent_orig), Some(child_orig)) = (parent_orig, child_orig) {
let root_e_opt = root_frame_entity.as_ref().map(|r| r.0);
for (entity, integ_frame, role) in [
(evt.parent, parent_orig, "parent"),
(evt.child, child_orig, "child"),
] {
let is_root = root_e_opt == Some(integ_frame);
let is_legal = is_root || known_source_frames.contains(&integ_frame);
assert!(
is_legal,
"AttachEvent.{role} = {entity:?}: live integration-frame \
entity {integ_frame:?} (the ChildOf parent of the body's \
frame entity) is neither the root frame entity \
({root_e_opt:?}) nor a registered gravity source's frame \
entity. The body-frame entity must be parented under one \
of those — register the source via PlanetBundle (which \
inserts GravitySourceC and FrameEntityC) before spawning \
the body, or attach the body under the root frame entity."
);
}
if let Some(root_e) = root_e_opt {
let fold_root_equivalent = |parent: Entity| -> Entity {
if crate::validation::is_root_equivalent_entity(
parent,
root_e,
&parents,
&frame_states,
) {
root_e
} else {
parent
}
};
let parent_frame = fold_root_equivalent(parent_orig);
let child_frame = fold_root_equivalent(child_orig);
if parent_frame != child_frame {
let resolve_integ_origin = |frame: Entity| -> (glam::DVec3, glam::DVec3) {
if frame == root_e {
(glam::DVec3::ZERO, glam::DVec3::ZERO)
} else {
let (p, v) = frame_origin.origin_in_root(root_e, frame);
(p.raw_si(), v.raw_si())
}
};
let (parent_integ_origin_pos, parent_integ_origin_vel) =
resolve_integ_origin(parent_frame);
let mut reparent_entries: Vec<CrossIntegReparentEntry> = Vec::new();
let mut subtree: Vec<astrodyn::MassBodyId> = vec![child_id];
let mut idx = 0;
while idx < subtree.len() {
let id = subtree[idx];
for child in tree.children(id) {
subtree.push(*child);
}
idx += 1;
}
for id in subtree {
if let Some(&entity) = id_to_entity.get(&id) {
let fe = body_frames.get(entity).ok();
let body_frame_entity = match fe {
Some(fe) => fe.0,
None => {
let (has_dyn_cfg, has_trans, _has_rot) =
eligibility.get(entity).unwrap_or((false, false, false));
assert!(
!(has_dyn_cfg && has_trans),
"staging_system: cross-integ-frame attach: \
descendant body {entity:?} carries DynamicsConfigC \
and TranslationalStateC (dynamic body) but has no \
FrameEntityC — registration race vs \
register_body_frames_system. The cross-integ-frame \
reparent would otherwise leave this descendant's \
stored TranslationalStateC interpreted under the \
pre-attach integ frame while every other body in \
the subtree gets shifted into the new frame, \
silently mixing coordinates across distinct \
integration frames. Spawn the body with a \
registered IntegSourceC (or under the root frame) \
and ensure register_body_frames_system has run \
before firing the AttachEvent."
);
continue;
}
};
let descendant_integ_frame = parents
.get(body_frame_entity)
.unwrap_or_else(|err| {
panic!(
"staging_system: cross-integ-frame attach: \
descendant body {entity:?} body-frame entity \
{body_frame_entity:?} has no ChildOf parent \
({err:?}). Every body-frame entity must be \
parented under its integration frame entity \
(set by register_body_frames_system)."
)
})
.parent();
let descendant_integ_frame_folded =
if crate::validation::is_root_equivalent_entity(
descendant_integ_frame,
root_e,
&parents,
&frame_states,
) {
root_e
} else {
descendant_integ_frame
};
let (old_pos, old_vel) = if descendant_integ_frame_folded == root_e {
(glam::DVec3::ZERO, glam::DVec3::ZERO)
} else {
let (p, v) =
frame_origin.origin_in_root(root_e, descendant_integ_frame);
(p.raw_si(), v.raw_si())
};
reparent_entries.push(CrossIntegReparentEntry {
body_entity: entity,
body_frame_entity,
old_integ_origin_pos: old_pos,
old_integ_origin_vel: old_vel,
});
}
}
let new_parent_frame_entity = parent_orig;
cross_integ = Some(CrossIntegFrameAttachWork {
parent_integ_origin_pos,
parent_integ_origin_vel,
new_parent_frame_entity,
reparent_entries,
});
}
}
}
let parent_body_frame_capture = body_frames.get(evt.parent).ok();
let (parent_integ_origin_pos_typed, parent_integ_origin_vel_typed) =
body_integ_origin_in_root_lazy(
parent_body_frame_capture,
&parents,
root_frame_entity.as_deref().map(|r| r.0),
&frame_origin,
);
let parent_integ_origin_pos = parent_integ_origin_pos_typed.raw_si();
let parent_integ_origin_vel = parent_integ_origin_vel_typed.raw_si();
let child_body_frame_capture = body_frames.get(subject_root_entity).ok();
let (child_integ_origin_pos_typed, child_integ_origin_vel_typed) =
body_integ_origin_in_root_lazy(
child_body_frame_capture,
&parents,
root_frame_entity.as_deref().map(|r| r.0),
&frame_origin,
);
let child_integ_origin_pos = child_integ_origin_pos_typed.raw_si();
let child_integ_origin_vel = child_integ_origin_vel_typed.raw_si();
let parent_position = parent_position_integ + parent_integ_origin_pos;
let parent_velocity = parent_velocity_integ + parent_integ_origin_vel;
let child_position = child_position_integ + child_integ_origin_pos;
let child_velocity = child_velocity_integ + child_integ_origin_vel;
let parent_t_struct_to_body = parent_mass.t_parent_this;
let parent_t_inertial_to_body = parent_quaternion.left_quat_to_transformation();
let parent_t_inertial_struct = astrodyn::compute_t_inertial_struct(
&parent_t_struct_to_body,
&parent_t_inertial_to_body,
);
affected_ids.push(subject_root_id);
affected_ids.extend(tree.ancestors_inclusive(parent_id));
if is_chained_attach {
assert!(
cross_integ.is_none(),
"AttachEvent: chained-attach (re-rooting) combined with cross-integration-frame \
attach is not yet supported. The subject root {subject_root_entity:?} (mass id \
{subject_root_id:?}) is being reparented under {:?} but the parent and child \
also live in different integration frames. Either align the two bodies' \
integration frames before the chained attach (e.g. via a frame switch), or \
detach the subject from its current parent before the cross-integ-frame attach \
so the simple root-subject path runs.",
evt.parent,
);
affected_ids.extend(tree.subtree_ids(subject_root_id));
}
let child_was_detached = detached_q.contains(subject_root_entity);
let parent_was_detached = detached_q.contains(evt.parent);
attach_work.push(AttachWork {
parent_entity: evt.parent,
subject_root_entity,
subject_root_id,
is_chained_attach,
parent_id,
parent_position,
parent_velocity,
parent_quaternion,
parent_ang_vel_body,
parent_mass,
orig_parent_cm_struct: parent_mass.position,
parent_t_inertial_struct,
child_position,
child_velocity,
child_quaternion,
child_ang_vel_body,
child_mass,
parent_integ_origin_pos,
parent_integ_origin_vel,
child_was_detached,
parent_was_detached,
cross_integ,
});
let _attached_root = tree.attach_with_reroot(
child_id,
parent_id,
evt.offset.raw_si(),
evt.t_parent_child.matrix(),
);
}
struct ParentShift {
tree_root_entity: Entity,
parent_pre_position: glam::DVec3,
parent_pre_velocity: glam::DVec3,
parent_pre_quat: astrodyn::JeodQuat,
parent_pre_ang_vel_body: glam::DVec3,
parent_pre_composite_props: astrodyn::MassProperties,
parent_was_detached: bool,
}
let mut parent_shifts: Vec<(astrodyn::MassBodyId, ParentShift)> = Vec::new();
for evt in detach_events.read() {
let (_, child_body_id, _, _, _) = bodies.get(evt.child).unwrap_or_else(|_| {
panic!(
"DetachEvent.child = {:?} is not a mass body — entity is missing MassBodyIdC \
and/or MassPropertiesC.",
evt.child
)
});
let child_id = child_body_id.0;
let mut tree_root_id = child_id;
while let Some(p) = tree.parent(tree_root_id) {
tree_root_id = p;
}
if tree_root_id == child_id {
panic!(
"DetachEvent.child = {:?} (mass id {:?}) has no parent in the mass tree — \
detaching a tree root is a no-op in JEOD and indicates a stale event \
(e.g. firing DetachEvent twice without a re-AttachEvent in between).",
evt.child, child_id,
);
}
let tree_root_entity = *id_to_entity.get(&tree_root_id).unwrap_or_else(|| {
panic!(
"DetachEvent.child = {:?}: tree root {:?} has no entity in the bodies query — \
every mass-tree node must be spawned with `MassBodyIdC` before any \
attach/detach event references it.",
evt.child, tree_root_id,
)
});
let (
parent_pre_position,
parent_pre_velocity,
parent_pre_quat,
parent_pre_ang_vel_body,
parent_pre_composite_props,
) = {
let (_, _, _, parent_trans, parent_rot) = bodies
.get(tree_root_entity)
.expect("id_to_entity points at a valid mass body");
let position = parent_trans
.as_ref()
.map(|t| t.0.position.raw_si())
.unwrap_or(glam::DVec3::ZERO);
let velocity = parent_trans
.as_ref()
.map(|t| t.0.velocity.raw_si())
.unwrap_or(glam::DVec3::ZERO);
let (q, w) = parent_rot
.as_ref()
.map(|r| {
let u = rot_typed_to_raw(&r.0);
(u.quaternion, u.ang_vel_body)
})
.unwrap_or((astrodyn::JeodQuat::identity(), glam::DVec3::ZERO));
let composite = tree.get(tree_root_id).composite_properties;
(position, velocity, q, w, composite)
};
let mut chain: Vec<astrodyn::MassBodyId> = Vec::new();
let mut walker = child_id;
while walker != tree_root_id {
chain.push(walker);
walker = tree
.parent(walker)
.expect("chain walk hit a parentless intermediate before reaching tree root");
}
chain.reverse();
let parent_body_frame = body_frames.get(tree_root_entity).ok();
let (parent_integ_origin_pos, parent_integ_origin_vel) = body_integ_origin_in_root_lazy(
parent_body_frame,
&parents,
root_frame_entity.as_deref().map(|r| r.0),
&frame_origin,
);
let parent_pre_position_inertial = parent_pre_position + parent_integ_origin_pos.raw_si();
let parent_pre_velocity_inertial = parent_pre_velocity + parent_integ_origin_vel.raw_si();
let parent_composite_state = astrodyn::RefFrameState {
trans: astrodyn::RefFrameTrans {
position: parent_pre_position_inertial,
velocity: parent_pre_velocity_inertial,
},
rot: astrodyn::RefFrameRot {
q_parent_this: parent_pre_quat,
t_parent_this: parent_pre_quat.left_quat_to_transformation(),
ang_vel_this: parent_pre_ang_vel_body,
},
};
let mut current_state = parent_composite_state;
let mut current_node_id = tree_root_id;
for next_id in &chain {
let next_node = tree.get(*next_id);
let current_node = tree.get(current_node_id);
let t_current_struct_to_body = current_node.composite_properties.t_parent_this;
let t_next_struct_to_body = next_node.composite_properties.t_parent_this;
let offset_struct =
next_node.composite_wrt_pstr.position - current_node.composite_properties.position;
let offset_in_current_body = t_current_struct_to_body * offset_struct;
let t_current_body_to_next_body = t_next_struct_to_body
* next_node.structure_point.t_parent_this
* t_current_struct_to_body.transpose();
let rel = astrodyn::MassPointState {
position: offset_in_current_body,
t_parent_this: t_current_body_to_next_body,
};
current_state = astrodyn::propagate_forward(¤t_state, &rel);
current_node_id = *next_id;
}
let subtree_state = current_state;
let captured = astrodyn::stage_detach_capture(
subtree_state.trans.position,
subtree_state.trans.velocity,
subtree_state.rot.q_parent_this,
subtree_state.rot.ang_vel_this,
);
detach_work.push((evt.child, captured));
let parent_was_detached_root = detached_q.contains(tree_root_entity);
parent_shifts.push((
tree_root_id,
ParentShift {
tree_root_entity,
parent_pre_position,
parent_pre_velocity,
parent_pre_quat,
parent_pre_ang_vel_body,
parent_pre_composite_props,
parent_was_detached: parent_was_detached_root,
},
));
affected_ids.push(child_id);
affected_ids.extend(tree.ancestors_inclusive(tree_root_id));
tree.detach(child_id);
}
if affected_ids.is_empty() && attach_work.is_empty() && detach_work.is_empty() {
return;
}
affected_ids.sort_unstable();
affected_ids.dedup();
for (_, body_id, mut mass, _, _) in &mut bodies {
if affected_ids.binary_search(&body_id.0).is_ok() {
*mass.bypass_change_detection() = MassPropertiesC::from(mass_raw_to_self_ref(
&tree.get(body_id.0).composite_properties,
));
}
}
for work in &attach_work {
let combined_mass = tree.get(work.parent_id).composite_properties;
let merged = astrodyn::stage_attach_combine(astrodyn::StageAttachInputs {
parent_position: work.parent_position,
parent_velocity: work.parent_velocity,
parent_quaternion: work.parent_quaternion,
parent_ang_vel_body: work.parent_ang_vel_body,
parent_mass: work.parent_mass,
orig_parent_cm_struct: work.orig_parent_cm_struct,
parent_t_inertial_struct: work.parent_t_inertial_struct,
child_position: work.child_position,
child_velocity: work.child_velocity,
child_quaternion: work.child_quaternion,
child_ang_vel_body: work.child_ang_vel_body,
child_mass: work.child_mass,
combined_mass,
});
let merged_position = merged.position - work.parent_integ_origin_pos;
let merged_velocity = merged.velocity - work.parent_integ_origin_vel;
if let Ok((_, _, _, mut trans, mut rot)) = bodies.get_mut(work.parent_entity) {
if let Some(ref mut t) = trans {
t.0 = trans_raw_to_planet::<P>(&TranslationalState {
position: merged_position,
velocity: merged_velocity,
});
}
if let Some(ref mut r) = rot {
r.0 = rot_raw_to_self_ref(&RotationalState {
quaternion: merged.quaternion,
ang_vel_body: merged.ang_vel_body,
});
}
}
if let Some(ci) = work.cross_integ.as_ref() {
apply_cross_integ_frame_attach::<P>(
ci,
&mut commands,
&mut bodies,
&frame_states,
work.parent_entity,
);
}
if work.child_was_detached {
commands
.entity(work.subject_root_entity)
.remove::<crate::DetachedSubtreeStateC>();
}
if work.is_chained_attach {
for id in tree.subtree_ids(work.subject_root_id) {
if let Some(&entity) = id_to_entity.get(&id) {
let (_has_dyn_cfg, _has_trans, has_rot) =
eligibility.get(entity).unwrap_or((false, false, false));
let body_frame = body_frames.get(entity).ok();
if body_frame.is_some() {
assert!(
has_rot,
"AttachEvent: chained-attach reroot: dynamic body {entity:?} \
(mass id {id:?}) is in the rerooted subtree under \
subject_root {subject:?} (mass id {subject_id:?}) but has no \
RotationalStateC. Kinematic propagation derives both \
TranslationalStateC and RotationalStateC from the integrating \
root, so any attached (non-root) body must be 6-DOF — a 3-DOF \
body's state would go stale post-attach. Make this body 6-DOF \
by inserting RotationalStateC before the chained attach, or \
restructure the topology so this body never becomes a non-root \
member of the merged tree.",
subject = work.subject_root_entity,
subject_id = work.subject_root_id,
);
}
}
}
for id in tree.subtree_ids(work.subject_root_id) {
let entity = match id_to_entity.get(&id) {
Some(&e) => e,
None => continue,
};
let arena_parent_id = tree.parent(id).expect(
"subtree_ids(subject_root) is the post-reroot subtree; the \
subject root now has `parent_entity` and every descendant has \
its in-subtree arena parent — both branches must yield Some(_)",
);
let arena_parent_entity = match id_to_entity.get(&arena_parent_id) {
Some(&e) => e,
None => continue,
};
let sp = tree.get(id).structure_point;
commands
.entity(entity)
.insert(crate::MassChildOf::with_rotation(
arena_parent_entity,
sp.position,
sp.t_parent_this,
));
}
}
if work.parent_was_detached {
use astrodyn::Vec3Ext as _;
let updated = astrodyn::DetachedSubtreeState {
composite_position: merged.position.m_at::<astrodyn::RootInertial>(),
composite_velocity: merged.velocity.m_per_s_at::<astrodyn::RootInertial>(),
composite_attitude: astrodyn::DetachedSubtreeState::attitude_from_raw_jeod_quat(
merged.quaternion,
),
composite_ang_vel_body: merged.ang_vel_body,
};
commands
.entity(work.parent_entity)
.insert(crate::DetachedSubtreeStateC(updated));
}
}
for (entity, captured) in detach_work {
commands
.entity(entity)
.insert(crate::DetachedSubtreeStateC(captured));
}
for (tree_root_id, shift) in parent_shifts {
let parent_post_composite_props = tree.get(tree_root_id).composite_properties;
let cm_delta_struct =
parent_post_composite_props.position - shift.parent_pre_composite_props.position;
let t_struct_to_body = shift.parent_pre_composite_props.t_parent_this;
let cm_delta_body = t_struct_to_body * cm_delta_struct;
let t_inertial_to_body = shift.parent_pre_quat.left_quat_to_transformation();
let cm_delta_inertial = t_inertial_to_body.transpose() * cm_delta_body;
let omega_body = shift.parent_pre_ang_vel_body;
let dvel_inertial = t_inertial_to_body.transpose() * omega_body.cross(cm_delta_body);
let new_position = shift.parent_pre_position + cm_delta_inertial;
let new_velocity = shift.parent_pre_velocity + dvel_inertial;
if let Ok((_, _, _, Some(mut t), _)) = bodies.get_mut(shift.tree_root_entity) {
t.0 = trans_raw_to_planet::<P>(&TranslationalState {
position: new_position,
velocity: new_velocity,
});
}
if shift.parent_was_detached {
let parent_body_frame_shift = body_frames.get(shift.tree_root_entity).ok();
let (parent_integ_origin_pos_shift, parent_integ_origin_vel_shift) =
body_integ_origin_in_root_lazy(
parent_body_frame_shift,
&parents,
root_frame_entity.as_deref().map(|r| r.0),
&frame_origin,
);
use astrodyn::Vec3Ext as _;
let updated = astrodyn::DetachedSubtreeState {
composite_position: (new_position + parent_integ_origin_pos_shift.raw_si())
.m_at::<astrodyn::RootInertial>(),
composite_velocity: (new_velocity + parent_integ_origin_vel_shift.raw_si())
.m_per_s_at::<astrodyn::RootInertial>(),
composite_attitude: astrodyn::DetachedSubtreeState::attitude_from_raw_jeod_quat(
shift.parent_pre_quat,
),
composite_ang_vel_body: shift.parent_pre_ang_vel_body,
};
commands
.entity(shift.tree_root_entity)
.insert(crate::DetachedSubtreeStateC(updated));
}
}
for (body_id, mut gj_opt, mut abm_opt) in &mut integrators {
if affected_ids.binary_search(&body_id.0).is_ok() {
if let Some(ref mut gj) = gj_opt {
gj.0.mark_topology_dirty();
}
if let Some(ref mut abm) = abm_opt {
abm.0.mark_topology_dirty();
}
}
}
for (body_id, mut gj_opt, mut abm_opt) in &mut integrators {
if affected_ids.binary_search(&body_id.0).is_ok() {
astrodyn::reset_integrators(
gj_opt.as_mut().map(|c| c.0.inner_mut()),
abm_opt.as_mut().map(|c| c.0.inner_mut()),
);
}
}
}
#[allow(clippy::too_many_arguments, clippy::type_complexity)]
pub fn step_detached_system<P: Planet>(
dt: Res<IntegrationDtR>,
sim_time: Res<SimulationTimeR>,
mut detached: Query<(
Entity,
&mut crate::DetachedSubtreeStateC,
Option<&mut TranslationalStateC<P>>,
Option<&mut RotationalStateC>,
)>,
body_frames: Query<&FrameEntityC>,
parents: Query<&ChildOf>,
frame_origin: FrameOrigin,
root_frame_entity: Option<Res<crate::RootFrameEntityR>>,
) {
let dt = dt.0;
if dt == 0.0 {
return;
}
let integ_dt = dt * sim_time.0.time_scale_factor;
for (entity, mut state, trans, rot) in &mut detached {
state.0.step_ballistic(integ_dt);
if let Some(mut t) = trans {
let body_frame = body_frames.get(entity).ok();
let (integ_origin_pos, integ_origin_vel) = body_integ_origin_in_root_lazy(
body_frame,
&parents,
root_frame_entity.as_deref().map(|r| r.0),
&frame_origin,
);
let position = state.0.composite_position.raw_si() - integ_origin_pos.raw_si();
let velocity = state.0.composite_velocity.raw_si() - integ_origin_vel.raw_si();
t.0 = trans_raw_to_planet::<P>(&TranslationalState { position, velocity });
}
if let Some(mut r) = rot {
r.0 = rot_raw_to_self_ref(&RotationalState {
quaternion: state.0.composite_attitude.to_jeod_quat(),
ang_vel_body: state.0.composite_ang_vel_body,
});
}
}
}