1#[cfg(test)] extern crate rand;
2
3use std::cmp::Ordering;
4use std::iter::{self, FromIterator};
5use std::collections::{BinaryHeap, binary_heap::PeekMut};
6
7#[cfg(test)]
8mod tests;
9
10pub trait BoundingVolume<P> {
11 fn min_corner(&self) -> P;
12 fn max_corner(&self) -> P;
13}
14
15pub trait CmpPoints<A, P> {
16 fn cmp_points(&self, axis: &A, a: &P, b: &P) -> Ordering;
17}
18
19impl<A, P, F> CmpPoints<A, P> for F where F: Fn(&A, &P, &P) -> Ordering {
20 fn cmp_points(&self, axis: &A, a: &P, b: &P) -> Ordering {
21 (self)(axis, a, b)
22 }
23}
24
25pub trait GetBoundingVolume<B, S> {
26 fn bounding_volume(&self, shape: &S) -> B;
27}
28
29impl<B, S, F> GetBoundingVolume<B, S> for F where F: Fn(&S) -> B {
30 fn bounding_volume(&self, shape: &S) -> B {
31 (self)(shape)
32 }
33}
34
35pub trait GetCutPoint<A, P> {
36 fn cut_point<I>(&mut self, cut_axis: &A, points: I) -> Option<P>
37 where I: Iterator<Item = P>;
38}
39
40impl<A, P, F> GetCutPoint<A, P> for F where F: FnMut(&A, &mut Iterator<Item = P>) -> Option<P> {
41 fn cut_point<I>(&mut self, cut_axis: &A, mut points: I) -> Option<P>
42 where I: Iterator<Item = P>
43 {
44 (self)(cut_axis, &mut points)
45 }
46}
47
48pub trait BoundingVolumesCutter<A, P, B, S> {
49 type Error;
50
51 fn cut(&mut self, shape: &S, fragment: &B, cut_axis: &A, cut_point: &P) -> Result<Option<(B, B)>, Self::Error>;
52}
53
54impl<A, P, B, S, F, E> BoundingVolumesCutter<A, P, B, S> for F where F: FnMut(&S, &B, &A, &P) -> Result<Option<(B, B)>, E> {
55 type Error = E;
56
57 fn cut(&mut self, shape: &S, fragment: &B, cut_axis: &A, cut_point: &P) -> Result<Option<(B, B)>, Self::Error> {
58 (self)(shape, fragment, cut_axis, cut_point)
59 }
60}
61
62pub trait DistanceBVCP<A, P, B> {
63 type Dist;
64
65 fn bv_to_cut_point_distance(&mut self, axis: &A, bounding_volume: &B, cut_point: &P) -> Self::Dist;
66}
67
68impl<A, P, B, F, D> DistanceBVCP<A, P, B> for F where F: FnMut(&A, &B, &P) -> D {
69 type Dist = D;
70
71 fn bv_to_cut_point_distance(&mut self, axis: &A, bounding_volume: &B, cut_point: &P) -> Self::Dist {
72 (self)(axis, bounding_volume, cut_point)
73 }
74}
75
76pub trait DistanceBVBV<BA, BB> {
77 type Dist;
78
79 fn bv_to_bv_distance(&mut self, bounding_volume_a: &BA, bounding_volume_b: &BB) -> Self::Dist;
80}
81
82impl<BA, BB, F, D> DistanceBVBV<BA, BB> for F where F: FnMut(&BA, &BB) -> D {
83 type Dist = D;
84
85 fn bv_to_bv_distance(&mut self, bounding_volume_a: &BA, bounding_volume_b: &BB) -> Self::Dist {
86 (self)(bounding_volume_a, bounding_volume_b)
87 }
88}
89
90pub struct KdvTree<A, P, B, S> {
91 axis: Vec<A>,
92 shapes: Vec<S>,
93 root: KdvNode<P, B>,
94}
95
96impl<A, P, B, S> KdvTree<A, P, B, S>
97 where B: BoundingVolume<P>,
98{
99 pub fn build<IA, II, CMF, BVF, CPF, CBF>(
100 axis_it: IA,
101 shapes_it: II,
102 cmp_p: CMF,
103 get_bv: BVF,
104 mut get_cp: CPF,
105 mut cut_bv: CBF,
106 )
107 -> Result<KdvTree<A, P, B, S>, CBF::Error>
108 where IA: IntoIterator<Item = A>,
109 II: IntoIterator<Item = S>,
110 CMF: CmpPoints<A, P>,
111 BVF: GetBoundingVolume<B, S>,
112 CPF: GetCutPoint<A, P>,
113 CBF: BoundingVolumesCutter<A, P, B, S>,
114 {
115 let axis: Vec<_> = axis_it.into_iter().collect();
116 let shapes: Vec<_> = shapes_it.into_iter().collect();
117 let root_shapes: Vec<_> = shapes
118 .iter()
119 .enumerate()
120 .map(|(i, s)| ShapeFragment {
121 bounding_volume: get_bv.bounding_volume(s),
122 shape_id: i,
123 })
124 .collect();
125
126 struct Op<P, B> {
127 node_shapes: Vec<ShapeFragment<B>>,
128 instruction: Instruction<P>,
129 }
130
131 enum Instruction<P> {
132 MakeNode { depth: usize, },
133 AssembleOnlyLeft { cut_point: CutPoint<P>, },
134 AssembleOnlyRight { cut_point: CutPoint<P>, },
135 AssembleBoth { cut_point: CutPoint<P>, },
136 }
137
138 let mut ops_stack = vec![Op { node_shapes: root_shapes, instruction: Instruction::MakeNode { depth: 0, }, }];
139 let mut ret_stack: Vec<KdvNode<P, B>> = vec![];
140 while let Some(op) = ops_stack.pop() {
141 match op {
142 Op { mut node_shapes, instruction: Instruction::MakeNode { depth, }, } => {
143 let cut_axis_index = depth % axis.len();
145 let cut_axis = &axis[cut_axis_index];
146 let maybe_cut_point = get_cp.cut_point(
147 cut_axis,
148 node_shapes
149 .iter()
150 .flat_map(|sf| {
151 let bvol = &sf.bounding_volume;
152 iter::once(bvol.min_corner())
153 .chain(iter::once(bvol.max_corner()))
154 }),
155 );
156
157 if let Some(cut_point) = maybe_cut_point {
158 let mut left_shapes = Vec::new();
160 let mut right_shapes = Vec::new();
161 let mut head = 0;
162 while head < node_shapes.len() {
163 let ShapeFragment { shape_id, bounding_volume, } = node_shapes.swap_remove(head);
164 let owner = shape_owner(&shapes[shape_id], bounding_volume, cut_axis, &cut_point, &cmp_p, &mut cut_bv)?;
165 match owner {
166 ShapeOwner::Me(bounding_volume) => {
167 let tail = node_shapes.len();
168 node_shapes.push(ShapeFragment { shape_id, bounding_volume, });
169 node_shapes.swap(head, tail);
170 head += 1;
171 },
172 ShapeOwner::Left(bounding_volume) =>
173 left_shapes.push(ShapeFragment { shape_id, bounding_volume, }),
174 ShapeOwner::Right(bounding_volume) =>
175 right_shapes.push(ShapeFragment { shape_id, bounding_volume, }),
176 ShapeOwner::Both { left_bvol, right_bvol, } => {
177 left_shapes.push(ShapeFragment { shape_id, bounding_volume: left_bvol, });
178 right_shapes.push(ShapeFragment { shape_id, bounding_volume: right_bvol, });
179 },
180 }
181 }
182 if left_shapes.is_empty() && right_shapes.is_empty() {
184 ret_stack.push(KdvNode {
185 shapes: node_shapes,
186 children: KdvNodeChildren::Missing,
187 });
188 } else if left_shapes.is_empty() {
189 ops_stack.push(Op {
190 node_shapes,
191 instruction: Instruction::AssembleOnlyRight {
192 cut_point: CutPoint { axis_index: cut_axis_index, point: cut_point, },
193 },
194 });
195 ops_stack.push(Op {
196 node_shapes: right_shapes,
197 instruction: Instruction::MakeNode { depth: depth + 1, },
198 });
199 } else if right_shapes.is_empty() {
200 ops_stack.push(Op {
201 node_shapes,
202 instruction: Instruction::AssembleOnlyLeft {
203 cut_point: CutPoint { axis_index: cut_axis_index, point: cut_point, },
204 },
205 });
206 ops_stack.push(Op {
207 node_shapes: left_shapes,
208 instruction: Instruction::MakeNode { depth: depth + 1, },
209 });
210 } else {
211 ops_stack.push(Op {
212 node_shapes,
213 instruction: Instruction::AssembleBoth {
214 cut_point: CutPoint { axis_index: cut_axis_index, point: cut_point, },
215 },
216 });
217 ops_stack.push(Op {
218 node_shapes: left_shapes,
219 instruction: Instruction::MakeNode { depth: depth + 1, },
220 });
221 ops_stack.push(Op {
222 node_shapes: right_shapes,
223 instruction: Instruction::MakeNode { depth: depth + 1, },
224 });
225 }
226 } else {
227 ret_stack.push(KdvNode {
229 shapes: node_shapes,
230 children: KdvNodeChildren::Missing,
231 });
232 }
233 },
234 Op { node_shapes, instruction: Instruction::AssembleOnlyLeft { cut_point, }, } => {
235 let child = ret_stack.pop().map(Box::new).unwrap_or_else(|| unreachable!());
236 ret_stack.push(KdvNode {
237 shapes: node_shapes,
238 children: KdvNodeChildren::OnlyLeft { cut_point, child },
239 });
240 },
241 Op { node_shapes, instruction: Instruction::AssembleOnlyRight { cut_point, }, } => {
242 let child = ret_stack.pop().map(Box::new).unwrap_or_else(|| unreachable!());
243 ret_stack.push(KdvNode {
244 shapes: node_shapes,
245 children: KdvNodeChildren::OnlyRight { cut_point, child, },
246 });
247 },
248 Op { node_shapes, instruction: Instruction::AssembleBoth { cut_point, }, } => {
249 let left = ret_stack.pop().map(Box::new).unwrap_or_else(|| unreachable!());
250 let right = ret_stack.pop().map(Box::new).unwrap_or_else(|| unreachable!());
251 ret_stack.push(KdvNode {
252 shapes: node_shapes,
253 children: KdvNodeChildren::Both { cut_point, left, right },
254 });
255 },
256 }
257 }
258
259 let root = ret_stack.pop().unwrap_or_else(|| unreachable!());
260 assert!(ret_stack.is_empty());
261 Ok(KdvTree { root, axis, shapes, })
262 }
263
264 pub fn intersects<'t, 's, SN, BN, CMF, BVF, CPF, CBF>(
265 &'t self,
266 shape: &'s SN,
267 cmp_p: CMF,
268 get_bv: BVF,
269 get_cp: CPF,
270 cut_bv: CBF,
271 )
272 -> IntersectIter<'t, 's, A, P, S, B, SN, BN, CMF, CPF, CBF>
273 where CMF: CmpPoints<A, P>,
274 BVF: GetBoundingVolume<BN, SN>,
275 CPF: GetCutPoint<A, P>,
276 CBF: BoundingVolumesCutter<A, P, BN, SN>,
277 {
278 IntersectIter {
279 needle: shape,
280 axis: &self.axis,
281 shapes: &self.shapes,
282 queue: vec![TraverseTask::Explore {
283 node: &self.root,
284 needle_fragment: get_bv.bounding_volume(shape),
285 }],
286 cmp_p,
287 get_cp,
288 cut_bv,
289 }
290 }
291
292 pub fn nearest<'t, 's, SN, BN, D, CMF, BVF, CBF, DPF, DVF>(
293 &'t self,
294 shape: &'s SN,
295 cmp_p: CMF,
296 get_bv: BVF,
297 cut_bv: CBF,
298 dist_cp: DPF,
299 dist_bv: DVF,
300 )
301 -> NearestIter<'t, 's, A, P, S, B, SN, BN, D, CMF, CBF, DPF, DVF>
302 where CMF: CmpPoints<A, P>,
303 BVF: GetBoundingVolume<BN, SN>,
304 CBF: BoundingVolumesCutter<A, P, BN, SN>,
305 DPF: DistanceBVCP<A, P, BN, Dist = D>,
306 DVF: DistanceBVBV<B, BN, Dist = D>,
307 D: PartialEq + PartialOrd,
308 {
309 NearestIter {
310 shape,
311 axis: &self.axis,
312 shapes: &self.shapes,
313 nodes_queue: BinaryHeap::from_iter(iter::once(NearestNode {
314 dist: None,
315 node: &self.root,
316 needle_bv: get_bv.bounding_volume(shape),
317 })),
318 neighbours: BinaryHeap::new(),
319 cmp_p,
320 cut_bv,
321 dist_cp,
322 dist_bv,
323 }
324 }
325
326 pub fn iter<'t>(&'t self) -> Iter<'t, P, B, S> {
327 Iter {
328 shapes: &self.shapes,
329 pending: vec![(0, &self.root)],
330 }
331 }
332}
333
334struct ShapeFragment<B> {
335 bounding_volume: B,
336 shape_id: usize,
337}
338
339struct KdvNode<P, B> {
340 shapes: Vec<ShapeFragment<B>>,
341 children: KdvNodeChildren<P, B>,
342}
343
344struct CutPoint<P> {
345 axis_index: usize,
346 point: P,
347}
348
349enum KdvNodeChildren<P, B> {
350 Missing,
351 OnlyLeft { cut_point: CutPoint<P>, child: Box<KdvNode<P, B>>, },
352 OnlyRight { cut_point: CutPoint<P>, child: Box<KdvNode<P, B>>, },
353 Both { cut_point: CutPoint<P>, left: Box<KdvNode<P, B>>, right: Box<KdvNode<P, B>>, },
354}
355
356enum ShapeOwner<B> {
357 Me(B),
358 Left(B),
359 Right(B),
360 Both { left_bvol: B, right_bvol: B, },
361}
362
363fn shape_owner<A, P, B, S, CMF, CBF>(shape: &S, fragment: B, cut_axis: &A, cut_point: &P, cmp_p: &CMF, cut_bv: &mut CBF) ->
364 Result<ShapeOwner<B>, CBF::Error>
365 where B: BoundingVolume<P>,
366 CMF: CmpPoints<A, P>,
367 CBF: BoundingVolumesCutter<A, P, B, S>,
368{
369 let min_corner = fragment.min_corner();
370 let max_corner = fragment.max_corner();
371 Ok(match (cmp_p.cmp_points(&cut_axis, &min_corner, cut_point), cmp_p.cmp_points(&cut_axis, &max_corner, cut_point)) {
372 (Ordering::Less, Ordering::Less) | (Ordering::Less, Ordering::Equal) =>
373 ShapeOwner::Left(fragment),
374 (Ordering::Greater, Ordering::Greater) | (Ordering::Equal, Ordering::Greater) =>
375 ShapeOwner::Right(fragment),
376 _ => if let Some((left_bvol, right_bvol)) = cut_bv.cut(shape, &fragment, cut_axis, cut_point)? {
377 ShapeOwner::Both { left_bvol, right_bvol, }
378 } else {
379 ShapeOwner::Me(fragment)
380 }
381 })
382}
383
384enum TraverseTask<'t, P: 't, BS: 't, BN> {
385 Explore { node: &'t KdvNode<P, BS>, needle_fragment: BN, },
386 Intersect { needle_fragment: BN, shape_fragment: &'t ShapeFragment<BS>, axis_counter: usize, },
387}
388
389pub struct IntersectIter<'t, 's, A: 't, P: 't, SS: 't, BS: 't, SN: 's, BN, CMF, CPF, CBF> {
390 needle: &'s SN,
391 axis: &'t [A],
392 shapes: &'t [SS],
393 queue: Vec<TraverseTask<'t, P, BS, BN>>,
394 cmp_p: CMF,
395 get_cp: CPF,
396 cut_bv: CBF,
397}
398
399#[derive(Clone, PartialEq, Debug)]
400pub struct Intersection<'t, SS: 't, BS: 't, BN> {
401 pub shape: &'t SS,
402 pub shape_fragment: &'t BS,
403 pub needle_fragment: BN,
404}
405
406impl<'t, 's, A, P, SS, BS, SN, BN, CMF, CPF, CBF> Iterator for IntersectIter<'t, 's, A, P, SS, BS, SN, BN, CMF, CPF, CBF>
407 where BS: BoundingVolume<P>,
408 BN: BoundingVolume<P> + Clone,
409 CMF: CmpPoints<A, P>,
410 CPF: GetCutPoint<A, P>,
411 CBF: BoundingVolumesCutter<A, P, BN, SN>,
412{
413 type Item = Result<Intersection<'t, SS, BS, BN>, CBF::Error>;
414
415 fn next(&mut self) -> Option<Self::Item> {
416 'outer: while let Some(task) = self.queue.pop() {
417 match task {
418 TraverseTask::Explore { node, needle_fragment, } => {
419 match node.children {
420 KdvNodeChildren::Missing =>
421 (),
422 KdvNodeChildren::OnlyLeft { ref cut_point, ref child, } => {
423 let cut_axis = &self.axis[cut_point.axis_index];
424 match shape_owner(self.needle, needle_fragment.clone(), cut_axis, &cut_point.point, &self.cmp_p, &mut self.cut_bv) {
425 Ok(ShapeOwner::Me(needle_fragment)) =>
426 self.queue.push(TraverseTask::Explore { node: child, needle_fragment, }),
427 Ok(ShapeOwner::Left(needle_fragment)) =>
428 self.queue.push(TraverseTask::Explore { node: child, needle_fragment, }),
429 Ok(ShapeOwner::Right(..)) =>
430 (),
431 Ok(ShapeOwner::Both { left_bvol: needle_fragment, .. }) =>
432 self.queue.push(TraverseTask::Explore { node: child, needle_fragment, }),
433 Err(error) => {
434 self.queue.clear();
435 return Some(Err(error));
436 },
437 }
438 },
439 KdvNodeChildren::OnlyRight { ref cut_point, ref child, } => {
440 let cut_axis = &self.axis[cut_point.axis_index];
441 match shape_owner(self.needle, needle_fragment.clone(), cut_axis, &cut_point.point, &self.cmp_p, &mut self.cut_bv) {
442 Ok(ShapeOwner::Me(needle_fragment)) =>
443 self.queue.push(TraverseTask::Explore { node: child, needle_fragment, }),
444 Ok(ShapeOwner::Left(..)) =>
445 (),
446 Ok(ShapeOwner::Right(needle_fragment)) =>
447 self.queue.push(TraverseTask::Explore { node: child, needle_fragment, }),
448 Ok(ShapeOwner::Both { right_bvol: needle_fragment, .. }) =>
449 self.queue.push(TraverseTask::Explore { node: child, needle_fragment, }),
450 Err(error) => {
451 self.queue.clear();
452 return Some(Err(error));
453 },
454 }
455 },
456 KdvNodeChildren::Both { ref cut_point, ref left, ref right, } => {
457 let cut_axis = &self.axis[cut_point.axis_index];
458 match shape_owner(self.needle, needle_fragment.clone(), cut_axis, &cut_point.point, &self.cmp_p, &mut self.cut_bv) {
459 Ok(ShapeOwner::Me(fragment)) => {
460 self.queue.push(TraverseTask::Explore { node: left, needle_fragment: fragment.clone(), });
461 self.queue.push(TraverseTask::Explore { node: right, needle_fragment: fragment, });
462 },
463 Ok(ShapeOwner::Left(needle_fragment)) =>
464 self.queue.push(TraverseTask::Explore { node: left, needle_fragment, }),
465 Ok(ShapeOwner::Right(needle_fragment)) =>
466 self.queue.push(TraverseTask::Explore { node: right, needle_fragment, }),
467 Ok(ShapeOwner::Both { left_bvol, right_bvol, }) => {
468 self.queue.push(TraverseTask::Explore { node: left, needle_fragment: left_bvol, });
469 self.queue.push(TraverseTask::Explore { node: right, needle_fragment: right_bvol, });
470 },
471 Err(error) => {
472 self.queue.clear();
473 return Some(Err(error));
474 },
475 }
476 },
477 }
478 for shape_fragment in node.shapes.iter() {
479 self.queue.push(TraverseTask::Intersect {
480 shape_fragment,
481 needle_fragment: needle_fragment.clone(),
482 axis_counter: 0,
483 });
484 }
485 },
486 TraverseTask::Intersect { shape_fragment, needle_fragment, mut axis_counter, } => {
487 let no_intersection = self.axis.iter().any(|axis| {
488 let needle_min = needle_fragment.min_corner();
489 let needle_max = needle_fragment.max_corner();
490 let shape_min = shape_fragment.bounding_volume.min_corner();
491 let shape_max = shape_fragment.bounding_volume.max_corner();
492 (self.cmp_p.cmp_points(axis, &needle_min, &shape_max) == Ordering::Greater ||
493 self.cmp_p.cmp_points(axis, &needle_max, &shape_min) == Ordering::Less)
494 });
495 if no_intersection {
496 continue;
497 }
498 let axis_total = self.axis.len();
499 for _ in 0 .. axis_total {
500 let cut_axis = &self.axis[axis_counter % axis_total];
501 axis_counter += 1;
502 let maybe_cut_point = self.get_cp.cut_point(
503 cut_axis,
504 iter::once(needle_fragment.min_corner())
505 .chain(iter::once(needle_fragment.max_corner())),
506 );
507 if let Some(cut_point) = maybe_cut_point {
508 match self.cut_bv.cut(self.needle, &needle_fragment, cut_axis, &cut_point) {
509 Ok(Some((left_fragment, right_fragment))) => {
510 self.queue.push(TraverseTask::Intersect {
511 shape_fragment: shape_fragment.clone(),
512 needle_fragment: left_fragment,
513 axis_counter,
514 });
515 self.queue.push(TraverseTask::Intersect {
516 shape_fragment: shape_fragment,
517 needle_fragment: right_fragment,
518 axis_counter,
519 });
520 continue 'outer;
521 },
522 Ok(None) =>
523 (),
524 Err(error) => {
525 self.queue.clear();
526 return Some(Err(error));
527 },
528 }
529 }
530 }
531 return Some(Ok(Intersection {
532 shape: &self.shapes[shape_fragment.shape_id],
533 shape_fragment: &shape_fragment.bounding_volume,
534 needle_fragment,
535 }));
536 },
537 }
538 }
539 None
540 }
541}
542
543struct NearestNode<'t, P: 't, B: 't, BN, D> where D: PartialEq + PartialOrd {
544 dist: Option<D>,
545 node: &'t KdvNode<P, B>,
546 needle_bv: BN,
547}
548
549#[derive(Clone, Debug)]
550pub struct NearestShape<'t, B: 't, S: 't, D> {
551 pub dist: D,
552 pub shape: &'t S,
553 pub shape_fragment: &'t B,
554}
555
556pub struct NearestIter<'t, 's, A: 't, P: 't, S: 't, B: 't, SN: 's, BN, D, CMF, CBF, DPF, DVF> where D: PartialEq + PartialOrd {
557 shape: &'s SN,
558 axis: &'t [A],
559 shapes: &'t [S],
560 nodes_queue: BinaryHeap<NearestNode<'t, P, B, BN, D>>,
561 neighbours: BinaryHeap<NearestShape<'t, B, S, D>>,
562 cmp_p: CMF,
563 cut_bv: CBF,
564 dist_cp: DPF,
565 dist_bv: DVF,
566}
567
568impl<'t, 's, A, P, S, B, SN, BN, D, CMF, CBF, DPF, DVF> Iterator for NearestIter<'t, 's, A, P, S, B, SN, BN, D, CMF, CBF, DPF, DVF>
569 where B: BoundingVolume<P>,
570 BN: BoundingVolume<P> + Clone,
571 D: PartialEq + PartialOrd,
572 CMF: CmpPoints<A, P>,
573 CBF: BoundingVolumesCutter<A, P, BN, SN>,
574 DPF: DistanceBVCP<A, P, BN, Dist = D>,
575 DVF: DistanceBVBV<B, BN, Dist = D>,
576{
577 type Item = Result<NearestShape<'t, B, S, D>, CBF::Error>;
578
579 fn next(&mut self) -> Option<Self::Item> {
580 loop {
581 let direction =
582 match (self.neighbours.peek_mut(), self.nodes_queue.peek_mut()) {
583 (None, None) =>
584 return None,
585 (Some(top_shape), None) =>
586 Ok(PeekMut::pop(top_shape)),
587 (None, Some(top_node)) =>
588 Err(PeekMut::pop(top_node)),
589 (Some(top_shape), Some(top_node)) => {
590 let shape_closer = if let Some(ref top_node_dist) = top_node.dist {
591 &top_shape.dist < top_node_dist
592 } else {
593 false
594 };
595 if shape_closer {
596 Ok(PeekMut::pop(top_shape))
597 } else {
598 Err(PeekMut::pop(top_node))
599 }
600 },
601 };
602 match direction {
603 Ok(nearest_shape) =>
604 return Some(Ok(nearest_shape)),
605 Err(nearest_node) => {
606 let shapes = self.shapes;
607 let dist_cp = &mut self.dist_cp;
608 let dist_bv = &mut self.dist_bv;
609 {
610 let needle_bv = &nearest_node.needle_bv;
611 self.neighbours.extend(
612 nearest_node.node.shapes.iter()
613 .map(|fragment| NearestShape {
614 dist: dist_bv.bv_to_bv_distance(&fragment.bounding_volume, needle_bv),
615 shape: &shapes[fragment.shape_id],
616 shape_fragment: &fragment.bounding_volume,
617 })
618 );
619 }
620 match nearest_node.node.children {
621 KdvNodeChildren::Missing =>
622 (),
623 KdvNodeChildren::OnlyLeft { ref cut_point, ref child, } => {
624 let cut_axis = &self.axis[cut_point.axis_index];
625 match shape_owner(self.shape, nearest_node.needle_bv, cut_axis, &cut_point.point, &self.cmp_p, &mut self.cut_bv) {
626 Ok(ShapeOwner::Me(needle_fragment)) =>
627 self.nodes_queue.push(NearestNode { dist: None, node: child, needle_bv: needle_fragment, }),
628 Ok(ShapeOwner::Left(needle_fragment)) =>
629 self.nodes_queue.push(NearestNode { dist: None, node: child, needle_bv: needle_fragment, }),
630 Ok(ShapeOwner::Right(..)) =>
631 (),
632 Ok(ShapeOwner::Both { left_bvol: needle_fragment, .. }) =>
633 self.nodes_queue.push(NearestNode { dist: None, node: child, needle_bv: needle_fragment, }),
634 Err(error) => {
635 self.neighbours.clear();
636 self.nodes_queue.clear();
637 return Some(Err(error));
638 },
639 }
640 },
641 KdvNodeChildren::OnlyRight { ref cut_point, ref child, } => {
642 let cut_axis = &self.axis[cut_point.axis_index];
643 match shape_owner(self.shape, nearest_node.needle_bv, cut_axis, &cut_point.point, &self.cmp_p, &mut self.cut_bv) {
644 Ok(ShapeOwner::Me(needle_fragment)) =>
645 self.nodes_queue.push(NearestNode { dist: None, node: child, needle_bv: needle_fragment, }),
646 Ok(ShapeOwner::Left(..)) =>
647 (),
648 Ok(ShapeOwner::Right(needle_fragment)) =>
649 self.nodes_queue.push(NearestNode { dist: None, node: child, needle_bv: needle_fragment, }),
650 Ok(ShapeOwner::Both { right_bvol: needle_fragment, .. }) =>
651 self.nodes_queue.push(NearestNode { dist: None, node: child, needle_bv: needle_fragment, }),
652 Err(error) => {
653 self.neighbours.clear();
654 self.nodes_queue.clear();
655 return Some(Err(error));
656 },
657 }
658 },
659 KdvNodeChildren::Both { ref cut_point, ref left, ref right, } => {
660 let cut_axis = &self.axis[cut_point.axis_index];
661 match shape_owner(self.shape, nearest_node.needle_bv, cut_axis, &cut_point.point, &self.cmp_p, &mut self.cut_bv) {
662 Ok(ShapeOwner::Me(needle_fragment)) => {
663 self.nodes_queue.push(NearestNode { dist: None, node: left, needle_bv: needle_fragment.clone(), });
664 self.nodes_queue.push(NearestNode { dist: None, node: right, needle_bv: needle_fragment, });
665 },
666 Ok(ShapeOwner::Left(needle_fragment)) => {
667 self.nodes_queue.push(NearestNode { dist: None, node: left, needle_bv: needle_fragment.clone(), });
668 self.nodes_queue.push(NearestNode {
669 dist: Some(dist_cp.bv_to_cut_point_distance(cut_axis, &needle_fragment, &cut_point.point)),
670 node: right,
671 needle_bv: needle_fragment,
672 });
673 },
674 Ok(ShapeOwner::Right(needle_fragment)) => {
675 self.nodes_queue.push(NearestNode {
676 dist: Some(dist_cp.bv_to_cut_point_distance(cut_axis, &needle_fragment, &cut_point.point)),
677 node: left,
678 needle_bv: needle_fragment.clone(),
679 });
680 self.nodes_queue.push(NearestNode { dist: None, node: right, needle_bv: needle_fragment, });
681 },
682 Ok(ShapeOwner::Both { left_bvol, right_bvol, }) => {
683 self.nodes_queue.push(NearestNode { dist: None, node: left, needle_bv: left_bvol, });
684 self.nodes_queue.push(NearestNode { dist: None, node: right, needle_bv: right_bvol, });
685 },
686 Err(error) => {
687 self.neighbours.clear();
688 self.nodes_queue.clear();
689 return Some(Err(error));
690 },
691 }
692 },
693 }
694 },
695 }
696 }
697 }
698}
699
700impl<'t, P, B, BN, D> Ord for NearestNode<'t, P, B, BN, D> where D: PartialOrd {
701 fn cmp(&self, other: &NearestNode<'t, P, B, BN, D>) -> Ordering {
702 match (&self.dist, &other.dist) {
703 (&None, &None) =>
704 Ordering::Equal,
705 (&None, &Some(..)) =>
706 Ordering::Greater,
707 (&Some(..), &None) =>
708 Ordering::Less,
709 (&Some(ref da), &Some(ref db)) =>
710 if da < db {
711 Ordering::Greater
712 } else if da > db {
713 Ordering::Less
714 } else {
715 Ordering::Equal
716 },
717 }
718 }
719}
720
721impl<'t, P, B, BN, D> PartialOrd for NearestNode<'t, P, B, BN, D> where D: PartialOrd {
722 fn partial_cmp(&self, other: &NearestNode<'t, P, B, BN, D>) -> Option<Ordering> {
723 Some(self.cmp(other))
724 }
725}
726
727impl<'t, P, B, BN, D> Eq for NearestNode<'t, P, B, BN, D> where D: PartialEq + PartialOrd { }
728
729impl<'t, P, B, BN, D> PartialEq for NearestNode<'t, P, B, BN, D> where D: PartialEq + PartialOrd {
730 fn eq(&self, other: &NearestNode<'t, P, B, BN, D>) -> bool {
731 self.dist == other.dist
732 }
733}
734
735impl<'t, B, S, D> Ord for NearestShape<'t, B, S, D> where D: PartialOrd {
736 fn cmp(&self, other: &NearestShape<'t, B, S, D>) -> Ordering {
737 if self.dist < other.dist {
738 Ordering::Greater
739 } else if self.dist > other.dist {
740 Ordering::Less
741 } else {
742 Ordering::Equal
743 }
744 }
745}
746
747impl<'t, B, S, D> PartialOrd for NearestShape<'t, B, S, D> where D: PartialOrd {
748 fn partial_cmp(&self, other: &NearestShape<'t, B, S, D>) -> Option<Ordering> {
749 Some(self.cmp(other))
750 }
751}
752
753impl<'t, B, S, D> Eq for NearestShape<'t, B, S, D> where D: PartialEq + PartialOrd { }
754
755impl<'t, B, S, D> PartialEq for NearestShape<'t, B, S, D> where D: PartialEq + PartialOrd {
756 fn eq(&self, other: &NearestShape<'t, B, S, D>) -> bool {
757 self.dist == other.dist
758 }
759}
760
761pub struct Iter<'t, P: 't, B: 't, S: 't> {
762 shapes: &'t [S],
763 pending: Vec<(usize, &'t KdvNode<P, B>)>,
764}
765
766impl<'t, P, B, S> Iterator for Iter<'t, P, B, S> {
767 type Item = KdvNodeRef<'t, B, S>;
768
769 fn next(&mut self) -> Option<Self::Item> {
770 if let Some((depth, node)) = self.pending.pop() {
771 match node.children {
772 KdvNodeChildren::Missing =>
773 (),
774 KdvNodeChildren::OnlyLeft { ref child, .. } =>
775 self.pending.push((depth + 1, &*child)),
776 KdvNodeChildren::OnlyRight { ref child, .. } =>
777 self.pending.push((depth + 1, &*child)),
778 KdvNodeChildren::Both { ref left, ref right, .. } => {
779 self.pending.push((depth + 1, &*right));
780 self.pending.push((depth + 1, &*left));
781 },
782 }
783 Some(KdvNodeRef {
784 shapes: self.shapes,
785 fragments: &node.shapes,
786 depth,
787 })
788 } else {
789 None
790 }
791 }
792}
793
794pub struct KdvNodeRef<'t, B: 't, S: 't> {
795 shapes: &'t [S],
796 fragments: &'t [ShapeFragment<B>],
797 depth: usize,
798}
799
800impl<'t, B, S> KdvNodeRef<'t, B, S> {
801 pub fn depth(&self) -> usize {
802 self.depth
803 }
804
805 pub fn shapes(self) -> impl Iterator<Item = (&'t S, &'t B)> {
806 self.fragments.into_iter()
807 .map(move |fragment| (&self.shapes[fragment.shape_id], &fragment.bounding_volume))
808 }
809}