parry3d/query/closest_points/
closest_points_halfspace_support_map.rs

1use crate::math::{Isometry, Real};
2use crate::query::ClosestPoints;
3use crate::shape::HalfSpace;
4use crate::shape::SupportMap;
5
6/// Closest points between a halfspace and a support-mapped shape (Cuboid, ConvexHull, etc.)
7pub fn closest_points_halfspace_support_map<G: ?Sized + SupportMap>(
8    pos12: &Isometry<Real>,
9    halfspace: &HalfSpace,
10    other: &G,
11    margin: Real,
12) -> ClosestPoints {
13    assert!(
14        margin >= 0.0,
15        "The proximity margin must be positive or null."
16    );
17
18    let deepest = other.support_point(pos12, &-halfspace.normal);
19    let distance = halfspace.normal.dot(&(-deepest.coords));
20
21    if distance >= -margin {
22        if distance >= 0.0 {
23            ClosestPoints::Intersecting
24        } else {
25            let p1 = deepest + *halfspace.normal * distance;
26            let p2 = pos12.inverse_transform_point(&deepest);
27            ClosestPoints::WithinMargin(p1, p2)
28        }
29    } else {
30        ClosestPoints::Disjoint
31    }
32}
33
34/// Closest points between a support-mapped shape (Cuboid, ConvexHull, etc.) and a halfspace.
35pub fn closest_points_support_map_halfspace<G: ?Sized + SupportMap>(
36    pos12: &Isometry<Real>,
37    other: &G,
38    halfspace: &HalfSpace,
39    margin: Real,
40) -> ClosestPoints {
41    closest_points_halfspace_support_map(&pos12.inverse(), halfspace, other, margin).flipped()
42}