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

parry2d/query/epa/
epa2.rs
