avian3d/collision/broad_phase/
bvh_broad_phase.rs

1use core::marker::PhantomData;
2
3use crate::{
4    collider_tree::{
5        ColliderTree, ColliderTreeProxy, ColliderTreeProxyFlags, ColliderTreeProxyKey,
6        ColliderTreeType, ColliderTrees, MovedProxies, ProxyId,
7    },
8    collision::{CollisionDiagnostics, contact_types::ContactEdgeFlags},
9    data_structures::pair_key::PairKey,
10    dynamics::solver::joint_graph::JointGraph,
11    prelude::*,
12};
13use bevy::{
14    ecs::system::{StaticSystemParam, SystemParamItem},
15    prelude::*,
16    tasks::{ComputeTaskPool, ParallelSlice},
17};
18use obvhs::aabb::Aabb;
19
20/// A [broad phase](crate::collision::broad_phase) plugin that uses a [Bounding Volume Hierarchy (BVH)][BVH]
21/// to efficiently find pairs of colliders with overlapping AABBs.
22///
23/// The BVH structures are provided by [`ColliderTrees`].
24///
25/// [`CollisionHooks`] can be provided with generics to apply custom filtering for collision pairs.
26///
27/// See the [`broad_phase`](crate::collision::broad_phase) module for more information
28/// and an example of creating a custom broad phase plugin.
29///
30/// [BVH]: https://en.wikipedia.org/wiki/Bounding_volume_hierarchy
31pub struct BvhBroadPhasePlugin<H: CollisionHooks = ()>(PhantomData<H>);
32
33impl<H: CollisionHooks> Default for BvhBroadPhasePlugin<H> {
34    fn default() -> Self {
35        Self(PhantomData)
36    }
37}
38
39impl<H: CollisionHooks + 'static> Plugin for BvhBroadPhasePlugin<H>
40where
41    for<'w, 's> SystemParamItem<'w, 's, H>: CollisionHooks,
42{
43    fn build(&self, app: &mut App) {
44        app.add_systems(
45            PhysicsSchedule,
46            collect_collision_pairs::<H>.in_set(BroadPhaseSystems::CollectCollisions),
47        );
48    }
49}
50
51fn collect_collision_pairs<H: CollisionHooks>(
52    trees: ResMut<ColliderTrees>,
53    moved_proxies: Res<MovedProxies>,
54    hooks: StaticSystemParam<H>,
55    par_commands: ParallelCommands,
56    mut contact_graph: ResMut<ContactGraph>,
57    joint_graph: Res<JointGraph>,
58    mut diagnostics: ResMut<CollisionDiagnostics>,
59) where
60    for<'w, 's> SystemParamItem<'w, 's, H>: CollisionHooks,
61{
62    let start = crate::utils::Instant::now();
63
64    let hooks = hooks.into_inner();
65    let mut broad_collision_pairs = Vec::<(ColliderTreeProxyKey, ColliderTreeProxyKey)>::new();
66
67    // Perform tree queries for all moving proxies.
68    // TODO. We could iterate moved proxies of each tree separately
69    //       to get rid of tree lookups and body type checks.
70    //       May not be worth it though?
71    let pairs = moved_proxies.proxies().par_splat_map(
72        ComputeTaskPool::get(),
73        None,
74        |_chunk_index, proxies| {
75            let mut pairs = Vec::new();
76
77            par_commands.command_scope(|mut commands| {
78                for proxy_key1 in proxies {
79                    let proxy_id1 = proxy_key1.id();
80                    let proxy_type1 = proxy_key1.tree_type();
81
82                    // Get the proxy from its appropriate tree.
83                    let tree = trees.tree_for_type(proxy_type1);
84                    let proxy1 = tree.get_proxy(proxy_key1.id()).unwrap();
85
86                    let Some(proxy_aabb1) = tree.get_proxy_aabb(proxy_id1) else {
87                        continue;
88                    };
89
90                    // Query dynamic tree.
91                    query_tree(
92                        &trees.dynamic_tree,
93                        ColliderTreeType::Dynamic,
94                        *proxy_key1,
95                        proxy_id1,
96                        proxy_type1,
97                        proxy_aabb1,
98                        proxy1,
99                        &moved_proxies,
100                        &hooks,
101                        &mut commands,
102                        &contact_graph,
103                        &joint_graph,
104                        &mut pairs,
105                    );
106
107                    // Query kinematic tree.
108                    query_tree(
109                        &trees.kinematic_tree,
110                        ColliderTreeType::Kinematic,
111                        *proxy_key1,
112                        proxy_id1,
113                        proxy_type1,
114                        proxy_aabb1,
115                        proxy1,
116                        &moved_proxies,
117                        &hooks,
118                        &mut commands,
119                        &contact_graph,
120                        &joint_graph,
121                        &mut pairs,
122                    );
123
124                    // Skip static-static body collisions unless sensors or standalone colliders are involved.
125                    if proxy_type1 != ColliderTreeType::Static || proxy1.is_sensor() {
126                        // Query static tree.
127                        query_tree(
128                            &trees.static_tree,
129                            ColliderTreeType::Static,
130                            *proxy_key1,
131                            proxy_id1,
132                            proxy_type1,
133                            proxy_aabb1,
134                            proxy1,
135                            &moved_proxies,
136                            &hooks,
137                            &mut commands,
138                            &contact_graph,
139                            &joint_graph,
140                            &mut pairs,
141                        );
142                    }
143
144                    // Query standalone tree (colliders with no body).
145                    query_tree(
146                        &trees.standalone_tree,
147                        ColliderTreeType::Standalone,
148                        *proxy_key1,
149                        proxy_id1,
150                        proxy_type1,
151                        proxy_aabb1,
152                        proxy1,
153                        &moved_proxies,
154                        &hooks,
155                        &mut commands,
156                        &contact_graph,
157                        &joint_graph,
158                        &mut pairs,
159                    );
160                }
161            });
162
163            pairs
164        },
165    );
166
167    // Drain the pairs into a single vector.
168    for mut chunk in pairs {
169        broad_collision_pairs.append(&mut chunk);
170    }
171
172    // Add the found collision pairs to the contact graph.
173    for (proxy_key1, proxy_key2) in broad_collision_pairs {
174        let proxy1 = trees.get_proxy(proxy_key1).unwrap();
175        let proxy2 = trees.get_proxy(proxy_key2).unwrap();
176
177        let mut contact_edge = ContactEdge::new(proxy1.collider, proxy2.collider);
178        contact_edge.body1 = proxy1.body;
179        contact_edge.body2 = proxy2.body;
180
181        let flags_union = proxy1.flags.union(proxy2.flags);
182
183        // Contact event flags
184        contact_edge.flags.set(
185            ContactEdgeFlags::CONTACT_EVENTS,
186            flags_union.contains(ColliderTreeProxyFlags::CONTACT_EVENTS),
187        );
188
189        contact_graph.add_edge_with(contact_edge, |contact_pair| {
190            contact_pair.body1 = proxy1.body;
191            contact_pair.body2 = proxy2.body;
192
193            contact_pair.flags.set(
194                ContactPairFlags::MODIFY_CONTACTS,
195                flags_union.contains(ColliderTreeProxyFlags::MODIFY_CONTACTS),
196            );
197
198            contact_pair.flags.set(
199                ContactPairFlags::GENERATE_CONSTRAINTS,
200                !flags_union.contains(ColliderTreeProxyFlags::BODY_DISABLED)
201                    && !flags_union.contains(ColliderTreeProxyFlags::SENSOR),
202            );
203        });
204    }
205
206    diagnostics.broad_phase += start.elapsed();
207}
208
209#[inline]
210fn query_tree(
211    tree: &ColliderTree,
212    tree_type: ColliderTreeType,
213    proxy_key1: ColliderTreeProxyKey,
214    proxy_id1: ProxyId,
215    proxy_type1: ColliderTreeType,
216    proxy_aabb1: Aabb,
217    proxy1: &ColliderTreeProxy,
218    moved_proxies: &MovedProxies,
219    hooks: &impl CollisionHooks,
220    commands: &mut Commands,
221    contact_graph: &ContactGraph,
222    joint_graph: &JointGraph,
223    pairs: &mut Vec<(ColliderTreeProxyKey, ColliderTreeProxyKey)>,
224) {
225    tree.bvh.aabb_traverse(proxy_aabb1, |bvh, node_index| {
226        let node = &bvh.nodes[node_index as usize];
227        let start = node.first_index as usize;
228        let end = start + node.prim_count as usize;
229
230        for node_primitive_index in start..end {
231            let proxy_id2 = ProxyId::new(tree.bvh.primitive_indices[node_primitive_index]);
232            let proxy_key2 = ColliderTreeProxyKey::new(proxy_id2, tree_type);
233
234            // Skip self-collision.
235            if proxy_key1 == proxy_key2 {
236                continue;
237            }
238
239            let proxy2 = tree.get_proxy(proxy_id2).unwrap();
240
241            // Avoid duplicate pairs for moving proxies.
242            //
243            // Most of the time, only dynamic and kinematic bodies will be moving, but static bodies
244            // can also be in the move set (ex: when spawned or teleported).
245            //
246            // If sensors are involved, we handle the pair here regardless of movement,
247            // just to be safe. Otherwise a static sensor colliding with a static body might be missed.
248            // TODO: There's probably a better way to handle sensors.
249            let proxy1_greater = ((tree_type as u8) < (proxy_type1 as u8))
250                || (tree_type == proxy_type1 && proxy_id2.id() < proxy_id1.id());
251            if proxy1_greater && moved_proxies.contains(proxy_key2) && !proxy1.is_sensor() {
252                // Both proxies are moving, so the other query will handle this pair.
253                continue;
254            }
255
256            // Check if the layers interact.
257            if !proxy1.layers.interacts_with(proxy2.layers) {
258                continue;
259            }
260
261            // No collisions between colliders on the same body.
262            if proxy1.body == proxy2.body {
263                continue;
264            }
265
266            let entity1 = proxy1.collider;
267            let entity2 = proxy2.collider;
268
269            // Avoid duplicate pairs.
270            let pair_key = PairKey::new(entity1.index_u32(), entity2.index_u32());
271            if contact_graph.contains_key(&pair_key) {
272                continue;
273            }
274
275            // Check if a joint disables contacts between the two bodies.
276            if let Some(body1) = proxy1.body
277                && let Some(body2) = proxy2.body
278                && joint_graph
279                    .joints_between(body1, body2)
280                    .any(|edge| edge.collision_disabled)
281            {
282                continue;
283            }
284
285            // Apply user-defined filter.
286            if proxy1
287                .flags
288                .union(proxy2.flags)
289                .contains(ColliderTreeProxyFlags::CUSTOM_FILTER)
290            {
291                let should_collide = hooks.filter_pairs(entity1, entity2, commands);
292                if !should_collide {
293                    continue;
294                }
295            }
296
297            pairs.push((proxy_key1, proxy_key2));
298        }
299
300        true
301    });
302}
avian3d/collision/broad_phase/bvh_broad_phase.rs

avian3d/collision/broad_phase/
bvh_broad_phase.rs
