ncollide3d/query/algorithms/
epa3.rs

Help
1//! Three-dimensional penetration depth queries using the Expanding Polytope Algorithm.
2
3use crate::math::{Isometry, Point, Vector};
4use crate::query::algorithms::special_support_maps::ConstantOrigin;
5use crate::query::algorithms::{gjk, CSOPoint, VoronoiSimplex};
6use crate::query::PointQueryWithLocation;
7use crate::shape::{SupportMap, Triangle, TrianglePointLocation};
8use crate::utils;
9use na::{self, RealField, Unit};
10use std::cmp::Ordering;
11use std::collections::BinaryHeap;
12
13#[derive(Copy, Clone, PartialEq)]
14struct FaceId<N: RealField + Copy> {
15    id: usize,
16    neg_dist: N,
17}
18
19impl<N: RealField + Copy> FaceId<N> {
20    fn new(id: usize, neg_dist: N) -> Option<Self> {
21        if neg_dist > gjk::eps_tol() {
22            None
23        } else {
24            Some(FaceId { id, neg_dist })
25        }
26    }
27}
28
29impl<N: RealField + Copy> Eq for FaceId<N> {}
30
31impl<N: RealField + Copy> PartialOrd for FaceId<N> {
32    #[inline]
33    fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
34        self.neg_dist.partial_cmp(&other.neg_dist)
35    }
36}
37
38impl<N: RealField + Copy> Ord for FaceId<N> {
39    #[inline]
40    fn cmp(&self, other: &Self) -> Ordering {
41        if self.neg_dist < other.neg_dist {
42            Ordering::Less
43        } else if self.neg_dist > other.neg_dist {
44            Ordering::Greater
45        } else {
46            Ordering::Equal
47        }
48    }
49}
50
51#[derive(Clone, Debug)]
52struct Face<N: RealField + Copy> {
53    pts: [usize; 3],
54    adj: [usize; 3],
55    normal: Unit<Vector<N>>,
56    #[allow(dead_code)]
57    proj: Point<N>,
58    bcoords: [N; 3],
59    deleted: bool,
60}
61
62impl<N: RealField + Copy> Face<N> {
63    pub fn new_with_proj(
64        vertices: &[CSOPoint<N>],
65        proj: Point<N>,
66        bcoords: [N; 3],
67        pts: [usize; 3],
68        adj: [usize; 3],
69    ) -> Self {
70        let normal;
71
72        if let Some(n) = utils::ccw_face_normal([
73            &vertices[pts[0]].point,
74            &vertices[pts[1]].point,
75            &vertices[pts[2]].point,
76        ]) {
77            normal = n;
78        } else {
79            // This is a bit of a hack for degenerate faces.
80            // TODO: It will work OK with our current code, though
81            // we should do this in another way to avoid any risk
82            // of misusing the face normal in the future.
83            normal = Unit::new_unchecked(na::zero());
84        }
85
86        Face {
87            pts,
88            proj,
89            bcoords,
90            adj,
91            normal,
92            deleted: false,
93        }
94    }
95
96    pub fn new(vertices: &[CSOPoint<N>], pts: [usize; 3], adj: [usize; 3]) -> (Self, bool) {
97        let tri = Triangle::new(
98            vertices[pts[0]].point,
99            vertices[pts[1]].point,
100            vertices[pts[2]].point,
101        );
102        let (proj, loc) =
103            tri.project_point_with_location(&Isometry::identity(), &Point::origin(), true);
104
105        match loc {
106            TrianglePointLocation::OnFace(_, bcoords) => (
107                Self::new_with_proj(vertices, proj.point, bcoords, pts, adj),
108                true,
109            ),
110            _ => (
111                Self::new_with_proj(vertices, proj.point, [N::zero(); 3], pts, adj),
112                false,
113            ),
114        }
115    }
116
117    pub fn closest_points(&self, vertices: &[CSOPoint<N>]) -> (Point<N>, Point<N>) {
118        (
119            vertices[self.pts[0]].orig1 * self.bcoords[0]
120                + vertices[self.pts[1]].orig1.coords * self.bcoords[1]
121                + vertices[self.pts[2]].orig1.coords * self.bcoords[2],
122            vertices[self.pts[0]].orig2 * self.bcoords[0]
123                + vertices[self.pts[1]].orig2.coords * self.bcoords[1]
124                + vertices[self.pts[2]].orig2.coords * self.bcoords[2],
125        )
126    }
127
128    pub fn contains_point(&self, id: usize) -> bool {
129        self.pts[0] == id || self.pts[1] == id || self.pts[2] == id
130    }
131
132    pub fn next_ccw_pt_id(&self, id: usize) -> usize {
133        if self.pts[0] == id {
134            1
135        } else if self.pts[1] == id {
136            2
137        } else {
138            assert_eq!(self.pts[2], id);
139            0
140        }
141    }
142
143    pub fn can_be_seen_by(&self, vertices: &[CSOPoint<N>], point: usize, opp_pt_id: usize) -> bool {
144        let p0 = &vertices[self.pts[opp_pt_id]].point;
145        let p1 = &vertices[self.pts[(opp_pt_id + 1) % 3]].point;
146        let p2 = &vertices[self.pts[(opp_pt_id + 2) % 3]].point;
147        let pt = &vertices[point].point;
148
149        // NOTE: it is important that we return true for the case where
150        // the dot product is zero. This is because degenerate faces will
151        // have a zero normal, causing the dot product to be zero.
152        // So return true for these case will let us skip the triangle
153        // during silhouette computation.
154        (*pt - *p0).dot(&self.normal) >= -gjk::eps_tol::<N>()
155            || utils::is_affinely_dependent_triangle(p1, p2, pt)
156    }
157}
158
159struct SilhouetteEdge {
160    face_id: usize,
161    opp_pt_id: usize,
162}
163
164impl SilhouetteEdge {
165    pub fn new(face_id: usize, opp_pt_id: usize) -> Self {
166        SilhouetteEdge { face_id, opp_pt_id }
167    }
168}
169
170/// The Expanding Polytope Algorithm in 3D.
171pub struct EPA<N: RealField + Copy> {
172    vertices: Vec<CSOPoint<N>>,
173    faces: Vec<Face<N>>,
174    silhouette: Vec<SilhouetteEdge>,
175    heap: BinaryHeap<FaceId<N>>,
176}
177
178impl<N: RealField + Copy> EPA<N> {
179    /// Creates a new instance of the 3D Expanding Polytope Algorithm.
180    pub fn new() -> Self {
181        EPA {
182            vertices: Vec::new(),
183            faces: Vec::new(),
184            silhouette: Vec::new(),
185            heap: BinaryHeap::new(),
186        }
187    }
188
189    fn reset(&mut self) {
190        self.vertices.clear();
191        self.faces.clear();
192        self.heap.clear();
193        self.silhouette.clear();
194    }
195
196    /// Projects the origin on boundary of the given shape.
197    ///
198    /// The origin is assumed to be inside of the shape. If it is outside
199    /// use the GJK algorithm instead.
200    /// Return `None` if the origin is not inside of the shape or if
201    /// the EPA algorithm failed to compute the projection.
202    pub fn project_origin<G: ?Sized>(
203        &mut self,
204        m: &Isometry<N>,
205        g: &G,
206        simplex: &VoronoiSimplex<N>,
207    ) -> Option<Point<N>>
208    where
209        G: SupportMap<N>,
210    {
211        self.closest_points(m, g, &Isometry::identity(), &ConstantOrigin, simplex)
212            .map(|(p, _, _)| p)
213    }
214
215    /// Projects the origin on a shape unsing the EPA algorithm.
216    ///
217    /// The origin is assumed to be located inside of the shape.
218    /// Returns `None` if the EPA fails to converge or if `g1` and `g2` are not penetrating.
219    pub fn closest_points<G1: ?Sized, G2: ?Sized>(
220        &mut self,
221        m1: &Isometry<N>,
222        g1: &G1,
223        m2: &Isometry<N>,
224        g2: &G2,
225        simplex: &VoronoiSimplex<N>,
226    ) -> Option<(Point<N>, Point<N>, Unit<Vector<N>>)>
227    where
228        G1: SupportMap<N>,
229        G2: SupportMap<N>,
230    {
231        let _eps = N::default_epsilon();
232        let _eps_tol = _eps * na::convert(100.0f64);
233
234        self.reset();
235
236        /*
237         * Initialization.
238         */
239        for i in 0..simplex.dimension() + 1 {
240            self.vertices.push(*simplex.point(i));
241        }
242
243        if simplex.dimension() == 0 {
244            let mut n: Vector<N> = na::zero();
245            n[1] = na::one();
246            return Some((Point::origin(), Point::origin(), Unit::new_unchecked(n)));
247        } else if simplex.dimension() == 3 {
248            let dp1 = self.vertices[1] - self.vertices[0];
249            let dp2 = self.vertices[2] - self.vertices[0];
250            let dp3 = self.vertices[3] - self.vertices[0];
251
252            if dp1.cross(&dp2).dot(&dp3) > na::zero() {
253                self.vertices.swap(1, 2)
254            }
255
256            let pts1 = [0, 1, 2];
257            let pts2 = [1, 3, 2];
258            let pts3 = [0, 2, 3];
259            let pts4 = [0, 3, 1];
260
261            let adj1 = [3, 1, 2];
262            let adj2 = [3, 2, 0];
263            let adj3 = [0, 1, 3];
264            let adj4 = [2, 1, 0];
265
266            let (face1, proj_inside1) = Face::new(&self.vertices, pts1, adj1);
267            let (face2, proj_inside2) = Face::new(&self.vertices, pts2, adj2);
268            let (face3, proj_inside3) = Face::new(&self.vertices, pts3, adj3);
269            let (face4, proj_inside4) = Face::new(&self.vertices, pts4, adj4);
270
271            self.faces.push(face1);
272            self.faces.push(face2);
273            self.faces.push(face3);
274            self.faces.push(face4);
275
276            if proj_inside1 {
277                let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords);
278                self.heap.push(FaceId::new(0, -dist1)?);
279            }
280
281            if proj_inside2 {
282                let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords);
283                self.heap.push(FaceId::new(1, -dist2)?);
284            }
285
286            if proj_inside3 {
287                let dist3 = self.faces[2].normal.dot(&self.vertices[2].point.coords);
288                self.heap.push(FaceId::new(2, -dist3)?);
289            }
290
291            if proj_inside4 {
292                let dist4 = self.faces[3].normal.dot(&self.vertices[3].point.coords);
293                self.heap.push(FaceId::new(3, -dist4)?);
294            }
295        } else {
296            if simplex.dimension() == 1 {
297                let dpt = self.vertices[1] - self.vertices[0];
298
299                Vector::orthonormal_subspace_basis(&[dpt], |dir| {
300                    // XXX: dir should already be unit on nalgebra!
301                    let dir = Unit::new_unchecked(*dir);
302                    self.vertices
303                        .push(CSOPoint::from_shapes(m1, g1, m2, g2, &dir));
304                    false
305                });
306            }
307
308            let pts1 = [0, 1, 2];
309            let pts2 = [0, 2, 1];
310
311            let adj1 = [1, 1, 1];
312            let adj2 = [0, 0, 0];
313
314            let (face1, _) = Face::new(&self.vertices, pts1, adj1);
315            let (face2, _) = Face::new(&self.vertices, pts2, adj2);
316            self.faces.push(face1);
317            self.faces.push(face2);
318
319            self.heap.push(FaceId::new(0, na::zero())?);
320            self.heap.push(FaceId::new(1, na::zero())?);
321        }
322
323        let mut niter = 0;
324        let mut max_dist = N::max_value().unwrap();
325        let mut best_face_id = *self.heap.peek().unwrap();
326
327        /*
328         * Run the expansion.
329         */
330        while let Some(face_id) = self.heap.pop() {
331            // Create new faces.
332            let face = self.faces[face_id.id].clone();
333
334            if face.deleted {
335                continue;
336            }
337
338            let cso_point = CSOPoint::from_shapes(m1, g1, m2, g2, &face.normal);
339            let support_point_id = self.vertices.len();
340            self.vertices.push(cso_point);
341
342            let candidate_max_dist = cso_point.point.coords.dot(&face.normal);
343
344            if candidate_max_dist < max_dist {
345                best_face_id = face_id;
346                max_dist = candidate_max_dist;
347            }
348
349            let curr_dist = -face_id.neg_dist;
350
351            if max_dist - curr_dist < _eps_tol {
352                let best_face = &self.faces[best_face_id.id];
353                let points = best_face.closest_points(&self.vertices);
354                return Some((points.0, points.1, best_face.normal));
355            }
356
357            self.faces[face_id.id].deleted = true;
358
359            let adj_opp_pt_id1 = self.faces[face.adj[0]].next_ccw_pt_id(face.pts[0]);
360            let adj_opp_pt_id2 = self.faces[face.adj[1]].next_ccw_pt_id(face.pts[1]);
361            let adj_opp_pt_id3 = self.faces[face.adj[2]].next_ccw_pt_id(face.pts[2]);
362
363            self.compute_silhouette(support_point_id, face.adj[0], adj_opp_pt_id1);
364            self.compute_silhouette(support_point_id, face.adj[1], adj_opp_pt_id2);
365            self.compute_silhouette(support_point_id, face.adj[2], adj_opp_pt_id3);
366
367            let first_new_face_id = self.faces.len();
368
369            if self.silhouette.len() == 0 {
370                // FIXME: Something went very wrong because we failed to extract a silhouette…
371                return None;
372            }
373
374            for edge in &self.silhouette {
375                if !self.faces[edge.face_id].deleted {
376                    let new_face_id = self.faces.len();
377                    let new_face;
378
379                    // FIXME: NLL
380                    {
381                        let face_adj = &mut self.faces[edge.face_id];
382                        let pt_id1 = face_adj.pts[(edge.opp_pt_id + 2) % 3];
383                        let pt_id2 = face_adj.pts[(edge.opp_pt_id + 1) % 3];
384
385                        let pts = [pt_id1, pt_id2, support_point_id];
386                        let adj = [edge.face_id, new_face_id + 1, new_face_id - 1];
387                        new_face = Face::new(&self.vertices, pts, adj);
388
389                        face_adj.adj[(edge.opp_pt_id + 1) % 3] = new_face_id;
390                    }
391
392                    self.faces.push(new_face.0);
393
394                    if new_face.1 {
395                        let pt = self.vertices[self.faces[new_face_id].pts[0]].point.coords;
396                        let dist = self.faces[new_face_id].normal.dot(&pt);
397                        if dist < curr_dist {
398                            // FIXME: if we reach this point, there were issues due to
399                            // numerical errors.
400                            let points = face.closest_points(&self.vertices);
401                            return Some((points.0, points.1, face.normal));
402                        }
403
404                        self.heap.push(FaceId::new(new_face_id, -dist)?);
405                    }
406                }
407            }
408
409            if first_new_face_id == self.faces.len() {
410                // Something went very wrong because all the edges
411                // from the silhouette belonged to deleted faces.
412                return None;
413            }
414
415            self.faces[first_new_face_id].adj[2] = self.faces.len() - 1;
416            self.faces.last_mut().unwrap().adj[1] = first_new_face_id;
417
418            self.silhouette.clear();
419            // self.check_topology(); // NOTE: for debugging only.
420
421            niter += 1;
422            if niter > 10000 {
423                return None;
424            }
425        }
426
427        let best_face = &self.faces[best_face_id.id];
428        let points = best_face.closest_points(&self.vertices);
429        return Some((points.0, points.1, best_face.normal));
430    }
431
432    fn compute_silhouette(&mut self, point: usize, id: usize, opp_pt_id: usize) {
433        if !self.faces[id].deleted {
434            if !self.faces[id].can_be_seen_by(&self.vertices, point, opp_pt_id) {
435                self.silhouette.push(SilhouetteEdge::new(id, opp_pt_id));
436            } else {
437                self.faces[id].deleted = true;
438
439                let adj_pt_id1 = (opp_pt_id + 2) % 3;
440                let adj_pt_id2 = opp_pt_id;
441
442                let adj1 = self.faces[id].adj[adj_pt_id1];
443                let adj2 = self.faces[id].adj[adj_pt_id2];
444
445                let adj_opp_pt_id1 =
446                    self.faces[adj1].next_ccw_pt_id(self.faces[id].pts[adj_pt_id1]);
447                let adj_opp_pt_id2 =
448                    self.faces[adj2].next_ccw_pt_id(self.faces[id].pts[adj_pt_id2]);
449
450                self.compute_silhouette(point, adj1, adj_opp_pt_id1);
451                self.compute_silhouette(point, adj2, adj_opp_pt_id2);
452            }
453        }
454    }
455
456    #[allow(dead_code)]
457    fn print_silhouette(&self) {
458        print!("Silhouette points: ");
459        for i in 0..self.silhouette.len() {
460            let edge = &self.silhouette[i];
461            let face = &self.faces[edge.face_id];
462
463            if !face.deleted {
464                print!(
465                    "({}, {}) ",
466                    face.pts[(edge.opp_pt_id + 2) % 3],
467                    face.pts[(edge.opp_pt_id + 1) % 3]
468                );
469            }
470        }
471        println!("");
472    }
473
474    #[allow(dead_code)]
475    fn check_topology(&self) {
476        for i in 0..self.faces.len() {
477            let face = &self.faces[i];
478            if face.deleted {
479                continue;
480            }
481
482            println!("checking {}-th face.", i);
483            let adj1 = &self.faces[face.adj[0]];
484            let adj2 = &self.faces[face.adj[1]];
485            let adj3 = &self.faces[face.adj[2]];
486
487            assert!(!adj1.deleted);
488            assert!(!adj2.deleted);
489            assert!(!adj3.deleted);
490
491            assert!(face.pts[0] != face.pts[1]);
492            assert!(face.pts[0] != face.pts[2]);
493            assert!(face.pts[1] != face.pts[2]);
494
495            assert!(adj1.contains_point(face.pts[0]));
496            assert!(adj1.contains_point(face.pts[1]));
497
498            assert!(adj2.contains_point(face.pts[1]));
499            assert!(adj2.contains_point(face.pts[2]));
500
501            assert!(adj3.contains_point(face.pts[2]));
502            assert!(adj3.contains_point(face.pts[0]));
503
504            let opp_pt_id1 = adj1.next_ccw_pt_id(face.pts[0]);
505            let opp_pt_id2 = adj2.next_ccw_pt_id(face.pts[1]);
506            let opp_pt_id3 = adj3.next_ccw_pt_id(face.pts[2]);
507
508            assert!(!face.contains_point(adj1.pts[opp_pt_id1]));
509            assert!(!face.contains_point(adj2.pts[opp_pt_id2]));
510            assert!(!face.contains_point(adj3.pts[opp_pt_id3]));
511
512            assert!(adj1.adj[(opp_pt_id1 + 1) % 3] == i);
513            assert!(adj2.adj[(opp_pt_id2 + 1) % 3] == i);
514            assert!(adj3.adj[(opp_pt_id3 + 1) % 3] == i);
515        }
516    }
517}
ncollide3d/query/algorithms/epa3.rs

ncollide3d/query/algorithms/
epa3.rs
