parry3d/query/gjk/
cso_point.rs

1use crate::math::{Isometry, Point, Real, Vector};
2use crate::shape::SupportMap;
3use core::ops::Sub;
4use na::Unit;
5
6/// A point of a Configuration-Space Obstacle.
7///
8/// A Configuration-Space Obstacle (CSO) is the result of the
9/// Minkowski Difference of two solids. In other words, each of its
10/// points correspond to the difference of two point, each belonging
11/// to a different solid.
12#[derive(Copy, Clone, Debug, PartialEq)]
13pub struct CSOPoint {
14    /// The point on the CSO. This is equal to `self.orig1 - self.orig2`, unless this CSOPoint
15    /// has been translated with self.translate.
16    pub point: Point<Real>,
17    /// The original point on the first shape used to compute `self.point`.
18    pub orig1: Point<Real>,
19    /// The original point on the second shape used to compute `self.point`.
20    pub orig2: Point<Real>,
21}
22
23impl CSOPoint {
24    /// Initializes a CSO point with `orig1 - orig2`.
25    pub fn new(orig1: Point<Real>, orig2: Point<Real>) -> Self {
26        let point = Point::from(orig1 - orig2);
27        Self::new_with_point(point, orig1, orig2)
28    }
29
30    /// Initializes a CSO point with all information provided.
31    ///
32    /// It is assumed, but not checked, that `point == orig1 - orig2`.
33    pub fn new_with_point(point: Point<Real>, orig1: Point<Real>, orig2: Point<Real>) -> Self {
34        CSOPoint {
35            point,
36            orig1,
37            orig2,
38        }
39    }
40
41    /// Initializes a CSO point where both original points are equal.
42    pub fn single_point(point: Point<Real>) -> Self {
43        Self::new_with_point(point, point, Point::origin())
44    }
45
46    /// CSO point where all components are set to zero.
47    pub fn origin() -> Self {
48        CSOPoint::new(Point::origin(), Point::origin())
49    }
50
51    /// Computes the support point of the CSO of `g1` and `g2` toward the unit direction `dir`.
52    pub fn from_shapes_toward<G1, G2>(
53        pos12: &Isometry<Real>,
54        g1: &G1,
55        g2: &G2,
56        dir: &Unit<Vector<Real>>,
57    ) -> Self
58    where
59        G1: ?Sized + SupportMap,
60        G2: ?Sized + SupportMap,
61    {
62        let sp1 = g1.local_support_point_toward(dir);
63        let sp2 = g2.support_point_toward(pos12, &-*dir);
64
65        CSOPoint::new(sp1, sp2)
66    }
67
68    /// Computes the support point of the CSO of `g1` and `g2` toward the direction `dir`.
69    pub fn from_shapes<G1, G2>(pos12: &Isometry<Real>, g1: &G1, g2: &G2, dir: &Vector<Real>) -> Self
70    where
71        G1: ?Sized + SupportMap,
72        G2: ?Sized + SupportMap,
73    {
74        let sp1 = g1.local_support_point(dir);
75        let sp2 = g2.support_point(pos12, &-*dir);
76
77        CSOPoint::new(sp1, sp2)
78    }
79
80    /// Translate the CSO point.
81    pub fn translate(&self, dir: &Vector<Real>) -> Self {
82        CSOPoint::new_with_point(self.point + dir, self.orig1, self.orig2)
83    }
84
85    /// Translate in-place the CSO point.
86    pub fn translate_mut(&mut self, dir: &Vector<Real>) {
87        self.point += dir;
88    }
89}
90
91impl Sub<CSOPoint> for CSOPoint {
92    type Output = Vector<Real>;
93
94    #[inline]
95    fn sub(self, rhs: CSOPoint) -> Vector<Real> {
96        self.point - rhs.point
97    }
98}
parry3d/query/gjk/cso_point.rs

parry3d/query/gjk/
cso_point.rs
