ncollide3d/query/algorithms/
voronoi_simplex3.rs

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

ncollide3d/query/algorithms/
voronoi_simplex3.rs
