#[cfg(test)] extern crate rand;
use std::cmp::Ordering;
use std::iter::{self, FromIterator};
use std::collections::{BinaryHeap, binary_heap::PeekMut};
#[cfg(test)]
mod tests;
pub trait BoundingVolume<P> {
fn min_corner(&self) -> P;
fn max_corner(&self) -> P;
}
pub trait CmpPoints<A, P> {
fn cmp_points(&self, axis: &A, a: &P, b: &P) -> Ordering;
}
impl<A, P, F> CmpPoints<A, P> for F where F: Fn(&A, &P, &P) -> Ordering {
fn cmp_points(&self, axis: &A, a: &P, b: &P) -> Ordering {
(self)(axis, a, b)
}
}
pub trait GetBoundingVolume<B, S> {
fn bounding_volume(&self, shape: &S) -> B;
}
impl<B, S, F> GetBoundingVolume<B, S> for F where F: Fn(&S) -> B {
fn bounding_volume(&self, shape: &S) -> B {
(self)(shape)
}
}
pub trait GetCutPoint<A, P> {
fn cut_point<I>(&mut self, cut_axis: &A, points: I) -> Option<P>
where I: Iterator<Item = P>;
}
impl<A, P, F> GetCutPoint<A, P> for F where F: FnMut(&A, &mut Iterator<Item = P>) -> Option<P> {
fn cut_point<I>(&mut self, cut_axis: &A, mut points: I) -> Option<P>
where I: Iterator<Item = P>
{
(self)(cut_axis, &mut points)
}
}
pub trait BoundingVolumesCutter<A, P, B, S> {
type Error;
fn cut(&mut self, shape: &S, fragment: &B, cut_axis: &A, cut_point: &P) -> Result<Option<(B, B)>, Self::Error>;
}
impl<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> {
type Error = E;
fn cut(&mut self, shape: &S, fragment: &B, cut_axis: &A, cut_point: &P) -> Result<Option<(B, B)>, Self::Error> {
(self)(shape, fragment, cut_axis, cut_point)
}
}
pub trait DistanceBVCP<A, P, B> {
type Dist;
fn bv_to_cut_point_distance(&mut self, axis: &A, bounding_volume: &B, cut_point: &P) -> Self::Dist;
}
impl<A, P, B, F, D> DistanceBVCP<A, P, B> for F where F: FnMut(&A, &B, &P) -> D {
type Dist = D;
fn bv_to_cut_point_distance(&mut self, axis: &A, bounding_volume: &B, cut_point: &P) -> Self::Dist {
(self)(axis, bounding_volume, cut_point)
}
}
pub trait DistanceBVBV<BA, BB> {
type Dist;
fn bv_to_bv_distance(&mut self, bounding_volume_a: &BA, bounding_volume_b: &BB) -> Self::Dist;
}
impl<BA, BB, F, D> DistanceBVBV<BA, BB> for F where F: FnMut(&BA, &BB) -> D {
type Dist = D;
fn bv_to_bv_distance(&mut self, bounding_volume_a: &BA, bounding_volume_b: &BB) -> Self::Dist {
(self)(bounding_volume_a, bounding_volume_b)
}
}
pub struct KdvTree<A, P, B, S> {
axis: Vec<A>,
shapes: Vec<S>,
root: KdvNode<P, B>,
}
impl<A, P, B, S> KdvTree<A, P, B, S>
where B: BoundingVolume<P>,
{
pub fn build<IA, II, CMF, BVF, CPF, CBF>(
axis_it: IA,
shapes_it: II,
cmp_p: CMF,
get_bv: BVF,
mut get_cp: CPF,
mut cut_bv: CBF,
)
-> Result<KdvTree<A, P, B, S>, CBF::Error>
where IA: IntoIterator<Item = A>,
II: IntoIterator<Item = S>,
CMF: CmpPoints<A, P>,
BVF: GetBoundingVolume<B, S>,
CPF: GetCutPoint<A, P>,
CBF: BoundingVolumesCutter<A, P, B, S>,
{
let axis: Vec<_> = axis_it.into_iter().collect();
let shapes: Vec<_> = shapes_it.into_iter().collect();
let root_shapes: Vec<_> = shapes
.iter()
.enumerate()
.map(|(i, s)| ShapeFragment {
bounding_volume: get_bv.bounding_volume(s),
shape_id: i,
})
.collect();
struct Op<P, B> {
node_shapes: Vec<ShapeFragment<B>>,
instruction: Instruction<P>,
}
enum Instruction<P> {
MakeNode { depth: usize, },
AssembleOnlyLeft { cut_point: CutPoint<P>, },
AssembleOnlyRight { cut_point: CutPoint<P>, },
AssembleBoth { cut_point: CutPoint<P>, },
}
let mut ops_stack = vec![Op { node_shapes: root_shapes, instruction: Instruction::MakeNode { depth: 0, }, }];
let mut ret_stack: Vec<KdvNode<P, B>> = vec![];
while let Some(op) = ops_stack.pop() {
match op {
Op { mut node_shapes, instruction: Instruction::MakeNode { depth, }, } => {
let cut_axis_index = depth % axis.len();
let cut_axis = &axis[cut_axis_index];
let maybe_cut_point = get_cp.cut_point(
cut_axis,
node_shapes
.iter()
.flat_map(|sf| {
let bvol = &sf.bounding_volume;
iter::once(bvol.min_corner())
.chain(iter::once(bvol.max_corner()))
}),
);
if let Some(cut_point) = maybe_cut_point {
let mut left_shapes = Vec::new();
let mut right_shapes = Vec::new();
let mut head = 0;
while head < node_shapes.len() {
let ShapeFragment { shape_id, bounding_volume, } = node_shapes.swap_remove(head);
let owner = shape_owner(&shapes[shape_id], bounding_volume, cut_axis, &cut_point, &cmp_p, &mut cut_bv)?;
match owner {
ShapeOwner::Me(bounding_volume) => {
let tail = node_shapes.len();
node_shapes.push(ShapeFragment { shape_id, bounding_volume, });
node_shapes.swap(head, tail);
head += 1;
},
ShapeOwner::Left(bounding_volume) =>
left_shapes.push(ShapeFragment { shape_id, bounding_volume, }),
ShapeOwner::Right(bounding_volume) =>
right_shapes.push(ShapeFragment { shape_id, bounding_volume, }),
ShapeOwner::Both { left_bvol, right_bvol, } => {
left_shapes.push(ShapeFragment { shape_id, bounding_volume: left_bvol, });
right_shapes.push(ShapeFragment { shape_id, bounding_volume: right_bvol, });
},
}
}
if left_shapes.is_empty() && right_shapes.is_empty() {
ret_stack.push(KdvNode {
shapes: node_shapes,
children: KdvNodeChildren::Missing,
});
} else if left_shapes.is_empty() {
ops_stack.push(Op {
node_shapes,
instruction: Instruction::AssembleOnlyRight {
cut_point: CutPoint { axis_index: cut_axis_index, point: cut_point, },
},
});
ops_stack.push(Op {
node_shapes: right_shapes,
instruction: Instruction::MakeNode { depth: depth + 1, },
});
} else if right_shapes.is_empty() {
ops_stack.push(Op {
node_shapes,
instruction: Instruction::AssembleOnlyLeft {
cut_point: CutPoint { axis_index: cut_axis_index, point: cut_point, },
},
});
ops_stack.push(Op {
node_shapes: left_shapes,
instruction: Instruction::MakeNode { depth: depth + 1, },
});
} else {
ops_stack.push(Op {
node_shapes,
instruction: Instruction::AssembleBoth {
cut_point: CutPoint { axis_index: cut_axis_index, point: cut_point, },
},
});
ops_stack.push(Op {
node_shapes: left_shapes,
instruction: Instruction::MakeNode { depth: depth + 1, },
});
ops_stack.push(Op {
node_shapes: right_shapes,
instruction: Instruction::MakeNode { depth: depth + 1, },
});
}
} else {
ret_stack.push(KdvNode {
shapes: node_shapes,
children: KdvNodeChildren::Missing,
});
}
},
Op { node_shapes, instruction: Instruction::AssembleOnlyLeft { cut_point, }, } => {
let child = ret_stack.pop().map(Box::new).unwrap_or_else(|| unreachable!());
ret_stack.push(KdvNode {
shapes: node_shapes,
children: KdvNodeChildren::OnlyLeft { cut_point, child },
});
},
Op { node_shapes, instruction: Instruction::AssembleOnlyRight { cut_point, }, } => {
let child = ret_stack.pop().map(Box::new).unwrap_or_else(|| unreachable!());
ret_stack.push(KdvNode {
shapes: node_shapes,
children: KdvNodeChildren::OnlyRight { cut_point, child, },
});
},
Op { node_shapes, instruction: Instruction::AssembleBoth { cut_point, }, } => {
let left = ret_stack.pop().map(Box::new).unwrap_or_else(|| unreachable!());
let right = ret_stack.pop().map(Box::new).unwrap_or_else(|| unreachable!());
ret_stack.push(KdvNode {
shapes: node_shapes,
children: KdvNodeChildren::Both { cut_point, left, right },
});
},
}
}
let root = ret_stack.pop().unwrap_or_else(|| unreachable!());
assert!(ret_stack.is_empty());
Ok(KdvTree { root, axis, shapes, })
}
pub fn intersects<'t, 's, SN, BN, CMF, BVF, CPF, CBF>(
&'t self,
shape: &'s SN,
cmp_p: CMF,
get_bv: BVF,
get_cp: CPF,
cut_bv: CBF,
)
-> IntersectIter<'t, 's, A, P, S, B, SN, BN, CMF, CPF, CBF>
where CMF: CmpPoints<A, P>,
BVF: GetBoundingVolume<BN, SN>,
CPF: GetCutPoint<A, P>,
CBF: BoundingVolumesCutter<A, P, BN, SN>,
{
IntersectIter {
needle: shape,
axis: &self.axis,
shapes: &self.shapes,
queue: vec![TraverseTask::Explore {
node: &self.root,
needle_fragment: get_bv.bounding_volume(shape),
}],
cmp_p,
get_cp,
cut_bv,
}
}
pub fn nearest<'t, 's, SN, BN, D, CMF, BVF, CBF, DPF, DVF>(
&'t self,
shape: &'s SN,
cmp_p: CMF,
get_bv: BVF,
cut_bv: CBF,
dist_cp: DPF,
dist_bv: DVF,
)
-> NearestIter<'t, 's, A, P, S, B, SN, BN, D, CMF, CBF, DPF, DVF>
where CMF: CmpPoints<A, P>,
BVF: GetBoundingVolume<BN, SN>,
CBF: BoundingVolumesCutter<A, P, BN, SN>,
DPF: DistanceBVCP<A, P, BN, Dist = D>,
DVF: DistanceBVBV<B, BN, Dist = D>,
D: PartialEq + PartialOrd,
{
NearestIter {
shape,
axis: &self.axis,
shapes: &self.shapes,
nodes_queue: BinaryHeap::from_iter(iter::once(NearestNode {
dist: None,
node: &self.root,
needle_bv: get_bv.bounding_volume(shape),
})),
neighbours: BinaryHeap::new(),
cmp_p,
cut_bv,
dist_cp,
dist_bv,
}
}
pub fn iter<'t>(&'t self) -> Iter<'t, P, B, S> {
Iter {
shapes: &self.shapes,
pending: vec![(0, &self.root)],
}
}
}
struct ShapeFragment<B> {
bounding_volume: B,
shape_id: usize,
}
struct KdvNode<P, B> {
shapes: Vec<ShapeFragment<B>>,
children: KdvNodeChildren<P, B>,
}
struct CutPoint<P> {
axis_index: usize,
point: P,
}
enum KdvNodeChildren<P, B> {
Missing,
OnlyLeft { cut_point: CutPoint<P>, child: Box<KdvNode<P, B>>, },
OnlyRight { cut_point: CutPoint<P>, child: Box<KdvNode<P, B>>, },
Both { cut_point: CutPoint<P>, left: Box<KdvNode<P, B>>, right: Box<KdvNode<P, B>>, },
}
enum ShapeOwner<B> {
Me(B),
Left(B),
Right(B),
Both { left_bvol: B, right_bvol: B, },
}
fn 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) ->
Result<ShapeOwner<B>, CBF::Error>
where B: BoundingVolume<P>,
CMF: CmpPoints<A, P>,
CBF: BoundingVolumesCutter<A, P, B, S>,
{
let min_corner = fragment.min_corner();
let max_corner = fragment.max_corner();
Ok(match (cmp_p.cmp_points(&cut_axis, &min_corner, cut_point), cmp_p.cmp_points(&cut_axis, &max_corner, cut_point)) {
(Ordering::Less, Ordering::Less) | (Ordering::Less, Ordering::Equal) =>
ShapeOwner::Left(fragment),
(Ordering::Greater, Ordering::Greater) | (Ordering::Equal, Ordering::Greater) =>
ShapeOwner::Right(fragment),
_ => if let Some((left_bvol, right_bvol)) = cut_bv.cut(shape, &fragment, cut_axis, cut_point)? {
ShapeOwner::Both { left_bvol, right_bvol, }
} else {
ShapeOwner::Me(fragment)
}
})
}
enum TraverseTask<'t, P: 't, BS: 't, BN> {
Explore { node: &'t KdvNode<P, BS>, needle_fragment: BN, },
Intersect { needle_fragment: BN, shape_fragment: &'t ShapeFragment<BS>, axis_counter: usize, },
}
pub struct IntersectIter<'t, 's, A: 't, P: 't, SS: 't, BS: 't, SN: 's, BN, CMF, CPF, CBF> {
needle: &'s SN,
axis: &'t [A],
shapes: &'t [SS],
queue: Vec<TraverseTask<'t, P, BS, BN>>,
cmp_p: CMF,
get_cp: CPF,
cut_bv: CBF,
}
#[derive(Clone, PartialEq, Debug)]
pub struct Intersection<'t, SS: 't, BS: 't, BN> {
pub shape: &'t SS,
pub shape_fragment: &'t BS,
pub needle_fragment: BN,
}
impl<'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>
where BS: BoundingVolume<P>,
BN: BoundingVolume<P> + Clone,
CMF: CmpPoints<A, P>,
CPF: GetCutPoint<A, P>,
CBF: BoundingVolumesCutter<A, P, BN, SN>,
{
type Item = Result<Intersection<'t, SS, BS, BN>, CBF::Error>;
fn next(&mut self) -> Option<Self::Item> {
'outer: while let Some(task) = self.queue.pop() {
match task {
TraverseTask::Explore { node, needle_fragment, } => {
match node.children {
KdvNodeChildren::Missing =>
(),
KdvNodeChildren::OnlyLeft { ref cut_point, ref child, } => {
let cut_axis = &self.axis[cut_point.axis_index];
match shape_owner(self.needle, needle_fragment.clone(), cut_axis, &cut_point.point, &self.cmp_p, &mut self.cut_bv) {
Ok(ShapeOwner::Me(needle_fragment)) =>
self.queue.push(TraverseTask::Explore { node: child, needle_fragment, }),
Ok(ShapeOwner::Left(needle_fragment)) =>
self.queue.push(TraverseTask::Explore { node: child, needle_fragment, }),
Ok(ShapeOwner::Right(..)) =>
(),
Ok(ShapeOwner::Both { left_bvol: needle_fragment, .. }) =>
self.queue.push(TraverseTask::Explore { node: child, needle_fragment, }),
Err(error) => {
self.queue.clear();
return Some(Err(error));
},
}
},
KdvNodeChildren::OnlyRight { ref cut_point, ref child, } => {
let cut_axis = &self.axis[cut_point.axis_index];
match shape_owner(self.needle, needle_fragment.clone(), cut_axis, &cut_point.point, &self.cmp_p, &mut self.cut_bv) {
Ok(ShapeOwner::Me(needle_fragment)) =>
self.queue.push(TraverseTask::Explore { node: child, needle_fragment, }),
Ok(ShapeOwner::Left(..)) =>
(),
Ok(ShapeOwner::Right(needle_fragment)) =>
self.queue.push(TraverseTask::Explore { node: child, needle_fragment, }),
Ok(ShapeOwner::Both { right_bvol: needle_fragment, .. }) =>
self.queue.push(TraverseTask::Explore { node: child, needle_fragment, }),
Err(error) => {
self.queue.clear();
return Some(Err(error));
},
}
},
KdvNodeChildren::Both { ref cut_point, ref left, ref right, } => {
let cut_axis = &self.axis[cut_point.axis_index];
match shape_owner(self.needle, needle_fragment.clone(), cut_axis, &cut_point.point, &self.cmp_p, &mut self.cut_bv) {
Ok(ShapeOwner::Me(fragment)) => {
self.queue.push(TraverseTask::Explore { node: left, needle_fragment: fragment.clone(), });
self.queue.push(TraverseTask::Explore { node: right, needle_fragment: fragment, });
},
Ok(ShapeOwner::Left(needle_fragment)) =>
self.queue.push(TraverseTask::Explore { node: left, needle_fragment, }),
Ok(ShapeOwner::Right(needle_fragment)) =>
self.queue.push(TraverseTask::Explore { node: right, needle_fragment, }),
Ok(ShapeOwner::Both { left_bvol, right_bvol, }) => {
self.queue.push(TraverseTask::Explore { node: left, needle_fragment: left_bvol, });
self.queue.push(TraverseTask::Explore { node: right, needle_fragment: right_bvol, });
},
Err(error) => {
self.queue.clear();
return Some(Err(error));
},
}
},
}
for shape_fragment in node.shapes.iter() {
self.queue.push(TraverseTask::Intersect {
shape_fragment,
needle_fragment: needle_fragment.clone(),
axis_counter: 0,
});
}
},
TraverseTask::Intersect { shape_fragment, needle_fragment, mut axis_counter, } => {
let no_intersection = self.axis.iter().any(|axis| {
let needle_min = needle_fragment.min_corner();
let needle_max = needle_fragment.max_corner();
let shape_min = shape_fragment.bounding_volume.min_corner();
let shape_max = shape_fragment.bounding_volume.max_corner();
(self.cmp_p.cmp_points(axis, &needle_min, &shape_max) == Ordering::Greater ||
self.cmp_p.cmp_points(axis, &needle_max, &shape_min) == Ordering::Less)
});
if no_intersection {
continue;
}
let axis_total = self.axis.len();
for _ in 0 .. axis_total {
let cut_axis = &self.axis[axis_counter % axis_total];
axis_counter += 1;
let maybe_cut_point = self.get_cp.cut_point(
cut_axis,
iter::once(needle_fragment.min_corner())
.chain(iter::once(needle_fragment.max_corner())),
);
if let Some(cut_point) = maybe_cut_point {
match self.cut_bv.cut(self.needle, &needle_fragment, cut_axis, &cut_point) {
Ok(Some((left_fragment, right_fragment))) => {
self.queue.push(TraverseTask::Intersect {
shape_fragment: shape_fragment.clone(),
needle_fragment: left_fragment,
axis_counter,
});
self.queue.push(TraverseTask::Intersect {
shape_fragment: shape_fragment,
needle_fragment: right_fragment,
axis_counter,
});
continue 'outer;
},
Ok(None) =>
(),
Err(error) => {
self.queue.clear();
return Some(Err(error));
},
}
}
}
return Some(Ok(Intersection {
shape: &self.shapes[shape_fragment.shape_id],
shape_fragment: &shape_fragment.bounding_volume,
needle_fragment,
}));
},
}
}
None
}
}
struct NearestNode<'t, P: 't, B: 't, BN, D> where D: PartialEq + PartialOrd {
dist: Option<D>,
node: &'t KdvNode<P, B>,
needle_bv: BN,
}
#[derive(Clone, Debug)]
pub struct NearestShape<'t, B: 't, S: 't, D> {
pub dist: D,
pub shape: &'t S,
pub shape_fragment: &'t B,
}
pub struct NearestIter<'t, 's, A: 't, P: 't, S: 't, B: 't, SN: 's, BN, D, CMF, CBF, DPF, DVF> where D: PartialEq + PartialOrd {
shape: &'s SN,
axis: &'t [A],
shapes: &'t [S],
nodes_queue: BinaryHeap<NearestNode<'t, P, B, BN, D>>,
neighbours: BinaryHeap<NearestShape<'t, B, S, D>>,
cmp_p: CMF,
cut_bv: CBF,
dist_cp: DPF,
dist_bv: DVF,
}
impl<'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>
where B: BoundingVolume<P>,
BN: BoundingVolume<P> + Clone,
D: PartialEq + PartialOrd,
CMF: CmpPoints<A, P>,
CBF: BoundingVolumesCutter<A, P, BN, SN>,
DPF: DistanceBVCP<A, P, BN, Dist = D>,
DVF: DistanceBVBV<B, BN, Dist = D>,
{
type Item = Result<NearestShape<'t, B, S, D>, CBF::Error>;
fn next(&mut self) -> Option<Self::Item> {
loop {
let direction =
match (self.neighbours.peek_mut(), self.nodes_queue.peek_mut()) {
(None, None) =>
return None,
(Some(top_shape), None) =>
Ok(PeekMut::pop(top_shape)),
(None, Some(top_node)) =>
Err(PeekMut::pop(top_node)),
(Some(top_shape), Some(top_node)) => {
let shape_closer = if let Some(ref top_node_dist) = top_node.dist {
&top_shape.dist < top_node_dist
} else {
false
};
if shape_closer {
Ok(PeekMut::pop(top_shape))
} else {
Err(PeekMut::pop(top_node))
}
},
};
match direction {
Ok(nearest_shape) =>
return Some(Ok(nearest_shape)),
Err(nearest_node) => {
let shapes = self.shapes;
let dist_cp = &mut self.dist_cp;
let dist_bv = &mut self.dist_bv;
{
let needle_bv = &nearest_node.needle_bv;
self.neighbours.extend(
nearest_node.node.shapes.iter()
.map(|fragment| NearestShape {
dist: dist_bv.bv_to_bv_distance(&fragment.bounding_volume, needle_bv),
shape: &shapes[fragment.shape_id],
shape_fragment: &fragment.bounding_volume,
})
);
}
match nearest_node.node.children {
KdvNodeChildren::Missing =>
(),
KdvNodeChildren::OnlyLeft { ref cut_point, ref child, } => {
let cut_axis = &self.axis[cut_point.axis_index];
match shape_owner(self.shape, nearest_node.needle_bv, cut_axis, &cut_point.point, &self.cmp_p, &mut self.cut_bv) {
Ok(ShapeOwner::Me(needle_fragment)) =>
self.nodes_queue.push(NearestNode { dist: None, node: child, needle_bv: needle_fragment, }),
Ok(ShapeOwner::Left(needle_fragment)) =>
self.nodes_queue.push(NearestNode { dist: None, node: child, needle_bv: needle_fragment, }),
Ok(ShapeOwner::Right(..)) =>
(),
Ok(ShapeOwner::Both { left_bvol: needle_fragment, .. }) =>
self.nodes_queue.push(NearestNode { dist: None, node: child, needle_bv: needle_fragment, }),
Err(error) => {
self.neighbours.clear();
self.nodes_queue.clear();
return Some(Err(error));
},
}
},
KdvNodeChildren::OnlyRight { ref cut_point, ref child, } => {
let cut_axis = &self.axis[cut_point.axis_index];
match shape_owner(self.shape, nearest_node.needle_bv, cut_axis, &cut_point.point, &self.cmp_p, &mut self.cut_bv) {
Ok(ShapeOwner::Me(needle_fragment)) =>
self.nodes_queue.push(NearestNode { dist: None, node: child, needle_bv: needle_fragment, }),
Ok(ShapeOwner::Left(..)) =>
(),
Ok(ShapeOwner::Right(needle_fragment)) =>
self.nodes_queue.push(NearestNode { dist: None, node: child, needle_bv: needle_fragment, }),
Ok(ShapeOwner::Both { right_bvol: needle_fragment, .. }) =>
self.nodes_queue.push(NearestNode { dist: None, node: child, needle_bv: needle_fragment, }),
Err(error) => {
self.neighbours.clear();
self.nodes_queue.clear();
return Some(Err(error));
},
}
},
KdvNodeChildren::Both { ref cut_point, ref left, ref right, } => {
let cut_axis = &self.axis[cut_point.axis_index];
match shape_owner(self.shape, nearest_node.needle_bv, cut_axis, &cut_point.point, &self.cmp_p, &mut self.cut_bv) {
Ok(ShapeOwner::Me(needle_fragment)) => {
self.nodes_queue.push(NearestNode { dist: None, node: left, needle_bv: needle_fragment.clone(), });
self.nodes_queue.push(NearestNode { dist: None, node: right, needle_bv: needle_fragment, });
},
Ok(ShapeOwner::Left(needle_fragment)) => {
self.nodes_queue.push(NearestNode { dist: None, node: left, needle_bv: needle_fragment.clone(), });
self.nodes_queue.push(NearestNode {
dist: Some(dist_cp.bv_to_cut_point_distance(cut_axis, &needle_fragment, &cut_point.point)),
node: right,
needle_bv: needle_fragment,
});
},
Ok(ShapeOwner::Right(needle_fragment)) => {
self.nodes_queue.push(NearestNode {
dist: Some(dist_cp.bv_to_cut_point_distance(cut_axis, &needle_fragment, &cut_point.point)),
node: left,
needle_bv: needle_fragment.clone(),
});
self.nodes_queue.push(NearestNode { dist: None, node: right, needle_bv: needle_fragment, });
},
Ok(ShapeOwner::Both { left_bvol, right_bvol, }) => {
self.nodes_queue.push(NearestNode { dist: None, node: left, needle_bv: left_bvol, });
self.nodes_queue.push(NearestNode { dist: None, node: right, needle_bv: right_bvol, });
},
Err(error) => {
self.neighbours.clear();
self.nodes_queue.clear();
return Some(Err(error));
},
}
},
}
},
}
}
}
}
impl<'t, P, B, BN, D> Ord for NearestNode<'t, P, B, BN, D> where D: PartialOrd {
fn cmp(&self, other: &NearestNode<'t, P, B, BN, D>) -> Ordering {
match (&self.dist, &other.dist) {
(&None, &None) =>
Ordering::Equal,
(&None, &Some(..)) =>
Ordering::Greater,
(&Some(..), &None) =>
Ordering::Less,
(&Some(ref da), &Some(ref db)) =>
if da < db {
Ordering::Greater
} else if da > db {
Ordering::Less
} else {
Ordering::Equal
},
}
}
}
impl<'t, P, B, BN, D> PartialOrd for NearestNode<'t, P, B, BN, D> where D: PartialOrd {
fn partial_cmp(&self, other: &NearestNode<'t, P, B, BN, D>) -> Option<Ordering> {
Some(self.cmp(other))
}
}
impl<'t, P, B, BN, D> Eq for NearestNode<'t, P, B, BN, D> where D: PartialEq + PartialOrd { }
impl<'t, P, B, BN, D> PartialEq for NearestNode<'t, P, B, BN, D> where D: PartialEq + PartialOrd {
fn eq(&self, other: &NearestNode<'t, P, B, BN, D>) -> bool {
self.dist == other.dist
}
}
impl<'t, B, S, D> Ord for NearestShape<'t, B, S, D> where D: PartialOrd {
fn cmp(&self, other: &NearestShape<'t, B, S, D>) -> Ordering {
if self.dist < other.dist {
Ordering::Greater
} else if self.dist > other.dist {
Ordering::Less
} else {
Ordering::Equal
}
}
}
impl<'t, B, S, D> PartialOrd for NearestShape<'t, B, S, D> where D: PartialOrd {
fn partial_cmp(&self, other: &NearestShape<'t, B, S, D>) -> Option<Ordering> {
Some(self.cmp(other))
}
}
impl<'t, B, S, D> Eq for NearestShape<'t, B, S, D> where D: PartialEq + PartialOrd { }
impl<'t, B, S, D> PartialEq for NearestShape<'t, B, S, D> where D: PartialEq + PartialOrd {
fn eq(&self, other: &NearestShape<'t, B, S, D>) -> bool {
self.dist == other.dist
}
}
pub struct Iter<'t, P: 't, B: 't, S: 't> {
shapes: &'t [S],
pending: Vec<(usize, &'t KdvNode<P, B>)>,
}
impl<'t, P, B, S> Iterator for Iter<'t, P, B, S> {
type Item = KdvNodeRef<'t, B, S>;
fn next(&mut self) -> Option<Self::Item> {
if let Some((depth, node)) = self.pending.pop() {
match node.children {
KdvNodeChildren::Missing =>
(),
KdvNodeChildren::OnlyLeft { ref child, .. } =>
self.pending.push((depth + 1, &*child)),
KdvNodeChildren::OnlyRight { ref child, .. } =>
self.pending.push((depth + 1, &*child)),
KdvNodeChildren::Both { ref left, ref right, .. } => {
self.pending.push((depth + 1, &*right));
self.pending.push((depth + 1, &*left));
},
}
Some(KdvNodeRef {
shapes: self.shapes,
fragments: &node.shapes,
depth,
})
} else {
None
}
}
}
pub struct KdvNodeRef<'t, B: 't, S: 't> {
shapes: &'t [S],
fragments: &'t [ShapeFragment<B>],
depth: usize,
}
impl<'t, B, S> KdvNodeRef<'t, B, S> {
pub fn depth(&self) -> usize {
self.depth
}
pub fn shapes(self) -> impl Iterator<Item = (&'t S, &'t B)> {
self.fragments.into_iter()
.map(move |fragment| (&self.shapes[fragment.shape_id], &fragment.bounding_volume))
}
}