parry3d/partitioning/bvh/bvh_refit.rs
1use super::bvh_tree::{BvhNodeIndex, BvhNodeVec, BvhNodeWide};
2use super::{Bvh, BvhNode, BvhWorkspace};
3use crate::utils::VecMap;
4use alloc::vec::Vec;
5
6impl Bvh {
7 /// Updates the BVH's internal node AABBs after leaf changes.
8 ///
9 /// Refitting ensures that every internal node's AABB tightly encloses the AABBs of its
10 /// children. This operation is essential after updating leaf positions with
11 /// [`insert_or_update_partially`] and is much faster than rebuilding the entire tree.
12 ///
13 /// In addition to updating AABBs, this method:
14 /// - Reorders nodes in depth-first order for better cache locality during queries
15 /// - Ensures leaf counts on each node are correct
16 /// - Propagates change flags from leaves to ancestors (for change detection)
17 ///
18 /// # When to Use
19 ///
20 /// Call `refit` after:
21 /// - Bulk updates with [`insert_or_update_partially`]
22 /// - Any operation that modifies leaf AABBs without updating ancestor nodes
23 /// - When you want to optimize tree layout for better query performance
24 ///
25 /// **Don't call `refit` after**:
26 /// - Regular [`insert`] calls (they already update ancestors)
27 /// - [`remove`] calls (they already maintain tree validity)
28 ///
29 /// # Arguments
30 ///
31 /// * `workspace` - A reusable workspace to avoid allocations. Can be shared across
32 /// multiple BVH operations for better performance.
33 ///
34 /// # Performance
35 ///
36 /// - **Time**: O(n) where n is the number of nodes
37 /// - **Space**: O(n) temporary storage in workspace
38 /// - Much faster than rebuilding the tree from scratch
39 /// - Essential for maintaining good query performance in dynamic scenes
40 ///
41 /// # Examples
42 ///
43 /// ## After bulk updates
44 ///
45 /// ```
46 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
47 /// use parry3d::partitioning::{Bvh, BvhWorkspace};
48 /// use parry3d::bounding_volume::Aabb;
49 /// use nalgebra::Point3;
50 ///
51 /// let mut bvh = Bvh::new();
52 /// let mut workspace = BvhWorkspace::default();
53 ///
54 /// // Insert initial objects
55 /// for i in 0..100 {
56 /// let aabb = Aabb::new(
57 /// Point3::new(i as f32, 0.0, 0.0),
58 /// Point3::new(i as f32 + 1.0, 1.0, 1.0)
59 /// );
60 /// bvh.insert(aabb, i);
61 /// }
62 ///
63 /// // Update all objects without tree propagation (faster)
64 /// for i in 0..100 {
65 /// let offset = 0.1;
66 /// let aabb = Aabb::new(
67 /// Point3::new(i as f32 + offset, 0.0, 0.0),
68 /// Point3::new(i as f32 + 1.0 + offset, 1.0, 1.0)
69 /// );
70 /// bvh.insert_or_update_partially(aabb, i, 0.0);
71 /// }
72 ///
73 /// // Now update the tree in one efficient pass
74 /// bvh.refit(&mut workspace);
75 /// # }
76 /// ```
77 ///
78 /// ## In a game loop
79 ///
80 /// ```
81 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
82 /// use parry3d::partitioning::{Bvh, BvhWorkspace};
83 /// use parry3d::bounding_volume::Aabb;
84 /// use nalgebra::Point3;
85 ///
86 /// let mut bvh = Bvh::new();
87 /// let mut workspace = BvhWorkspace::default();
88 ///
89 /// // Game initialization - add objects
90 /// for i in 0..1000 {
91 /// let aabb = Aabb::new(
92 /// Point3::new(i as f32, 0.0, 0.0),
93 /// Point3::new(i as f32 + 1.0, 1.0, 1.0)
94 /// );
95 /// bvh.insert(aabb, i);
96 /// }
97 ///
98 /// // Game loop - update objects each frame
99 /// for frame in 0..100 {
100 /// // Update physics, AI, etc.
101 /// for i in 0..1000 {
102 /// let time = frame as f32 * 0.016; // ~60 FPS
103 /// let pos = time.sin() * 10.0;
104 /// let aabb = Aabb::new(
105 /// Point3::new(i as f32 + pos, 0.0, 0.0),
106 /// Point3::new(i as f32 + pos + 1.0, 1.0, 1.0)
107 /// );
108 /// bvh.insert_or_update_partially(aabb, i, 0.0);
109 /// }
110 ///
111 /// // Refit once per frame for all updates
112 /// bvh.refit(&mut workspace);
113 ///
114 /// // Now perform collision detection queries...
115 /// }
116 /// # }
117 /// ```
118 ///
119 /// ## With change detection margin
120 ///
121 /// ```
122 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
123 /// use parry3d::partitioning::{Bvh, BvhWorkspace};
124 /// use parry3d::bounding_volume::Aabb;
125 /// use nalgebra::Point3;
126 ///
127 /// let mut bvh = Bvh::new();
128 /// let mut workspace = BvhWorkspace::default();
129 ///
130 /// // Add an object
131 /// let aabb = Aabb::new(Point3::origin(), Point3::new(1.0, 1.0, 1.0));
132 /// bvh.insert(aabb, 0);
133 ///
134 /// // Update with a margin - tree won't update if movement is small
135 /// let margin = 0.5;
136 /// let new_aabb = Aabb::new(Point3::new(0.1, 0.0, 0.0), Point3::new(1.1, 1.0, 1.0));
137 /// bvh.insert_or_update_partially(new_aabb, 0, margin);
138 ///
139 /// // Refit propagates the change detection flags
140 /// bvh.refit(&mut workspace);
141 /// # }
142 /// ```
143 ///
144 /// # Comparison with `refit_without_opt`
145 ///
146 /// This method reorganizes the tree in memory for better cache performance.
147 /// If you only need to update AABBs without reordering, use [`refit_without_opt`](Self::refit_without_opt)
148 /// which is faster but doesn't improve memory layout.
149 ///
150 /// # Notes
151 ///
152 /// - Reuses the provided `workspace` to avoid allocations
153 /// - Safe to call even if no leaves were modified (just reorganizes tree)
154 /// - Does not change the tree's topology, only AABBs and layout
155 /// - Call this before [`optimize_incremental`] for best results
156 ///
157 /// # See Also
158 ///
159 /// - [`insert_or_update_partially`](Bvh::insert_or_update_partially) - Update leaves
160 /// without propagation
161 /// - [`refit_without_opt`](Self::refit_without_opt) - Faster refit without memory
162 /// reorganization
163 /// - [`optimize_incremental`](Bvh::optimize_incremental) - Improve tree quality
164 /// - [`BvhWorkspace`] - Reusable workspace for operations
165 ///
166 /// [`insert_or_update_partially`]: Bvh::insert_or_update_partially
167 /// [`insert`]: Bvh::insert
168 /// [`remove`]: Bvh::remove
169 /// [`optimize_incremental`]: Bvh::optimize_incremental
170 pub fn refit(&mut self, workspace: &mut BvhWorkspace) {
171 Self::refit_buffers(
172 &mut self.nodes,
173 &mut workspace.refit_tmp,
174 &mut self.leaf_node_indices,
175 &mut self.parents,
176 );
177
178 // Swap the old nodes with the refitted ones.
179 core::mem::swap(&mut self.nodes, &mut workspace.refit_tmp);
180 }
181
182 pub(super) fn refit_buffers(
183 source: &mut BvhNodeVec,
184 target: &mut BvhNodeVec,
185 leaf_data: &mut VecMap<BvhNodeIndex>,
186 parents: &mut Vec<BvhNodeIndex>,
187 ) {
188 if source.is_empty() {
189 target.clear();
190 parents.clear();
191 } else if source[0].leaf_count() <= 2 {
192 // No actual refit to apply, just copy the root wide node.
193 target.clear();
194 parents.clear();
195 target.push(source[0]);
196 target[0].left.data.resolve_pending_change();
197 if target[0].right.leaf_count() > 0 {
198 target[0].right.data.resolve_pending_change();
199 }
200 parents.push(BvhNodeIndex::default());
201 } else if !source.is_empty() && source[0].leaf_count() > 2 {
202 target.resize(
203 source.len(),
204 BvhNodeWide {
205 left: BvhNode::zeros(),
206 right: BvhNode::zeros(),
207 },
208 );
209
210 let mut len = 1;
211
212 // Start with a special case for the root then recurse.
213 let left_child_id = source[0].left.children;
214 let right_child_id = source[0].right.children;
215
216 if !source[0].left.is_leaf() {
217 Self::refit_recurse(
218 source,
219 target,
220 leaf_data,
221 parents,
222 left_child_id,
223 &mut len,
224 BvhNodeIndex::left(0),
225 );
226 } else {
227 target[0].left = source[0].left;
228 target[0].left.data.resolve_pending_change();
229
230 // NOTE: updating the leaf_data shouldn’t be needed here since the root
231 // is always at 0.
232 // *self.leaf_data.get_mut_unknown_gen(left_child_id).unwrap() = BvhNodeIndex::left(0);
233 }
234
235 if !source[0].right.is_leaf() {
236 Self::refit_recurse(
237 source,
238 target,
239 leaf_data,
240 parents,
241 right_child_id,
242 &mut len,
243 BvhNodeIndex::right(0),
244 );
245 } else {
246 target[0].right = source[0].right;
247 target[0].right.data.resolve_pending_change();
248 // NOTE: updating the leaf_data shouldn’t be needed here since the root
249 // is always at 0.
250 // *self.leaf_data.get_mut_unknown_gen(right_child_id).unwrap() = BvhNodeIndex::right(0);
251 }
252
253 source.truncate(len as usize);
254 target.truncate(len as usize);
255 parents.truncate(len as usize);
256 }
257 }
258
259 fn refit_recurse(
260 source: &BvhNodeVec,
261 target: &mut BvhNodeVec,
262 leaf_data: &mut VecMap<BvhNodeIndex>,
263 parents: &mut [BvhNodeIndex],
264 source_id: u32,
265 target_id_mut: &mut u32,
266 parent: BvhNodeIndex,
267 ) {
268 let target_id = *target_id_mut;
269 *target_id_mut += 1;
270
271 let node = &source[source_id as usize];
272 let left_is_leaf = node.left.is_leaf();
273 let right_is_leaf = node.right.is_leaf();
274 let left_source_id = node.left.children;
275 let right_source_id = node.right.children;
276
277 if !left_is_leaf {
278 Self::refit_recurse(
279 source,
280 target,
281 leaf_data,
282 parents,
283 left_source_id,
284 target_id_mut,
285 BvhNodeIndex::left(target_id),
286 );
287 } else {
288 let node = &source[source_id as usize];
289 target[target_id as usize].left = node.left;
290 target[target_id as usize]
291 .left
292 .data
293 .resolve_pending_change();
294 leaf_data[node.left.children as usize] = BvhNodeIndex::left(target_id);
295 }
296
297 if !right_is_leaf {
298 Self::refit_recurse(
299 source,
300 target,
301 leaf_data,
302 parents,
303 right_source_id,
304 target_id_mut,
305 BvhNodeIndex::right(target_id),
306 );
307 } else {
308 let node = &source[source_id as usize];
309 target[target_id as usize].right = node.right;
310 target[target_id as usize]
311 .right
312 .data
313 .resolve_pending_change();
314 leaf_data[node.right.children as usize] = BvhNodeIndex::right(target_id);
315 }
316
317 let node = &target[target_id as usize];
318 target[parent] = node.left.merged(&node.right, target_id);
319 parents[target_id as usize] = parent;
320 }
321
322 /// Similar to [`Self::refit`] but without any optimization of the internal node storage layout.
323 ///
324 /// This can be faster than [`Self::refit`] but doesn’t reorder node to be more cache-efficient
325 /// on tree traversals.
326 pub fn refit_without_opt(&mut self) {
327 if self.leaf_count() > 2 {
328 let root = &self.nodes[0];
329 let left = root.left.children;
330 let right = root.right.children;
331 let left_is_leaf = root.left.is_leaf();
332 let right_is_leaf = root.right.is_leaf();
333
334 if !left_is_leaf {
335 self.recurse_refit_without_opt(left, BvhNodeIndex::left(0));
336 }
337 if !right_is_leaf {
338 self.recurse_refit_without_opt(right, BvhNodeIndex::right(0));
339 }
340 }
341 }
342
343 fn recurse_refit_without_opt(&mut self, node_id: u32, parent: BvhNodeIndex) {
344 let node = &self.nodes[node_id as usize];
345 let left = &node.left;
346 let right = &node.right;
347 let left_is_leaf = left.is_leaf();
348 let right_is_leaf = right.is_leaf();
349 let left_children = left.children;
350 let right_children = right.children;
351
352 if !left_is_leaf {
353 self.recurse_refit_without_opt(left_children, BvhNodeIndex::left(node_id));
354 } else {
355 self.nodes[node_id as usize]
356 .left
357 .data
358 .resolve_pending_change();
359 }
360 if !right_is_leaf {
361 self.recurse_refit_without_opt(right_children, BvhNodeIndex::right(node_id));
362 } else {
363 self.nodes[node_id as usize]
364 .right
365 .data
366 .resolve_pending_change();
367 }
368
369 let node = &self.nodes[node_id as usize];
370 let left = &node.left;
371 let right = &node.right;
372 let merged = left.merged(right, node_id);
373
374 self.nodes[parent] = merged;
375 }
376}