GJK & EPA Contact Physics

1import numpy as np
2
3def support(shape_a, shape_b, d):
4    """Minkowski Difference support point: S_{A-B}(d) = S_A(d) - S_B(-d)."""
5    pt_a = max(shape_a, key=lambda p: np.dot(p, d))
6    pt_b = max(shape_b, key=lambda p: np.dot(p, -d))
7    return np.asarray(pt_a) - np.asarray(pt_b)
8
9def gjk_collision_test(shape_a, shape_b, max_iter=25):
10    """
11    Gilbert-Johnson-Keerthi (GJK) 2D convex collision detection.
12    Iteratively builds a 2-simplex (triangle) in Minkowski Difference C = A - B
13    to test whether C encloses the origin (0, 0).
14    """
15    d = np.mean(shape_b, axis=0) - np.mean(shape_a, axis=0)
16    if np.linalg.norm(d) < 1e-8:
17        d = np.array([1.0, 0.0])
18    simplex = [support(shape_a, shape_b, d)]
19    d = -simplex[0]
20
21    for _ in range(max_iter):
22        a = support(shape_a, shape_b, d)
23        if np.dot(a, d) < 0:
24            return False, simplex  # Separating axis found
25        simplex.append(a)
26        if len(simplex) == 2:
27            ab, ao = simplex[0] - a, -a
28            d = np.array([-ab[1], ab[0]])
29            if np.dot(d, ao) < 0:
30                d = -d
31        elif len(simplex) == 3:
32            c, b, a = simplex
33            ab, ac, ao = b - a, c - a, -a
34            ab_perp = np.array([-ab[1], ab[0]]) * (-1 if np.dot([-ab[1], ab[0]], ac) > 0 else 1)
35            ac_perp = np.array([-ac[1], ac[0]]) * (-1 if np.dot([-ac[1], ac[0]], ab) > 0 else 1)
36            if np.dot(ab_perp, ao) > 0:
37                simplex.pop(0)
38                d = ab_perp
39            elif np.dot(ac_perp, ao) > 0:
40                simplex.pop(1)
41                d = ac_perp
42            else:
43                return True, simplex  # Origin enclosed inside 2-simplex
44    return False, simplex

1def epa_penetration_vector(initial_simplex, shape_a, shape_b, tol=1e-4):
2    """
3    Expanding Polytope Algorithm (EPA) for collision contact manifolds.
4    Takes the terminating GJK simplex enclosing the origin.
5    Iteratively finds the closest facet to origin, shoots ray in facet normal direction,
6    queries support mapping, and expands the polytope until convergence.
7    Returns: penetration_depth (scalar) and contact_normal (unit vector).
8    """
9    polytope = list(initial_simplex)
10    while True:
11        closest_facet, normal, dist = find_closest_facet_to_origin(polytope)
12        p = support(shape_a, normal) - support(shape_b, -normal)
13        d = np.dot(p, normal)
14        if d - dist < tol:
15            return dist, normal  # Exact penetration depth and separation direction
16        polytope = expand_polytope_with_point(polytope, p)