GJK - EPA

Simplex becomes a polytope when you start adding vertices to it.

The Expanding Polytope Algorithm (EPA) is a method used in computational geometry and collision detection to estimate the penetration depth (PD) between two convex shapes. Let's break it down:

1. Minkowski Difference:

   - We start with two closed, convex shapes, denoted as A₁ and A₂ in n-dimensional space (usually 2D or 3D).

   - The Minkowski Difference D₁₂ between these shapes is defined as:

     \[ D₁₂ = A₁ \ominus A₂ = \{ v = v₁ - v₂ \mid v₁ \in A₁, v₂ \in A₂ \} \]

   - Essentially, it represents the set of all possible relative positions of points from A₁ to points in A₂.

2. Expanding Polytope Algorithm (EPA):

   - EPA estimates the penetration depth by iteratively expanding a polytope (or polygon) within the Minkowski Difference until it reaches the edge of the difference.

   - The key idea is to expand the closest feature of the polytope to the origin.

   -penetration depth accurately.

3. How EPA Works:

   - Start with an initial simplex (a small polytope) within the Minkowski Difference.

   - Iteratively expand the simplex by adding new vertices along the edges or faces of the simplex.

   - The expansion continues until the simplex contains the origin (indicating the penetration depth).

   - The closest feature (vertex, edge, or face) to the origin is used for expansion.

   - EPA can be more efficient than other methods like the Gilbert–Johnson–Keerthi (GJK) algorithm.

Loop through each edge

calculate normal to the edge pointing tot he origin

find teh distanec of the edge to the origin

pick the one with the smallest distance

For iterative loop

find the closest edge

find the support point in the direction of the edge normal

find the distance of the origin to P, alogn the normal

if the distane is zero, contineut o epxnad polytop until you can no longer epand

collision normal is the norla

depth is d

class EPAResult {
    constructor() {
        this.penetrationDepth;
        this.contactNormal = new Vector2();
    
    }
}


// Returns contact data if collide, otherwise returns null
function detectCollision(a /*: RigidBody*/, b/*: RigidBody*/) /*: ContactManifold | null*/ {
   
    const gjkResult = gjk(a, b);

    if (!gjkResult.collide) {
        return null;
    }
    else
    {
        // If the gjk termination simplex has vertices less than 3, expand to full simplex
        // Because EPA needs a full n-simplex to get started
        let simplex = gjkResult.simplex;

        switch (simplex.count) {

            case 1:
                let v = simplex.vertices[0];
                let randomSupport = csoSupport(a, b, new Vector2(1, 0));

                if (randomSupport.equals(v))
                    randomSupport = csoSupport(a, b, new Vector2(-1, 0));

                simplex.addVertex(randomSupport);
            case 2:
                let e = new Edge(simplex.vertices[0], simplex.vertices[1]);
                let normalSupport = csoSupport(a, b, e.normal);

                if (simplex.containsVertex(normalSupport))
                    simplex.addVertex(csoSupport(a, b, e.normal.inverted()));
                else
                    simplex.addVertex(normalSupport);
        }

        const epaResult /*: EPAResult*/ = epa(a, b, gjkResult.simplex);

        let flipped = false;
        // Apply axis weight to improve coherence
        if (epaResult.contactNormal.dot(new Vector2(0, -1)) < 0) {
            let tmp = a;
            a = b;
            b = tmp;
            epaResult.contactNormal.invert();
            flipped = true;
        }

        // Remove floating point error
        epaResult.contactNormal.fix(Settings.EPA_TOLERANCE);

        let contactPoints = findContactPoints(epaResult.contactNormal, a, b);

        let contact = new ContactManifold(a, b, contactPoints, epaResult.penetrationDepth, epaResult.contactNormal, flipped);

        return contact;
    }

}

EPA

// gjkResult - simplex object
// return : EPAResult object

function epa(b1, b2, gjkResult) {
    
    let polytope = new Polytope(gjkResult);

    let closestEdge /*: ClosestEdgeInfo*/ = { index: 0, distance: Infinity, normal: new Vec2(0, 0) };

    for (let i = 0; i < Settings.EPA_MAX_ITERATION; i++) {
        closestEdge = polytope.getClosestEdge();
        let supportPoint = csoSupport(b1, b2, closestEdge.normal);
        let newDistance = closestEdge.normal.dot(supportPoint);

        if (Math.abs(closestEdge.distance - newDistance) > Settings.EPA_TOLERANCE)
        {
            // Insert the support vertex so that it expands our polytope
            polytope.vertices.splice(closestEdge.index + 1, 0, supportPoint);
        }
        else
        {
            // If you didn't expand edge, it means you reached the closest outer edge
            break;
        }
    }

    return {
        penetrationDepth: closestEdge.distance,
        contactNormal: closestEdge.normal,
    };
}

