parry3d/query/distance/
distance_support_map_support_map.rs

1use crate::math::{Isometry, Real, Vector};
2use crate::query::gjk::{self, CSOPoint, GJKResult, VoronoiSimplex};
3use crate::shape::SupportMap;
4
5use na::{self, Unit};
6use num::Bounded;
7
8/// Distance between support-mapped shapes.
9pub fn distance_support_map_support_map<G1, G2>(pos12: &Isometry<Real>, g1: &G1, g2: &G2) -> Real
10where
11    G1: ?Sized + SupportMap,
12    G2: ?Sized + SupportMap,
13{
14    distance_support_map_support_map_with_params(pos12, g1, g2, &mut VoronoiSimplex::new(), None)
15}
16
17/// Distance between support-mapped shapes.
18///
19/// This allows a more fine grained control other the underlying GJK algorigtm.
20pub fn distance_support_map_support_map_with_params<G1, G2>(
21    pos12: &Isometry<Real>,
22    g1: &G1,
23    g2: &G2,
24    simplex: &mut VoronoiSimplex,
25    init_dir: Option<Vector<Real>>,
26) -> Real
27where
28    G1: ?Sized + SupportMap,
29    G2: ?Sized + SupportMap,
30{
31    // TODO: or m2.translation - m1.translation ?
32    let dir = init_dir.unwrap_or_else(|| -pos12.translation.vector);
33
34    if let Some(dir) = Unit::try_new(dir, crate::math::DEFAULT_EPSILON) {
35        simplex.reset(CSOPoint::from_shapes(pos12, g1, g2, &dir));
36    } else {
37        simplex.reset(CSOPoint::from_shapes(
38            pos12,
39            g1,
40            g2,
41            &Vector::<Real>::x_axis(),
42        ));
43    }
44
45    match gjk::closest_points(pos12, g1, g2, Real::max_value(), true, simplex) {
46        GJKResult::Intersection => 0.0,
47        GJKResult::ClosestPoints(p1, p2, _) => na::distance(&p1, &p2),
48        GJKResult::Proximity(_) => unreachable!(),
49        GJKResult::NoIntersection(_) => 0.0, // TODO: GJK did not converge.
50    }
51}
parry3d/query/distance/distance_support_map_support_map.rs

parry3d/query/distance/
distance_support_map_support_map.rs
