1use crate::bounding_volume::BoundingVolume;
4use crate::math::{Point, DIM};
5use crate::partitioning::BVH;
6use crate::utils;
7use simba::scalar::RealField;
8use std::collections::VecDeque;
9use std::iter;
10use std::usize;
11
12#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
14#[derive(Clone)]
15pub struct BVT<T, BV> {
16 root: BVTNodeId,
17 internals: Vec<BVTInternal<BV>>,
18 leaves: Vec<BVTLeaf<T, BV>>,
19 deformation_timestamp: usize,
23 deformation_infos: Vec<BVTDeformationInfo>,
24 parents_to_update: VecDeque<usize>,
26}
27
28#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
30#[derive(Copy, Clone, Hash, PartialEq, Eq, Debug)]
31pub enum BVTNodeId {
32 Internal(usize),
34 Leaf(usize),
36}
37
38#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
39#[derive(Clone)]
40struct BVTInternal<BV> {
41 bounding_volume: BV,
42 right: BVTNodeId,
43 left: BVTNodeId,
44}
45
46#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
48#[derive(Clone)]
49pub struct BVTLeaf<T, BV> {
50 bounding_volume: BV,
51 data: T,
52}
53
54impl<T, BV> BVTLeaf<T, BV> {
55 #[inline]
57 pub fn bounding_volume(&self) -> &BV {
58 &self.bounding_volume
59 }
60
61 #[inline]
63 pub fn data(&self) -> &T {
64 &self.data
65 }
66}
67
68#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
69#[derive(Clone)]
70struct BVTDeformationInfo {
71 parent: usize,
72 timestamp: usize,
73}
74
75pub enum BinaryPartition<T, BV> {
77 Part(T),
79 Parts(Vec<(T, BV)>, Vec<(T, BV)>),
81}
82
83impl<T, BV> BVT<T, BV> {
84 #[deprecated(note = "please use `from_partitioning` instead")]
86 pub fn new_with_partitioning<F: FnMut(usize, Vec<(T, BV)>) -> (BV, BinaryPartition<T, BV>)>(
87 elements: Vec<(T, BV)>,
88 partitioning: &mut F,
89 ) -> BVT<T, BV> {
90 Self::from_partitioning(elements, partitioning)
91 }
92
93 pub fn from_partitioning(
96 elements: Vec<(T, BV)>,
97 partitioning: &mut impl FnMut(usize, Vec<(T, BV)>) -> (BV, BinaryPartition<T, BV>),
98 ) -> BVT<T, BV> {
99 if elements.len() == 0 {
100 BVT {
101 root: BVTNodeId::Leaf(0),
102 internals: Vec::new(),
103 leaves: Vec::new(),
104 deformation_timestamp: 1,
105 deformation_infos: Vec::new(),
106 parents_to_update: VecDeque::new(),
107 }
108 } else {
109 let mut internals = Vec::new();
110 let mut leaves = Vec::new();
111 let root =
112 Self::_from_partitioning(0, elements, &mut internals, &mut leaves, partitioning);
113 internals.shrink_to_fit();
114 leaves.shrink_to_fit();
115
116 BVT {
117 root,
118 internals,
119 leaves,
120 deformation_timestamp: 1,
121 deformation_infos: Vec::new(),
122 parents_to_update: VecDeque::new(),
123 }
124 }
125 }
126
127 #[inline]
129 pub fn leaves(&self) -> &[BVTLeaf<T, BV>] {
130 &self.leaves
131 }
132
133 #[inline]
135 pub fn leaf(&self, i: usize) -> &BVTLeaf<T, BV> {
136 &self.leaves[i]
137 }
138
139 pub fn root_bounding_volume(&self) -> Option<&BV> {
141 if self.leaves.is_empty() {
142 return None;
143 }
144
145 match self.root {
146 BVTNodeId::Leaf(i) => Some(&self.leaves[i].bounding_volume),
147 BVTNodeId::Internal(i) => Some(&self.internals[i].bounding_volume),
148 }
149 }
150
151 pub fn set_leaf_bounding_volume<N: RealField + Copy>(
159 &mut self,
160 i: usize,
161 bv: BV,
162 refit_now: bool,
163 ) where
164 BV: BoundingVolume<N>,
165 {
166 self.init_deformation_infos();
167 self.leaves[i].bounding_volume = bv;
168
169 if refit_now {
170 let mut curr = self.deformation_infos[self.internals.len() + i].parent;
171
172 while curr != usize::max_value() {
173 let new_bv = match (self.internals[curr].left, self.internals[curr].right) {
174 (BVTNodeId::Internal(i), BVTNodeId::Internal(j)) => self.internals[i]
175 .bounding_volume
176 .merged(&self.internals[j].bounding_volume),
177 (BVTNodeId::Internal(i), BVTNodeId::Leaf(j)) => self.internals[i]
178 .bounding_volume
179 .merged(&self.leaves[j].bounding_volume),
180 (BVTNodeId::Leaf(i), BVTNodeId::Internal(j)) => self.leaves[i]
181 .bounding_volume
182 .merged(&self.internals[j].bounding_volume),
183 (BVTNodeId::Leaf(i), BVTNodeId::Leaf(j)) => self.leaves[i]
184 .bounding_volume
185 .merged(&self.leaves[j].bounding_volume),
186 };
187 self.internals[curr].bounding_volume = new_bv;
188 curr = self.deformation_infos[curr].parent;
189 }
190 } else {
191 if self.leaves.len() != 1 {
192 self.parents_to_update
193 .push_back(self.deformation_infos[self.internals.len() + i].parent)
194 }
195 }
196 }
197
198 pub fn refit<N: RealField + Copy>(&mut self, margin: N)
207 where
208 BV: BoundingVolume<N>,
209 {
210 assert!(margin >= N::zero(), "Cannot set a negative margin.");
211
212 self.deformation_timestamp += 1;
213
214 while let Some(curr) = self.parents_to_update.pop_front() {
215 let infos = &mut self.deformation_infos[curr];
216 if infos.timestamp < self.deformation_timestamp {
217 infos.timestamp = self.deformation_timestamp;
219
220 let mut new_bv = match (self.internals[curr].left, self.internals[curr].right) {
221 (BVTNodeId::Internal(i), BVTNodeId::Internal(j)) => self.internals[i]
222 .bounding_volume
223 .merged(&self.internals[j].bounding_volume),
224 (BVTNodeId::Internal(i), BVTNodeId::Leaf(j)) => self.internals[i]
225 .bounding_volume
226 .merged(&self.leaves[j].bounding_volume),
227 (BVTNodeId::Leaf(i), BVTNodeId::Internal(j)) => self.leaves[i]
228 .bounding_volume
229 .merged(&self.internals[j].bounding_volume),
230 (BVTNodeId::Leaf(i), BVTNodeId::Leaf(j)) => self.leaves[i]
231 .bounding_volume
232 .merged(&self.leaves[j].bounding_volume),
233 };
234
235 if !self.internals[curr].bounding_volume.contains(&new_bv) {
236 if !margin.is_zero() {
237 new_bv.loosen(margin)
238 }
239
240 self.internals[curr].bounding_volume = new_bv;
241
242 if infos.parent != usize::max_value() {
243 self.parents_to_update.push_back(infos.parent);
245 }
246 }
247 }
248 }
249 }
250
251 fn init_deformation_infos(&mut self) {
252 if self.deformation_infos.is_empty() {
253 self.deformation_infos = iter::repeat(BVTDeformationInfo {
254 parent: usize::max_value(),
255 timestamp: 0,
256 })
257 .take(self.internals.len() + self.leaves.len())
258 .collect();
259
260 for (i, internal) in self.internals.iter().enumerate() {
261 match internal.left {
262 BVTNodeId::Internal(j) => self.deformation_infos[j].parent = i,
263 BVTNodeId::Leaf(j) => {
264 self.deformation_infos[self.internals.len() + j].parent = i
265 }
266 }
267
268 match internal.right {
269 BVTNodeId::Internal(j) => self.deformation_infos[j].parent = i,
270 BVTNodeId::Leaf(j) => {
271 self.deformation_infos[self.internals.len() + j].parent = i
272 }
273 }
274 }
275 }
276 }
277}
278
279impl<T, BV> BVT<T, BV> {
280 pub fn new_balanced<N>(leaves: Vec<(T, BV)>) -> BVT<T, BV>
282 where
283 N: RealField + Copy,
284 BV: BoundingVolume<N> + Clone,
285 {
286 BVT::from_partitioning(leaves, &mut Self::median_partitioning)
287 }
288
289 pub fn median_partitioning_with_centers<N, F: FnMut(&T, &BV) -> Point<N>>(
291 depth: usize,
292 leaves: Vec<(T, BV)>,
293 center: &mut F,
294 ) -> (BV, BinaryPartition<T, BV>)
295 where
296 N: RealField + Copy,
297 BV: BoundingVolume<N> + Clone,
298 {
299 if leaves.len() == 0 {
300 panic!("Cannot build a tree without leaves.");
301 } else if leaves.len() == 1 {
302 let (b, bv) = leaves.into_iter().next().unwrap();
303 (bv, BinaryPartition::Part(b))
304 } else {
305 let sep_axis = depth % DIM;
306
307 let mut median = Vec::new();
309
310 for l in leaves.iter() {
311 let c = (*center)(&l.0, &l.1);
312 median.push(c[sep_axis]);
313 }
314
315 let median = utils::median(&mut median[..]);
316
317 let mut right = Vec::new();
319 let mut left = Vec::new();
320 let mut bounding_bounding_volume = leaves[0].1.clone();
321
322 let mut insert_left = false;
323
324 for (b, bv) in leaves.into_iter() {
325 bounding_bounding_volume.merge(&bv);
326
327 let pos = (*center)(&b, &bv)[sep_axis];
328
329 if pos < median || (pos == median && insert_left) {
330 left.push((b, bv));
331 insert_left = false;
332 } else {
333 right.push((b, bv));
334 insert_left = true;
335 }
336 }
337
338 if left.len() == 0 {
340 left.push(right.pop().unwrap());
341 } else if right.len() == 0 {
342 right.push(left.pop().unwrap());
343 }
344
345 (
346 bounding_bounding_volume,
347 BinaryPartition::Parts(left, right),
348 )
349 }
350 }
351
352 pub fn median_partitioning<N>(
354 depth: usize,
355 leaves: Vec<(T, BV)>,
356 ) -> (BV, BinaryPartition<T, BV>)
357 where
358 N: RealField + Copy,
359 BV: BoundingVolume<N> + Clone,
360 {
361 Self::median_partitioning_with_centers(depth, leaves, &mut |_, bv| bv.center())
362 }
363
364 fn _from_partitioning<F: FnMut(usize, Vec<(T, BV)>) -> (BV, BinaryPartition<T, BV>)>(
365 depth: usize,
366 leaves: Vec<(T, BV)>,
367 out_internals: &mut Vec<BVTInternal<BV>>,
368 out_leaves: &mut Vec<BVTLeaf<T, BV>>,
369 partitioning: &mut F,
370 ) -> BVTNodeId {
371 let (bv, partitions) = partitioning(depth, leaves);
372
373 match partitions {
374 BinaryPartition::Part(b) => {
375 out_leaves.push(BVTLeaf {
376 bounding_volume: bv,
377 data: b,
378 });
379 BVTNodeId::Leaf(out_leaves.len() - 1)
380 }
381 BinaryPartition::Parts(left, right) => {
382 let left = Self::_from_partitioning(
383 depth + 1,
384 left,
385 out_internals,
386 out_leaves,
387 partitioning,
388 );
389 let right = Self::_from_partitioning(
390 depth + 1,
391 right,
392 out_internals,
393 out_leaves,
394 partitioning,
395 );
396 out_internals.push(BVTInternal {
397 bounding_volume: bv,
398 left,
399 right,
400 });
401 BVTNodeId::Internal(out_internals.len() - 1)
402 }
403 }
404 }
405}
406
407impl<'a, T, BV> BVH<T, BV> for BVT<T, BV> {
408 type Node = BVTNodeId;
409
410 fn root(&self) -> Option<Self::Node> {
411 if self.leaves.len() != 0 {
412 Some(self.root)
413 } else {
414 None
415 }
416 }
417
418 fn num_children(&self, node: Self::Node) -> usize {
419 match node {
420 BVTNodeId::Internal(_) => 2,
421 BVTNodeId::Leaf(_) => 0,
422 }
423 }
424
425 fn child(&self, i: usize, node: Self::Node) -> Self::Node {
426 match node {
427 BVTNodeId::Internal(node_id) => {
428 if i == 0 {
429 self.internals[node_id].left
430 } else {
431 self.internals[node_id].right
432 }
433 }
434 BVTNodeId::Leaf(_) => panic!("DBVT child index out of bounds."),
435 }
436 }
437
438 fn content(&self, node: Self::Node) -> (&BV, Option<&T>) {
439 match node {
440 BVTNodeId::Internal(i) => {
441 let node = &self.internals[i];
442 (&node.bounding_volume, None)
443 }
444 BVTNodeId::Leaf(i) => {
445 let node = &self.leaves[i];
446 (&node.bounding_volume, Some(&node.data))
447 }
448 }
449 }
450}