parry3d/query/gjk/
voronoi_simplex3.rs

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

parry3d/query/gjk/
voronoi_simplex3.rs