Polytope class

class ClosestEdgeInfo {

    constructor() {
        this.index;
        this.distance;
        this.normal = new Vector2();
    
    }

}

class Polytope {
    constructor(simplex /* Simplex object */) {
        if (simplex.count != 3) throw "Input simplex isn't a triangle";

        this.vertices = [
            simplex.vertices[0].clone(),
            simplex.vertices[1].clone(),
            simplex.vertices[2].clone()
        ];
    }

    get count() {
        return this.vertices.length;
    }

    // Returns the edge closest to the origin - ClosestEdgeInfo object
    getClosestEdge()    {
        let minIndex = 0;
        let minDistance = Infinity;
        let minNormal = new Vector2();

        for (let i = 0; i < this.count; i++) {
            let j = (i + 1) % this.count;

            let vertexI = this.vertices[i];
            let vertexJ = this.vertices[j];

            let edge = vertexJ.sub(vertexI);

            let normal = new Vector2(-edge.y, edge.x).normalized();
            let distance = normal.dot(vertexI);

            if (distance < 0) {
                distance *= -1;
                normal.invert();
            }

            if (distance < minDistance) {
                minDistance = distance;
                minNormal = normal;
                minIndex = i;
            }
        }

        return { index: minIndex, distance: minDistance, normal: minNormal };
    }
}

Find Contact Points

const TANGENT_MIN_LENGTH = 0.01;

class ContactPoint {
    constructor() {
        this.point = new Vec2();
        this.id;
    
    }
}

// Since the findFarthestEdge function returns a edge with a minimum length of 0.01 for circle,
// merging threshold should be greater than sqrt(2) * minimum edge length
const CONTACT_MERGE_THRESHOLD = 1.415 * TANGENT_MIN_LENGTH;

function findContactPoints(n /*: Vector2*/, a /*: RigidBody*/, b/*: RigidBody*/) /*: ContactPoint[]*/ {

    let edgeA = findFarthestEdge(a, n);
    let edgeB = findFarthestEdge(b, n.inverted());

    let ref = edgeA; // Reference edge
    let inc = edgeB; // Incidence edge
    let flip = false;

    let aPerpendicularness = Math.abs(edgeA.dir.dot(n));
    let bPerpendicularness = Math.abs(edgeB.dir.dot(n));

    if (aPerpendicularness >= bPerpendicularness)    {
        ref = edgeB;
        inc = edgeA;
        flip = true;
    }

    clipEdge(inc, ref.p1, ref.dir);
    clipEdge(inc, ref.p2, ref.dir.inverted());
    clipEdge(inc, ref.p1, flip ? n : n.inverted(), true);

    let contactPoints /*: ContactPoint[]*/;

    // If two points are closer than threshold, merge them into one point
    if (inc.length <= CONTACT_MERGE_THRESHOLD) {
        contactPoints = [{ point: inc.p1, id: inc.id1 }];
    }
    else
    {
        contactPoints = [{ point: inc.p1, id: inc.id1 }, { point: inc.p2, id: inc.id2 }];
    }

    return contactPoints;
}

Clipping Edge

function clipEdge(edge/*: Edge*/, p/*: Vector2*/, dir/*: Vector2*/, remove/*: boolean*/ = false) {
    let d1 = edge.p1.sub(p).dot(dir);
    let d2 = edge.p2.sub(p).dot(dir);

    if (d1 >= 0 && d2 >= 0) return;

    let per = Math.abs(d1) + Math.abs(d2);

    if (d1 < 0) {
        if (remove)        {
            edge.p1 = edge.p2;
            edge.id1 = edge.id2;
        }
        else
        {
            edge.p1 = edge.p1.add(Vec2.Subtract(edge.p2, edge.p1).mul(-d1 / per));
        }
    }
    else
    if (d2 < 0) {
        if (remove) {
            edge.p2 = edge.p1;
            edge.id2 = edge.id1;
        }
        else
        {
            edge.p2 = edge.p2.add(Vec2.Subtract(edge.p1, edge.p2).mul(-d2 / per));
        }
    }

}

Edge class

class Edge {

    constructor(p1, p2, id1 = -1, id2 = -1)    {
        this.p1 = p1.clone();
        this.p2 = p2.clone();

        if (this.p1.equals(this.p2))
            this.dir = new Vec2(0, 0);
        else
            this.dir = Vec2.Subtract(p2, p1).normalize();

        this.id1 = id1;
        this.id2 = id2;
    }

    get length()    {
        return Vec2.Subtract(this.p2, this.p1).length();
    }

    get normal()     {
        return Util.cross(1, this.dir);
    }

}

Contact Manifold

