parry2d/query/gjk/
voronoi_simplex2.rs

1use crate::math::{Point, Real};
2use crate::query::gjk::{self, CSOPoint};
3use crate::query::{PointQuery, PointQueryWithLocation};
4use crate::shape::{Segment, SegmentPointLocation, Triangle, TrianglePointLocation};
5
6/// A simplex of dimension up to 2 using Voronoï regions for computing point projections.
7#[derive(Clone, Debug)]
8pub struct VoronoiSimplex {
9    prev_vertices: [usize; 3],
10    prev_dim: usize,
11    prev_proj: [Real; 2],
12
13    vertices: [CSOPoint; 3],
14    proj: [Real; 2],
15    dim: usize,
16}
17
18impl Default for VoronoiSimplex {
19    fn default() -> Self {
20        Self::new()
21    }
22}
23
24impl VoronoiSimplex {
25    /// Crates a new empty simplex.
26    pub fn new() -> VoronoiSimplex {
27        VoronoiSimplex {
28            prev_vertices: [0, 1, 2],
29            prev_proj: [0.0; 2],
30            prev_dim: 0,
31            vertices: [CSOPoint::origin(); 3],
32            proj: [0.0; 2],
33            dim: 0,
34        }
35    }
36
37    /// Swap two vertices of this simplex.
38    pub fn swap(&mut self, i1: usize, i2: usize) {
39        self.vertices.swap(i1, i2);
40        self.prev_vertices.swap(i1, i2);
41    }
42
43    /// Resets this simplex to a single point.
44    pub fn reset(&mut self, pt: CSOPoint) {
45        self.prev_dim = 0;
46        self.dim = 0;
47        self.vertices[0] = pt;
48    }
49
50    /// Add a point to this simplex.
51    pub fn add_point(&mut self, pt: CSOPoint) -> bool {
52        self.prev_dim = self.dim;
53        self.prev_proj = self.proj;
54        self.prev_vertices = [0, 1, 2];
55
56        for i in 0..self.dim + 1 {
57            if (self.vertices[i].point - pt.point).norm_squared() < gjk::eps_tol() {
58                return false;
59            }
60        }
61
62        self.dim += 1;
63        self.vertices[self.dim] = pt;
64        true
65    }
66
67    /// Retrieves the barycentric coordinate associated to the `i`-th by the last call to `project_origin_and_reduce`.
68    pub fn proj_coord(&self, i: usize) -> Real {
69        assert!(i <= self.dim, "Index out of bounds.");
70        self.proj[i]
71    }
72
73    /// The i-th point of this simplex.
74    pub fn point(&self, i: usize) -> &CSOPoint {
75        assert!(i <= self.dim, "Index out of bounds.");
76        &self.vertices[i]
77    }
78
79    /// Retrieves the barycentric coordinate associated to the `i`-th before the last call to `project_origin_and_reduce`.
80    pub fn prev_proj_coord(&self, i: usize) -> Real {
81        assert!(i <= self.prev_dim, "Index out of bounds.");
82        self.prev_proj[i]
83    }
84
85    /// The i-th point of the simplex before the last call to `project_origin_and_reduce`.
86    pub fn prev_point(&self, i: usize) -> &CSOPoint {
87        assert!(i <= self.prev_dim, "Index out of bounds.");
88        &self.vertices[self.prev_vertices[i]]
89    }
90
91    /// Projects the origin on the boundary of this simplex and reduces `self` the smallest subsimplex containing the origin.
92    ///
93    /// Returns the result of the projection or Point::origin() if the origin lies inside of the simplex.
94    /// The state of the simplex before projection is saved, and can be retrieved using the methods prefixed
95    /// by `prev_`.
96    pub fn project_origin_and_reduce(&mut self) -> Point<Real> {
97        if self.dim == 0 {
98            self.proj[0] = 1.0;
99            self.vertices[0].point
100        } else if self.dim == 1 {
101            let (proj, location) = Segment::new(self.vertices[0].point, self.vertices[1].point)
102                .project_local_point_and_get_location(&Point::<Real>::origin(), true);
103
104            match location {
105                SegmentPointLocation::OnVertex(0) => {
106                    self.proj[0] = 1.0;
107                    self.dim = 0;
108                }
109                SegmentPointLocation::OnVertex(1) => {
110                    self.proj[0] = 1.0;
111                    self.swap(0, 1);
112                    self.dim = 0;
113                }
114                SegmentPointLocation::OnEdge(coords) => {
115                    self.proj = coords;
116                }
117                _ => unreachable!(),
118            }
119
120            proj.point
121        } else {
122            assert!(self.dim == 2);
123            let (proj, location) = Triangle::new(
124                self.vertices[0].point,
125                self.vertices[1].point,
126                self.vertices[2].point,
127            )
128            .project_local_point_and_get_location(&Point::<Real>::origin(), true);
129
130            match location {
131                TrianglePointLocation::OnVertex(i) => {
132                    self.swap(0, i as usize);
133                    self.proj[0] = 1.0;
134                    self.dim = 0;
135                }
136                TrianglePointLocation::OnEdge(0, coords) => {
137                    self.proj = coords;
138                    self.dim = 1;
139                }
140                TrianglePointLocation::OnEdge(1, coords) => {
141                    self.swap(0, 2);
142                    self.proj[0] = coords[1];
143                    self.proj[1] = coords[0];
144                    self.dim = 1;
145                }
146                TrianglePointLocation::OnEdge(2, coords) => {
147                    self.swap(1, 2);
148                    self.proj = coords;
149                    self.dim = 1;
150                }
151                _ => {}
152            }
153
154            proj.point
155        }
156    }
157
158    /// Compute the projection of the origin on the boundary of this simplex.
159    pub fn project_origin(&mut self) -> Point<Real> {
160        if self.dim == 0 {
161            self.vertices[0].point
162        } else if self.dim == 1 {
163            let seg = Segment::new(self.vertices[0].point, self.vertices[1].point);
164            seg.project_local_point(&Point::<Real>::origin(), true)
165                .point
166        } else {
167            assert!(self.dim == 2);
168            let tri = Triangle::new(
169                self.vertices[0].point,
170                self.vertices[1].point,
171                self.vertices[2].point,
172            );
173            tri.project_local_point(&Point::<Real>::origin(), true)
174                .point
175        }
176    }
177
178    /// Tests if the given point is already a vertex of this simplex.
179    pub fn contains_point(&self, pt: &Point<Real>) -> bool {
180        for i in 0..self.dim + 1 {
181            if self.vertices[i].point == *pt {
182                return true;
183            }
184        }
185
186        false
187    }
188
189    /// The dimension of the smallest subspace that can contain this simplex.
190    pub fn dimension(&self) -> usize {
191        self.dim
192    }
193
194    /// The dimension of the simplex before the last call to `project_origin_and_reduce`.
195    pub fn prev_dimension(&self) -> usize {
196        self.prev_dim
197    }
198
199    /// The maximum squared length of the vertices of this simplex.
200    pub fn max_sq_len(&self) -> Real {
201        let mut max_sq_len = 0.0;
202
203        for i in 0..self.dim + 1 {
204            let norm = self.vertices[i].point.coords.norm_squared();
205
206            if norm > max_sq_len {
207                max_sq_len = norm
208            }
209        }
210
211        max_sq_len
212    }
213
214    /// Apply a function to all the vertices of this simplex.
215    pub fn modify_pnts(&mut self, f: &dyn Fn(&mut CSOPoint)) {
216        for i in 0..self.dim + 1 {
217            f(&mut self.vertices[i])
218        }
219    }
220}
parry2d/query/gjk/voronoi_simplex2.rs

parry2d/query/gjk/
voronoi_simplex2.rs
