ncollide3d/partitioning/
bvh.rs

1use crate::partitioning::{
2    BVTNodeId, BestFirstVisitStatus, BestFirstVisitor, DBVTNodeId, SimultaneousVisitor,
3    VisitStatus, Visitor, BVT, DBVT,
4};
5use na::RealField;
6use std::cmp::Ordering;
7use std::collections::BinaryHeap;
8
9/// Trait implemented by Bounding Volume Hierarchy.
10pub trait BVH<T, BV> {
11    /// Type of a node identifiers on this BVH.
12    type Node: Copy;
13
14    /// The root of the BVH.
15    fn root(&self) -> Option<Self::Node>;
16    /// The number of children of the given node.
17    fn num_children(&self, node: Self::Node) -> usize;
18    /// The i-th child of the given node.
19    fn child(&self, i: usize, node: Self::Node) -> Self::Node;
20    /// The bounding volume and data contained by the given node.
21    fn content(&self, node: Self::Node) -> (&BV, Option<&T>);
22
23    /// Traverses this BVH using a visitor.
24    fn visit(&self, visitor: &mut impl Visitor<T, BV>) {
25        // FIXME: find a way to avoid the allocation.
26        let mut stack = Vec::new();
27
28        if let Some(root) = self.root() {
29            stack.push(root);
30
31            while let Some(node) = stack.pop() {
32                let content = self.content(node);
33
34                match visitor.visit(content.0, content.1) {
35                    VisitStatus::Continue => {
36                        for i in 0..self.num_children(node) {
37                            stack.push(self.child(i, node))
38                        }
39                    }
40                    VisitStatus::ExitEarly => return,
41                    VisitStatus::Stop => {}
42                }
43            }
44        }
45    }
46
47    /// Visits the bounding volume test tree implicitly formed with `other`.
48    fn visit_bvtt(&self, other: &impl BVH<T, BV>, visitor: &mut impl SimultaneousVisitor<T, BV>) {
49        // FIXME: find a way to avoid the allocation.
50        let mut stack = Vec::new();
51
52        if let (Some(root1), Some(root2)) = (self.root(), other.root()) {
53            stack.push((root1, root2));
54
55            while let Some((node1, node2)) = stack.pop() {
56                let content1 = self.content(node1);
57                let content2 = other.content(node2);
58
59                match visitor.visit(content1.0, content1.1, content2.0, content2.1) {
60                    VisitStatus::Continue => {
61                        let nchild1 = self.num_children(node1);
62                        let nchild2 = other.num_children(node2);
63
64                        match (nchild1, nchild2) {
65                            (0, 0) => {}
66                            (0, _) => {
67                                for j in 0..nchild2 {
68                                    let n2 = other.child(j, node2);
69                                    stack.push((node1, n2))
70                                }
71                            }
72                            (_, 0) => {
73                                for i in 0..nchild1 {
74                                    let n1 = self.child(i, node1);
75                                    stack.push((n1, node2))
76                                }
77                            }
78                            (_, _) => {
79                                for i in 0..nchild1 {
80                                    let n1 = self.child(i, node1);
81
82                                    for j in 0..nchild2 {
83                                        let n2 = other.child(j, node2);
84                                        stack.push((n1, n2))
85                                    }
86                                }
87                            }
88                        }
89                    }
90                    VisitStatus::ExitEarly => return,
91                    VisitStatus::Stop => {}
92                }
93            }
94        }
95    }
96
97    /// Performs a best-first-search on the BVH.
98    ///
99    /// Returns the content of the leaf with the smallest associated cost, and a result of
100    /// user-defined type.
101    fn best_first_search<N, BFS>(&self, visitor: &mut BFS) -> Option<(Self::Node, BFS::Result)>
102    where
103        N: RealField + Copy,
104        BFS: BestFirstVisitor<N, T, BV>,
105    {
106        let mut queue: BinaryHeap<WeightedValue<N, Self::Node>> = BinaryHeap::new();
107        // The lowest cost collision with actual scene geometry.
108        let mut best_cost = N::max_value().unwrap();
109        let mut best_result = None;
110
111        if let Some(root) = self.root() {
112            let (root_bv, root_data) = self.content(root);
113
114            match visitor.visit(best_cost, root_bv, root_data) {
115                BestFirstVisitStatus::Continue { cost, result } => {
116                    // Root may be a leaf node
117                    if let Some(res) = result {
118                        best_cost = cost;
119                        best_result = Some((root, res));
120                    }
121
122                    queue.push(WeightedValue::new(root, -cost))
123                }
124                BestFirstVisitStatus::Stop => return None,
125                BestFirstVisitStatus::ExitEarly(result) => return result.map(|res| (root, res)),
126            }
127
128            while let Some(entry) = queue.pop() {
129                if -entry.cost >= best_cost {
130                    // No BV left in the tree that has a lower cost than best_result
131                    break; // Solution found.
132                }
133
134                for i in 0..self.num_children(entry.value) {
135                    let child = self.child(i, entry.value);
136                    let (child_bv, child_data) = self.content(child);
137
138                    match visitor.visit(best_cost, child_bv, child_data) {
139                        BestFirstVisitStatus::Continue { cost, result } => {
140                            if cost < best_cost {
141                                if result.is_some() {
142                                    // This is the nearest collision so far
143                                    best_cost = cost;
144                                    best_result = result.map(|res| (child, res));
145                                }
146                                // BV may have a child with lower cost, evaluate it next.
147                                queue.push(WeightedValue::new(child, -cost))
148                            }
149                        }
150                        BestFirstVisitStatus::ExitEarly(result) => {
151                            return result.map(|res| (child, res)).or(best_result)
152                        }
153                        BestFirstVisitStatus::Stop => {}
154                    }
155                }
156            }
157        }
158
159        best_result
160    }
161}
162
163/// An enum grouping references to all the BVH implementations on ncollide.
164#[derive(Copy, Clone)]
165pub enum BVHImpl<'a, N: 'a + RealField + Copy, T: 'a, BV: 'a> {
166    /// A static binary bounding volume tree.
167    BVT(&'a BVT<T, BV>),
168    /// A dynamic binary bounding volume tree.
169    DBVT(&'a DBVT<N, T, BV>),
170}
171
172/// The Id of a node of a BVH.
173pub enum BVHNodeId {
174    // The Id of a BVT.
175    BVTNodeId(BVTNodeId),
176    // The Id of a DBVT.
177    DBVTNodeId(DBVTNodeId),
178}
179
180impl<'a, N: RealField + Copy, T, BV> BVHImpl<'a, N, T, BV> {
181    /// Gets the underlying reference to a BVT, or panics if this is not a `BVTImpl::BVT`.
182    #[inline]
183    pub fn unwrap_bvt(self) -> &'a BVT<T, BV> {
184        match self {
185            BVHImpl::BVT(bvt) => bvt,
186            _ => panic!("This BVTImpl is not a BVT."),
187        }
188    }
189
190    /// Gets the underlying reference to a DBVT, or panics if this is not a `BVTImpl::DBVT`.
191    #[inline]
192    pub fn unwrap_dbvt(self) -> &'a DBVT<N, T, BV> {
193        match self {
194            BVHImpl::DBVT(dbvt) => dbvt,
195            _ => panic!("This BVTImpl is not a DBVT."),
196        }
197    }
198
199    /// Traverses this tree using a visitor.
200    pub fn visit(self, visitor: &mut impl Visitor<T, BV>) {
201        match self {
202            BVHImpl::BVT(bvt) => bvt.visit(visitor),
203            BVHImpl::DBVT(dbvt) => dbvt.visit(visitor),
204        }
205    }
206
207    /// Visits the bounding volume traversal tree implicitly formed with `other`.
208    pub fn visit_bvtt(
209        self,
210        other: BVHImpl<N, T, BV>,
211        visitor: &mut impl SimultaneousVisitor<T, BV>,
212    ) {
213        // Note: the dispatch on each pair is split into two method to avoid
214        // having to write a manually a match over each possible pair.
215        match other {
216            BVHImpl::BVT(bvh2) => self.visit_bvtt_dispatch(bvh2, visitor),
217            BVHImpl::DBVT(bvh2) => self.visit_bvtt_dispatch(bvh2, visitor),
218        }
219    }
220
221    fn visit_bvtt_dispatch(
222        self,
223        bvh2: &impl BVH<T, BV>,
224        visitor: &mut impl SimultaneousVisitor<T, BV>,
225    ) {
226        match self {
227            BVHImpl::BVT(bvh1) => bvh1.visit_bvtt(bvh2, visitor),
228            BVHImpl::DBVT(bvh1) => bvh1.visit_bvtt(bvh2, visitor),
229        }
230    }
231
232    /// Performs a best-fist-search on the tree.
233    ///
234    /// Returns the content of the leaf with the smallest associated cost, and a result of
235    /// user-defined type.
236    pub fn best_first_search<BFS>(self, visitor: &mut BFS) -> Option<(BVHNodeId, BFS::Result)>
237    where
238        BFS: BestFirstVisitor<N, T, BV>,
239    {
240        match self {
241            BVHImpl::BVT(bvt) => bvt
242                .best_first_search(visitor)
243                .map(|res| (BVHNodeId::BVTNodeId(res.0), res.1)),
244            BVHImpl::DBVT(dbvt) => dbvt
245                .best_first_search(visitor)
246                .map(|res| (BVHNodeId::DBVTNodeId(res.0), res.1)),
247        }
248    }
249}
250
251struct WeightedValue<N, T> {
252    pub value: T,
253    pub cost: N,
254}
255
256impl<N, T> WeightedValue<N, T> {
257    /// Creates a new reference packed with a cost value.
258    #[inline]
259    pub fn new(value: T, cost: N) -> WeightedValue<N, T> {
260        WeightedValue {
261            value: value,
262            cost: cost,
263        }
264    }
265}
266
267impl<N: PartialEq, T> PartialEq for WeightedValue<N, T> {
268    #[inline]
269    fn eq(&self, other: &WeightedValue<N, T>) -> bool {
270        self.cost.eq(&other.cost)
271    }
272}
273
274impl<N: PartialEq, T> Eq for WeightedValue<N, T> {}
275
276impl<N: PartialOrd, T> PartialOrd for WeightedValue<N, T> {
277    #[inline]
278    fn partial_cmp(&self, other: &WeightedValue<N, T>) -> Option<Ordering> {
279        self.cost.partial_cmp(&other.cost)
280    }
281}
282
283impl<N: PartialOrd, T> Ord for WeightedValue<N, T> {
284    #[inline]
285    fn cmp(&self, other: &WeightedValue<N, T>) -> Ordering {
286        if self.cost < other.cost {
287            Ordering::Less
288        } else if self.cost > other.cost {
289            Ordering::Greater
290        } else {
291            Ordering::Equal
292        }
293    }
294}
ncollide3d/partitioning/bvh.rs

ncollide3d/partitioning/
bvh.rs
