ncollide3d/query/algorithms/
cso_point.rs

1use crate::math::{Isometry, Point, Vector};
2use crate::shape::SupportMap;
3use na::{RealField, Unit};
4use std::ops::Sub;
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<N: RealField + Copy> {
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<N>,
17    /// The original point on the first shape used to compute `self.point`.
18    pub orig1: Point<N>,
19    /// The original point on the second shape used to compute `self.point`.
20    pub orig2: Point<N>,
21}
22
23impl<N: RealField + Copy> CSOPoint<N> {
24    /// Initializes a CSO point with `orig1 - orig2`.
25    pub fn new(orig1: Point<N>, orig2: Point<N>) -> 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<N>, orig1: Point<N>, orig2: Point<N>) -> 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<N>) -> 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: ?Sized, G2: ?Sized>(
53        m1: &Isometry<N>,
54        g1: &G1,
55        m2: &Isometry<N>,
56        g2: &G2,
57        dir: &Unit<Vector<N>>,
58    ) -> Self
59    where
60        G1: SupportMap<N>,
61        G2: SupportMap<N>,
62    {
63        let sp1 = g1.support_point_toward(m1, dir);
64        let sp2 = g2.support_point_toward(m2, &-*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: ?Sized, G2: ?Sized>(
71        m1: &Isometry<N>,
72        g1: &G1,
73        m2: &Isometry<N>,
74        g2: &G2,
75        dir: &Vector<N>,
76    ) -> Self
77    where
78        G1: SupportMap<N>,
79        G2: SupportMap<N>,
80    {
81        let sp1 = g1.support_point(m1, dir);
82        let sp2 = g2.support_point(m2, &-*dir);
83
84        CSOPoint::new(sp1, sp2)
85    }
86
87    /// Translate the CSO point.
88    pub fn translate(&self, dir: &Vector<N>) -> Self {
89        CSOPoint::new_with_point(self.point + dir, self.orig1, self.orig2)
90    }
91
92    /// Translate in-place the CSO point.
93    pub fn translate_mut(&mut self, dir: &Vector<N>) {
94        self.point += dir;
95    }
96}
97
98impl<N: RealField + Copy> Sub<CSOPoint<N>> for CSOPoint<N> {
99    type Output = Vector<N>;
100
101    #[inline]
102    fn sub(self, rhs: CSOPoint<N>) -> Vector<N> {
103        self.point - rhs.point
104    }
105}
ncollide3d/query/algorithms/cso_point.rs

ncollide3d/query/algorithms/
cso_point.rs