class ContactInfo {
    constructor() {
        this.other /*: RigidBody*/;
        this.numContacts /*: number*/;
        this.contactDir /*: Vector2*/;
        this.contactPoints /*: Vector2[]*/;
        this.impulse /*: number*/;
    
    }
}

class ContactManifold extends Constraint {
  
    // param; bodyA, bodyB = RidigBody, contactPoints = array of ContactPoint
    constructor(bodyA, bodyB,  contactPoints, penetrationDepth, contactNormal, featureFlipped)    {
        super(bodyA, bodyB);

        // Contact informations
        this.contactPoints = contactPoints;
        this.penetrationDepth = penetrationDepth;
        this.contactNormal = contactNormal;
        this.contactTangent = new Vec2(-contactNormal.y, contactNormal.x);
        this.featureFlipped = featureFlipped;

        this.normalContacts /*: ContactSolver[]*/ = [];
        this.tangentContacts /*: ContactSolver[]*/ = [];
        /* private readonly blockSolver!: BlockSolver;*/

        this.persistent = false;

        for (let i = 0; i < this.numContacts; i++)        {
            this.normalContacts.push(new ContactSolver(this, contactPoints[i].point));
            this.tangentContacts.push(new ContactSolver(this, contactPoints[i].point));
        }

        if (this.numContacts == 2 && Settings.blockSolve) {
            this.blockSolver = new BlockSolver(this);
        }
    }

    prepare()    {

        for (let i = 0; i < this.numContacts; i++) {
            this.normalContacts[i].prepare(this.contactNormal, ContactType.Normal, this.featureFlipped);
            this.tangentContacts[i].prepare(this.contactTangent, ContactType.Tangent, this.featureFlipped);
        }

        // If we have two contact points, then prepare the block solver.
        if (this.numContacts == 2 && Settings.blockSolve)
        {
            this.blockSolver.prepare(this.normalContacts);
        }
    }

    solve() {
        // Solve tangent constraint first
        for (let i = 0; i < this.numContacts; i++)
        {
            this.tangentContacts[i].solve(this.normalContacts[i]);
        }

        if (this.numContacts == 1 || !Settings.blockSolve)
        {
            for (let i = 0; i < this.numContacts; i++)
            {
                this.normalContacts[i].solve();
            }
        }
        else // Solve two contact constraint in one shot using block solver
        {
            this.blockSolver.solve();
        }
    }

    tryWarmStart(oldManifold /*: ContactManifold*/) {
        for (let n = 0; n < this.numContacts; n++)
        {
            let o = 0;
            for (; o < oldManifold.numContacts; o++)
            {
                if (this.contactPoints[n].id == oldManifold.contactPoints[o].id)
                {
                    if (Settings.applyWarmStartingThreshold)
                    {
                        let dist = Util.squared_distance(this.contactPoints[n].point, oldManifold.contactPoints[o].point);
                        // If contact points are close enough, warm start.
                        // Otherwise, it means it's penetrating too deeply, skip the warm starting to prevent the overshoot
                        if (dist < Settings.warmStartingThreshold)
                            break;
                    }
                    else
                    {
                        break;
                    }
                }
            }

            if (o < oldManifold.numContacts)
            {
                this.normalContacts[n].impulseSum = oldManifold.normalContacts[o].impulseSum;
                this.tangentContacts[n].impulseSum = oldManifold.tangentContacts[o].impulseSum;

                this.persistent = true;
            }
        }
    }

    get numContacts() {
        return this.contactPoints.length;
    }

    // return contactinfo
    getContactInfo(flip)    {
        let contactInfo = new ContactInfo(
            /* other / flip ? this.bodyB : this.bodyA, 
            /numContacts: /this.numContacts,
            /contactDir:*/ flip ? this.contactNormal.inverted() : this.contactNormal.clone(),
            /*impulse: */0,
            )

        for (let i = 0; i < this.numContacts; i++) {
            contactInfo.contactPoints.push(this.contactPoints[i].point.copy());
            contactInfo.impulse += this.normalContacts[i].impulseSum;
        }

        return contactInfo;
    }

}

Useful References

(1) Efficient Incremental Penetration Depth Estimation between .... https://arxiv.org/pdf/2304.07357.pdf.

(2) EPA (Expanding Polytope Algorithm) | dyn4j. https://dyn4j.org/2010/05/epa-expanding-polytope-algorithm/.

(3) [2304.07357] Efficient Incremental Penetration Depth .... https://arxiv.org/abs/2304.07357.

(4) Expanding Polytope Algorithm - Real-Time Physics .... https://pybullet.org/Bullet/phpBB3/viewtopic.php?t=3322.

(5) Kevin Tracy , Taylor A. Howell , and Zachary Manchester .... https://arxiv.org/pdf/2207.00669.pdf.