ncollide2d_updated/query/algorithms/
voronoi_simplex2.rs

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

ncollide2d_updated/query/algorithms/
voronoi_simplex2.rs
