parry3d/query/closest_points/
closest_points_support_map_support_map.rs

1use crate::math::{Isometry, Real, Vector};
2use crate::query::gjk::{self, CSOPoint, GJKResult, VoronoiSimplex};
3use crate::query::ClosestPoints;
4use crate::shape::SupportMap;
5
6use na::Unit;
7
8/// Closest points between support-mapped shapes (`Cuboid`, `ConvexHull`, etc.)
9pub fn closest_points_support_map_support_map<G1, G2>(
10    pos12: &Isometry<Real>,
11    g1: &G1,
12    g2: &G2,
13    prediction: Real,
14) -> ClosestPoints
15where
16    G1: ?Sized + SupportMap,
17    G2: ?Sized + SupportMap,
18{
19    match closest_points_support_map_support_map_with_params(
20        pos12,
21        g1,
22        g2,
23        prediction,
24        &mut VoronoiSimplex::new(),
25        None,
26    ) {
27        GJKResult::ClosestPoints(pt1, pt2, _) => {
28            ClosestPoints::WithinMargin(pt1, pos12.inverse_transform_point(&pt2))
29        }
30        GJKResult::NoIntersection(_) => ClosestPoints::Disjoint,
31        GJKResult::Intersection => ClosestPoints::Intersecting,
32        GJKResult::Proximity(_) => unreachable!(),
33    }
34}
35
36/// Closest points between support-mapped shapes (`Cuboid`, `ConvexHull`, etc.)
37///
38/// This allows a more fine grained control other the underlying GJK algorigtm.
39pub fn closest_points_support_map_support_map_with_params<G1, G2>(
40    pos12: &Isometry<Real>,
41    g1: &G1,
42    g2: &G2,
43    prediction: Real,
44    simplex: &mut VoronoiSimplex,
45    init_dir: Option<Vector<Real>>,
46) -> GJKResult
47where
48    G1: ?Sized + SupportMap,
49    G2: ?Sized + SupportMap,
50{
51    let dir = match init_dir {
52        // TODO: or pos12.translation.vector (without the minus sign) ?
53        None => -pos12.translation.vector,
54        Some(dir) => dir,
55    };
56
57    if let Some(dir) = Unit::try_new(dir, crate::math::DEFAULT_EPSILON) {
58        simplex.reset(CSOPoint::from_shapes(pos12, g1, g2, &dir));
59    } else {
60        simplex.reset(CSOPoint::from_shapes(
61            pos12,
62            g1,
63            g2,
64            &Vector::<Real>::x_axis(),
65        ));
66    }
67
68    gjk::closest_points(pos12, g1, g2, prediction, true, simplex)
69}
parry3d/query/closest_points/closest_points_support_map_support_map.rs

parry3d/query/closest_points/
closest_points_support_map_support_map.rs
