parry3d/query/gjk/
voronoi_simplex3.rs

1use crate::math::{Real, Vector};
2use crate::query::gjk::{self, CsoPoint};
3use crate::query::{PointQuery, PointQueryWithLocation};
4use crate::shape::{
5    Segment, SegmentPointLocation, Tetrahedron, TetrahedronPointLocation, Triangle,
6    TrianglePointLocation,
7};
8
9/// A simplex of dimension up to 3 that uses Voronoï regions for computing point projections.
10#[derive(Clone, Debug)]
11pub struct VoronoiSimplex {
12    prev_vertices: [usize; 4],
13    prev_proj: [Real; 3],
14    prev_dim: usize,
15
16    vertices: [CsoPoint; 4],
17    proj: [Real; 3],
18    dim: usize,
19}
20
21impl Default for VoronoiSimplex {
22    fn default() -> Self {
23        Self::new()
24    }
25}
26
27impl VoronoiSimplex {
28    /// Creates a new empty simplex.
29    pub fn new() -> VoronoiSimplex {
30        VoronoiSimplex {
31            prev_vertices: [0, 1, 2, 3],
32            prev_proj: [0.0; 3],
33            prev_dim: 0,
34            vertices: [CsoPoint::ZERO; 4],
35            proj: [0.0; 3],
36            dim: 0,
37        }
38    }
39
40    /// Swap two vertices of this simplex.
41    pub fn swap(&mut self, i1: usize, i2: usize) {
42        self.vertices.swap(i1, i2);
43        self.prev_vertices.swap(i1, i2);
44    }
45
46    /// Resets this simplex to a single point.
47    pub fn reset(&mut self, pt: CsoPoint) {
48        self.dim = 0;
49        self.prev_dim = 0;
50        self.vertices[0] = pt;
51    }
52
53    /// Add a point to this simplex.
54    pub fn add_point(&mut self, pt: CsoPoint) -> bool {
55        self.prev_dim = self.dim;
56        self.prev_proj = self.proj;
57        self.prev_vertices = [0, 1, 2, 3];
58
59        match self.dim {
60            0 => {
61                if (self.vertices[0] - pt).length_squared() < gjk::eps_tol() {
62                    return false;
63                }
64            }
65            1 => {
66                let ab = self.vertices[1] - self.vertices[0];
67                let ac = pt - self.vertices[0];
68
69                if ab.cross(ac).length_squared() < gjk::eps_tol() {
70                    return false;
71                }
72            }
73            2 => {
74                let ab = self.vertices[1] - self.vertices[0];
75                let ac = self.vertices[2] - self.vertices[0];
76                let ap = pt - self.vertices[0];
77                let n = ab.cross(ac).normalize();
78
79                if n.dot(ap).abs() < gjk::eps_tol() {
80                    return false;
81                }
82            }
83            _ => unreachable!(),
84        }
85
86        self.dim += 1;
87        self.vertices[self.dim] = pt;
88        true
89    }
90
91    /// Retrieves the barycentric coordinate associated to the `i`-th by the last call to `project_origin_and_reduce`.
92    pub fn proj_coord(&self, i: usize) -> Real {
93        assert!(i <= self.dim, "Index out of bounds.");
94        self.proj[i]
95    }
96
97    /// The i-th point of this simplex.
98    pub fn point(&self, i: usize) -> &CsoPoint {
99        assert!(i <= self.dim, "Index out of bounds.");
100        &self.vertices[i]
101    }
102
103    /// Retrieves the barycentric coordinate associated to the `i`-th before the last call to `project_origin_and_reduce`.
104    pub fn prev_proj_coord(&self, i: usize) -> Real {
105        assert!(i <= self.prev_dim, "Index out of bounds.");
106        self.prev_proj[i]
107    }
108
109    /// The i-th point of the simplex before the last call to `project_origin_and_reduce`.
110    pub fn prev_point(&self, i: usize) -> &CsoPoint {
111        assert!(i <= self.prev_dim, "Index out of bounds.");
112        &self.vertices[self.prev_vertices[i]]
113    }
114
115    /// Projects the origin on the boundary of this simplex and reduces `self` the smallest subsimplex containing the origin.
116    ///
117    /// Returns the result of the projection or `Vector::ZERO` if the origin lies inside of the simplex.
118    /// The state of the simplex before projection is saved, and can be retrieved using the methods prefixed
119    /// by `prev_`.
120    pub fn project_origin_and_reduce(&mut self) -> Vector {
121        if self.dim == 0 {
122            self.proj[0] = 1.0;
123            self.vertices[0].point
124        } else if self.dim == 1 {
125            let (proj, location) = Segment::new(self.vertices[0].point, self.vertices[1].point)
126                .project_local_point_and_get_location(Vector::ZERO, true);
127
128            match location {
129                SegmentPointLocation::OnVertex(0) => {
130                    self.proj[0] = 1.0;
131                    self.dim = 0;
132                }
133                SegmentPointLocation::OnVertex(1) => {
134                    self.swap(0, 1);
135                    self.proj[0] = 1.0;
136                    self.dim = 0;
137                }
138                SegmentPointLocation::OnEdge(coords) => {
139                    self.proj[0] = coords[0];
140                    self.proj[1] = coords[1];
141                }
142                _ => unreachable!(),
143            }
144
145            proj.point
146        } else if self.dim == 2 {
147            let (proj, location) = Triangle::new(
148                self.vertices[0].point,
149                self.vertices[1].point,
150                self.vertices[2].point,
151            )
152            .project_local_point_and_get_location(Vector::ZERO, true);
153
154            match location {
155                TrianglePointLocation::OnVertex(i) => {
156                    self.swap(0, i as usize);
157                    self.proj[0] = 1.0;
158                    self.dim = 0;
159                }
160                TrianglePointLocation::OnEdge(0, coords) => {
161                    self.proj[0] = coords[0];
162                    self.proj[1] = coords[1];
163                    self.dim = 1;
164                }
165                TrianglePointLocation::OnEdge(1, coords) => {
166                    self.swap(0, 2);
167                    self.proj[0] = coords[1];
168                    self.proj[1] = coords[0];
169                    self.dim = 1;
170                }
171                TrianglePointLocation::OnEdge(2, coords) => {
172                    self.swap(1, 2);
173                    self.proj[0] = coords[0];
174                    self.proj[1] = coords[1];
175                    self.dim = 1;
176                }
177                TrianglePointLocation::OnFace(_, coords) => {
178                    self.proj = coords;
179                }
180                _ => {}
181            }
182
183            proj.point
184        } else {
185            assert!(self.dim == 3);
186            let (proj, location) = Tetrahedron::new(
187                self.vertices[0].point,
188                self.vertices[1].point,
189                self.vertices[2].point,
190                self.vertices[3].point,
191            )
192            .project_local_point_and_get_location(Vector::ZERO, true);
193
194            match location {
195                TetrahedronPointLocation::OnVertex(i) => {
196                    self.swap(0, i as usize);
197                    self.proj[0] = 1.0;
198                    self.dim = 0;
199                }
200                TetrahedronPointLocation::OnEdge(i, coords) => {
201                    match i {
202                        0 => {
203                            // ab
204                        }
205                        1 => {
206                            // ac
207                            self.swap(1, 2)
208                        }
209                        2 => {
210                            // ad
211                            self.swap(1, 3)
212                        }
213                        3 => {
214                            // bc
215                            self.swap(0, 2)
216                        }
217                        4 => {
218                            // bd
219                            self.swap(0, 3)
220                        }
221                        5 => {
222                            // cd
223                            self.swap(0, 2);
224                            self.swap(1, 3);
225                        }
226                        _ => unreachable!(),
227                    }
228
229                    match i {
230                        0 | 1 | 2 | 5 => {
231                            self.proj[0] = coords[0];
232                            self.proj[1] = coords[1];
233                        }
234                        3 | 4 => {
235                            self.proj[0] = coords[1];
236                            self.proj[1] = coords[0];
237                        }
238                        _ => unreachable!(),
239                    }
240                    self.dim = 1;
241                }
242                TetrahedronPointLocation::OnFace(i, coords) => {
243                    match i {
244                        0 => {
245                            // abc
246                            self.proj = coords;
247                        }
248                        1 => {
249                            // abd
250                            self.vertices[2] = self.vertices[3];
251                            self.proj = coords;
252                        }
253                        2 => {
254                            // acd
255                            self.vertices[1] = self.vertices[3];
256                            self.proj[0] = coords[0];
257                            self.proj[1] = coords[2];
258                            self.proj[2] = coords[1];
259                        }
260                        3 => {
261                            // bcd
262                            self.vertices[0] = self.vertices[3];
263                            self.proj[0] = coords[2];
264                            self.proj[1] = coords[0];
265                            self.proj[2] = coords[1];
266                        }
267                        _ => unreachable!(),
268                    }
269                    self.dim = 2;
270                }
271                _ => {}
272            }
273
274            proj.point
275        }
276    }
277
278    /// Compute the projection of the origin on the boundary of this simplex.
279    pub fn project_origin(&mut self) -> Vector {
280        if self.dim == 0 {
281            self.vertices[0].point
282        } else if self.dim == 1 {
283            let seg = Segment::new(self.vertices[0].point, self.vertices[1].point);
284            seg.project_local_point(Vector::ZERO, true).point
285        } else if self.dim == 2 {
286            let tri = Triangle::new(
287                self.vertices[0].point,
288                self.vertices[1].point,
289                self.vertices[2].point,
290            );
291            tri.project_local_point(Vector::ZERO, true).point
292        } else {
293            let tetr = Tetrahedron::new(
294                self.vertices[0].point,
295                self.vertices[1].point,
296                self.vertices[2].point,
297                self.vertices[3].point,
298            );
299            tetr.project_local_point(Vector::ZERO, true).point
300        }
301    }
302
303    /// Tests if the given point is already a vertex of this simplex.
304    pub fn contains_point(&self, pt: Vector) -> bool {
305        for i in 0..self.dim + 1 {
306            if self.vertices[i].point == pt {
307                return true;
308            }
309        }
310
311        false
312    }
313
314    /// The dimension of the smallest subspace that can contain this simplex.
315    pub fn dimension(&self) -> usize {
316        self.dim
317    }
318
319    /// The dimension of the simplex before the last call to `project_origin_and_reduce`.
320    pub fn prev_dimension(&self) -> usize {
321        self.prev_dim
322    }
323
324    /// The maximum squared length of the vertices of this simplex.
325    pub fn max_sq_len(&self) -> Real {
326        let mut max_sq_len = 0.0;
327
328        for i in 0..self.dim + 1 {
329            let norm = self.vertices[i].point.length_squared();
330
331            if norm > max_sq_len {
332                max_sq_len = norm
333            }
334        }
335
336        max_sq_len
337    }
338
339    /// Apply a function to all the vertices of this simplex.
340    pub fn modify_pnts(&mut self, f: &dyn Fn(&mut CsoPoint)) {
341        for i in 0..self.dim + 1 {
342            f(&mut self.vertices[i])
343        }
344    }
345}
parry3d/query/gjk/voronoi_simplex3.rs

parry3d/query/gjk/
voronoi_simplex3.rs
