parry2d/partitioning/bvh/
bvh_ploc_build.rs

1use super::bvh_tree::{BvhNodeIndex, BvhNodeWide};
2use super::BvhNode;
3use crate::bounding_volume::{Aabb, BoundingVolume};
4use crate::math::Real;
5use crate::partitioning::Bvh;
6use crate::utils::morton;
7use alloc::{vec, vec::Vec};
8
9impl Bvh {
10    pub(crate) fn rebuild_range_ploc(&mut self, target_node_id: u32, leaves: &mut Vec<BvhNode>) {
11        // Compute the centroids aabb.
12        let aabb = Aabb::from_points(leaves.iter().map(|l| l.center()));
13        let inv_extents = aabb.extents().map(|e| 1.0 / e);
14
15        // Sort the leaves.
16        leaves.sort_by_cached_key(|node| {
17            let center = (node.center() - aabb.mins) * (inv_extents);
18            morton::morton_encode_u64_unorm(center)
19        });
20
21        // Build all the levels.
22        const SEARCH_RADIUS: usize = 16;
23        let mut merge_candidates = vec![usize::MAX; leaves.len()];
24        let mut next_leaves = Vec::with_capacity(leaves.len());
25
26        while leaves.len() > 1 {
27            // Find merge candidates.
28            for i in 0..leaves.len() {
29                let mut best_sah = Real::MAX;
30                let mut best_candidate = usize::MAX;
31                for k in i.saturating_sub(SEARCH_RADIUS)..=(i + SEARCH_RADIUS).min(leaves.len() - 1)
32                {
33                    if k != i {
34                        let node_i = &leaves[i];
35                        let node_k = &leaves[k];
36                        let sah = node_i
37                            .aabb()
38                            .merged(&node_k.aabb())
39                            .half_area_or_perimeter();
40                        if sah < best_sah {
41                            best_sah = sah;
42                            best_candidate = k;
43                        }
44                    }
45                }
46                merge_candidates[i] = best_candidate;
47            }
48
49            // Group nodes with matching merge candidates.
50            for i in 0..leaves.len() {
51                let k = merge_candidates[i];
52                if merge_candidates[k] == i {
53                    if i > k {
54                        continue;
55                    }
56
57                    // Merge nodes k and i:
58                    let left = leaves[i];
59                    let right = leaves[k];
60                    let wide_node = BvhNodeWide { left, right };
61
62                    let id = if leaves.len() == 2 {
63                        self.nodes[target_node_id as usize] = wide_node;
64                        target_node_id
65                    } else {
66                        let id = self.nodes.len() as u32;
67                        let parent = wide_node.merged(id);
68                        self.nodes.push(wide_node);
69                        self.parents.push(BvhNodeIndex::default()); // Will be set when the parent is created.
70                        next_leaves.push(parent);
71                        id
72                    };
73
74                    if left.is_leaf() {
75                        self.leaf_node_indices[left.children as usize] = BvhNodeIndex::left(id);
76                    } else {
77                        self.parents[left.children as usize] = BvhNodeIndex::left(id);
78                    }
79                    if right.is_leaf() {
80                        self.leaf_node_indices[right.children as usize] = BvhNodeIndex::right(id);
81                    } else {
82                        self.parents[right.children as usize] = BvhNodeIndex::right(id);
83                    }
84                } else {
85                    next_leaves.push(leaves[i]);
86                }
87            }
88
89            // Swap for next step.
90            core::mem::swap(leaves, &mut next_leaves);
91            next_leaves.clear();
92        }
93    }
94}
parry2d/partitioning/bvh/bvh_ploc_build.rs

parry2d/partitioning/bvh/
bvh_ploc_build.rs
