1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271
//! Collects pairs of potentially colliding entities into [`BroadCollisionPairs`] using
//! [AABB](ColliderAabb) intersection checks.
//!
//! See [`BroadPhasePlugin`].
use crate::prelude::*;
use bevy::{
ecs::entity::{EntityMapper, MapEntities},
prelude::*,
};
/// Collects pairs of potentially colliding entities into [`BroadCollisionPairs`] using
/// [AABB](ColliderAabb) intersection checks. This speeds up narrow phase collision detection,
/// as the number of precise collision checks required is greatly reduced.
///
/// Currently, the broad phase uses the [sweep and prune](https://en.wikipedia.org/wiki/Sweep_and_prune) algorithm.
///
/// The broad phase systems run in [`PhysicsStepSet::BroadPhase`].
pub struct BroadPhasePlugin;
impl Plugin for BroadPhasePlugin {
fn build(&self, app: &mut App) {
app.init_resource::<BroadCollisionPairs>()
.init_resource::<AabbIntervals>();
app.configure_sets(
PhysicsSchedule,
(
BroadPhaseSet::First,
BroadPhaseSet::UpdateStructures,
BroadPhaseSet::CollectCollisions,
BroadPhaseSet::Last,
)
.chain()
.in_set(PhysicsStepSet::BroadPhase),
);
let physics_schedule = app
.get_schedule_mut(PhysicsSchedule)
.expect("add PhysicsSchedule first");
physics_schedule.add_systems(
(update_aabb_intervals, add_new_aabb_intervals)
.chain()
.in_set(BroadPhaseSet::UpdateStructures),
);
physics_schedule
.add_systems(collect_collision_pairs.in_set(BroadPhaseSet::CollectCollisions));
}
}
/// System sets for systems running in [`PhysicsStepSet::BroadPhase`].
#[derive(SystemSet, Clone, Copy, Debug, PartialEq, Eq, Hash)]
pub enum BroadPhaseSet {
/// Runs at the start of the broad phase. Empty by default.
First,
/// Updates acceleration structures and other data needed for broad phase collision detection.
UpdateStructures,
/// Detects potential intersections between entities and adds them to the [`BroadCollisionPairs`] resource.
CollectCollisions,
/// Runs at the end of the broad phase. Empty by default.
Last,
}
/// A list of entity pairs for potential collisions collected during the broad phase.
#[derive(Reflect, Resource, Debug, Default, Deref, DerefMut)]
#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))]
#[reflect(Resource)]
pub struct BroadCollisionPairs(pub Vec<(Entity, Entity)>);
/// Contains the entities whose AABBs intersect the AABB of this entity.
/// Updated automatically during broad phase collision detection.
///
/// Note that this component is only added to bodies with [`SweptCcd`] by default,
/// but can be added to any entity.
#[derive(Component, Clone, Debug, Default, Deref, DerefMut, Reflect)]
#[reflect(Component)]
pub struct AabbIntersections(pub Vec<Entity>);
/// True if the rigid body should store [`AabbIntersections`].
type StoreAabbIntersections = bool;
/// True if the rigid body hasn't moved.
type IsBodyInactive = bool;
/// Entities with [`ColliderAabb`]s sorted along an axis by their extents.
#[derive(Resource, Default)]
struct AabbIntervals(
Vec<(
Entity,
ColliderParent,
ColliderAabb,
CollisionLayers,
StoreAabbIntersections,
IsBodyInactive,
)>,
);
impl MapEntities for AabbIntervals {
fn map_entities<M: EntityMapper>(&mut self, entity_mapper: &mut M) {
for interval in self.0.iter_mut() {
interval.0 = entity_mapper.map_entity(interval.0);
}
}
}
/// Updates [`AabbIntervals`] to keep them in sync with the [`ColliderAabb`]s.
#[allow(clippy::type_complexity)]
fn update_aabb_intervals(
aabbs: Query<(
&ColliderAabb,
Option<&ColliderParent>,
Option<&CollisionLayers>,
Has<AabbIntersections>,
Has<Sleeping>,
)>,
rbs: Query<&RigidBody>,
mut intervals: ResMut<AabbIntervals>,
) {
intervals.0.retain_mut(
|(collider_entity, collider_parent, aabb, layers, store_intersections, is_inactive)| {
if let Ok((new_aabb, new_parent, new_layers, new_store_intersections, is_sleeping)) =
aabbs.get(*collider_entity)
{
if !new_aabb.min.is_finite() || !new_aabb.max.is_finite() {
return false;
}
*aabb = *new_aabb;
*collider_parent = new_parent.map_or(ColliderParent(*collider_entity), |p| *p);
*layers = new_layers.map_or(CollisionLayers::default(), |layers| *layers);
let is_static =
new_parent.is_some_and(|p| rbs.get(p.get()).is_ok_and(RigidBody::is_static));
*is_inactive = is_static || is_sleeping;
*store_intersections = new_store_intersections;
true
} else {
false
}
},
);
}
/// Adds new [`ColliderAabb`]s to [`AabbIntervals`].
#[allow(clippy::type_complexity)]
fn add_new_aabb_intervals(
aabbs: Query<
(
Entity,
Option<&ColliderParent>,
&ColliderAabb,
Option<&RigidBody>,
Option<&CollisionLayers>,
Has<AabbIntersections>,
),
Added<ColliderAabb>,
>,
mut intervals: ResMut<AabbIntervals>,
) {
let aabbs = aabbs
.iter()
.map(|(ent, parent, aabb, rb, layers, store_intersections)| {
(
ent,
parent.map_or(ColliderParent(ent), |p| *p),
*aabb,
// Default to treating collider as immovable/static for filtering unnecessary collision checks
layers.map_or(CollisionLayers::default(), |layers| *layers),
rb.map_or(false, |rb| rb.is_static()),
store_intersections,
)
});
intervals.0.extend(aabbs);
}
/// Collects bodies that are potentially colliding.
fn collect_collision_pairs(
intervals: ResMut<AabbIntervals>,
mut broad_collision_pairs: ResMut<BroadCollisionPairs>,
mut aabb_intersection_query: Query<&mut AabbIntersections>,
) {
for mut intersections in &mut aabb_intersection_query {
intersections.clear();
}
sweep_and_prune(
intervals,
&mut broad_collision_pairs.0,
&mut aabb_intersection_query,
);
}
/// Sorts the entities by their minimum extents along an axis and collects the entity pairs that have intersecting AABBs.
///
/// Sweep and prune exploits temporal coherence, as bodies are unlikely to move significantly between two simulation steps. Insertion sort is used, as it is good at sorting nearly sorted lists efficiently.
fn sweep_and_prune(
mut intervals: ResMut<AabbIntervals>,
broad_collision_pairs: &mut Vec<(Entity, Entity)>,
aabb_intersection_query: &mut Query<&mut AabbIntersections>,
) {
// Sort bodies along the x-axis using insertion sort, a sorting algorithm great for sorting nearly sorted lists.
insertion_sort(&mut intervals.0, |a, b| a.2.min.x > b.2.min.x);
// Clear broad phase collisions from previous iteration.
broad_collision_pairs.clear();
// Find potential collisions by checking for AABB intersections along all axes.
for (i, (ent1, parent1, aabb1, layers1, store_intersections1, inactive1)) in
intervals.0.iter().enumerate()
{
for (ent2, parent2, aabb2, layers2, store_intersections2, inactive2) in
intervals.0.iter().skip(i + 1)
{
// x doesn't intersect; check this first so we can discard as soon as possible
if aabb2.min.x > aabb1.max.x {
break;
}
// No collisions between bodies that haven't moved or colliders with incompatible layers or colliders with the same parent
if (*inactive1 && *inactive2) || !layers1.interacts_with(*layers2) || parent1 == parent2
{
continue;
}
// y doesn't intersect
if aabb1.min.y > aabb2.max.y || aabb1.max.y < aabb2.min.y {
continue;
}
#[cfg(feature = "3d")]
// z doesn't intersect
if aabb1.min.z > aabb2.max.z || aabb1.max.z < aabb2.min.z {
continue;
}
if *ent1 < *ent2 {
broad_collision_pairs.push((*ent1, *ent2));
} else {
broad_collision_pairs.push((*ent2, *ent1));
}
if *store_intersections1 {
if let Ok(mut intersections) = aabb_intersection_query.get_mut(*ent1) {
intersections.push(*ent2);
}
}
if *store_intersections2 {
if let Ok(mut intersections) = aabb_intersection_query.get_mut(*ent2) {
intersections.push(*ent1);
}
}
}
}
}
/// Sorts a list iteratively using comparisons. In an ascending sort order, when a smaller value is encountered, it is moved lower in the list until it is larger than the item before it.
///
/// This is relatively slow for large lists, but very efficient in cases where the list is already mostly sorted.
fn insertion_sort<T>(items: &mut [T], comparison: fn(&T, &T) -> bool) {
for i in 1..items.len() {
let mut j = i;
while j > 0 && comparison(&items[j - 1], &items[j]) {
items.swap(j - 1, j);
j -= 1;
}
}
}