avian3d/collision/collider/collider_transform/
plugin.rs

1use crate::{
2    collision::narrow_phase::NarrowPhaseSet,
3    prelude::*,
4    prepare::{match_any, PrepareSet},
5    sync::ancestor_marker::{AncestorMarker, AncestorMarkerPlugin},
6};
7use bevy::{
8    ecs::{intern::Interned, schedule::ScheduleLabel},
9    prelude::*,
10    transform::systems::{mark_dirty_trees, propagate_parent_transforms, sync_simple_transforms},
11};
12
13/// A plugin for propagating and updating transforms for colliders.
14///
15/// - Propagates [`Transform`] to [`GlobalTransform`] and [`ColliderTransform`].
16/// - Updates [`Position`] and [`Rotation`] for colliders.
17///
18/// This plugin requires that colliders have the [`ColliderMarker`] component,
19/// which is added automatically for colliders if the [`ColliderBackendPlugin`] is enabled.
20pub struct ColliderTransformPlugin {
21    schedule: Interned<dyn ScheduleLabel>,
22}
23
24impl ColliderTransformPlugin {
25    /// Creates a [`ColliderTransformPlugin`] with the schedule that is used for running the [`PhysicsSchedule`].
26    ///
27    /// The default schedule is `FixedPostUpdate`.
28    pub fn new(schedule: impl ScheduleLabel) -> Self {
29        Self {
30            schedule: schedule.intern(),
31        }
32    }
33}
34
35impl Default for ColliderTransformPlugin {
36    fn default() -> Self {
37        Self {
38            schedule: FixedPostUpdate.intern(),
39        }
40    }
41}
42
43impl Plugin for ColliderTransformPlugin {
44    fn build(&self, app: &mut App) {
45        // Mark ancestors of colliders with `AncestorMarker<ColliderMarker>`.
46        // This is used to speed up `ColliderTransform` propagation by skipping
47        // trees that have no colliders.
48        app.add_plugins(AncestorMarkerPlugin::<ColliderMarker>::default());
49
50        // Run transform propagation if new colliders without rigid bodies have been added.
51        // The `PreparePlugin` should handle transform propagation for new rigid bodies.
52        app.add_systems(
53            self.schedule,
54            (
55                mark_dirty_trees,
56                propagate_parent_transforms,
57                sync_simple_transforms,
58            )
59                .chain()
60                .run_if(match_any::<(Added<ColliderMarker>, Without<RigidBody>)>)
61                .in_set(PrepareSet::PropagateTransforms)
62                .ambiguous_with_all(),
63        );
64
65        // Propagate `ColliderTransform`s if there are new colliders.
66        // Only traverses trees with `AncestorMarker<ColliderMarker>`.
67        app.add_systems(
68            self.schedule,
69            (
70                propagate_collider_transforms,
71                update_child_collider_position.run_if(match_any::<Added<ColliderMarker>>),
72            )
73                .chain()
74                .after(PrepareSet::InitTransforms)
75                .before(PrepareSet::Finalize),
76        );
77
78        let physics_schedule = app
79            .get_schedule_mut(PhysicsSchedule)
80            .expect("add PhysicsSchedule first");
81
82        // Update child collider positions before narrow phase collision detection.
83        // Only traverses trees with `AncestorMarker<ColliderMarker>`.
84        physics_schedule.add_systems(update_child_collider_position.in_set(NarrowPhaseSet::First));
85    }
86}
87
88#[allow(clippy::type_complexity)]
89pub(crate) fn update_child_collider_position(
90    mut collider_query: Query<
91        (
92            &ColliderTransform,
93            &mut Position,
94            &mut Rotation,
95            &ColliderOf,
96        ),
97        Without<RigidBody>,
98    >,
99    rb_query: Query<(&Position, &Rotation), (With<RigidBody>, With<Children>)>,
100) {
101    for (collider_transform, mut position, mut rotation, collider_of) in &mut collider_query {
102        let Ok((rb_pos, rb_rot)) = rb_query.get(collider_of.body) else {
103            continue;
104        };
105
106        position.0 = rb_pos.0 + rb_rot * collider_transform.translation;
107        #[cfg(feature = "2d")]
108        {
109            *rotation = *rb_rot * collider_transform.rotation;
110        }
111        #[cfg(feature = "3d")]
112        {
113            *rotation = (rb_rot.0 * collider_transform.rotation.0)
114                .normalize()
115                .into();
116        }
117    }
118}
119
120// `ColliderTransform` propagation should only be continued if the child
121// is a collider or is a `AncestorMarker<ColliderMarker>`.
122type ShouldPropagate = Or<(With<AncestorMarker<ColliderMarker>>, With<ColliderMarker>)>;
123
124/// Updates [`ColliderTransform`]s based on entity hierarchies. Each transform is computed by recursively
125/// traversing the children of each rigid body and adding their transforms together to form
126/// the total transform relative to the body.
127///
128/// This is largely a clone of `propagate_transforms` in `bevy_transform`.
129#[allow(clippy::type_complexity)]
130pub(crate) fn propagate_collider_transforms(
131    mut root_query: Query<
132        (Entity, Ref<Transform>, &Children),
133        (Without<ChildOf>, With<AncestorMarker<ColliderMarker>>),
134    >,
135    collider_query: Query<
136        (
137            Ref<Transform>,
138            Option<&mut ColliderTransform>,
139            Option<&Children>,
140        ),
141        (With<ChildOf>, ShouldPropagate),
142    >,
143    parent_query: Query<(Entity, Ref<Transform>, Has<RigidBody>, Ref<ChildOf>), ShouldPropagate>,
144) {
145    root_query.par_iter_mut().for_each(
146        |(entity, transform, children)| {
147            for (child, child_transform, is_child_rb, child_of) in parent_query.iter_many(children) {
148                assert_eq!(
149                    child_of.parent(), entity,
150                    "Malformed hierarchy. This probably means that your hierarchy has been improperly maintained, or contains a cycle"
151                );
152                let changed = transform.is_changed() || child_of.is_changed();
153                let parent_transform = ColliderTransform::from(*transform);
154                let child_transform = ColliderTransform::from(*child_transform);
155                let scale = parent_transform.scale * child_transform.scale;
156
157                // SAFETY:
158                // - `child` must have consistent parentage, or the above assertion would panic.
159                // Since `child` is parented to a root entity, the entire hierarchy leading to it is consistent.
160                // - We may operate as if all descendants are consistent, since `propagate_collider_transform_recursive` will panic before
161                //   continuing to propagate if it encounters an entity with inconsistent parentage.
162                // - Since each root entity is unique and the hierarchy is consistent and forest-like,
163                //   other root entities' `propagate_collider_transform_recursive` calls will not conflict with this one.
164                // - Since this is the only place where `collider_query` gets used, there will be no conflicting fetches elsewhere.
165                unsafe {
166                    propagate_collider_transforms_recursive(
167                        if is_child_rb {
168                            ColliderTransform {
169                                scale,
170                                ..default()
171                            }
172                        } else {
173                            ColliderTransform {
174                                translation: parent_transform.scale * child_transform.translation,
175                                rotation: child_transform.rotation,
176                                scale,
177                            }
178                        },
179                        &collider_query,
180                        &parent_query,
181                        child,
182                        changed,
183                    );
184                }
185            }
186        },
187    );
188}
189
190/// Recursively computes the [`ColliderTransform`] for `entity` and all of its descendants
191/// by propagating transforms.
192///
193/// This is largely a clone of `propagate_recursive` in `bevy_transform`.
194///
195/// # Panics
196///
197/// If `entity`'s descendants have a malformed hierarchy, this function will panic occur before propagating
198/// the transforms of any malformed entities and their descendants.
199///
200/// # Safety
201///
202/// - While this function is running, `collider_query` must not have any fetches for `entity`,
203///   nor any of its descendants.
204/// - The caller must ensure that the hierarchy leading to `entity`
205///   is well-formed and must remain as a tree or a forest. Each entity must have at most one parent.
206#[allow(clippy::type_complexity)]
207unsafe fn propagate_collider_transforms_recursive(
208    transform: ColliderTransform,
209    collider_query: &Query<
210        (
211            Ref<Transform>,
212            Option<&mut ColliderTransform>,
213            Option<&Children>,
214        ),
215        (With<ChildOf>, ShouldPropagate),
216    >,
217    parent_query: &Query<(Entity, Ref<Transform>, Has<RigidBody>, Ref<ChildOf>), ShouldPropagate>,
218    entity: Entity,
219    mut changed: bool,
220) {
221    let children = {
222        // SAFETY: This call cannot create aliased mutable references.
223        //   - The top level iteration parallelizes on the roots of the hierarchy.
224        //   - The caller ensures that each child has one and only one unique parent throughout the entire
225        //     hierarchy.
226        //
227        // For example, consider the following malformed hierarchy:
228        //
229        //     A
230        //   /   \
231        //  B     C
232        //   \   /
233        //     D
234        //
235        // D has two parents, B and C. If the propagation passes through C, but the ChildOf component on D points to B,
236        // the above check will panic as the origin parent does match the recorded parent.
237        //
238        // Also consider the following case, where A and B are roots:
239        //
240        //  A       B
241        //   \     /
242        //    C   D
243        //     \ /
244        //      E
245        //
246        // Even if these A and B start two separate tasks running in parallel, one of them will panic before attempting
247        // to mutably access E.
248        let Ok((transform_ref, collider_transform, children)) =
249            (unsafe { collider_query.get_unchecked(entity) })
250        else {
251            return;
252        };
253
254        changed |= transform_ref.is_changed();
255        if changed {
256            if let Some(mut collider_transform) = collider_transform {
257                if *collider_transform != transform {
258                    *collider_transform = transform;
259                }
260            }
261        }
262
263        children
264    };
265
266    let Some(children) = children else { return };
267    for (child, child_transform, is_rb, child_of) in parent_query.iter_many(children) {
268        assert_eq!(
269            child_of.parent(), entity,
270            "Malformed hierarchy. This probably means that your hierarchy has been improperly maintained, or contains a cycle"
271        );
272
273        let child_transform = ColliderTransform::from(*child_transform);
274        let scale = transform.scale * child_transform.scale;
275
276        // SAFETY: The caller guarantees that `collider_query` will not be fetched
277        // for any descendants of `entity`, so it is safe to call `propagate_collider_transforms_recursive` for each child.
278        //
279        // The above assertion ensures that each child has one and only one unique parent throughout the
280        // entire hierarchy.
281        unsafe {
282            propagate_collider_transforms_recursive(
283                if is_rb {
284                    ColliderTransform { scale, ..default() }
285                } else {
286                    ColliderTransform {
287                        translation: transform.transform_point(child_transform.translation),
288                        #[cfg(feature = "2d")]
289                        rotation: transform.rotation * child_transform.rotation,
290                        #[cfg(feature = "3d")]
291                        rotation: Rotation(transform.rotation.0 * child_transform.rotation.0),
292                        scale,
293                    }
294                },
295                collider_query,
296                parent_query,
297                child,
298                changed || child_of.is_changed(),
299            );
300        }
301    }
302}
303
304// TODO: Add thorough tests for propagation. It's pretty error-prone and changes are risky.