parry3d/query/closest_points/
closest_points_halfspace_support_map.rs1use crate::math::{Isometry, Real};
2use crate::query::ClosestPoints;
3use crate::shape::HalfSpace;
4use crate::shape::SupportMap;
5
6pub 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
34pub 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}