parry3d/query/epa/epa3.rs
1//! Three-dimensional penetration depth queries using the Expanding Polytope Algorithm.
2//!
3//! This module provides the 3D-specific implementation of EPA, which works with
4//! polyhedra (triangular faces) rather than polygons (edges) as in the 2D version.
5
6use crate::math::{Pose, Real, Vector};
7use crate::query::gjk::{self, ConstantOrigin, CsoPoint, VoronoiSimplex};
8use crate::query::PointQueryWithLocation;
9use crate::shape::{SupportMap, Triangle, TrianglePointLocation};
10use crate::utils;
11use alloc::collections::BinaryHeap;
12use alloc::vec::Vec;
13use core::cmp::Ordering;
14use num::Bounded;
15
16#[derive(Copy, Clone, PartialEq)]
17struct FaceId {
18 id: usize,
19 neg_dist: Real,
20}
21
22impl FaceId {
23 /// Creates a new face id, rejecting faces whose `neg_dist` is positive beyond `tol`.
24 fn new(id: usize, neg_dist: Real, tol: Real) -> Option<Self> {
25 if neg_dist > tol {
26 None
27 } else {
28 Some(FaceId { id, neg_dist })
29 }
30 }
31}
32
33impl Eq for FaceId {}
34
35impl PartialOrd for FaceId {
36 #[inline]
37 fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
38 Some(self.cmp(other))
39 }
40}
41
42impl Ord for FaceId {
43 #[inline]
44 fn cmp(&self, other: &Self) -> Ordering {
45 if self.neg_dist < other.neg_dist {
46 Ordering::Less
47 } else if self.neg_dist > other.neg_dist {
48 Ordering::Greater
49 } else {
50 Ordering::Equal
51 }
52 }
53}
54
55#[derive(Clone, Debug)]
56struct Face {
57 pts: [usize; 3],
58 adj: [usize; 3],
59 normal: Vector,
60 bcoords: [Real; 3],
61 deleted: bool,
62}
63
64impl Face {
65 pub fn new_with_proj(
66 vertices: &[CsoPoint],
67 bcoords: [Real; 3],
68 pts: [usize; 3],
69 adj: [usize; 3],
70 ) -> Self {
71 let normal;
72
73 if let Some(n) = utils::ccw_face_normal([
74 vertices[pts[0]].point,
75 vertices[pts[1]].point,
76 vertices[pts[2]].point,
77 ]) {
78 normal = n;
79 } else {
80 // This is a bit of a hack for degenerate faces.
81 // TODO: It will work OK with our current code, though
82 // we should do this in another way to avoid any risk
83 // of misusing the face normal in the future.
84 normal = Vector::ZERO;
85 }
86
87 Face {
88 pts,
89 bcoords,
90 adj,
91 normal,
92 deleted: false,
93 }
94 }
95
96 pub fn new(vertices: &[CsoPoint], 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) = tri.project_local_point_and_get_location(Vector::ZERO, true);
103
104 match loc {
105 TrianglePointLocation::OnVertex(_) | TrianglePointLocation::OnEdge(_, _) => {
106 let eps_tol = crate::math::DEFAULT_EPSILON * 100.0; // Same as in closest_points
107 (
108 // barycentric_coordinates is guaranteed to work in OnVertex and OnEdge locations
109 Self::new_with_proj(vertices, loc.barycentric_coordinates().unwrap(), pts, adj),
110 proj.is_inside_eps(Vector::ZERO, eps_tol),
111 )
112 }
113 TrianglePointLocation::OnFace(_, bcoords) => {
114 (Self::new_with_proj(vertices, bcoords, pts, adj), true)
115 }
116 _ => (Self::new_with_proj(vertices, [0.0; 3], pts, adj), false),
117 }
118 }
119
120 pub fn closest_points(&self, vertices: &[CsoPoint]) -> (Vector, Vector) {
121 (
122 vertices[self.pts[0]].orig1 * self.bcoords[0]
123 + vertices[self.pts[1]].orig1 * self.bcoords[1]
124 + vertices[self.pts[2]].orig1 * self.bcoords[2],
125 vertices[self.pts[0]].orig2 * self.bcoords[0]
126 + vertices[self.pts[1]].orig2 * self.bcoords[1]
127 + vertices[self.pts[2]].orig2 * self.bcoords[2],
128 )
129 }
130
131 pub fn contains_point(&self, id: usize) -> bool {
132 self.pts[0] == id || self.pts[1] == id || self.pts[2] == id
133 }
134
135 pub fn next_ccw_pt_id(&self, id: usize) -> usize {
136 if self.pts[0] == id {
137 1
138 } else if self.pts[1] == id {
139 2
140 } else {
141 if self.pts[2] != id {
142 log::debug!(
143 "Hit unexpected state in EPA: found index {}, expected: {}.",
144 self.pts[2],
145 id
146 );
147 }
148
149 0
150 }
151 }
152
153 pub fn can_be_seen_by(&self, vertices: &[CsoPoint], point: usize, opp_pt_id: usize) -> bool {
154 let p0 = &vertices[self.pts[opp_pt_id]].point;
155 let p1 = &vertices[self.pts[(opp_pt_id + 1) % 3]].point;
156 let p2 = &vertices[self.pts[(opp_pt_id + 2) % 3]].point;
157 let pt = &vertices[point].point;
158
159 // NOTE: it is important that we return true for the case where
160 // the dot product is zero. This is because degenerate faces will
161 // have a zero normal, causing the dot product to be zero.
162 // So return true for these case will let us skip the triangle
163 // during silhouette computation.
164 (*pt - *p0).dot(self.normal) >= -gjk::eps_tol()
165 || Triangle::new(*p1, *p2, *pt).is_affinely_dependent()
166 }
167}
168
169struct SilhouetteEdge {
170 face_id: usize,
171 opp_pt_id: usize,
172}
173
174impl SilhouetteEdge {
175 pub fn new(face_id: usize, opp_pt_id: usize) -> Self {
176 SilhouetteEdge { face_id, opp_pt_id }
177 }
178}
179
180/// The Expanding Polytope Algorithm in 3D.
181///
182/// This structure computes the penetration depth between two shapes when they are overlapping.
183/// It's used after GJK (Gilbert-Johnson-Keerthi) determines that two shapes are penetrating.
184///
185/// # What does EPA do?
186///
187/// EPA finds:
188/// - The **penetration depth**: How far the shapes are overlapping
189/// - The **contact normal**: The direction to separate the shapes
190/// - The **contact points**: Where the shapes are touching on each surface
191///
192/// # How it works in 3D
193///
194/// In 3D, EPA maintains a convex polyhedron (made of triangular faces) in the Minkowski
195/// difference space (CSO - Configuration Space Obstacle) that encloses the origin. It iteratively:
196///
197/// 1. Finds the triangular face closest to the origin
198/// 2. Expands the polyhedron by adding a new support point in the direction of that face's normal
199/// 3. Removes faces that can be "seen" from the new point (they're now inside)
200/// 4. Creates new faces connecting the boundary edges (silhouette) to the new point
201/// 5. Repeats until the polyhedron cannot expand further (convergence)
202///
203/// The final closest face provides the penetration depth (distance to origin) and contact normal.
204///
205/// # Example
206///
207/// ```
208/// # #[cfg(all(feature = "dim3", feature = "f32"))] {
209/// use parry3d::query::epa::EPA;
210/// use parry3d::query::gjk::VoronoiSimplex;
211/// use parry3d::shape::Ball;
212/// use parry3d::math::Pose;
213///
214/// let ball1 = Ball::new(1.0);
215/// let ball2 = Ball::new(1.0);
216/// let pos12 = Pose::translation(1.5, 0.0, 0.0); // Overlapping spheres
217///
218/// // After GJK determines penetration and fills a simplex:
219/// let mut epa = EPA::new();
220/// let simplex = VoronoiSimplex::new(); // Would be filled by GJK
221///
222/// // EPA computes the contact details
223/// // if let Some((pt1, pt2, normal)) = epa.closest_points(&pos12, &ball1, &ball2, &simplex) {
224/// // println!("Penetration depth: {}", (pt2 - pt1).length());
225/// // println!("Contact normal: {}", normal);
226/// // }
227/// # }
228/// ```
229///
230/// # Reusability
231///
232/// The `EPA` structure can be reused across multiple queries to avoid allocations.
233/// Internal buffers are cleared and reused in each call to [`closest_points`](EPA::closest_points).
234///
235/// # Convergence and Failure Cases
236///
237/// EPA may return `None` in these situations:
238/// - The shapes are not actually penetrating (GJK should be used instead)
239/// - Degenerate or nearly-degenerate geometry causes numerical instability
240/// - The initial simplex from GJK is invalid
241/// - The algorithm fails to converge after 100 iterations
242/// - Silhouette extraction fails (topology issues)
243///
244/// When `None` is returned, the shapes may be touching at a single point, edge, or face,
245/// or there may be numerical precision issues with the input geometry.
246///
247/// # Complexity
248///
249/// The 3D EPA implementation is more complex than 2D because:
250/// - It maintains a 3D mesh topology with face adjacency information
251/// - It needs to compute silhouettes (visible edges from a point)
252/// - It handles more degenerate cases (coplanar faces, edge cases)
253#[derive(Default)]
254pub struct EPA {
255 vertices: Vec<CsoPoint>,
256 faces: Vec<Face>,
257 silhouette: Vec<SilhouetteEdge>,
258 heap: BinaryHeap<FaceId>,
259}
260
261impl EPA {
262 /// Creates a new instance of the 3D Expanding Polytope Algorithm.
263 ///
264 /// This allocates internal data structures (vertices, faces, silhouette buffer, and a priority heap).
265 /// The same `EPA` instance can be reused for multiple queries to avoid repeated allocations.
266 ///
267 /// # Example
268 ///
269 /// ```
270 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
271 /// use parry3d::query::epa::EPA;
272 ///
273 /// let mut epa = EPA::new();
274 /// // Use epa for multiple queries...
275 /// # }
276 /// ```
277 pub fn new() -> Self {
278 Self::default()
279 }
280
281 fn reset(&mut self) {
282 self.vertices.clear();
283 self.faces.clear();
284 self.heap.clear();
285 self.silhouette.clear();
286 }
287
288 /// Projects the origin onto the boundary of the given shape.
289 ///
290 /// This is a specialized version of [`closest_points`](EPA::closest_points) for projecting
291 /// a point (the origin) onto a single shape's surface.
292 ///
293 /// # Parameters
294 ///
295 /// - `m`: The position and orientation of the shape in world space
296 /// - `g`: The shape to project onto (must implement [`SupportMap`])
297 /// - `simplex`: A Voronoi simplex from GJK that encloses the origin (indicating the origin
298 /// is inside the shape)
299 ///
300 /// # Returns
301 ///
302 /// - `Some(point)`: The closest point on the shape's boundary to the origin, in the local
303 /// space of `g`
304 /// - `None`: If the origin is not inside the shape, or if EPA failed to converge
305 ///
306 /// # Prerequisites
307 ///
308 /// The origin **must be inside** the shape. If it's outside, use GJK instead.
309 /// Typically, you would:
310 /// 1. Run GJK to detect if a point is inside a shape
311 /// 2. If inside, use this method to find the closest boundary point
312 ///
313 /// # Example
314 ///
315 /// ```
316 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
317 /// use parry3d::query::epa::EPA;
318 /// use parry3d::query::gjk::VoronoiSimplex;
319 /// use parry3d::shape::Ball;
320 /// use parry3d::math::Pose;
321 ///
322 /// let ball = Ball::new(2.0);
323 /// let pos = Pose::identity();
324 ///
325 /// // Assume GJK determined the origin is inside and filled simplex
326 /// let simplex = VoronoiSimplex::new();
327 /// let mut epa = EPA::new();
328 ///
329 /// // Find the closest point on the ball's surface to the origin
330 /// // if let Some(surface_point) = epa.project_origin(&pos, &ball, &simplex) {
331 /// // println!("Closest surface point: {:?}", surface_point);
332 /// // }
333 /// # }
334 /// ```
335 pub fn project_origin<G: ?Sized + SupportMap>(
336 &mut self,
337 m: &Pose,
338 g: &G,
339 simplex: &VoronoiSimplex,
340 ) -> Option<Vector> {
341 self.closest_points(&m.inverse(), g, &ConstantOrigin, simplex)
342 .map(|(p, _, _)| p)
343 }
344
345 /// Computes the closest points between two penetrating shapes and their contact normal.
346 ///
347 /// This is the main EPA method that computes detailed contact information for overlapping shapes.
348 /// It should be called after GJK determines that two shapes are penetrating.
349 ///
350 /// # Parameters
351 ///
352 /// - `pos12`: The relative position/orientation from `g2`'s frame to `g1`'s frame
353 /// (typically computed as `pos1.inverse() * pos2`)
354 /// - `g1`: The first shape (must implement [`SupportMap`])
355 /// - `g2`: The second shape (must implement [`SupportMap`])
356 /// - `simplex`: A Voronoi simplex from GJK that encloses the origin, indicating penetration
357 ///
358 /// # Returns
359 ///
360 /// Returns `Some((point1, point2, normal))` where:
361 /// - `point1`: Contact point on shape `g1` in `g1`'s local frame
362 /// - `point2`: Contact point on shape `g2` in `g2`'s local frame
363 /// - `normal`: Contact normal pointing from `g2` toward `g1`, normalized
364 ///
365 /// The **penetration depth** can be computed as `(point1 - point2).length()` after transforming
366 /// both points to the same coordinate frame.
367 ///
368 /// Returns `None` if:
369 /// - The shapes are not actually penetrating
370 /// - EPA fails to converge (degenerate geometry, numerical issues)
371 /// - The simplex is invalid or empty
372 /// - The algorithm reaches the maximum iteration limit (100 iterations)
373 /// - Silhouette extraction fails (indicates topology corruption)
374 ///
375 /// # Prerequisites
376 ///
377 /// The shapes **must be penetrating**. The typical workflow is:
378 /// 1. Run GJK to check if shapes intersect
379 /// 2. If GJK detects penetration and returns a simplex enclosing the origin
380 /// 3. Use EPA with that simplex to compute detailed contact information
381 ///
382 /// # Example
383 ///
384 /// ```
385 /// # #[cfg(all(feature = "dim3", feature = "f32"))] {
386 /// use parry3d::query::epa::EPA;
387 /// use parry3d::query::gjk::{GJKResult, VoronoiSimplex};
388 /// use parry3d::shape::Ball;
389 /// use parry3d::math::Pose;
390 ///
391 /// let ball1 = Ball::new(1.0);
392 /// let ball2 = Ball::new(1.0);
393 ///
394 /// let pos1 = Pose::identity();
395 /// let pos2 = Pose::translation(1.5, 0.0, 0.0); // Overlapping
396 /// let pos12 = pos1.inverse() * pos2;
397 ///
398 /// // After GJK detects penetration:
399 /// let mut simplex = VoronoiSimplex::new();
400 /// // ... simplex would be filled by GJK ...
401 ///
402 /// let mut epa = EPA::new();
403 /// // if let Some((pt1, pt2, normal)) = epa.closest_points(&pos12, &ball1, &ball2, &simplex) {
404 /// // println!("Contact on shape 1: {:?}", pt1);
405 /// // println!("Contact on shape 2: {:?}", pt2);
406 /// // println!("Contact normal: {}", normal);
407 /// // println!("Penetration depth: ~0.5");
408 /// // }
409 /// # }
410 /// ```
411 ///
412 /// # Technical Details
413 ///
414 /// The algorithm works in the **Minkowski difference space** (also called the Configuration
415 /// Space Obstacle or CSO), where the difference between support points of the two shapes
416 /// forms a new shape. When shapes penetrate, this CSO contains the origin.
417 ///
418 /// EPA iteratively expands a convex polyhedron (in 3D) that surrounds the origin. At each
419 /// iteration:
420 /// 1. It finds the triangular face closest to the origin
421 /// 2. Computes a support point in that face's normal direction
422 /// 3. Determines which existing faces are visible from the new point (the **silhouette**)
423 /// 4. Removes visible faces and creates new ones connecting the silhouette boundary to the new point
424 ///
425 /// This process maintains a valid convex hull that progressively tightens around the
426 /// origin until convergence, at which point the closest face defines the penetration
427 /// depth and contact normal.
428 pub fn closest_points<G1, G2>(
429 &mut self,
430 pos12: &Pose,
431 g1: &G1,
432 g2: &G2,
433 simplex: &VoronoiSimplex,
434 ) -> Option<(Vector, Vector, Vector)>
435 where
436 G1: ?Sized + SupportMap,
437 G2: ?Sized + SupportMap,
438 {
439 let _eps = crate::math::DEFAULT_EPSILON;
440 let _eps_tol = _eps * 100.0;
441
442 self.reset();
443
444 /*
445 * Initialization.
446 */
447 for i in 0..simplex.dimension() + 1 {
448 self.vertices.push(*simplex.point(i));
449 }
450
451 if simplex.dimension() == 0 {
452 // The GJK simplex degenerated to a single point on the CSO boundary, typically
453 // an exactly-touching contact. Bootstrap a second vertex from an axis-aligned
454 // support direction so the expansion below can find the normal.
455 let candidates = [
456 Vector::X,
457 Vector::Y,
458 Vector::Z,
459 -Vector::X,
460 -Vector::Y,
461 -Vector::Z,
462 ];
463 let sep_tol = gjk::eps_tol();
464 let bootstrap = candidates.iter().find_map(|dir| {
465 let pt = CsoPoint::from_shapes(pos12, g1, g2, *dir);
466 ((pt.point - self.vertices[0].point).length_squared() > sep_tol * sep_tol)
467 .then_some(pt)
468 });
469
470 match bootstrap {
471 Some(pt) => self.vertices.push(pt),
472 None => {
473 // The CSO is degenerate (a single point): no meaningful normal exists.
474 let v = self.vertices[0];
475 return Some((v.orig1, v.orig2, Vector::Y));
476 }
477 }
478 }
479
480 // Tolerance used to reject degenerate faces. It is scaled relative to the magnitude of
481 // the simplex coordinates: dot products used to compute face distances accumulate
482 // rounding errors proportional to the coordinate magnitudes, so an absolute tolerance
483 // would spuriously abort EPA for large coordinates.
484 // See <https://github.com/dimforge/parry/issues/415>.
485 let dist_tol = {
486 let scale = self
487 .vertices
488 .iter()
489 .map(|v| v.point.length())
490 .fold(0.0, Real::max);
491 gjk::eps_tol() * scale.max(1.0)
492 };
493
494 if simplex.dimension() == 3 {
495 let dp1 = self.vertices[1] - self.vertices[0];
496 let dp2 = self.vertices[2] - self.vertices[0];
497 let dp3 = self.vertices[3] - self.vertices[0];
498
499 if dp1.cross(dp2).dot(dp3) > 0.0 {
500 self.vertices.swap(1, 2)
501 }
502
503 let pts1 = [0, 1, 2];
504 let pts2 = [1, 3, 2];
505 let pts3 = [0, 2, 3];
506 let pts4 = [0, 3, 1];
507
508 let adj1 = [3, 1, 2];
509 let adj2 = [3, 2, 0];
510 let adj3 = [0, 1, 3];
511 let adj4 = [2, 1, 0];
512
513 let (face1, proj_inside1) = Face::new(&self.vertices, pts1, adj1);
514 let (face2, proj_inside2) = Face::new(&self.vertices, pts2, adj2);
515 let (face3, proj_inside3) = Face::new(&self.vertices, pts3, adj3);
516 let (face4, proj_inside4) = Face::new(&self.vertices, pts4, adj4);
517
518 self.faces.push(face1);
519 self.faces.push(face2);
520 self.faces.push(face3);
521 self.faces.push(face4);
522
523 if proj_inside1 {
524 let dist1 = self.faces[0].normal.dot(self.vertices[0].point);
525 self.heap.push(FaceId::new(0, -dist1, dist_tol)?);
526 }
527
528 if proj_inside2 {
529 let dist2 = self.faces[1].normal.dot(self.vertices[1].point);
530 self.heap.push(FaceId::new(1, -dist2, dist_tol)?);
531 }
532
533 if proj_inside3 {
534 let dist3 = self.faces[2].normal.dot(self.vertices[2].point);
535 self.heap.push(FaceId::new(2, -dist3, dist_tol)?);
536 }
537
538 if proj_inside4 {
539 let dist4 = self.faces[3].normal.dot(self.vertices[3].point);
540 self.heap.push(FaceId::new(3, -dist4, dist_tol)?);
541 }
542
543 if !(proj_inside1 || proj_inside2 || proj_inside3 || proj_inside4) {
544 // Related issues:
545 // https://github.com/dimforge/parry/issues/253
546 // https://github.com/dimforge/parry/issues/246
547 log::debug!("Hit unexpected state in EPA: failed to project the origin on the initial simplex.");
548 return None;
549 }
550 } else {
551 // NOTE: len == 2 covers both a 1-dimensional GJK simplex and a 0-dimensional
552 // one completed by the bootstrap above.
553 if self.vertices.len() == 2 {
554 let dpt = self.vertices[1] - self.vertices[0];
555
556 crate::math::orthonormal_subspace_basis(&[dpt], |dir| {
557 // dir is already normalized by orthonormal_subspace_basis
558 self.vertices
559 .push(CsoPoint::from_shapes(pos12, g1, g2, dir));
560 false
561 });
562 }
563
564 let pts1 = [0, 1, 2];
565 let pts2 = [0, 2, 1];
566
567 let adj1 = [1, 1, 1];
568 let adj2 = [0, 0, 0];
569
570 let (face1, _) = Face::new(&self.vertices, pts1, adj1);
571 let (face2, _) = Face::new(&self.vertices, pts2, adj2);
572 self.faces.push(face1);
573 self.faces.push(face2);
574
575 self.heap.push(FaceId::new(0, 0.0, dist_tol)?);
576 self.heap.push(FaceId::new(1, 0.0, dist_tol)?);
577 }
578
579 let mut niter = 0;
580 let mut max_dist = Real::max_value();
581 let mut best_face_id = *self.heap.peek()?;
582 let mut old_dist = 0.0;
583
584 /*
585 * Run the expansion.
586 */
587 while let Some(face_id) = self.heap.pop() {
588 // Create new faces.
589 let face = self.faces[face_id.id].clone();
590
591 if face.deleted {
592 continue;
593 }
594
595 let cso_point = CsoPoint::from_shapes(pos12, g1, g2, face.normal);
596 let support_point_id = self.vertices.len();
597 self.vertices.push(cso_point);
598
599 let candidate_max_dist = cso_point.point.dot(face.normal);
600
601 if candidate_max_dist < max_dist {
602 best_face_id = face_id;
603 max_dist = candidate_max_dist;
604 }
605
606 let curr_dist = -face_id.neg_dist;
607
608 if max_dist - curr_dist < _eps_tol ||
609 // Accept the intersection as the algorithm is stuck and no new points will be found
610 // This happens because of numerical stability issue
611 ((curr_dist - old_dist).abs() < _eps && candidate_max_dist < max_dist)
612 {
613 let best_face = &self.faces[best_face_id.id];
614 let points = best_face.closest_points(&self.vertices);
615 return Some((points.0, points.1, best_face.normal));
616 }
617
618 old_dist = curr_dist;
619
620 self.faces[face_id.id].deleted = true;
621
622 let adj_opp_pt_id1 = self.faces[face.adj[0]].next_ccw_pt_id(face.pts[0]);
623 let adj_opp_pt_id2 = self.faces[face.adj[1]].next_ccw_pt_id(face.pts[1]);
624 let adj_opp_pt_id3 = self.faces[face.adj[2]].next_ccw_pt_id(face.pts[2]);
625
626 self.compute_silhouette(support_point_id, face.adj[0], adj_opp_pt_id1);
627 self.compute_silhouette(support_point_id, face.adj[1], adj_opp_pt_id2);
628 self.compute_silhouette(support_point_id, face.adj[2], adj_opp_pt_id3);
629
630 let first_new_face_id = self.faces.len();
631
632 if self.silhouette.is_empty() {
633 // TODO: Something went very wrong because we failed to extract a silhouette…
634 return None;
635 }
636
637 for edge in &self.silhouette {
638 if !self.faces[edge.face_id].deleted {
639 let new_face_id = self.faces.len();
640
641 let face_adj = &mut self.faces[edge.face_id];
642 let pt_id1 = face_adj.pts[(edge.opp_pt_id + 2) % 3];
643 let pt_id2 = face_adj.pts[(edge.opp_pt_id + 1) % 3];
644
645 let pts = [pt_id1, pt_id2, support_point_id];
646 let adj = [edge.face_id, new_face_id + 1, new_face_id - 1];
647 let new_face = Face::new(&self.vertices, pts, adj);
648
649 face_adj.adj[(edge.opp_pt_id + 1) % 3] = new_face_id;
650
651 self.faces.push(new_face.0);
652
653 if new_face.1 {
654 let pt = self.vertices[self.faces[new_face_id].pts[0]].point;
655 let dist = self.faces[new_face_id].normal.dot(pt);
656 if dist < curr_dist {
657 // TODO: if we reach this point, there were issues due to
658 // numerical errors.
659 let points = face.closest_points(&self.vertices);
660 return Some((points.0, points.1, face.normal));
661 }
662
663 self.heap.push(FaceId::new(new_face_id, -dist, dist_tol)?);
664 }
665 }
666 }
667
668 if first_new_face_id == self.faces.len() {
669 // Something went very wrong because all the edges
670 // from the silhouette belonged to deleted faces.
671 return None;
672 }
673
674 self.faces[first_new_face_id].adj[2] = self.faces.len() - 1;
675 self.faces.last_mut().unwrap().adj[1] = first_new_face_id;
676
677 self.silhouette.clear();
678 // self.check_topology(); // NOTE: for debugging only.
679
680 niter += 1;
681 if niter > 100 {
682 // if we reached this point, our algorithm didn't converge to what precision we wanted.
683 // still return an intersection point, as it's probably close enough.
684 break;
685 }
686 }
687
688 let best_face = &self.faces[best_face_id.id];
689 let points = best_face.closest_points(&self.vertices);
690 Some((points.0, points.1, best_face.normal))
691 }
692
693 fn compute_silhouette(&mut self, point: usize, id: usize, opp_pt_id: usize) {
694 if !self.faces[id].deleted {
695 if !self.faces[id].can_be_seen_by(&self.vertices, point, opp_pt_id) {
696 self.silhouette.push(SilhouetteEdge::new(id, opp_pt_id));
697 } else {
698 self.faces[id].deleted = true;
699
700 let adj_pt_id1 = (opp_pt_id + 2) % 3;
701 let adj_pt_id2 = opp_pt_id;
702
703 let adj1 = self.faces[id].adj[adj_pt_id1];
704 let adj2 = self.faces[id].adj[adj_pt_id2];
705
706 let adj_opp_pt_id1 =
707 self.faces[adj1].next_ccw_pt_id(self.faces[id].pts[adj_pt_id1]);
708 let adj_opp_pt_id2 =
709 self.faces[adj2].next_ccw_pt_id(self.faces[id].pts[adj_pt_id2]);
710
711 self.compute_silhouette(point, adj1, adj_opp_pt_id1);
712 self.compute_silhouette(point, adj2, adj_opp_pt_id2);
713 }
714 }
715 }
716
717 #[allow(dead_code)]
718 #[cfg(feature = "std")]
719 fn print_silhouette(&self) {
720 use std::{print, println};
721
722 print!("Silhouette points: ");
723 for i in 0..self.silhouette.len() {
724 let edge = &self.silhouette[i];
725 let face = &self.faces[edge.face_id];
726
727 if !face.deleted {
728 print!(
729 "({}, {}) ",
730 face.pts[(edge.opp_pt_id + 2) % 3],
731 face.pts[(edge.opp_pt_id + 1) % 3]
732 );
733 }
734 }
735 println!();
736 }
737
738 #[allow(dead_code)]
739 fn check_topology(&self) {
740 for i in 0..self.faces.len() {
741 let face = &self.faces[i];
742 if face.deleted {
743 continue;
744 }
745
746 // println!("checking {}-th face.", i);
747 let adj1 = &self.faces[face.adj[0]];
748 let adj2 = &self.faces[face.adj[1]];
749 let adj3 = &self.faces[face.adj[2]];
750
751 assert!(!adj1.deleted);
752 assert!(!adj2.deleted);
753 assert!(!adj3.deleted);
754
755 assert!(face.pts[0] != face.pts[1]);
756 assert!(face.pts[0] != face.pts[2]);
757 assert!(face.pts[1] != face.pts[2]);
758
759 assert!(adj1.contains_point(face.pts[0]));
760 assert!(adj1.contains_point(face.pts[1]));
761
762 assert!(adj2.contains_point(face.pts[1]));
763 assert!(adj2.contains_point(face.pts[2]));
764
765 assert!(adj3.contains_point(face.pts[2]));
766 assert!(adj3.contains_point(face.pts[0]));
767
768 let opp_pt_id1 = adj1.next_ccw_pt_id(face.pts[0]);
769 let opp_pt_id2 = adj2.next_ccw_pt_id(face.pts[1]);
770 let opp_pt_id3 = adj3.next_ccw_pt_id(face.pts[2]);
771
772 assert!(!face.contains_point(adj1.pts[opp_pt_id1]));
773 assert!(!face.contains_point(adj2.pts[opp_pt_id2]));
774 assert!(!face.contains_point(adj3.pts[opp_pt_id3]));
775
776 assert!(adj1.adj[(opp_pt_id1 + 1) % 3] == i);
777 assert!(adj2.adj[(opp_pt_id2 + 1) % 3] == i);
778 assert!(adj3.adj[(opp_pt_id3 + 1) % 3] == i);
779 }
780 }
781}