parry2d/partitioning/bvh/
bvh_binned_build.rs

1use super::bvh_tree::{BvhBuildStrategy, BvhNodeIndex, BvhNodeWide};
2use super::{Bvh, BvhNode, BvhWorkspace};
3use crate::bounding_volume::{Aabb, BoundingVolume};
4use crate::math::{Real, VectorExt};
5
6impl Bvh {
7    /// Fully rebuilds this BVH using the given strategy.
8    ///
9    /// This rebuilds a BVH with the same leaves, but different intermediate nodes and depth using
10    /// the specified building strategy.
11    pub fn rebuild(&mut self, workspace: &mut BvhWorkspace, strategy: BvhBuildStrategy) {
12        if self.nodes.len() < 2 {
13            // Nothing to rebuild if the tree is empty or only contains the root.
14            // This takes care of the case where we have a partial root too.
15            return;
16        }
17
18        workspace.rebuild_leaves.clear();
19        for node in self.nodes.iter() {
20            if node.left.is_leaf() {
21                workspace.rebuild_leaves.push(node.left);
22            }
23            if node.right.is_leaf() {
24                workspace.rebuild_leaves.push(node.right);
25            }
26        }
27
28        self.nodes.clear();
29        self.parents.clear();
30        self.free_wide_nodes.clear();
31        self.nodes.push(BvhNodeWide::zeros());
32        self.parents.push(BvhNodeIndex::default());
33
34        match strategy {
35            BvhBuildStrategy::Binned => self.rebuild_range_binned(0, &mut workspace.rebuild_leaves),
36            BvhBuildStrategy::Ploc => self.rebuild_range_ploc(0, &mut workspace.rebuild_leaves),
37        }
38    }
39
40    pub(crate) fn rebuild_range_binned(&mut self, target_node_id: u32, leaves: &mut [BvhNode]) {
41        // PERF: calculate an optimal bin count dynamically based on the number of leaves to split?
42        //       The paper suggests (4 + 2 * sqrt(num_leaves).floor()).min(16)
43        const NUM_BINS: usize = 8;
44        const BIN_EPSILON: Real = 1.0e-5;
45
46        let mut bins = [BvhBin::default(); NUM_BINS];
47
48        assert!(leaves.len() > 1);
49
50        let centroid_aabb = Aabb::from_points(leaves.iter().map(|node| node.center()));
51        let bins_axis = centroid_aabb.extents().max_position();
52        let bins_range = [
53            centroid_aabb.mins.vget(bins_axis),
54            centroid_aabb.maxs.vget(bins_axis),
55        ];
56
57        // Compute bins characteristics.
58        let k1 = NUM_BINS as Real * (1.0 - BIN_EPSILON) / (bins_range[1] - bins_range[0]);
59        let k0 = bins_range[0];
60        // NOTE: the clamp guards against degenerate leaf AABBs, whose non-finite or very
61        //       large coordinates would push the bin index outside [0, NUM_BINS - 1]
62        //       (`as usize` saturates).
63        let bin_id_unclamped = |center: Real| (k1 * (center - k0)) as usize;
64        for leaf in &*leaves {
65            let bin_id = bin_id_unclamped(leaf.center().vget(bins_axis)).min(NUM_BINS - 1);
66            let bin = &mut bins[bin_id];
67            bin.aabb.merge(&leaf.aabb());
68            bin.leaf_count += 1;
69        }
70
71        // Select the best splitting plane (there are NUM_BINS - 1 splitting planes) based on SAH.
72        let mut right_merges = bins;
73        let mut right_acc = bins[NUM_BINS - 1];
74
75        for i in 1..NUM_BINS - 1 {
76            right_acc.aabb.merge(&right_merges[NUM_BINS - 1 - i].aabb);
77            right_acc.leaf_count += &right_merges[NUM_BINS - 1 - i].leaf_count;
78            right_merges[NUM_BINS - 1 - i] = right_acc;
79        }
80
81        let mut best_cost = Real::MAX;
82        let mut best_plane = 0;
83        let mut left_merge = bins[0];
84        let mut best_leaf_count = bins[0].leaf_count;
85
86        for i in 0..NUM_BINS - 1 {
87            let right = &right_merges[i + 1];
88
89            let cost = left_merge.aabb.volume() * left_merge.leaf_count as Real
90                + right.aabb.volume() * right.leaf_count as Real;
91            if cost < best_cost {
92                best_cost = cost;
93                best_plane = i;
94                best_leaf_count = left_merge.leaf_count;
95            }
96
97            left_merge.aabb.merge(&bins[i + 1].aabb);
98            left_merge.leaf_count += bins[i + 1].leaf_count;
99        }
100
101        // With the splitting plane selected, sort & split the leaves in place.
102        let mut mid = best_leaf_count as usize;
103
104        // In degenerate cases where all the node end up on the same bin,
105        // just split the range in two.
106        if mid == 0 || mid == leaves.len() {
107            mid = leaves.len() / 2;
108        } else {
109            // Sort in-place.
110            // TODO PERF: try with using teh leaves_tmp instead of in-place sorting.
111            let bin = |leaves: &mut [BvhNode], id: usize| {
112                let node = &leaves[id];
113                bin_id_unclamped(node.center().vget(bins_axis)).min(NUM_BINS - 1)
114            };
115
116            let mut left_id = 0;
117            let mut right_id = mid;
118
119            'outer: while left_id != mid && right_id != leaves.len() {
120                while bin(leaves, left_id) <= best_plane {
121                    left_id += 1;
122
123                    if left_id == mid {
124                        break 'outer;
125                    }
126                }
127
128                while bin(leaves, right_id) > best_plane {
129                    right_id += 1;
130
131                    if right_id == leaves.len() {
132                        break 'outer;
133                    }
134                }
135
136                leaves.swap(left_id, right_id);
137                left_id += 1;
138                right_id += 1;
139            }
140        }
141
142        // Recurse.
143        let (left_leaves, right_leaves) = leaves.split_at_mut(mid);
144
145        assert!(!left_leaves.is_empty() && !right_leaves.is_empty());
146
147        // Recurse.
148        if left_leaves.len() == 1 {
149            let target = &mut self.nodes[target_node_id as usize];
150            target.left = left_leaves[0];
151
152            if target.left.is_leaf() {
153                self.leaf_node_indices[target.left.children as usize] =
154                    BvhNodeIndex::left(target_node_id);
155            } else {
156                self.parents[target.left.children as usize] = BvhNodeIndex::left(target_node_id);
157            }
158        } else {
159            let left_id = self.nodes.len() as u32;
160            self.nodes.push(BvhNodeWide::zeros());
161            self.parents.push(BvhNodeIndex::left(target_node_id));
162            self.rebuild_range_binned(left_id, left_leaves);
163            self.nodes[target_node_id as usize].left = self.nodes[left_id as usize].merged(left_id);
164        }
165
166        if right_leaves.len() == 1 {
167            let target = &mut self.nodes[target_node_id as usize];
168            target.right = right_leaves[0];
169
170            if target.right.is_leaf() {
171                self.leaf_node_indices[target.right.children as usize] =
172                    BvhNodeIndex::right(target_node_id);
173            } else {
174                self.parents[target.right.children as usize] = BvhNodeIndex::right(target_node_id);
175            }
176        } else {
177            let right_id = self.nodes.len() as u32;
178            self.nodes.push(BvhNodeWide::zeros());
179            self.parents.push(BvhNodeIndex::right(target_node_id));
180            self.rebuild_range_binned(right_id, right_leaves);
181            self.nodes[target_node_id as usize].right =
182                self.nodes[right_id as usize].merged(right_id);
183        }
184    }
185}
186
187#[derive(Copy, Clone, Debug)]
188struct BvhBin {
189    aabb: Aabb,
190    leaf_count: u32,
191}
192
193impl Default for BvhBin {
194    fn default() -> Self {
195        Self {
196            aabb: Aabb::new_invalid(),
197            leaf_count: 0,
198        }
199    }
200}
parry2d/partitioning/bvh/bvh_binned_build.rs

parry2d/partitioning/bvh/
bvh_binned_build.rs
