From 7975464e3b5db070cd381ea8fbc85b5758c61e15 Mon Sep 17 00:00:00 2001 From: Joona Aalto Date: Sat, 22 Jun 2024 00:08:27 +0300 Subject: [PATCH] Refactor and speed up transform propagation and hierarchies further (#380) # Objective #377 refactored collider hierarchy logic and extracted it into a `ColliderHierarchyPlugin`, along with significantly speeding up `ColliderTransform` propagation using a `ColliderAncestor` marker component to ignore trees that don't have colliders. However, a similar marker component approach can *also* be used to speed up the many copies of transform propagation that the engine has. By limiting the propagation runs to physics entities, we could drastically reduce the overhead for scenes with lots of non-physics entities, further addressing #356. ## Solution - Extract the ancestor marker logic from `ColliderHierarchyPlugin` into an `AncestorMarkerPlugin` that adds an `AncestorMarker` component for all ancestors of entities with the given component `C`. The logic was also refactored to be more robust and fix some edge cases and fully support hierarchy changes, along with adding more tests. - Use this plugin in `SyncPlugin` to mark rigid body ancestors. - Define custom transform propagation systems that *only* traverse trees that have physics entities (rigid bodies or colliders). See the comments in `sync.rs` above the propagation systems to see how this works. - To support custom collision backends without having to add generics to all the propagation systems, and to avoid needing to unnecessarily duplicate systems, the `ColliderBackendPlugin` now adds a general, automatically managed `ColliderMarker` component for all colliders, which allows filtering collider entities without knowing the collider type. This lets us get rid of the generics on `ColliderHierarchyPlugin`! - I also changed some scheduling and system sets in the `PrepareSet` to account for some changes and to be more logical. ## Results Test scene: 12 substeps, 1 root entity, 100,000 child entities. All entities have just a `SpatialBundle`, and one child entity also has a `Collider`. - Before [#377](https://github.com/Jondolf/bevy_xpbd/pull/377): ~22 FPS - After [#377](https://github.com/Jondolf/bevy_xpbd/pull/377): ~200 FPS - After this PR: ~490 FPS Of course, this scene is not very representative of an actual game scene, but it does show just how much of an impact the transform propagation was having. A *lot* of games (probably most of them) do have many more non-physics entities than physics entities, and the overhead added by the marker components and new checks should be very minimal in comparison. --- ## Migration Guide Some `PrepareSet` system sets have changed order. Before: 1. `PreInit` 2. `PropagateTransforms` 3. `InitRigidBodies` 4. `InitMassProperties` 5. `InitColliders` 6. `InitTransforms` 7. `Finalize` After: 1. `PreInit` 2. `InitRigidBodies` 3. `InitColliders` 4. `PropagateTransforms` 5. `InitMassProperties` 6. `InitTransforms` 7. `Finalize` Additionally, the `ColliderHierarchyPlugin` added in #377 no longer needs generics. The plugin is new anyway however, so this isn't really a breaking change. --- src/plugins/collision/collider/backend.rs | 66 ++- src/plugins/collision/collider/hierarchy.rs | 294 ++---------- src/plugins/collision/collider/mod.rs | 2 +- src/plugins/mod.rs | 2 +- src/plugins/prepare.rs | 32 +- src/plugins/sync/ancestor_marker.rs | 493 ++++++++++++++++++++ src/plugins/{sync.rs => sync/mod.rs} | 323 ++++++++++++- src/tests.rs | 13 +- 8 files changed, 946 insertions(+), 279 deletions(-) create mode 100644 src/plugins/sync/ancestor_marker.rs rename src/plugins/{sync.rs => sync/mod.rs} (52%) diff --git a/src/plugins/collision/collider/backend.rs b/src/plugins/collision/collider/backend.rs index e712d9f9..cb8e34c0 100644 --- a/src/plugins/collision/collider/backend.rs +++ b/src/plugins/collision/collider/backend.rs @@ -15,11 +15,13 @@ use bevy::{ ecs::{intern::Interned, system::SystemId}, prelude::*, }; +use sync::SyncSet; /// A plugin for handling generic collider backend logic. /// /// - Initializes colliders, including [`AsyncCollider`] and [`AsyncSceneCollider`]. /// - Updates [`ColliderAabb`]s. +/// - Updates collider scale based on `Transform` scale. /// - Updates collider mass properties, also updating rigid bodies accordingly. /// /// This plugin should typically be used together with the [`ColliderHierarchyPlugin`]. @@ -27,7 +29,8 @@ use bevy::{ /// ## Custom collision backends /// /// By default, [`PhysicsPlugins`] adds this plugin for the [`Collider`] component. -/// You can also create custom collider backends by implementing the [`AnyCollider`] trait for a type. +/// You can also create custom collider backends by implementing the [`AnyCollider`] +/// and [`ScalableCollider`] traits for a type. /// /// To use a custom collider backend, simply add the [`ColliderBackendPlugin`] with your collider type: /// @@ -54,12 +57,9 @@ use bevy::{ /// } /// ``` /// -/// Assuming you have implemented [`AnyCollider`] correctly, +/// Assuming you have implemented the required traits correctly, /// it should now work with the rest of the engine just like normal [`Collider`]s! /// -/// Remember to also add the [`ColliderHierarchyPlugin`] for your custom collider -/// type if you want transforms to work for them. -/// /// **Note**: [Spatial queries](spatial_query) are not supported for custom colliders yet. pub struct ColliderBackendPlugin { @@ -98,10 +98,16 @@ impl Plugin for ColliderBackendPlugin { // Register a component hook that updates mass properties of rigid bodies // when the colliders attached to them are removed. + // Also removes `ColliderMarker` components. app.world_mut() .register_component_hooks::() .on_remove(|mut world, entity, _| { - let entity_ref = world.entity(entity); + // Remove the `ColliderMarker` associated with the collider. + // TODO: If the same entity had multiple *different* types of colliders, this would + // get removed even if just one collider was removed. This is a very niche edge case though. + world.commands().entity(entity).remove::(); + + let entity_ref = world.entity_mut(entity); // Get the needed collider components. // TODO: Is there an efficient way to do this with QueryState? @@ -138,6 +144,14 @@ impl Plugin for ColliderBackendPlugin { ), ); + // Update colliders based on the scale from `ColliderTransform`. + app.add_systems( + self.schedule, + update_collider_scale:: + .after(SyncSet::Update) + .before(SyncSet::Last), + ); + let physics_schedule = app .get_schedule_mut(PhysicsSchedule) .expect("add PhysicsSchedule first"); @@ -161,6 +175,12 @@ impl Plugin for ColliderBackendPlugin { } } +/// A marker component for colliders. Inserted and removed automatically. +/// +/// This is useful for filtering collider entities regardless of the [collider backend](ColliderBackendPlugin). +#[derive(Reflect, Component, Clone, Copy, Debug)] +pub struct ColliderMarker; + /// Initializes missing components for [colliders](Collider). #[allow(clippy::type_complexity)] pub(crate) fn init_colliders( @@ -183,6 +203,7 @@ pub(crate) fn init_colliders( density, *mass_properties.unwrap_or(&collider.mass_properties(density.0)), CollidingEntities::default(), + ColliderMarker, )); } } @@ -394,6 +415,39 @@ fn update_aabb( } } +/// Updates the scale of colliders based on [`Transform`] scale. +#[allow(clippy::type_complexity)] +pub fn update_collider_scale( + mut colliders: ParamSet<( + // Root bodies + Query<(&Transform, &mut C), Without>, + // Child colliders + Query<(&ColliderTransform, &mut C), With>, + )>, +) { + // Update collider scale for root bodies + for (transform, mut collider) in &mut colliders.p0() { + #[cfg(feature = "2d")] + let scale = transform.scale.truncate().adjust_precision(); + #[cfg(feature = "3d")] + let scale = transform.scale.adjust_precision(); + if scale != collider.scale() { + // TODO: Support configurable subdivision count for shapes that + // can't be represented without approximations after scaling. + collider.set_scale(scale, 10); + } + } + + // Update collider scale for child colliders + for (collider_transform, mut collider) in &mut colliders.p1() { + if collider_transform.scale != collider.scale() { + // TODO: Support configurable subdivision count for shapes that + // can't be represented without approximations after scaling. + collider.set_scale(collider_transform.scale, 10); + } + } +} + /// A resource that stores the system ID for the system that reacts to collider removals. #[derive(Resource)] struct ColliderRemovalSystem(SystemId<(ColliderParent, ColliderMassProperties, ColliderTransform)>); diff --git a/src/plugins/collision/collider/hierarchy.rs b/src/plugins/collision/collider/hierarchy.rs index 96659757..69336655 100644 --- a/src/plugins/collision/collider/hierarchy.rs +++ b/src/plugins/collision/collider/hierarchy.rs @@ -2,101 +2,69 @@ //! //! See [`ColliderHierarchyPlugin`]. -use std::marker::PhantomData; - use crate::{ prelude::*, prepare::{match_any, PrepareSet}, - sync::SyncSet, }; use bevy::{ecs::intern::Interned, prelude::*}; +use sync::ancestor_marker::{AncestorMarker, AncestorMarkerPlugin}; /// A plugin for managing the collider hierarchy and related updates. /// /// - Updates [`ColliderParent`]. /// - Propagates [`ColliderTransform`]. -/// - Updates collider scale based on `Transform` scale. -/// -/// By default, [`PhysicsPlugins`] adds this plugin for the [`Collider`] component. -/// You can also use a custom collider backend by adding this plugin for any type -/// that implements the [`ScalableCollider`] trait. /// -/// This plugin should typically be used together with the [`ColliderBackendPlugin`]. -pub struct ColliderHierarchyPlugin { +/// This plugin requires Bevy's `HierarchyPlugin` and that colliders have the `ColliderMarker` component, +/// which is added automatically for colliders if the [`ColliderBackendPlugin`] is enabled. +pub struct ColliderHierarchyPlugin { schedule: Interned, - _phantom: PhantomData, } -impl ColliderHierarchyPlugin { +impl ColliderHierarchyPlugin { /// Creates a [`ColliderHierarchyPlugin`] with the schedule that is used for running the [`PhysicsSchedule`]. /// /// The default schedule is `PostUpdate`. pub fn new(schedule: impl ScheduleLabel) -> Self { Self { schedule: schedule.intern(), - _phantom: PhantomData, } } } -impl Default for ColliderHierarchyPlugin { +impl Default for ColliderHierarchyPlugin { fn default() -> Self { Self { schedule: PostUpdate.intern(), - _phantom: PhantomData, } } } -impl Plugin for ColliderHierarchyPlugin { +#[derive(SystemSet, Clone, Copy, Debug, PartialEq, Eq, Hash)] +struct MarkColliderAncestors; + +impl Plugin for ColliderHierarchyPlugin { fn build(&self, app: &mut App) { - // Mark ancestors of added colliders with the `ColliderAncestor` component. - // This is used to speed up `ColliderTransform` propagation. - app.add_systems( - self.schedule, - mark_collider_ancestors:: - .after(super::backend::init_colliders::) - .in_set(PrepareSet::InitColliders), + // Mark ancestors of colliders with `AncestorMarker`. + // This is used to speed up `ColliderTransform` propagation by skipping + // trees that have no colliders. + app.add_plugins( + AncestorMarkerPlugin::::new(self.schedule) + .add_markers_in_set(MarkColliderAncestors), ); - // Remove `ColliderAncestor` markers from removed colliders and their ancestors, - // until an ancestor that has other `ColliderAncestor` entities as children is encountered. - #[allow(clippy::type_complexity)] - app.observe( - |trigger: Trigger, - mut commands: Commands, - child_query: Query<&Children>, - parent_query: Query<&Parent>, - ancestor_query: Query< - (Entity, Has), - Or<(With, With)>, - >| { - let entity = trigger.entity(); - - // Iterate over ancestors, removing `ColliderAncestor` markers until - // an entity that has other `ColliderAncestor` children is encountered. - let mut previous_parent = entity; - for parent_entity in parent_query.iter_ancestors(entity) { - if let Ok(children) = child_query.get(parent_entity) { - // Keep the marker if `parent_entity` has a child that is a collider ancestor - // or a collider, but not the one that was removed. - let keep_marker = - ancestor_query - .iter_many(children) - .any(|(child, is_collider)| { - child != previous_parent || (is_collider && child != entity) - }); - - if keep_marker { - return; - } else { - commands.entity(parent_entity).remove::(); - } - } + app.configure_sets( + self.schedule, + MarkColliderAncestors + .after(PrepareSet::InitColliders) + .before(PrepareSet::PropagateTransforms), + ); - previous_parent = parent_entity; - } - }, + // Update collider parents. + app.add_systems( + self.schedule, + update_collider_parents + .after(PrepareSet::InitColliders) + .before(PrepareSet::Finalize), ); // Run transform propagation if new colliders without rigid bodies have been added. @@ -104,28 +72,17 @@ impl Plugin for ColliderHierarchyPlugin { app.add_systems( self.schedule, ( - bevy::transform::systems::sync_simple_transforms, - bevy::transform::systems::propagate_transforms, + sync::sync_simple_transforms_physics, + sync::propagate_transforms_physics, ) .chain() - .run_if(match_any::<(Added, Without)>) + .run_if(match_any::<(Added, Without)>) .in_set(PrepareSet::PropagateTransforms) - // Allowing ambiguities is required so that it's possible - // to have multiple collision backends at the same time. .ambiguous_with_all(), ); - // Update collider parents. - app.add_systems( - self.schedule, - update_collider_parents:: - // TODO: Decouple the ordering here. - .after(super::backend::init_colliders::) - .in_set(PrepareSet::InitColliders), - ); - // Propagate `ColliderTransform`s if there are new colliders. - // Only traverses trees with `ColliderAncestor`. + // Only traverses trees with `AncestorMarker`. app.add_systems( self.schedule, ( @@ -133,18 +90,9 @@ impl Plugin for ColliderHierarchyPlugin { update_child_collider_position, ) .chain() - .run_if(match_any::>) - // TODO: Decouple the ordering here. - .before(super::backend::update_collider_mass_properties::) - .in_set(PrepareSet::Finalize), - ); - - // Update colliders based on the scale from `ColliderTransform`. - app.add_systems( - self.schedule, - update_collider_scale:: - .after(SyncSet::Update) - .before(SyncSet::Last), + .run_if(match_any::>) + .after(PrepareSet::InitTransforms) + .before(PrepareSet::Finalize), ); let physics_schedule = app @@ -159,7 +107,7 @@ impl Plugin for ColliderHierarchyPlugin { .expect("add SubstepSchedule first"); // Propagate `ColliderTransform`s before narrow phase collision detection. - // Only traverses trees with `ColliderAncestor`. + // Only traverses trees with `AncestorMarker`. substep_schedule.add_systems( ( propagate_collider_transforms, @@ -167,53 +115,24 @@ impl Plugin for ColliderHierarchyPlugin { ) .chain() .after(SubstepSet::Integrate) - .before(SubstepSet::NarrowPhase) - .ambiguous_with_all(), + .before(SubstepSet::NarrowPhase), ); } } -/// A marker component that marks an entity as an ancestor of an entity with a collider. -/// -/// This is used to avoid unnecessary work when propagating transforms for colliders. -#[derive(Reflect, Clone, Copy, Component)] -pub struct ColliderAncestor; - -// TODO: This could also be an observer, but it'd need to have the appropriate filters -// and trigger for `Parent` changes, which doesn't seem to be possible yet. -// Unless we perhaps added an `OnColliderParentChanged` trigger. -/// Marks ancestors of added colliders with the `ColliderAncestor` component. -/// This is used to speed up `ColliderTransform` propagation. -#[allow(clippy::type_complexity)] -fn mark_collider_ancestors( - mut commands: Commands, - collider_query: Query, Changed)>, - parent_query: Query<&Parent>, - ancestor_query: Query<(), With>, -) { - for entity in &collider_query { - // Traverse up the tree, marking entities with `ColliderAncestor` - // until an entity that already has it is encountered. - for parent_entity in parent_query.iter_ancestors(entity) { - if ancestor_query.contains(parent_entity) { - return; - } else { - commands.entity(parent_entity).insert(ColliderAncestor); - } - } - } -} - #[derive(Reflect, Clone, Copy, Component, Debug, Default, Deref, DerefMut, PartialEq)] #[reflect(Component)] pub(crate) struct PreviousColliderTransform(pub ColliderTransform); #[allow(clippy::type_complexity)] -fn update_collider_parents( +fn update_collider_parents( mut commands: Commands, - mut bodies: Query<(Entity, Option<&mut ColliderParent>, Has), With>, + mut bodies: Query<(Entity, Option<&mut ColliderParent>, Has), With>, children: Query<&Children>, - mut child_colliders: Query, (With, Without)>, + mut child_colliders: Query< + Option<&mut ColliderParent>, + (With, Without), + >, ) { for (entity, collider_parent, has_collider) in &mut bodies { if has_collider { @@ -222,7 +141,7 @@ fn update_collider_parents( } else { commands.entity(entity).try_insert(( ColliderParent(entity), - // Todo: This probably causes a one frame delay. Compute real value? + // TODO: This probably causes a one frame delay. Compute real value? ColliderTransform::default(), PreviousColliderTransform::default(), )); @@ -235,7 +154,7 @@ fn update_collider_parents( } else { commands.entity(child).insert(( ColliderParent(entity), - // Todo: This probably causes a one frame delay. Compute real value? + // TODO: This probably causes a one frame delay. Compute real value? ColliderTransform::default(), PreviousColliderTransform::default(), )); @@ -302,42 +221,9 @@ pub(crate) fn update_child_collider_position( } } -/// Updates the scale of colliders based on [`Transform`] scale. -#[allow(clippy::type_complexity)] -pub fn update_collider_scale( - mut colliders: ParamSet<( - // Root bodies - Query<(&Transform, &mut C), Without>, - // Child colliders - Query<(&ColliderTransform, &mut C), With>, - )>, -) { - // Update collider scale for root bodies - for (transform, mut collider) in &mut colliders.p0() { - #[cfg(feature = "2d")] - let scale = transform.scale.truncate().adjust_precision(); - #[cfg(feature = "3d")] - let scale = transform.scale.adjust_precision(); - if scale != collider.scale() { - // TODO: Support configurable subdivision count for shapes that - // can't be represented without approximations after scaling. - collider.set_scale(scale, 10); - } - } - - // Update collider scale for child colliders - for (collider_transform, mut collider) in &mut colliders.p1() { - if collider_transform.scale != collider.scale() { - // TODO: Support configurable subdivision count for shapes that - // can't be represented without approximations after scaling. - collider.set_scale(collider_transform.scale, 10); - } - } -} - // `ColliderTransform` propagation should only be continued if the child -// is a collider (has a `ColliderTransform`) or is a `ColliderAncestor`. -type ShouldPropagate = Or<(With, With)>; +// is a collider or is a `AncestorMarker`. +type ShouldPropagate = Or<(With>, With)>; /// Updates [`ColliderTransform`]s based on entity hierarchies. Each transform is computed by recursively /// traversing the children of each rigid body and adding their transforms together to form @@ -348,7 +234,7 @@ type ShouldPropagate = Or<(With, With)>; pub(crate) fn propagate_collider_transforms( mut root_query: Query< (Entity, Ref, &Children), - (Without, With), + (Without, With>), >, collider_query: Query< ( @@ -361,7 +247,7 @@ pub(crate) fn propagate_collider_transforms( parent_query: Query<(Entity, Ref, Has, Ref), ShouldPropagate>, ) { root_query.par_iter_mut().for_each( - |(entity, transform,children)| { + |(entity, transform, children)| { for (child, child_transform, is_child_rb, parent) in parent_query.iter_many(children) { assert_eq!( parent.get(), entity, @@ -521,84 +407,4 @@ unsafe fn propagate_collider_transforms_recursive( } } -#[cfg(test)] -mod tests { - use super::*; - - #[test] - fn collider_ancestor_markers() { - let mut app = App::new(); - - app.init_schedule(PhysicsSchedule); - app.init_schedule(SubstepSchedule); - - app.add_plugins(ColliderHierarchyPlugin::::new(PostUpdate)); - - let collider = Collider::capsule(2.0, 0.5); - - // Set up an entity tree like the following: - // - // AN - // / \ - // BN CY - // / \ - // DN EN - // / \ - // FY GY - // - // where Y means that the entity has a collider, - // and N means that the entity does not have a collider. - - let an = app.world_mut().spawn_empty().id(); - - let bn = app.world_mut().spawn_empty().set_parent(an).id(); - let cy = app.world_mut().spawn(collider.clone()).set_parent(an).id(); - - let dn = app.world_mut().spawn_empty().set_parent(cy).id(); - let en = app.world_mut().spawn_empty().set_parent(cy).id(); - - let fy = app.world_mut().spawn(collider.clone()).set_parent(dn).id(); - let gy = app.world_mut().spawn(collider.clone()).set_parent(dn).id(); - - app.world_mut().run_schedule(PostUpdate); - - // Check that the correct entities have the `ColliderAncestor` component. - assert!(app.world().entity(an).contains::()); - assert!(!app.world().entity(bn).contains::()); - assert!(app.world().entity(cy).contains::()); - assert!(app.world().entity(dn).contains::()); - assert!(!app.world().entity(en).contains::()); - assert!(!app.world().entity(fy).contains::()); - assert!(!app.world().entity(gy).contains::()); - - // Remove the collider from FY. DN, CY, and AN should all keep the `ColliderAncestor` marker. - let mut entity_mut = app.world_mut().entity_mut(fy); - entity_mut.remove::(); - - app.world_mut().run_schedule(PostUpdate); - - assert!(app.world().entity(dn).contains::()); - assert!(app.world().entity(cy).contains::()); - assert!(app.world().entity(an).contains::()); - - // Remove the collider from GY. The `ColliderAncestor` marker should - // now be removed from DN and CY, but it should remain on AN. - let mut entity_mut = app.world_mut().entity_mut(gy); - entity_mut.remove::(); - - app.world_mut().run_schedule(PostUpdate); - - assert!(!app.world().entity(dn).contains::()); - assert!(!app.world().entity(cy).contains::()); - assert!(app.world().entity(an).contains::()); - - // Remove the collider from CY. The `ColliderAncestor` marker should - // now be removed from AN. - let mut entity_mut = app.world_mut().entity_mut(cy); - entity_mut.remove::(); - - app.world_mut().run_schedule(PostUpdate); - - assert!(!app.world().entity(an).contains::()); - } -} +// TODO: Add thorough tests for propagation. It's pretty error-prone and changes are risky. diff --git a/src/plugins/collision/collider/mod.rs b/src/plugins/collision/collider/mod.rs index 11ddf921..ed1e5895 100644 --- a/src/plugins/collision/collider/mod.rs +++ b/src/plugins/collision/collider/mod.rs @@ -12,7 +12,7 @@ use bevy::{ mod backend; mod hierarchy; -pub use backend::ColliderBackendPlugin; +pub use backend::{ColliderBackendPlugin, ColliderMarker}; pub use hierarchy::ColliderHierarchyPlugin; pub(crate) use hierarchy::PreviousColliderTransform; diff --git a/src/plugins/mod.rs b/src/plugins/mod.rs index 5ef08024..22bed450 100644 --- a/src/plugins/mod.rs +++ b/src/plugins/mod.rs @@ -201,7 +201,7 @@ impl PluginGroup for PhysicsPlugins { ))] let builder = builder .add(ColliderBackendPlugin::::new(self.schedule)) - .add(ColliderHierarchyPlugin::::new(self.schedule)) + .add(ColliderHierarchyPlugin::new(self.schedule)) .add(NarrowPhasePlugin::::default()); builder diff --git a/src/plugins/prepare.rs b/src/plugins/prepare.rs index 61c9ab14..0fafc0f9 100644 --- a/src/plugins/prepare.rs +++ b/src/plugins/prepare.rs @@ -53,10 +53,10 @@ impl Default for PreparePlugin { /// without having to worry about implementation details. /// /// 1. `PreInit`: Used for systems that must run before initialization. -/// 2. `PropagateTransforms`: Responsible for propagating transforms. -/// 3. `InitRigidBodies`: Responsible for initializing missing [`RigidBody`] components. -/// 4. `InitMassProperties`: Responsible for initializing missing mass properties for [`RigidBody`] components. -/// 5. `InitColliders`: Responsible for initializing missing [`Collider`] components. +/// 2. `InitRigidBodies`: Responsible for initializing missing [`RigidBody`] components. +/// 3. `InitColliders`: Responsible for initializing missing [`Collider`] components. +/// 4. `PropagateTransforms`: Responsible for propagating transforms. +/// 5. `InitMassProperties`: Responsible for initializing missing mass properties for [`RigidBody`] components. /// 6. `InitTransforms`: Responsible for initializing [`Transform`] based on [`Position`] and [`Rotation`] /// or vice versa. /// 7. `Finalize`: Responsible for performing final updates after everything is initialized. @@ -64,14 +64,14 @@ impl Default for PreparePlugin { pub enum PrepareSet { /// Used for systems that must run before initialization. PreInit, - /// Responsible for propagating transforms. - PropagateTransforms, /// Responsible for initializing missing [`RigidBody`] components. InitRigidBodies, - /// Responsible for initializing missing mass properties for [`RigidBody`] components. - InitMassProperties, /// Responsible for initializing missing [`Collider`] components. InitColliders, + /// Responsible for propagating transforms. + PropagateTransforms, + /// Responsible for initializing missing mass properties for [`RigidBody`] components. + InitMassProperties, /// Responsible for initializing [`Transform`] based on [`Position`] and [`Rotation`] /// or vice versa. Parts of this system can be disabled with [`PrepareConfig`]. /// Schedule your system with this to implement custom behavior for initializing transforms. @@ -87,10 +87,10 @@ impl Plugin for PreparePlugin { self.schedule, ( PrepareSet::PreInit, - PrepareSet::PropagateTransforms, PrepareSet::InitRigidBodies, - PrepareSet::InitMassProperties, PrepareSet::InitColliders, + PrepareSet::InitMassProperties, + PrepareSet::PropagateTransforms, PrepareSet::InitTransforms, PrepareSet::Finalize, ) @@ -104,17 +104,13 @@ impl Plugin for PreparePlugin { // Note: Collider logic is handled by the `ColliderBackendPlugin` app.add_systems( self.schedule, + // Run transform propagation if new bodies have been added ( - apply_deferred, - // Run transform propagation if new bodies have been added - ( - bevy::transform::systems::sync_simple_transforms, - bevy::transform::systems::propagate_transforms, - ) - .chain() - .run_if(match_any::>), + sync::sync_simple_transforms_physics, + sync::propagate_transforms_physics, ) .chain() + .run_if(match_any::>) .in_set(PrepareSet::PropagateTransforms), ) .add_systems( diff --git a/src/plugins/sync/ancestor_marker.rs b/src/plugins/sync/ancestor_marker.rs new file mode 100644 index 00000000..bf308495 --- /dev/null +++ b/src/plugins/sync/ancestor_marker.rs @@ -0,0 +1,493 @@ +//! Functionality for marking ancestors of entities with marker components. + +use std::marker::PhantomData; + +use bevy::{ + ecs::{intern::Interned, schedule::ScheduleLabel}, + hierarchy::HierarchyEvent, + prelude::*, +}; + +/// A plugin that marks the ancestors of entities that have the given component `C` +/// with the [`AncestorMarker`] component. +/// +/// One use case is speeding up transform propagation: we only need to propagate +/// down trees that have a certain type of entity, like a collider or a rigid body. +pub struct AncestorMarkerPlugin { + schedule: Interned, + system_set: Option>, + _phantom: PhantomData, +} + +impl AncestorMarkerPlugin { + /// Creates a new [`AncestorMarkerPlugin`] with the schedule that the system + /// adding markers should run in. + pub fn new(schedule: impl ScheduleLabel) -> Self { + Self { + schedule: schedule.intern(), + system_set: None, + _phantom: PhantomData, + } + } + + /// Configures which system set the system adding the [`AncestorMarker`]s should run in. + /// + /// Note: Unlike the normal `in_set` for system configurations, this *overwrites* the set, + /// so the system can only be added to a single system set at a time. + pub fn add_markers_in_set(mut self, set: impl SystemSet) -> Self { + self.system_set = Some(set.intern()); + self + } +} + +impl Plugin for AncestorMarkerPlugin { + fn build(&self, app: &mut App) { + // Add `AncestorMarker` for the ancestors of added colliders, + // until an ancestor that has other `AncestorMarker` entities as children is encountered. + app.observe( + |trigger: Trigger, + mut commands: Commands, + parent_query: Query<&Parent>, + ancestor_query: Query<(), With>>| { + let entity = trigger.entity(); + if parent_query.contains(entity) { + add_ancestor_markers( + entity, + &mut commands, + &parent_query, + &ancestor_query, + false, + ); + } + }, + ); + + // Remove `AncestorMarker` from removed colliders and their ancestors, + // until an ancestor that has other `AncestorMarker` entities as children is encountered. + #[allow(clippy::type_complexity)] + app.observe( + |trigger: Trigger, + mut commands: Commands, + child_query: Query<&Children>, + parent_query: Query<&Parent>, + ancestor_query: Query< + (Entity, Has), + Or<(With>, With)> + >| { + remove_ancestor_markers(trigger.entity(), &mut commands, &parent_query, &child_query, &ancestor_query, false); + }, + ); + + // Update markers when changes are made to the hierarchy. + // TODO: This should be an observer. It'd remove the need for this scheduling nonsense + // and make the implementation more robust. + if let Some(set) = self.system_set { + app.add_systems( + self.schedule, + update_markers_on_hierarchy_changes::.in_set(set), + ); + } else { + app.add_systems(self.schedule, update_markers_on_hierarchy_changes::); + } + } + + fn finish(&self, app: &mut App) { + assert!( + app.is_plugin_added::(), + "`AncestorMarkerPlugin` requires Bevy's `HierarchyPlugin` to function.", + ); + } +} + +/// A marker component that marks an entity as an ancestor of an entity with the given component `C`. +/// +/// This is added and removed automatically by the [`AncestorMarkerPlugin`] if it is enabled. +#[derive(Component, Reflect)] +pub struct AncestorMarker { + _phantom: PhantomData, +} + +impl Default for AncestorMarker { + fn default() -> Self { + Self { + _phantom: PhantomData, + } + } +} + +// TODO: This should be an observer once there is a trigger for hierarchy changes. +// See https://github.com/bevyengine/bevy/pull/13925 for an implementation of that trigger. +/// Marks ancestors of entities that have the given component `C` with the `AncestorMarker` component. +#[allow(clippy::type_complexity)] +fn update_markers_on_hierarchy_changes( + mut commands: Commands, + entity_query: Query<(), With>, + parent_query: Query<&Parent>, + child_query: Query<&Children>, + ancestor_query_1: Query<(), With>>, + ancestor_query_2: Query<(Entity, Has), Or<(With>, With)>>, + mut hierarchy_event: EventReader, +) { + for event in hierarchy_event.read().cloned() { + match event { + HierarchyEvent::ChildAdded { child, parent } => { + if entity_query.contains(child) { + // Mark the child's ancestors. + add_ancestor_markers( + parent, + &mut commands, + &parent_query, + &ancestor_query_1, + true, + ); + } + } + + HierarchyEvent::ChildRemoved { child, parent } => { + if entity_query.contains(child) { + // Remove markers from the parent and its ancestors. + remove_ancestor_markers( + parent, + &mut commands, + &parent_query, + &child_query, + &ancestor_query_2, + true, + ); + } + } + + HierarchyEvent::ChildMoved { + child, + previous_parent, + new_parent, + } => { + if entity_query.contains(child) { + // Remove markers from the previous parent and its ancestors. + remove_ancestor_markers( + previous_parent, + &mut commands, + &parent_query, + &child_query, + &ancestor_query_2, + true, + ); + + // Mark the new parent and its ancestors. + add_ancestor_markers( + new_parent, + &mut commands, + &parent_query, + &ancestor_query_1, + true, + ); + } + } + } + } +} + +fn add_ancestor_markers( + entity: Entity, + commands: &mut Commands, + parent_query: &Query<&Parent>, + ancestor_query: &Query<(), With>>, + include_self: bool, +) { + if include_self { + commands + .entity(entity) + .insert(AncestorMarker::::default()); + } + + // Traverse up the tree, marking entities with `AncestorMarker` + // until an entity that already has it is encountered. + for parent_entity in parent_query.iter_ancestors(entity) { + if ancestor_query.contains(parent_entity) { + break; + } else { + commands + .entity(parent_entity) + .insert(AncestorMarker::::default()); + } + } +} + +#[allow(clippy::type_complexity)] +fn remove_ancestor_markers( + entity: Entity, + commands: &mut Commands, + parent_query: &Query<&Parent>, + child_query: &Query<&Children>, + ancestor_query: &Query<(Entity, Has), Or<(With>, With)>>, + include_self: bool, +) { + if include_self { + // Remove the marker from the `parent` unless a sibling of the `child` + // is also a marked ancestor or has `C`. + if let Ok(children) = child_query.get(entity) { + let keep_marker = ancestor_query + .iter_many(children) + .any(|(parent_child, _has_c)| parent_child != entity); + if keep_marker { + return; + } else { + commands.entity(entity).remove::>(); + } + } else { + // The parent has no children, so it cannot be an ancestor. + commands.entity(entity).remove::>(); + } + } + + // Iterate over ancestors, removing `AncestorMarker` markers until + // an entity that has other `AncestorMarker` children is encountered. + let mut previous_parent = entity; + for parent_entity in parent_query.iter_ancestors(entity) { + if let Ok(children) = child_query.get(parent_entity) { + // Keep the marker if `parent_entity` has a child that is a marked ancestor + // or an entity that has `C`, but not the one that was removed. + let keep_marker = ancestor_query + .iter_many(children) + .any(|(child, has_c)| child != previous_parent || (has_c && child != entity)); + + if keep_marker { + return; + } else { + commands.entity(parent_entity).remove::>(); + } + } else { + // The parent has no children, so it cannot be an ancestor. + commands.entity(parent_entity).remove::>(); + } + + previous_parent = parent_entity; + } +} + +#[cfg(test)] +mod tests { + use super::*; + + #[derive(Component)] + struct C; + + #[test] + fn add_and_remove_component() { + let mut app = App::new(); + + app.add_plugins((AncestorMarkerPlugin::::new(PostUpdate), HierarchyPlugin)); + + // Set up an entity tree like the following: + // + // AN + // / \ + // BN CY + // / \ + // DN EN + // / \ + // FY GY + // + // where Y means that the entity has `C`, + // and N means that the entity does not have `C`. + + let an = app.world_mut().spawn_empty().id(); + + let bn = app.world_mut().spawn_empty().set_parent(an).id(); + let cy = app.world_mut().spawn(C).set_parent(an).id(); + + let dn = app.world_mut().spawn_empty().set_parent(cy).id(); + let en = app.world_mut().spawn_empty().set_parent(cy).id(); + + let fy = app.world_mut().spawn(C).set_parent(dn).id(); + let gy = app.world_mut().spawn(C).set_parent(dn).id(); + + app.world_mut().run_schedule(PostUpdate); + + // Check that the correct entities have the `AncestorMarker` component. + assert!(app.world().entity(an).contains::>()); + assert!(!app.world().entity(bn).contains::>()); + assert!(app.world().entity(cy).contains::>()); + assert!(app.world().entity(dn).contains::>()); + assert!(!app.world().entity(en).contains::>()); + assert!(!app.world().entity(fy).contains::>()); + assert!(!app.world().entity(gy).contains::>()); + + // Remove `C` from FY. DN, CY, and AN should all keep the `AncestorMarker` marker. + let mut entity_mut = app.world_mut().entity_mut(fy); + entity_mut.remove::(); + + app.world_mut().run_schedule(PostUpdate); + + assert!(app.world().entity(dn).contains::>()); + assert!(app.world().entity(cy).contains::>()); + assert!(app.world().entity(an).contains::>()); + + // Remove `C` from GY. The `AncestorMarker` marker should + // now be removed from DN and CY, but it should remain on AN. + let mut entity_mut = app.world_mut().entity_mut(gy); + entity_mut.remove::(); + + app.world_mut().run_schedule(PostUpdate); + + assert!(!app.world().entity(dn).contains::>()); + assert!(!app.world().entity(cy).contains::>()); + assert!(app.world().entity(an).contains::>()); + + // Remove `C` from CY. The `AncestorMarker` marker should + // now be removed from AN. + let mut entity_mut = app.world_mut().entity_mut(cy); + entity_mut.remove::(); + + app.world_mut().run_schedule(PostUpdate); + + assert!(!app.world().entity(an).contains::>()); + } + + #[test] + fn remove_children() { + let mut app = App::new(); + + app.add_plugins((AncestorMarkerPlugin::::new(PostUpdate), HierarchyPlugin)); + + // Set up an entity tree like the following: + // + // AN + // / \ + // BN CY + // / \ + // DN EN + // / \ + // FY GY + // + // where Y means that the entity has `C`, + // and N means that the entity does not have `C`. + + let an = app.world_mut().spawn_empty().id(); + + let bn = app.world_mut().spawn_empty().set_parent(an).id(); + let cy = app.world_mut().spawn(C).set_parent(an).id(); + + let dn = app.world_mut().spawn_empty().set_parent(cy).id(); + let en = app.world_mut().spawn_empty().set_parent(cy).id(); + + let fy = app.world_mut().spawn(C).set_parent(dn).id(); + let gy = app.world_mut().spawn(C).set_parent(dn).id(); + + app.world_mut().run_schedule(PostUpdate); + + // Check that the correct entities have the `AncestorMarker` component. + assert!(app.world().entity(an).contains::>()); + assert!(!app.world().entity(bn).contains::>()); + assert!(app.world().entity(cy).contains::>()); + assert!(app.world().entity(dn).contains::>()); + assert!(!app.world().entity(en).contains::>()); + assert!(!app.world().entity(fy).contains::>()); + assert!(!app.world().entity(gy).contains::>()); + + // Make FY an orphan. + let mut entity_mut = app.world_mut().entity_mut(dn); + entity_mut.remove_children(&[fy]); + + app.world_mut().run_schedule(PostUpdate); + + assert!(app.world().entity(dn).contains::>()); + assert!(app.world().entity(cy).contains::>()); + assert!(app.world().entity(an).contains::>()); + + // Make GY an orphan. The `AncestorMarker` marker should + // now be removed from DN and CY, but it should remain on AN. + let mut entity_mut = app.world_mut().entity_mut(dn); + entity_mut.remove_children(&[gy]); + + app.world_mut().run_schedule(PostUpdate); + + assert!(!app.world().entity(dn).contains::>()); + assert!(!app.world().entity(cy).contains::>()); + assert!(app.world().entity(an).contains::>()); + + // Make CY an orphan. The `AncestorMarker` marker should + // now be removed from AN. + let mut entity_mut = app.world_mut().entity_mut(an); + entity_mut.remove_children(&[cy]); + + app.world_mut().run_schedule(PostUpdate); + + assert!(!app.world().entity(an).contains::>()); + } + + #[test] + fn move_children() { + let mut app = App::new(); + + app.add_plugins((AncestorMarkerPlugin::::new(PostUpdate), HierarchyPlugin)); + + // Set up an entity tree like the following: + // + // AN + // / \ + // BN CY + // / \ + // DN EN + // / \ + // FY GY + // + // where Y means that the entity has `C`, + // and N means that the entity does not have `C`. + + let an = app.world_mut().spawn_empty().id(); + + let bn = app.world_mut().spawn_empty().set_parent(an).id(); + let cy = app.world_mut().spawn(C).set_parent(an).id(); + + let dn = app.world_mut().spawn_empty().set_parent(cy).id(); + let en = app.world_mut().spawn_empty().set_parent(cy).id(); + + let fy = app.world_mut().spawn(C).set_parent(dn).id(); + let gy = app.world_mut().spawn(C).set_parent(dn).id(); + + app.world_mut().run_schedule(PostUpdate); + + // Check that the correct entities have the `AncestorMarker` component. + assert!(app.world().entity(an).contains::>()); + assert!(!app.world().entity(bn).contains::>()); + assert!(app.world().entity(cy).contains::>()); + assert!(app.world().entity(dn).contains::>()); + assert!(!app.world().entity(en).contains::>()); + assert!(!app.world().entity(fy).contains::>()); + assert!(!app.world().entity(gy).contains::>()); + + // Move FY to be a child of BN. BN should get the `AncestorMarker` component. + let mut entity_mut = app.world_mut().entity_mut(bn); + entity_mut.add_child(fy); + + app.world_mut().run_schedule(PostUpdate); + + assert!(app.world().entity(bn).contains::>()); + assert!(app.world().entity(dn).contains::>()); + assert!(app.world().entity(cy).contains::>()); + assert!(app.world().entity(an).contains::>()); + + // Move GY to be a child of BN. The `AncestorMarker` marker should + // now be removed from DN and CY. + let mut entity_mut = app.world_mut().entity_mut(bn); + entity_mut.add_child(gy); + + app.world_mut().run_schedule(PostUpdate); + + assert!(!app.world().entity(dn).contains::>()); + assert!(!app.world().entity(cy).contains::>()); + assert!(app.world().entity(an).contains::>()); + + // Move all children from BN to CY. The `AncestorMarker` marker should + // now be removed from BN, and CY should get it back. + let mut entity_mut = app.world_mut().entity_mut(cy); + entity_mut.push_children(&[fy, gy]); + + app.world_mut().run_schedule(PostUpdate); + + assert!(!app.world().entity(bn).contains::>()); + assert!(app.world().entity(cy).contains::>()); + assert!(app.world().entity(an).contains::>()); + } +} diff --git a/src/plugins/sync.rs b/src/plugins/sync/mod.rs similarity index 52% rename from src/plugins/sync.rs rename to src/plugins/sync/mod.rs index 822ab416..2d726fb7 100644 --- a/src/plugins/sync.rs +++ b/src/plugins/sync/mod.rs @@ -4,7 +4,12 @@ //! See [`SyncPlugin`]. use crate::{prelude::*, utils::get_pos_translation}; +use ancestor_marker::{AncestorMarker, AncestorMarkerPlugin}; use bevy::{ecs::intern::Interned, prelude::*}; +use prepare::PrepareSet; + +// TODO: Where should this be? +pub mod ancestor_marker; /// Responsible for synchronizing physics components with other data, like keeping [`Position`] /// and [`Rotation`] in sync with `Transform`. @@ -47,6 +52,9 @@ impl Default for SyncPlugin { } } +#[derive(SystemSet, Clone, Copy, Debug, PartialEq, Eq, Hash)] +struct MarkRigidBodyAncestors; + impl Plugin for SyncPlugin { fn build(&self, app: &mut App) { app.init_resource::() @@ -65,13 +73,26 @@ impl Plugin for SyncPlugin { .in_set(PhysicsSet::Sync), ); + // Mark ancestors of colliders with `AncestorMarker`. + // This is used to speed up transform propagation by skipping + // trees that have no rigid bodies. + app.add_plugins( + AncestorMarkerPlugin::::new(self.schedule) + .add_markers_in_set(MarkRigidBodyAncestors), + ); + + app.configure_sets( + self.schedule, + MarkRigidBodyAncestors.in_set(PrepareSet::PreInit), + ); + // Initialize `PreviousGlobalTransform` and apply `Transform` changes that happened // between the end of the previous physics frame and the start of this physics frame. app.add_systems( self.schedule, ( - bevy::transform::systems::sync_simple_transforms, - bevy::transform::systems::propagate_transforms, + sync_simple_transforms_physics, + propagate_transforms_physics, init_previous_global_transform, transform_to_position, // Update `PreviousGlobalTransform` for the physics step's `GlobalTransform` change detection @@ -83,12 +104,13 @@ impl Plugin for SyncPlugin { .run_if(|config: Res| config.transform_to_position), ); - // Apply `Transform` changes to `Position` and `Rotation` + // Apply `Transform` changes to `Position` and `Rotation`. + // TODO: Do we need this? app.add_systems( self.schedule, ( - bevy::transform::systems::sync_simple_transforms, - bevy::transform::systems::propagate_transforms, + sync_simple_transforms_physics, + propagate_transforms_physics, transform_to_position, ) .chain() @@ -108,8 +130,8 @@ impl Plugin for SyncPlugin { app.add_systems( self.schedule, ( - bevy::transform::systems::sync_simple_transforms, - bevy::transform::systems::propagate_transforms, + sync_simple_transforms_physics, + propagate_transforms_physics, update_previous_global_transforms, ) .chain() @@ -356,3 +378,290 @@ pub fn update_previous_global_transforms( previous_transform.0 = *transform; } } + +// Below are copies of Bevy's transform propagation systems, but optimized to only traverse trees with rigid bodies. +// Propagation is unnecessary for everything else, because the physics engine should only modify the positions +// of rigid bodies and their descendants. Bevy runs its own propagation near the end of the frame. + +/// Updates the [`GlobalTransform`] component of physics entities that aren't in the hierarchy. +#[allow(clippy::type_complexity)] +pub fn sync_simple_transforms_physics( + mut query: ParamSet<( + Query< + (&Transform, &mut GlobalTransform), + ( + Or<(Changed, Added)>, + Without, + Without, + Or<(With, With)>, + ), + >, + Query< + (Ref, &mut GlobalTransform), + ( + Without, + Without, + Or<(With, With)>, + ), + >, + )>, + mut orphaned: RemovedComponents, +) { + // Update changed entities. + query + .p0() + .par_iter_mut() + .for_each(|(transform, mut global_transform)| { + *global_transform = GlobalTransform::from(*transform); + }); + // Update orphaned entities. + let mut query = query.p1(); + let mut iter = query.iter_many_mut(orphaned.read()); + while let Some((transform, mut global_transform)) = iter.fetch_next() { + if !transform.is_changed() && !global_transform.is_added() { + *global_transform = GlobalTransform::from(*transform); + } + } +} + +// Below is a diagram of an example hierarchy. +// +// A +// / \ +// N A +// / \ +// P N +// / \ +// N N +// +// P = a physics entity +// A = a physics entity ancestor +// N = not a physics entity ancestor +// +// We can stop propagation, if: +// +// 1. we encounter an N that doesn't have any P as an ancestor. +// 2. we encounter a P with no children. + +// TODO: A general `PhysicsMarker` for both rigid bodies and colliders could be nice. + +type TransformQueryData = ( + Ref<'static, Transform>, + &'static mut GlobalTransform, + Option<&'static Children>, + Has, + Has, +); + +type ParentQueryData = ( + Entity, + Ref<'static, Parent>, + Has, + Has, +); + +type PhysicsObjectOrAncestorFilter = Or<( + Or<(With, With>)>, + Or<(With, With>)>, +)>; + +/// Update [`GlobalTransform`] component of physics entities based on entity hierarchy and +/// [`Transform`] component. +#[allow(clippy::type_complexity)] +pub fn propagate_transforms_physics( + mut root_query: Query< + ( + Entity, + &Children, + Ref, + &mut GlobalTransform, + Has, + Has, + ), + ( + Without, + Or<( + With>, + With>, + )>, + ), + >, + mut orphaned: RemovedComponents, + transform_query: Query>, + // This is used if the entity has no physics entity ancestor. + parent_query_1: Query, + // This is used if the entity is a physics entity with children *or* if any ancestor is a physics entity. + parent_query_2: Query, + mut orphaned_entities: Local>, +) { + orphaned_entities.clear(); + orphaned_entities.extend(orphaned.read()); + orphaned_entities.sort_unstable(); + root_query.par_iter_mut().for_each( + |(entity, children, transform, mut global_transform, is_root_rb, is_root_collider)| { + let changed = transform.is_changed() || global_transform.is_added() || orphaned_entities.binary_search(&entity).is_ok(); + if changed { + *global_transform = GlobalTransform::from(*transform); + } + + let handle = |(child, actual_parent, is_parent_rb, is_parent_collider): (Entity, Ref, bool, bool)| { + assert_eq!( + actual_parent.get(), entity, + "Malformed hierarchy. This probably means that your hierarchy has been improperly maintained, or contains a cycle" + ); + // SAFETY: + // - `child` must have consistent parentage, or the above assertion would panic. + // Since `child` is parented to a root entity, the entire hierarchy leading to it is consistent. + // - We may operate as if all descendants are consistent, since `propagate_recursive` will panic before + // continuing to propagate if it encounters an entity with inconsistent parentage. + // - Since each root entity is unique and the hierarchy is consistent and forest-like, + // other root entities' `propagate_recursive` calls will not conflict with this one. + // - Since this is the only place where `transform_query` gets used, there will be no conflicting fetches elsewhere. + #[allow(unsafe_code)] + unsafe { + propagate_transforms_physics_recursive( + &global_transform, + &transform_query, + &parent_query_1, + &parent_query_2, + child, + changed || actual_parent.is_changed(), + is_parent_rb || is_parent_collider, + ); + } + }; + + if is_root_rb || is_root_collider { + parent_query_1.iter_many(children).for_each(handle); + } else { + parent_query_2.iter_many(children).for_each(handle); + } + }, + ); +} + +/// Recursively propagates the transforms for `entity` and all of its descendants. +/// +/// # Panics +/// +/// If `entity`'s descendants have a malformed hierarchy, this function will panic occur before propagating +/// the transforms of any malformed entities and their descendants. +/// +/// # Safety +/// +/// - While this function is running, `transform_query` must not have any fetches for `entity`, +/// nor any of its descendants. +/// - The caller must ensure that the hierarchy leading to `entity` +/// is well-formed and must remain as a tree or a forest. Each entity must have at most one parent. +#[allow(clippy::type_complexity)] +unsafe fn propagate_transforms_physics_recursive( + parent: &GlobalTransform, + transform_query: &Query>, + // This is used if the entity has no physics entity ancestor. + parent_query_1: &Query, + // This is used if the entity is a physics entity with children *or* if any ancestor is a physics entity. + parent_query_2: &Query, + entity: Entity, + mut changed: bool, + mut any_ancestor_is_physics_entity: bool, +) { + let (global_matrix, children) = { + let Ok((transform, mut global_transform, children, is_rb, is_collider)) = + // SAFETY: This call cannot create aliased mutable references. + // - The top level iteration parallelizes on the roots of the hierarchy. + // - The caller ensures that each child has one and only one unique parent throughout the entire + // hierarchy. + // + // For example, consider the following malformed hierarchy: + // + // A + // / \ + // B C + // \ / + // D + // + // D has two parents, B and C. If the propagation passes through C, but the Parent component on D points to B, + // the above check will panic as the origin parent does match the recorded parent. + // + // Also consider the following case, where A and B are roots: + // + // A B + // \ / + // C D + // \ / + // E + // + // Even if these A and B start two separate tasks running in parallel, one of them will panic before attempting + // to mutably access E. + (unsafe { transform_query.get_unchecked(entity) }) else { + return; + }; + + if any_ancestor_is_physics_entity || is_rb || is_collider { + any_ancestor_is_physics_entity = true; + + changed |= transform.is_changed() || global_transform.is_added(); + if changed { + *global_transform = parent.mul_transform(*transform); + } + } + + (*global_transform, children) + }; + + let Some(children) = children else { return }; + + // If the entity has a physics entity ancestor, propagate down regardless of the child type. + // Otherwise, only propagate to entities that are physics entities or physics entity ancestors. + if any_ancestor_is_physics_entity { + for (child, actual_parent, is_parent_rb, is_parent_collider) in + parent_query_1.iter_many(children) + { + assert_eq!( + actual_parent.get(), entity, + "Malformed hierarchy. This probably means that your hierarchy has been improperly maintained, or contains a cycle" + ); + // SAFETY: The caller guarantees that `transform_query` will not be fetched + // for any descendants of `entity`, so it is safe to call `propagate_transforms_physics_recursive` for each child. + // + // The above assertion ensures that each child has one and only one unique parent throughout the + // entire hierarchy. + unsafe { + propagate_transforms_physics_recursive( + &global_matrix, + transform_query, + parent_query_1, + parent_query_2, + child, + changed || actual_parent.is_changed(), + any_ancestor_is_physics_entity || is_parent_rb || is_parent_collider, + ); + } + } + } else { + for (child, actual_parent, is_parent_rb, is_parent_collider) in + parent_query_2.iter_many(children) + { + assert_eq!( + actual_parent.get(), entity, + "Malformed hierarchy. This probably means that your hierarchy has been improperly maintained, or contains a cycle" + ); + // SAFETY: The caller guarantees that `transform_query` will not be fetched + // for any descendants of `entity`, so it is safe to call `propagate_transforms_physics_recursive` for each child. + // + // The above assertion ensures that each child has one and only one unique parent throughout the + // entire hierarchy. + unsafe { + propagate_transforms_physics_recursive( + &global_matrix, + transform_query, + parent_query_1, + parent_query_2, + child, + changed || actual_parent.is_changed(), + any_ancestor_is_physics_entity || is_parent_rb || is_parent_collider, + ); + } + } + } +} diff --git a/src/tests.rs b/src/tests.rs index b80f0791..26cd11d2 100644 --- a/src/tests.rs +++ b/src/tests.rs @@ -29,7 +29,12 @@ macro_rules! setup_insta { fn create_app() -> App { let mut app = App::new(); - app.add_plugins((MinimalPlugins, TransformPlugin, PhysicsPlugins::default())); + app.add_plugins(( + MinimalPlugins, + TransformPlugin, + HierarchyPlugin, + PhysicsPlugins::default(), + )); #[cfg(feature = "async-collider")] { app.add_plugins(( @@ -250,7 +255,11 @@ fn no_ambiguity_errors() { let mut app = App::new(); - app.add_plugins((MinimalPlugins, PhysicsPlugins::new(DeterministicSchedule))); + app.add_plugins(( + MinimalPlugins, + HierarchyPlugin, + PhysicsPlugins::new(DeterministicSchedule), + )); #[cfg(feature = "async-collider")] {