1use crate::bounding_volume::BoundingVolume;
2use crate::math::Point;
3use crate::partitioning::BVH;
4use na::{self, RealField};
5use slab::Slab;
6use std::ops::Index;
7
8#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)]
9pub struct DBVTLeafId(usize);
11
12impl DBVTLeafId {
13 #[inline]
15 pub fn new_invalid() -> Self {
16 DBVTLeafId(usize::max_value())
17 }
18
19 #[inline]
21 pub fn is_invalid(&self) -> bool {
22 let DBVTLeafId(val) = *self;
23 val == usize::max_value()
24 }
25}
26
27#[derive(Copy, Clone, Debug)]
28enum UpdateStatus {
29 NeedsShrink,
30 UpToDate,
31}
32
33#[derive(Copy, Clone, Debug, Hash)]
34enum DBVTInternalId {
35 RightChildOf(usize),
36 LeftChildOf(usize),
37 Root,
38}
39
40#[derive(Copy, Clone, Debug, Hash)]
42pub enum DBVTNodeId {
43 Leaf(usize),
45 Internal(usize),
47}
48
49#[derive(Clone)]
51pub struct DBVT<N: RealField + Copy, T, BV> {
52 root: DBVTNodeId,
53 leaves: Slab<DBVTLeaf<N, T, BV>>,
54 internals: Slab<DBVTInternal<N, BV>>,
55}
56
57#[derive(Clone)]
59pub struct DBVTLeaf<N: RealField + Copy, T, BV> {
60 pub bounding_volume: BV,
62 pub center: Point<N>,
64 pub data: T,
66 parent: DBVTInternalId,
68}
69
70#[derive(Clone)]
72struct DBVTInternal<N: RealField + Copy, BV> {
73 bounding_volume: BV,
75 center: Point<N>,
77 left: DBVTNodeId,
79 right: DBVTNodeId,
81 parent: DBVTInternalId,
83
84 state: UpdateStatus,
85}
86
87impl<N: RealField + Copy, T, BV: BoundingVolume<N>> DBVTLeaf<N, T, BV> {
88 pub fn new(bounding_volume: BV, data: T) -> DBVTLeaf<N, T, BV> {
90 DBVTLeaf {
91 center: bounding_volume.center(),
92 bounding_volume: bounding_volume,
93 data: data,
94 parent: DBVTInternalId::Root,
95 }
96 }
97
98 pub fn is_root(&self) -> bool {
100 match self.parent {
101 DBVTInternalId::Root => true,
102 _ => false,
103 }
104 }
105}
106
107impl<N: RealField + Copy, BV: BoundingVolume<N>> DBVTInternal<N, BV> {
108 fn new(
110 bounding_volume: BV,
111 parent: DBVTInternalId,
112 left: DBVTNodeId,
113 right: DBVTNodeId,
114 ) -> DBVTInternal<N, BV> {
115 DBVTInternal {
116 center: bounding_volume.center(),
117 bounding_volume: bounding_volume,
118 left: left,
119 right: right,
120 parent: parent,
121 state: UpdateStatus::UpToDate,
122 }
123 }
124}
125
126impl<N: RealField + Copy, T, BV: BoundingVolume<N>> DBVT<N, T, BV> {
127 pub fn new() -> DBVT<N, T, BV> {
129 DBVT {
130 root: DBVTNodeId::Leaf(0),
131 leaves: Slab::new(),
132 internals: Slab::new(),
133 }
134 }
135
136 #[inline]
140 pub fn root_bounding_volume(&self) -> Option<&BV> {
141 if self.leaves.len() == 0 {
142 return None;
143 }
144
145 match self.root {
146 DBVTNodeId::Leaf(i) => Some(&self.leaves[i].bounding_volume),
147 DBVTNodeId::Internal(i) => Some(&self.internals[i].bounding_volume),
148 }
149 }
150
151 #[inline]
153 pub fn is_empty(&self) -> bool {
154 self.leaves.is_empty()
155 }
156
157 pub fn insert(&mut self, leaf: DBVTLeaf<N, T, BV>) -> DBVTLeafId {
159 if self.is_empty() {
160 let new_id = self.leaves.insert(leaf);
161 self.leaves[new_id].parent = DBVTInternalId::Root;
162 self.root = DBVTNodeId::Leaf(new_id);
163
164 return DBVTLeafId(new_id);
165 }
166
167 match self.root {
168 DBVTNodeId::Internal(_) => {
169 let mut curr = self.root;
170
171 loop {
172 match curr {
173 DBVTNodeId::Internal(id) => {
174 let (left, right) = {
176 let node = &mut self.internals[id];
177 node.bounding_volume.merge(&leaf.bounding_volume);
178 (node.left, node.right)
179 };
180
181 let dist1 = match left {
182 DBVTNodeId::Leaf(l) => {
183 na::distance_squared(&self.leaves[l].center, &leaf.center)
184 }
185 DBVTNodeId::Internal(i) => {
186 na::distance_squared(&self.internals[i].center, &leaf.center)
187 }
188 };
189
190 let dist2 = match right {
191 DBVTNodeId::Leaf(l) => {
192 na::distance_squared(&self.leaves[l].center, &leaf.center)
193 }
194 DBVTNodeId::Internal(i) => {
195 na::distance_squared(&self.internals[i].center, &leaf.center)
196 }
197 };
198
199 curr = if dist1 < dist2 { left } else { right };
200 }
201 DBVTNodeId::Leaf(id) => {
202 let parent_bv = self.leaves[id]
203 .bounding_volume
204 .merged(&leaf.bounding_volume);
205 let grand_parent = self.leaves[id].parent;
206
207 let new_id = self.leaves.insert(leaf);
208 let parent = DBVTInternal::new(
209 parent_bv,
210 grand_parent,
211 curr,
212 DBVTNodeId::Leaf(new_id),
213 );
214 let parent_id = self.internals.insert(parent);
215 self.leaves[id].parent = DBVTInternalId::LeftChildOf(parent_id);
216 self.leaves[new_id].parent = DBVTInternalId::RightChildOf(parent_id);
217
218 match grand_parent {
219 DBVTInternalId::LeftChildOf(pp) => {
220 self.internals[pp].left = DBVTNodeId::Internal(parent_id)
221 }
222 DBVTInternalId::RightChildOf(pp) => {
223 self.internals[pp].right = DBVTNodeId::Internal(parent_id)
224 }
225 _ => unreachable!(),
226 }
227
228 break DBVTLeafId(new_id);
229 }
230 }
231 }
232 }
233 DBVTNodeId::Leaf(id) => {
234 let new_id = self.leaves.insert(leaf);
235
236 let root_bv = self.leaves[id]
238 .bounding_volume
239 .merged(&self.leaves[new_id].bounding_volume);
240 let root = DBVTInternal::new(
241 root_bv,
242 DBVTInternalId::Root,
243 DBVTNodeId::Leaf(id),
244 DBVTNodeId::Leaf(new_id),
245 );
246
247 let root_id = self.internals.insert(root);
248 self.leaves[id].parent = DBVTInternalId::LeftChildOf(root_id);
249 self.leaves[new_id].parent = DBVTInternalId::RightChildOf(root_id);
250 self.root = DBVTNodeId::Internal(root_id);
251
252 DBVTLeafId(new_id)
253 }
254 }
255 }
256
257 pub fn remove(&mut self, leaf_id: DBVTLeafId) -> DBVTLeaf<N, T, BV> {
261 let DBVTLeafId(leaf_id) = leaf_id;
262 let leaf = self.leaves.remove(leaf_id);
263
264 if !leaf.is_root() {
265 let p;
266 let other;
267
268 match leaf.parent {
269 DBVTInternalId::RightChildOf(parent) => {
270 other = self.internals[parent].left;
271 p = parent;
272 }
273 DBVTInternalId::LeftChildOf(parent) => {
274 other = self.internals[parent].right;
275 p = parent;
276 }
277 DBVTInternalId::Root => unreachable!(),
278 }
279
280 match self.internals[p].parent {
281 DBVTInternalId::RightChildOf(pp) => {
282 match other {
283 DBVTNodeId::Internal(id) => {
284 self.internals[id].parent = DBVTInternalId::RightChildOf(pp)
285 }
286 DBVTNodeId::Leaf(id) => {
287 self.leaves[id].parent = DBVTInternalId::RightChildOf(pp)
288 }
289 }
290
291 self.internals[pp].right = other;
292 self.internals[pp].state = UpdateStatus::NeedsShrink;
293 }
294 DBVTInternalId::LeftChildOf(pp) => {
295 match other {
296 DBVTNodeId::Internal(id) => {
297 self.internals[id].parent = DBVTInternalId::LeftChildOf(pp)
298 }
299 DBVTNodeId::Leaf(id) => {
300 self.leaves[id].parent = DBVTInternalId::LeftChildOf(pp)
301 }
302 }
303
304 self.internals[pp].left = other;
305 self.internals[pp].state = UpdateStatus::NeedsShrink;
306 }
307 DBVTInternalId::Root => {
308 match other {
310 DBVTNodeId::Leaf(id) => self.leaves[id].parent = DBVTInternalId::Root,
311 DBVTNodeId::Internal(id) => {
312 self.internals[id].parent = DBVTInternalId::Root
313 }
314 }
315
316 self.root = other;
317 }
318 }
319
320 let _ = self.internals.remove(p);
321 } else {
322 self.leaves.clear();
324 self.internals.clear();
325 }
326
327 leaf
328 }
329
330 #[inline]
332 pub fn get(&self, DBVTLeafId(id): DBVTLeafId) -> Option<&DBVTLeaf<N, T, BV>> {
333 self.leaves.get(id)
334 }
335}
336
337impl<N: RealField + Copy, T, BV> Index<DBVTLeafId> for DBVT<N, T, BV> {
338 type Output = DBVTLeaf<N, T, BV>;
339
340 #[inline]
341 fn index(&self, DBVTLeafId(id): DBVTLeafId) -> &Self::Output {
342 &self.leaves[id]
343 }
344}
345
346impl<'a, N: RealField + Copy, T, BV> BVH<T, BV> for DBVT<N, T, BV> {
347 type Node = DBVTNodeId;
348
349 fn root(&self) -> Option<Self::Node> {
350 if self.leaves.len() != 0 {
351 Some(self.root)
352 } else {
353 None
354 }
355 }
356
357 fn num_children(&self, node: Self::Node) -> usize {
358 match node {
359 DBVTNodeId::Internal(_) => 2,
360 DBVTNodeId::Leaf(_) => 0,
361 }
362 }
363
364 fn child(&self, i: usize, node: Self::Node) -> Self::Node {
365 match node {
366 DBVTNodeId::Internal(node_id) => {
367 if i == 0 {
368 self.internals[node_id].left
369 } else {
370 self.internals[node_id].right
371 }
372 }
373 DBVTNodeId::Leaf(_) => panic!("DBVT child index out of bounds."),
374 }
375 }
376
377 fn content(&self, node: Self::Node) -> (&BV, Option<&T>) {
378 match node {
379 DBVTNodeId::Internal(i) => {
380 let node = &self.internals[i];
381 (&node.bounding_volume, None)
382 }
383 DBVTNodeId::Leaf(i) => {
384 let node = &self.leaves[i];
385 (&node.bounding_volume, Some(&node.data))
386 }
387 }
388 }
389}