parry2d/query/epa/
epa2.rs

1//! Two-dimensional penetration depth queries using the Expanding Polytope Algorithm.
2//!
3//! This module provides the 2D-specific implementation of EPA, which works with
4//! polygons (edges) rather than polyhedra (faces) as in the 3D version.
5
6use alloc::{collections::BinaryHeap, vec::Vec};
7use core::cmp::Ordering;
8
9use na::{self, Unit};
10use num::Bounded;
11
12use crate::math::{Isometry, Point, Real, Vector};
13use crate::query::gjk::{self, CSOPoint, ConstantOrigin, VoronoiSimplex};
14use crate::shape::SupportMap;
15use crate::utils;
16
17#[derive(Copy, Clone, PartialEq)]
18struct FaceId {
19    id: usize,
20    neg_dist: Real,
21}
22
23impl FaceId {
24    fn new(id: usize, neg_dist: Real) -> Option<Self> {
25        if neg_dist > gjk::eps_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; 2],
58    normal: Unit<Vector<Real>>,
59    proj: Point<Real>,
60    bcoords: [Real; 2],
61    deleted: bool,
62}
63
64impl Face {
65    pub fn new(vertices: &[CSOPoint], pts: [usize; 2]) -> (Self, bool) {
66        if let Some((proj, bcoords)) =
67            project_origin(&vertices[pts[0]].point, &vertices[pts[1]].point)
68        {
69            (Self::new_with_proj(vertices, proj, bcoords, pts), true)
70        } else {
71            (
72                Self::new_with_proj(vertices, Point::origin(), [0.0; 2], pts),
73                false,
74            )
75        }
76    }
77
78    pub fn new_with_proj(
79        vertices: &[CSOPoint],
80        proj: Point<Real>,
81        bcoords: [Real; 2],
82        pts: [usize; 2],
83    ) -> Self {
84        let normal;
85        let deleted;
86
87        if let Some(n) = utils::ccw_face_normal([&vertices[pts[0]].point, &vertices[pts[1]].point])
88        {
89            normal = n;
90            deleted = false;
91        } else {
92            normal = Unit::new_unchecked(na::zero());
93            deleted = true;
94        }
95
96        Face {
97            pts,
98            normal,
99            proj,
100            bcoords,
101            deleted,
102        }
103    }
104
105    pub fn closest_points(&self, vertices: &[CSOPoint]) -> (Point<Real>, Point<Real>) {
106        (
107            vertices[self.pts[0]].orig1 * self.bcoords[0]
108                + vertices[self.pts[1]].orig1.coords * self.bcoords[1],
109            vertices[self.pts[0]].orig2 * self.bcoords[0]
110                + vertices[self.pts[1]].orig2.coords * self.bcoords[1],
111        )
112    }
113}
114
115/// The Expanding Polytope Algorithm in 2D.
116///
117/// This structure computes the penetration depth between two shapes when they are overlapping.
118/// It's used after GJK (Gilbert-Johnson-Keerthi) determines that two shapes are penetrating.
119///
120/// # What does EPA do?
121///
122/// EPA finds:
123/// - The **penetration depth**: How far the shapes are overlapping
124/// - The **contact normal**: The direction to separate the shapes
125/// - The **contact points**: Where the shapes are touching on each surface
126///
127/// # How it works in 2D
128///
129/// In 2D, EPA maintains a polygon in the Minkowski difference space (CSO - Configuration Space
130/// Obstacle) that encloses the origin. It iteratively:
131///
132/// 1. Finds the edge closest to the origin
133/// 2. Expands the polygon by adding a new support point in the direction of that edge's normal
134/// 3. Updates the polygon structure with the new point
135/// 4. Repeats until the polygon cannot expand further (convergence)
136///
137/// The final closest edge provides the penetration depth (distance to origin) and contact normal.
138///
139/// # Example
140///
141/// ```
142/// # #[cfg(all(feature = "dim2", feature = "f32"))] {
143/// use parry2d::query::epa::EPA;
144/// use parry2d::query::gjk::VoronoiSimplex;
145/// use parry2d::shape::Ball;
146/// use parry2d::na::Isometry2;
147///
148/// let ball1 = Ball::new(1.0);
149/// let ball2 = Ball::new(1.0);
150/// let pos12 = Isometry2::translation(1.5, 0.0); // Overlapping circles
151///
152/// // After GJK determines penetration and fills a simplex:
153/// let mut epa = EPA::new();
154/// let simplex = VoronoiSimplex::new(); // Would be filled by GJK
155///
156/// // EPA computes the contact details
157/// // if let Some((pt1, pt2, normal)) = epa.closest_points(&pos12, &ball1, &ball2, &simplex) {
158/// //     println!("Penetration depth: {}", (pt2 - pt1).norm());
159/// //     println!("Contact normal: {}", normal);
160/// // }
161/// # }
162/// ```
163///
164/// # Reusability
165///
166/// The `EPA` structure can be reused across multiple queries to avoid allocations.
167/// Internal buffers are cleared and reused in each call to [`closest_points`](EPA::closest_points).
168///
169/// # Convergence and Failure Cases
170///
171/// EPA may return `None` in these situations:
172/// - The shapes are not actually penetrating (GJK should be used instead)
173/// - Degenerate or nearly-degenerate geometry causes numerical instability
174/// - The initial simplex from GJK is invalid
175/// - The algorithm fails to converge after 100 iterations
176///
177/// When `None` is returned, the shapes may be touching at a single point or edge, or there
178/// may be numerical precision issues with the input geometry.
179#[derive(Default)]
180pub struct EPA {
181    vertices: Vec<CSOPoint>,
182    faces: Vec<Face>,
183    heap: BinaryHeap<FaceId>,
184}
185
186impl EPA {
187    /// Creates a new instance of the 2D Expanding Polytope Algorithm.
188    ///
189    /// This allocates internal data structures (vertices, faces, and a priority heap).
190    /// The same `EPA` instance can be reused for multiple queries to avoid repeated allocations.
191    ///
192    /// # Example
193    ///
194    /// ```
195    /// # #[cfg(all(feature = "dim2", feature = "f32"))] {
196    /// use parry2d::query::epa::EPA;
197    ///
198    /// let mut epa = EPA::new();
199    /// // Use epa for multiple queries...
200    /// # }
201    /// ```
202    pub fn new() -> Self {
203        EPA::default()
204    }
205
206    fn reset(&mut self) {
207        self.vertices.clear();
208        self.faces.clear();
209        self.heap.clear();
210    }
211
212    /// Projects the origin onto the boundary of the given shape.
213    ///
214    /// This is a specialized version of [`closest_points`](EPA::closest_points) for projecting
215    /// a point (the origin) onto a single shape's surface.
216    ///
217    /// # Parameters
218    ///
219    /// - `m`: The position and orientation of the shape in world space
220    /// - `g`: The shape to project onto (must implement [`SupportMap`])
221    /// - `simplex`: A Voronoi simplex from GJK that encloses the origin (indicating the origin
222    ///   is inside the shape)
223    ///
224    /// # Returns
225    ///
226    /// - `Some(point)`: The closest point on the shape's boundary to the origin, in the local
227    ///   space of `g`
228    /// - `None`: If the origin is not inside the shape, or if EPA failed to converge
229    ///
230    /// # Prerequisites
231    ///
232    /// The origin **must be inside** the shape. If it's outside, use GJK instead.
233    /// Typically, you would:
234    /// 1. Run GJK to detect if a point is inside a shape
235    /// 2. If inside, use this method to find the closest boundary point
236    ///
237    /// # Example
238    ///
239    /// ```
240    /// # #[cfg(all(feature = "dim2", feature = "f32"))] {
241    /// use parry2d::query::epa::EPA;
242    /// use parry2d::query::gjk::VoronoiSimplex;
243    /// use parry2d::shape::Ball;
244    /// use parry2d::na::Isometry2;
245    ///
246    /// let ball = Ball::new(2.0);
247    /// let pos: Isometry2<f32> = Isometry2::identity();
248    ///
249    /// // Assume GJK determined the origin is inside and filled simplex
250    /// let simplex = VoronoiSimplex::new();
251    /// let mut epa = EPA::new();
252    ///
253    /// // Find the closest point on the ball's surface to the origin
254    /// // if let Some(surface_point) = epa.project_origin(&pos, &ball, &simplex) {
255    /// //     println!("Closest surface point: {:?}", surface_point);
256    /// // }
257    /// # }
258    /// ```
259    pub fn project_origin<G: ?Sized + SupportMap>(
260        &mut self,
261        m: &Isometry<Real>,
262        g: &G,
263        simplex: &VoronoiSimplex,
264    ) -> Option<Point<Real>> {
265        self.closest_points(&m.inverse(), g, &ConstantOrigin, simplex)
266            .map(|(p, _, _)| p)
267    }
268
269    /// Computes the closest points between two penetrating shapes and their contact normal.
270    ///
271    /// This is the main EPA method that computes detailed contact information for overlapping shapes.
272    /// It should be called after GJK determines that two shapes are penetrating.
273    ///
274    /// # Parameters
275    ///
276    /// - `pos12`: The relative position/orientation from `g2`'s frame to `g1`'s frame
277    ///   (typically computed as `pos1.inverse() * pos2`)
278    /// - `g1`: The first shape (must implement [`SupportMap`])
279    /// - `g2`: The second shape (must implement [`SupportMap`])
280    /// - `simplex`: A Voronoi simplex from GJK that encloses the origin, indicating penetration
281    ///
282    /// # Returns
283    ///
284    /// Returns `Some((point1, point2, normal))` where:
285    /// - `point1`: Contact point on shape `g1` in `g1`'s local frame
286    /// - `point2`: Contact point on shape `g2` in `g2`'s local frame
287    /// - `normal`: Contact normal pointing from `g2` toward `g1`, normalized
288    ///
289    /// The **penetration depth** can be computed as `(point1 - point2).norm()` after transforming
290    /// both points to the same coordinate frame.
291    ///
292    /// Returns `None` if:
293    /// - The shapes are not actually penetrating
294    /// - EPA fails to converge (degenerate geometry, numerical issues)
295    /// - The simplex is invalid or empty
296    /// - The algorithm reaches the maximum iteration limit (100 iterations)
297    ///
298    /// # Prerequisites
299    ///
300    /// The shapes **must be penetrating**. The typical workflow is:
301    /// 1. Run GJK to check if shapes intersect
302    /// 2. If GJK detects penetration and returns a simplex enclosing the origin
303    /// 3. Use EPA with that simplex to compute detailed contact information
304    ///
305    /// # Example
306    ///
307    /// ```
308    /// # #[cfg(all(feature = "dim2", feature = "f32"))] {
309    /// use parry2d::query::epa::EPA;
310    /// use parry2d::query::gjk::{GJKResult, VoronoiSimplex};
311    /// use parry2d::shape::Ball;
312    /// use parry2d::na::Isometry2;
313    ///
314    /// let ball1 = Ball::new(1.0);
315    /// let ball2 = Ball::new(1.0);
316    ///
317    /// let pos1 = Isometry2::identity();
318    /// let pos2 = Isometry2::translation(1.5, 0.0); // Overlapping
319    /// let pos12 = pos1.inverse() * pos2;
320    ///
321    /// // After GJK detects penetration:
322    /// let mut simplex = VoronoiSimplex::new();
323    /// // ... simplex would be filled by GJK ...
324    ///
325    /// let mut epa = EPA::new();
326    /// // if let Some((pt1, pt2, normal)) = epa.closest_points(&pos12, &ball1, &ball2, &simplex) {
327    /// //     println!("Contact on shape 1: {:?}", pt1);
328    /// //     println!("Contact on shape 2: {:?}", pt2);
329    /// //     println!("Contact normal: {}", normal);
330    /// //     println!("Penetration depth: ~0.5");
331    /// // }
332    /// # }
333    /// ```
334    ///
335    /// # Technical Details
336    ///
337    /// The algorithm works in the **Minkowski difference space** (also called the Configuration
338    /// Space Obstacle or CSO), where the difference between support points of the two shapes
339    /// forms a new shape. When shapes penetrate, this CSO contains the origin.
340    ///
341    /// EPA iteratively expands a polygon (in 2D) that surrounds the origin, finding the edge
342    /// closest to the origin at each iteration. This closest edge defines the penetration
343    /// depth and contact normal.
344    pub fn closest_points<G1, G2>(
345        &mut self,
346        pos12: &Isometry<Real>,
347        g1: &G1,
348        g2: &G2,
349        simplex: &VoronoiSimplex,
350    ) -> Option<(Point<Real>, Point<Real>, Unit<Vector<Real>>)>
351    where
352        G1: ?Sized + SupportMap,
353        G2: ?Sized + SupportMap,
354    {
355        let _eps: Real = crate::math::DEFAULT_EPSILON;
356        let _eps_tol = _eps * 100.0;
357
358        self.reset();
359
360        /*
361         * Initialization.
362         */
363        for i in 0..simplex.dimension() + 1 {
364            self.vertices.push(*simplex.point(i));
365        }
366
367        if simplex.dimension() == 0 {
368            const MAX_ITERS: usize = 100; // If there is no convergence, just use whatever direction was extracted so fare
369
370            // The contact is vertex-vertex.
371            // We need to determine a valid normal that lies
372            // on both vertices' normal cone.
373            let mut n = Vector::y_axis();
374
375            // First, find a vector on the first vertex tangent cone.
376            let orig1 = self.vertices[0].orig1;
377            for _ in 0..MAX_ITERS {
378                let supp1 = g1.local_support_point(&n);
379                if let Some(tangent) = Unit::try_new(supp1 - orig1, _eps_tol) {
380                    if n.dot(&tangent) < _eps_tol {
381                        break;
382                    }
383
384                    n = Unit::new_unchecked(Vector::new(-tangent.y, tangent.x));
385                } else {
386                    break;
387                }
388            }
389
390            // Second, ensure the direction lies on the second vertex's tangent cone.
391            let orig2 = self.vertices[0].orig2;
392            for _ in 0..MAX_ITERS {
393                let supp2 = g2.support_point(pos12, &-n);
394                if let Some(tangent) = Unit::try_new(supp2 - orig2, _eps_tol) {
395                    if (-n).dot(&tangent) < _eps_tol {
396                        break;
397                    }
398
399                    n = Unit::new_unchecked(Vector::new(-tangent.y, tangent.x));
400                } else {
401                    break;
402                }
403            }
404
405            return Some((Point::origin(), Point::origin(), n));
406        } else if simplex.dimension() == 2 {
407            let dp1 = self.vertices[1] - self.vertices[0];
408            let dp2 = self.vertices[2] - self.vertices[0];
409
410            if dp1.perp(&dp2) < 0.0 {
411                self.vertices.swap(1, 2)
412            }
413
414            let pts1 = [0, 1];
415            let pts2 = [1, 2];
416            let pts3 = [2, 0];
417
418            let (face1, proj_inside1) = Face::new(&self.vertices, pts1);
419            let (face2, proj_inside2) = Face::new(&self.vertices, pts2);
420            let (face3, proj_inside3) = Face::new(&self.vertices, pts3);
421
422            self.faces.push(face1);
423            self.faces.push(face2);
424            self.faces.push(face3);
425
426            if proj_inside1 {
427                let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords);
428                self.heap.push(FaceId::new(0, -dist1)?);
429            }
430
431            if proj_inside2 {
432                let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords);
433                self.heap.push(FaceId::new(1, -dist2)?);
434            }
435
436            if proj_inside3 {
437                let dist3 = self.faces[2].normal.dot(&self.vertices[2].point.coords);
438                self.heap.push(FaceId::new(2, -dist3)?);
439            }
440
441            if !(proj_inside1 || proj_inside2 || proj_inside3) {
442                // Related issues:
443                // https://github.com/dimforge/parry/issues/253
444                // https://github.com/dimforge/parry/issues/246
445                log::debug!("Hit unexpected state in EPA: failed to project the origin on the initial simplex.");
446                return None;
447            }
448        } else {
449            let pts1 = [0, 1];
450            let pts2 = [1, 0];
451
452            self.faces.push(Face::new_with_proj(
453                &self.vertices,
454                Point::origin(),
455                [1.0, 0.0],
456                pts1,
457            ));
458            self.faces.push(Face::new_with_proj(
459                &self.vertices,
460                Point::origin(),
461                [1.0, 0.0],
462                pts2,
463            ));
464
465            let dist1 = self.faces[0].normal.dot(&self.vertices[0].point.coords);
466            let dist2 = self.faces[1].normal.dot(&self.vertices[1].point.coords);
467
468            self.heap.push(FaceId::new(0, dist1)?);
469            self.heap.push(FaceId::new(1, dist2)?);
470        }
471
472        let mut niter = 0;
473        let mut max_dist = Real::max_value();
474        let mut best_face_id = *self.heap.peek().unwrap();
475        let mut old_dist = 0.0;
476
477        /*
478         * Run the expansion.
479         */
480        while let Some(face_id) = self.heap.pop() {
481            // Create new faces.
482            let face = self.faces[face_id.id].clone();
483
484            if face.deleted {
485                continue;
486            }
487
488            let cso_point = CSOPoint::from_shapes(pos12, g1, g2, &face.normal);
489            let support_point_id = self.vertices.len();
490            self.vertices.push(cso_point);
491
492            let candidate_max_dist = cso_point.point.coords.dot(&face.normal);
493
494            if candidate_max_dist < max_dist {
495                best_face_id = face_id;
496                max_dist = candidate_max_dist;
497            }
498
499            let curr_dist = -face_id.neg_dist;
500
501            if max_dist - curr_dist < _eps_tol ||
502                // Accept the intersection as the algorithm is stuck and no new points will be found
503                // This happens because of numerical stability issue
504                ((curr_dist - old_dist).abs() < _eps && candidate_max_dist < max_dist)
505            {
506                let best_face = &self.faces[best_face_id.id];
507                let cpts = best_face.closest_points(&self.vertices);
508                return Some((cpts.0, cpts.1, best_face.normal));
509            }
510
511            old_dist = curr_dist;
512
513            let pts1 = [face.pts[0], support_point_id];
514            let pts2 = [support_point_id, face.pts[1]];
515
516            let new_faces = [
517                Face::new(&self.vertices, pts1),
518                Face::new(&self.vertices, pts2),
519            ];
520
521            for f in new_faces.iter() {
522                if f.1 {
523                    let dist = f.0.normal.dot(&f.0.proj.coords);
524                    if dist < curr_dist {
525                        // TODO: if we reach this point, there were issues due to
526                        // numerical errors.
527                        let cpts = f.0.closest_points(&self.vertices);
528                        return Some((cpts.0, cpts.1, f.0.normal));
529                    }
530
531                    if !f.0.deleted {
532                        self.heap.push(FaceId::new(self.faces.len(), -dist)?);
533                    }
534                }
535
536                self.faces.push(f.0.clone());
537            }
538
539            niter += 1;
540            if niter > 100 {
541                // if we reached this point, our algorithm didn't converge to what precision we wanted.
542                // still return an intersection point, as it's probably close enough.
543                break;
544            }
545        }
546
547        let best_face = &self.faces[best_face_id.id];
548        let cpts = best_face.closest_points(&self.vertices);
549        Some((cpts.0, cpts.1, best_face.normal))
550    }
551}
552
553fn project_origin(a: &Point<Real>, b: &Point<Real>) -> Option<(Point<Real>, [Real; 2])> {
554    let ab = *b - *a;
555    let ap = -a.coords;
556    let ab_ap = ab.dot(&ap);
557    let sqnab = ab.norm_squared();
558
559    if sqnab == 0.0 {
560        return None;
561    }
562
563    let position_on_segment;
564
565    let _eps: Real = gjk::eps_tol();
566
567    if ab_ap < -_eps || ab_ap > sqnab + _eps {
568        // Voronoï region of vertex 'a' or 'b'.
569        None
570    } else {
571        // Voronoï region of the segment interior.
572        position_on_segment = ab_ap / sqnab;
573
574        let res = *a + ab * position_on_segment;
575
576        Some((res, [1.0 - position_on_segment, position_on_segment]))
577    }
578}
parry2d/query/epa/epa2.rs

parry2d/query/epa/
epa2.rs
