parry3d/query/gjk/
cso_point.rs

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

parry3d/query/gjk/
cso_point.rs
