ncollide3d/query/distance/
distance_composite_shape_shape.rs

1use crate::bounding_volume::AABB;
2use crate::math::{Isometry, Point, Vector};
3use crate::partitioning::{BestFirstVisitStatus, BestFirstVisitor};
4use crate::query::{self, PointQuery};
5use crate::shape::{CompositeShape, Shape};
6use na::{self, RealField};
7
8/// Smallest distance between a composite shape and any other shape.
9pub fn distance_composite_shape_shape<N, G1: ?Sized>(
10    m1: &Isometry<N>,
11    g1: &G1,
12    m2: &Isometry<N>,
13    g2: &dyn Shape<N>,
14) -> N
15where
16    N: RealField + Copy,
17    G1: CompositeShape<N>,
18{
19    let ls_m2 = m1.inverse() * m2.clone();
20    let ls_aabb2 = g2.aabb(&ls_m2);
21
22    let mut visitor = CompositeShapeAgainstAnyDistanceVisitor {
23        msum_shift: -ls_aabb2.center().coords,
24        msum_margin: ls_aabb2.half_extents(),
25        m1: m1,
26        g1: g1,
27        m2: m2,
28        g2: g2,
29    };
30
31    g1.bvh()
32        .best_first_search(&mut visitor)
33        .expect("The composite shape must not be empty.")
34        .1
35}
36
37/// Smallest distance between a shape and a composite shape.
38pub fn distance_shape_composite_shape<N, G2: ?Sized>(
39    m1: &Isometry<N>,
40    g1: &dyn Shape<N>,
41    m2: &Isometry<N>,
42    g2: &G2,
43) -> N
44where
45    N: RealField + Copy,
46    G2: CompositeShape<N>,
47{
48    distance_composite_shape_shape(m2, g2, m1, g1)
49}
50
51struct CompositeShapeAgainstAnyDistanceVisitor<'a, N: 'a + RealField + Copy, G1: ?Sized + 'a> {
52    msum_shift: Vector<N>,
53    msum_margin: Vector<N>,
54
55    m1: &'a Isometry<N>,
56    g1: &'a G1,
57    m2: &'a Isometry<N>,
58    g2: &'a dyn Shape<N>,
59}
60
61impl<'a, N, G1: ?Sized> BestFirstVisitor<N, usize, AABB<N>>
62    for CompositeShapeAgainstAnyDistanceVisitor<'a, N, G1>
63where
64    N: RealField + Copy,
65    G1: CompositeShape<N>,
66{
67    type Result = N;
68
69    fn visit(
70        &mut self,
71        best: N,
72        bv: &AABB<N>,
73        data: Option<&usize>,
74    ) -> BestFirstVisitStatus<N, Self::Result> {
75        // Compute the minkowski sum of the two AABBs.
76        let msum = AABB::new(
77            bv.mins + self.msum_shift + (-self.msum_margin),
78            bv.maxs + self.msum_shift + self.msum_margin,
79        );
80
81        let dist = msum.distance_to_point(&Isometry::identity(), &Point::origin(), true);
82
83        let mut res = BestFirstVisitStatus::Continue {
84            cost: dist,
85            result: None,
86        };
87
88        if let Some(b) = data {
89            if dist < best {
90                self.g1.map_part_at(*b, self.m1, &mut |m1, g1| {
91                    let distance = query::distance(m1, g1, self.m2, self.g2);
92                    res = BestFirstVisitStatus::Continue {
93                        cost: distance,
94                        result: Some(distance),
95                    }
96                });
97            }
98        }
99
100        res
101    }
102}
ncollide3d/query/distance/distance_composite_shape_shape.rs

ncollide3d/query/distance/
distance_composite_shape_shape.rs
