1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98

use crate::math::{Isometry, Point, Real, Vector};
use crate::shape::SupportMap;
use na::Unit;
use std::ops::Sub;

/// A point of a Configuration-Space Obstacle.
///
/// A Configuration-Space Obstacle (CSO) is the result of the
/// Minkowski Difference of two solids. In other words, each of its
/// points correspond to the difference of two point, each belonging
/// to a different solid.
#[derive(Copy, Clone, Debug, PartialEq)]
pub struct CSOPoint {
    /// The point on the CSO. This is equal to `self.orig1 - self.orig2`, unless this CSOPoint
    /// has been translated with self.translate.
    pub point: Point<Real>,
    /// The original point on the first shape used to compute `self.point`.
    pub orig1: Point<Real>,
    /// The original point on the second shape used to compute `self.point`.
    pub orig2: Point<Real>,
}

impl CSOPoint {
    /// Initializes a CSO point with `orig1 - orig2`.
    pub fn new(orig1: Point<Real>, orig2: Point<Real>) -> Self {
        let point = Point::from(orig1 - orig2);
        Self::new_with_point(point, orig1, orig2)
    }

    /// Initializes a CSO point with all information provided.
    ///
    /// It is assumed, but not checked, that `point == orig1 - orig2`.
    pub fn new_with_point(point: Point<Real>, orig1: Point<Real>, orig2: Point<Real>) -> Self {
        CSOPoint {
            point,
            orig1,
            orig2,
        }
    }

    /// Initializes a CSO point where both original points are equal.
    pub fn single_point(point: Point<Real>) -> Self {
        Self::new_with_point(point, point, Point::origin())
    }

    /// CSO point where all components are set to zero.
    pub fn origin() -> Self {
        CSOPoint::new(Point::origin(), Point::origin())
    }

    /// Computes the support point of the CSO of `g1` and `g2` toward the unit direction `dir`.
    pub fn from_shapes_toward<G1, G2>(
        pos12: &Isometry<Real>,
        g1: &G1,
        g2: &G2,
        dir: &Unit<Vector<Real>>,
    ) -> Self
    where
        G1: ?Sized + SupportMap,
        G2: ?Sized + SupportMap,
    {
        let sp1 = g1.local_support_point_toward(dir);
        let sp2 = g2.support_point_toward(pos12, &-*dir);

        CSOPoint::new(sp1, sp2)
    }

    /// Computes the support point of the CSO of `g1` and `g2` toward the direction `dir`.
    pub fn from_shapes<G1, G2>(pos12: &Isometry<Real>, g1: &G1, g2: &G2, dir: &Vector<Real>) -> Self
    where
        G1: ?Sized + SupportMap,
        G2: ?Sized + SupportMap,
    {
        let sp1 = g1.local_support_point(dir);
        let sp2 = g2.support_point(pos12, &-*dir);

        CSOPoint::new(sp1, sp2)
    }

    /// Translate the CSO point.
    pub fn translate(&self, dir: &Vector<Real>) -> Self {
        CSOPoint::new_with_point(self.point + dir, self.orig1, self.orig2)
    }

    /// Translate in-place the CSO point.
    pub fn translate_mut(&mut self, dir: &Vector<Real>) {
        self.point += dir;
    }
}

impl Sub<CSOPoint> for CSOPoint {
    type Output = Vector<Real>;

    #[inline]
    fn sub(self, rhs: CSOPoint) -> Vector<Real> {
        self.point - rhs.point
    }
}