parry3d/query/gjk/
special_support_maps.rs

1use na::Unit;
2
3use crate::math::{Isometry, Point, Real, Vector};
4use crate::shape::SupportMap;
5
6/// A support mapping that is a single point.
7pub struct ConstantPoint(pub Point<Real>);
8
9impl SupportMap for ConstantPoint {
10    #[inline]
11    fn support_point(&self, m: &Isometry<Real>, _: &Vector<Real>) -> Point<Real> {
12        m * self.0
13    }
14
15    #[inline]
16    fn support_point_toward(&self, m: &Isometry<Real>, _: &Unit<Vector<Real>>) -> Point<Real> {
17        m * self.0
18    }
19
20    #[inline]
21    fn local_support_point(&self, _: &Vector<Real>) -> Point<Real> {
22        self.0
23    }
24
25    #[inline]
26    fn local_support_point_toward(&self, _: &Unit<Vector<Real>>) -> Point<Real> {
27        self.0
28    }
29}
30
31/// A support mapping that is the point at (0.0, 0.0, 0.0).
32pub struct ConstantOrigin;
33
34impl SupportMap for ConstantOrigin {
35    #[inline]
36    fn support_point(&self, m: &Isometry<Real>, _: &Vector<Real>) -> Point<Real> {
37        m.translation.vector.into()
38    }
39
40    #[inline]
41    fn support_point_toward(&self, m: &Isometry<Real>, _: &Unit<Vector<Real>>) -> Point<Real> {
42        m.translation.vector.into()
43    }
44
45    #[inline]
46    fn local_support_point(&self, _: &Vector<Real>) -> Point<Real> {
47        Point::origin()
48    }
49
50    #[inline]
51    fn local_support_point_toward(&self, _: &Unit<Vector<Real>>) -> Point<Real> {
52        Point::origin()
53    }
54}
55
56/// The Minkowski sum of a shape and a ball.
57pub struct DilatedShape<'a, S: ?Sized + SupportMap> {
58    /// The shape involved in the Minkowski sum.
59    pub shape: &'a S,
60    /// The radius of the ball involved in the Minkoski sum.
61    pub radius: Real,
62}
63
64impl<S: ?Sized + SupportMap> SupportMap for DilatedShape<'_, S> {
65    #[inline]
66    fn support_point(&self, m: &Isometry<Real>, dir: &Vector<Real>) -> Point<Real> {
67        self.support_point_toward(m, &Unit::new_normalize(*dir))
68    }
69
70    #[inline]
71    fn support_point_toward(&self, m: &Isometry<Real>, dir: &Unit<Vector<Real>>) -> Point<Real> {
72        self.shape.support_point_toward(m, dir) + **dir * self.radius
73    }
74
75    #[inline]
76    fn local_support_point(&self, dir: &Vector<Real>) -> Point<Real> {
77        self.local_support_point_toward(&Unit::new_normalize(*dir))
78    }
79
80    #[inline]
81    fn local_support_point_toward(&self, dir: &Unit<Vector<Real>>) -> Point<Real> {
82        self.shape.local_support_point_toward(dir) + **dir * self.radius
83    }
84}