parry2d/query/closest_points/
closest_points_support_map_support_map.rs

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

parry2d/query/closest_points/
closest_points_support_map_support_map.rs
