pyforge3d 1.0.0__py3-none-any.whl

forge3d/collision/epa.py

@@ -0,0 +1,217 @@
+"""Expanding Polytope Algorithm (EPA) for penetration depth and contact normal.
+
+Given two intersecting convex shapes and a GJK simplex that encloses the
+Minkowski-difference origin, EPA expands the simplex into a full polytope
+and finds the minimum-penetration-depth contact.
+
+Returns:
+  depth  : float — penetration depth (> 0)
+  normal : (3,) ndarray — contact normal from body_b toward body_a
+
+Convention: normal points FROM b TOWARD a (same as ContactPoint).
+"""
+
+from __future__ import annotations
+
+from typing import Any
+
+import numpy as np
+
+# ── Internal helpers ──────────────────────────────────────────────────────────
+
+
+def _face_normal_and_dist(
+    v0: np.ndarray, v1: np.ndarray, v2: np.ndarray
+) -> tuple[np.ndarray, float]:
+    """Outward face normal and distance from origin for one triangle.
+
+    'Outward' means pointing AWAY from the interior of the polytope,
+    i.e. the signed distance dot(normal, v0) should be positive when
+    origin is inside.
+    """
+    edge1 = v1 - v0
+    edge2 = v2 - v0
+    n = np.cross(edge1, edge2)
+    n_len = float(np.linalg.norm(n))
+    if n_len < 1e-14:
+        return np.array([0.0, 0.0, 1.0]), 1e9
+    n = n / n_len
+    d = float(np.dot(n, v0))
+    if d < 0.0:  # flip so normal points away from origin
+        n = -n
+        d = -d
+    return n, d
+
+
+def _closest_face(
+    verts: list[np.ndarray], faces: list[tuple[int, int, int]]
+) -> tuple[int, np.ndarray, float]:
+    """Find the face (index, normal, dist) closest to the origin."""
+    min_dist = float("inf")
+    min_idx = 0
+    min_normal = np.array([0.0, 0.0, 1.0])
+
+    for i, (a, b, c) in enumerate(faces):
+        n, d = _face_normal_and_dist(verts[a], verts[b], verts[c])
+        if d < min_dist:
+            min_dist = d
+            min_idx = i
+            min_normal = n
+
+    return min_idx, min_normal, min_dist
+
+
+def _build_initial_polytope(
+    simplex: list[np.ndarray],
+    cso_fn: Any,  # callable(d) → point on CSO
+) -> tuple[list[np.ndarray], list[tuple[int, int, int]]]:
+    """Turn a GJK simplex into an initial tetrahedron polytope.
+
+    GJK may return a simplex with fewer than 4 vertices when the origin is
+    near a face/edge of the simplex.  We expand it here to a full tetrahedron.
+    """
+    verts = list(simplex)
+
+    # Expand to at least 4 non-coplanar vertices
+    directions = [
+        np.array([1.0, 0.0, 0.0]),
+        np.array([-1.0, 0.0, 0.0]),
+        np.array([0.0, 1.0, 0.0]),
+        np.array([0.0, -1.0, 0.0]),
+        np.array([0.0, 0.0, 1.0]),
+        np.array([0.0, 0.0, -1.0]),
+    ]
+    while len(verts) < 4:
+        best = None
+        best_dist = -1.0
+        for d in directions:
+            pt = cso_fn(d)
+            dist = float(np.dot(pt, d))
+            if best is None or dist > best_dist:
+                best_dist = dist
+                best = pt
+        if best is None:
+            break
+        # Only add if it doesn't duplicate an existing vertex
+        if all(float(np.linalg.norm(best - v)) > 1e-7 for v in verts):
+            verts.append(best)
+        else:
+            break
+
+    if len(verts) < 4:
+        # Degenerate: can't form a tetrahedron
+        return verts, []
+
+    # Build 4 triangular faces from tetrahedron (A, B, C, D).
+    # Ensure each face normal points outward (away from opposite vertex).
+    A, B, C, D = verts[:4]
+    faces: list[tuple[int, int, int]] = []
+
+    def add_face(i0: int, i1: int, i2: int, opposite: np.ndarray) -> None:
+        n, _ = _face_normal_and_dist(verts[i0], verts[i1], verts[i2])
+        # If normal points TOWARD opposite vertex, flip winding
+        if np.dot(n, opposite - verts[i0]) > 0:
+            faces.append((i0, i2, i1))  # reversed winding
+        else:
+            faces.append((i0, i1, i2))
+
+    add_face(0, 1, 2, D)  # ABC, opposite D
+    add_face(0, 1, 3, C)  # ABD, opposite C
+    add_face(0, 2, 3, B)  # ACD, opposite B
+    add_face(1, 2, 3, A)  # BCD, opposite A
+
+    return verts, faces
+
+
+def _expand_polytope(
+    verts: list[np.ndarray],
+    faces: list[tuple[int, int, int]],
+    new_pt: np.ndarray,
+    closest_face_idx: int,
+) -> tuple[list[np.ndarray], list[tuple[int, int, int]]]:
+    """Remove faces visible from new_pt and fill the hole.
+
+    A face is 'visible' from new_pt if new_pt is on the outward side of the face.
+    """
+    # Collect edges on the silhouette (border between visible/non-visible faces)
+    edge_count: dict[tuple[int, int], int] = {}
+
+    visible: set[int] = set()
+    for i, (a, b, c) in enumerate(faces):
+        n, _ = _face_normal_and_dist(verts[a], verts[b], verts[c])
+        if np.dot(n, new_pt - verts[a]) > 1e-10:
+            visible.add(i)
+            for ea, eb in [(a, b), (b, c), (c, a)]:
+                key = (min(ea, eb), max(ea, eb))
+                edge_count[key] = edge_count.get(key, 0) + 1
+
+    silhouette = [edge for edge, cnt in edge_count.items() if cnt == 1]
+
+    new_v_idx = len(verts)
+    verts.append(new_pt)
+
+    # Keep non-visible faces
+    new_faces = [f for i, f in enumerate(faces) if i not in visible]
+
+    # Add new triangles from silhouette to new_pt
+    for ea, eb in silhouette:
+        # Find correct winding so normal points away from origin
+        n, _ = _face_normal_and_dist(verts[ea], verts[eb], new_pt)
+        # normal should have positive dot with vertices (origin is inside polytope)
+        if np.dot(n, verts[ea]) < 0:
+            new_faces.append((ea, new_v_idx, eb))
+        else:
+            new_faces.append((ea, eb, new_v_idx))
+
+    return verts, new_faces
+
+
+# ── Public EPA interface ──────────────────────────────────────────────────────
+
+
+def epa(
+    body_a: Any,
+    body_b: Any,
+    gjk_simplex: list[np.ndarray],
+    max_iter: int = 64,
+    tolerance: float = 1e-5,
+) -> tuple[float, np.ndarray]:
+    """Run EPA to find penetration depth and contact normal.
+
+    Parameters
+    ----------
+    body_a, body_b : duck-typed bodies with shape_type, pos, quat, shape_params.
+    gjk_simplex    : simplex from GJK (must enclose the origin).
+    max_iter       : maximum expansion iterations.
+    tolerance      : convergence threshold.
+
+    Returns
+    -------
+    depth  : penetration depth (> 0).
+    normal : unit contact normal from body_b toward body_a.
+    """
+    from forge3d.collision.gjk import _cso_support
+
+    def cso_fn(d: np.ndarray) -> np.ndarray:
+        n = float(np.linalg.norm(d))
+        return _cso_support(body_a, body_b, d / n if n > 1e-12 else d)
+
+    verts, faces = _build_initial_polytope(gjk_simplex, cso_fn)
+
+    if len(faces) == 0:
+        # Degenerate — return a simple upward normal
+        return 0.01, np.array([0.0, 0.0, 1.0])
+
+    for _ in range(max_iter):
+        closest_idx, normal, dist = _closest_face(verts, faces)
+
+        new_pt = cso_fn(normal)
+        new_dist = float(np.dot(new_pt, normal))
+
+        if new_dist - dist < tolerance:
+            return dist, normal
+
+        verts, faces = _expand_polytope(verts, faces, new_pt, closest_idx)
+
+    closest_idx, normal, dist = _closest_face(verts, faces)
+    return dist, normal

forge3d/collision/gjk.py

@@ -0,0 +1,266 @@
+"""Gilbert-Johnson-Keerthi (GJK) distance algorithm.
+
+Determines whether two convex shapes intersect and, if not, computes the
+minimum distance between them.
+
+Supported shapes:
+  sphere       — represented by (center, radius)
+  box OBB      — represented by (position, rotation, half_extents)
+  mesh/capsule — convex hull support via hull_vertices in shape_params
+
+Algorithm:
+  Build a simplex in the Minkowski difference A ⊖ B.  If the simplex encloses
+  the origin, the shapes intersect.  Otherwise, the distance equals the length
+  of the nearest-point vector from the simplex to the origin.
+
+Convention:
+  Normal points from shape_b toward shape_a (consistent with ContactPoint).
+"""
+
+from __future__ import annotations
+
+from typing import Any
+
+import numpy as np
+
+# ── Support functions ─────────────────────────────────────────────────────────
+
+
+def _support_sphere(center: np.ndarray, radius: float, d: np.ndarray) -> np.ndarray:
+    n = np.linalg.norm(d)
+    if n < 1e-12:
+        return center + np.array([radius, 0.0, 0.0])
+    return center + (radius / n) * d
+
+
+def _support_box(pos: np.ndarray, R: np.ndarray, he: np.ndarray, d: np.ndarray) -> np.ndarray:
+    """Furthest point of an OBB in direction d."""
+    d_local = R.T @ d
+    s_local = np.where(d_local >= 0, he, -he)
+    return pos + R @ s_local
+
+
+def _support_convex_hull(
+    pos: np.ndarray, R: np.ndarray, hull_verts: np.ndarray, d: np.ndarray
+) -> np.ndarray:
+    """Furthest point of a convex hull in world-frame direction d.
+
+    hull_verts is (K, 3) in LOCAL frame.  O(K) — fast for small hulls.
+    """
+    d_local = R.T @ d          # direction in local frame
+    dots = hull_verts @ d_local  # (K,) — vectorised
+    return pos + R @ hull_verts[int(np.argmax(dots))]
+
+
+def _body_support(body: Any, d: np.ndarray) -> np.ndarray:
+    """Dispatch to the correct support function for a body."""
+    from forge3d.math.quaternion import quat_to_rot
+
+    if body.shape_type == "sphere":
+        return _support_sphere(body.pos, float(body.shape_params["radius"]), d)
+    if body.shape_type == "box":
+        R = quat_to_rot(body.quat)
+        return _support_box(body.pos, R, body.shape_params["half_extents"], d)
+    if body.shape_type in ("mesh", "capsule"):
+        R = quat_to_rot(body.quat)
+        hull_verts = body.shape_params["hull_vertices"]
+        return _support_convex_hull(body.pos, R, hull_verts, d)
+    raise ValueError(f"GJK: unsupported shape '{body.shape_type}'")
+
+
+def _cso_support(a: Any, b: Any, d: np.ndarray) -> np.ndarray:
+    """Minkowski difference support: support_a(d) - support_b(-d)."""
+    return _body_support(a, d) - _body_support(b, -d)
+
+
+# ── Simplex nearest-point helpers ─────────────────────────────────────────────
+
+
+def _nearest_on_line(p0: np.ndarray, p1: np.ndarray) -> tuple[list[np.ndarray], np.ndarray]:
+    """Nearest point on segment [p0, p1] to origin.  p1 is the newest vertex."""
+    p01 = p0 - p1
+    t = float(np.dot(-p1, p01)) / float(np.dot(p01, p01) + 1e-300)
+    t = max(0.0, min(1.0, t))
+    nearest = p1 + t * p01
+    return [p0, p1] if t > 0.0 else [p1], -nearest
+
+
+def _nearest_on_triangle(
+    p0: np.ndarray, p1: np.ndarray, p2: np.ndarray
+) -> tuple[list[np.ndarray], np.ndarray] | None:
+    """Nearest feature of triangle [p0, p1, p2] to origin (p2 newest).
+
+    Returns (reduced_simplex, direction) or None if origin inside the infinite
+    prism defined by the triangle (used when testing tetrahedron faces).
+    """
+    ab = p1 - p2
+    ac = p0 - p2
+    ao = -p2
+    n = np.cross(ab, ac)
+
+    # Check if origin is outside each edge (in the plane of the triangle)
+    if np.dot(np.cross(ab, n), ao) > 0:
+        # Outside edge AB (between p2 and p1)
+        if np.dot(ab, ao) > 0:
+            d = np.cross(np.cross(ab, ao), ab)
+            # parenthesise to avoid 3-tuple mis-parse
+            return ([p1, p2], d) if float(np.dot(d, d)) > 1e-20 else ([p1, p2], ao)
+        return _nearest_on_line(p2, p2)  # fallback: just vertex p2
+
+    if np.dot(np.cross(n, ac), ao) > 0:
+        # Outside edge AC (between p2 and p0)
+        if np.dot(ac, ao) > 0:
+            d = np.cross(np.cross(ac, ao), ac)
+            return ([p0, p2], d) if float(np.dot(d, d)) > 1e-20 else ([p0, p2], ao)
+        return [p2], ao
+
+    # Inside the triangle in the plane — check above/below face
+    dn = float(np.dot(n, ao))
+    if dn > 0:
+        return [p0, p1, p2], n
+    if dn < 0:
+        # Flip winding so normal points toward origin
+        return [p1, p0, p2], -n
+    # Origin ON the face
+    return None  # contained
+
+
+def _do_simplex(
+    simplex: list[np.ndarray], d: np.ndarray
+) -> tuple[list[np.ndarray], np.ndarray] | None:
+    """Evolve the GJK simplex toward origin.
+
+    Returns (new_simplex, new_direction) when not yet enclosed, or None when
+    the simplex contains the origin (→ intersection).
+    """
+    n = len(simplex)
+
+    if n == 1:
+        return simplex, -simplex[0]
+
+    if n == 2:
+        return _nearest_on_line(simplex[0], simplex[1])
+
+    if n == 3:
+        result = _nearest_on_triangle(simplex[0], simplex[1], simplex[2])
+        return result  # None → origin inside triangle (treat as intersection)
+
+    # n == 4: tetrahedron
+    D, C, B, A = simplex
+    AB = B - A
+    AC = C - A
+    AD = D - A
+    AO = -A
+
+    # Outward face normals (re-orient so they point away from the opposite vertex)
+    ABC = np.cross(AB, AC)
+    ACD = np.cross(AC, AD)
+    ADB = np.cross(AD, AB)
+    if np.dot(ABC, AD) > 0:
+        ABC = -ABC
+    if np.dot(ACD, AB) > 0:
+        ACD = -ACD
+    if np.dot(ADB, AC) > 0:
+        ADB = -ADB
+
+    in_abc = np.dot(ABC, AO) > 0
+    in_acd = np.dot(ACD, AO) > 0
+    in_adb = np.dot(ADB, AO) > 0
+
+    if not in_abc and not in_acd and not in_adb:
+        return None  # origin inside tetrahedron → intersection!
+
+    # Reduce to the face that the origin is "most in front of"
+    if in_abc:
+        return _nearest_on_triangle(C, B, A)
+    if in_acd:
+        return _nearest_on_triangle(D, C, A)
+    return _nearest_on_triangle(B, D, A)
+
+
+# ── Internal GJK returning simplex ───────────────────────────────────────────
+
+
+def _gjk_internal(
+    body_a: Any, body_b: Any, max_iter: int = 64
+) -> tuple[bool, float, list[np.ndarray]]:
+    """Run GJK and return (intersecting, distance, final_simplex).
+
+    The simplex is needed by EPA when bodies intersect.
+    """
+    d = body_b.pos - body_a.pos
+    if float(np.linalg.norm(d)) < 1e-10:
+        d = np.array([1.0, 0.0, 0.0])
+    d = d / np.linalg.norm(d)
+
+    s = _cso_support(body_a, body_b, d)
+    simplex: list[np.ndarray] = [s]
+    d = -s
+
+    for _ in range(max_iter):
+        d_len = float(np.linalg.norm(d))
+        if d_len < 1e-12:
+            return True, 0.0, simplex
+
+        new_pt = _cso_support(body_a, body_b, d / d_len)
+        if float(np.dot(new_pt, d)) < 0:
+            return False, d_len, simplex
+
+        simplex.append(new_pt)
+        result = _do_simplex(simplex, d)
+
+        if result is None:
+            return True, 0.0, simplex
+
+        simplex, d = result
+
+    return True, 0.0, simplex
+
+
+# ── Public GJK interface ──────────────────────────────────────────────────────
+
+
+def gjk(body_a: Any, body_b: Any, max_iter: int = 64) -> tuple[bool, float]:
+    """Run GJK between two convex bodies.
+
+    Parameters
+    ----------
+    body_a, body_b : bodies with ``shape_type``, ``pos``, ``quat``,
+                     ``shape_params`` attributes (duck-typed _Body).
+    max_iter       : maximum GJK iterations (default 64).
+
+    Returns
+    -------
+    intersecting : True if bodies overlap.
+    distance     : 0.0 if intersecting; minimum separation otherwise.
+    """
+    intersecting, dist, _ = _gjk_internal(body_a, body_b, max_iter)
+    return intersecting, dist
+
+
+def gjk_contact(
+    body_a: Any, body_b: Any, max_iter: int = 64
+) -> tuple[float, np.ndarray] | None:
+    """GJK + EPA contact query.
+
+    Returns (depth, normal) where normal points from body_b toward body_a,
+    or None if bodies are not intersecting.
+    """
+    from forge3d.collision.epa import epa
+
+    intersecting, _, simplex = _gjk_internal(body_a, body_b, max_iter)
+    if not intersecting:
+        return None
+    return epa(body_a, body_b, simplex)
+
+
+def gjk_intersect(body_a: Any, body_b: Any) -> bool:
+    """Boolean intersection test. True iff bodies overlap."""
+    intersecting, _ = gjk(body_a, body_b)
+    return intersecting
+
+
+def gjk_distance(body_a: Any, body_b: Any) -> float:
+    """Minimum separation distance. Returns 0.0 if bodies overlap."""
+    _, dist = gjk(body_a, body_b)
+    return dist

forge3d/collision/heightfield.py

@@ -0,0 +1,196 @@
+"""Heightfield terrain collision shape.
+
+Represents a terrain as a 2D grid of height values.
+Supports sphere and box collision against the terrain surface.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import TYPE_CHECKING, Any
+
+import numpy as np
+
+if TYPE_CHECKING:
+    from forge3d.collision.detection import ContactPoint
+
+
+@dataclass
+class Heightfield:
+    """A 2D grid of height values representing terrain.
+
+    Attributes:
+        heights: 2D float32 array of shape (rows, cols). heights[r, c] is
+            the terrain height (z) at grid cell (r, c).
+        cell_size: World-space size of each grid cell (m).
+        origin: World-space position of the (0, 0) grid corner (x, y, z).
+    """
+
+    heights: np.ndarray    # (rows, cols) float32
+    cell_size: float
+    origin: np.ndarray     # (3,) float64 — world position of grid corner
+
+    @property
+    def rows(self) -> int:
+        return self.heights.shape[0]
+
+    @property
+    def cols(self) -> int:
+        return self.heights.shape[1]
+
+    @property
+    def total_width(self) -> float:
+        return self.cell_size * (self.cols - 1)
+
+    @property
+    def total_depth(self) -> float:
+        return self.cell_size * (self.rows - 1)
+
+    def height_at(self, x: float, y: float) -> float:
+        """Bilinearly interpolated height at world-space (x, y)."""
+        lx = x - self.origin[0]
+        ly = y - self.origin[1]
+
+        # Grid coordinates (float)
+        gx = lx / self.cell_size
+        gy = ly / self.cell_size
+
+        # Integer cell indices
+        c0 = int(np.floor(gx))
+        r0 = int(np.floor(gy))
+
+        # Clamp to valid range
+        c0 = max(0, min(c0, self.cols - 2))
+        r0 = max(0, min(r0, self.rows - 2))
+        c1 = c0 + 1
+        r1 = r0 + 1
+
+        # Bilinear interpolation weights
+        tx = gx - c0
+        ty = gy - r0
+        tx = max(0.0, min(1.0, tx))
+        ty = max(0.0, min(1.0, ty))
+
+        h00 = float(self.heights[r0, c0])
+        h10 = float(self.heights[r1, c0])
+        h01 = float(self.heights[r0, c1])
+        h11 = float(self.heights[r1, c1])
+
+        return (
+            h00 * (1 - tx) * (1 - ty)
+            + h01 * tx * (1 - ty)
+            + h10 * (1 - tx) * ty
+            + h11 * tx * ty
+        ) + float(self.origin[2])
+
+    def normal_at(self, x: float, y: float) -> np.ndarray:
+        """Approximate surface normal at (x, y) from finite differences."""
+        dx = self.cell_size
+        hx0 = self.height_at(x - dx, y)
+        hx1 = self.height_at(x + dx, y)
+        hy0 = self.height_at(x, y - dx)
+        hy1 = self.height_at(x, y + dx)
+
+        dhx = (hx1 - hx0) / (2 * dx)
+        dhy = (hy1 - hy0) / (2 * dx)
+
+        n = np.array([-dhx, -dhy, 1.0])
+        norm = np.linalg.norm(n)
+        return n / norm if norm > 1e-10 else np.array([0.0, 0.0, 1.0])
+
+
+def sphere_vs_heightfield(
+    sphere_body: Any,
+    sphere_idx: int,
+    hf: "Heightfield",
+) -> list["ContactPoint"]:
+    """Collision between a sphere and a heightfield terrain.
+
+    Args:
+        sphere_body: A ``_Body`` with ``shape_type == "sphere"``.
+        sphere_idx: List index of the sphere body.
+        hf: The heightfield to test against.
+
+    Returns:
+        List of ``ContactPoint`` objects (0 or 1 contacts).
+    """
+    from forge3d.collision.detection import ContactPoint
+
+    pos = sphere_body.pos
+    radius = float(sphere_body.shape_params["radius"])
+
+    x, y = float(pos[0]), float(pos[1])
+    h_terrain = hf.height_at(x, y)
+    depth = h_terrain + radius - float(pos[2])
+
+    if depth <= 0:
+        return []
+
+    normal = hf.normal_at(x, y)  # points up (z+)
+    contact_pos = np.array([x, y, h_terrain])
+
+    return [
+        ContactPoint(
+            body_a_idx=sphere_idx,
+            body_b_idx=-1,           # static terrain
+            pos=contact_pos,
+            normal=normal,           # pushes sphere up
+            depth=depth,
+        )
+    ]
+
+
+def box_vs_heightfield(
+    box_body: Any,
+    box_idx: int,
+    hf: "Heightfield",
+) -> list["ContactPoint"]:
+    """Collision between a box and a heightfield terrain.
+
+    Approximates the box as 8 corner point samples against the heightfield.
+
+    Args:
+        box_body: A ``_Body`` with ``shape_type == "box"``.
+        box_idx: List index of the box body.
+        hf: The heightfield to test against.
+
+    Returns:
+        List of ``ContactPoint`` objects (0 to 4 contacts, highest penetration).
+    """
+    from forge3d.collision.detection import ContactPoint
+    from forge3d.math.quaternion import quat_to_rot
+
+    he = box_body.shape_params["half_extents"]
+    R = quat_to_rot(box_body.quat)
+    pos = box_body.pos
+
+    # 8 corners of the box in world frame
+    sx = [-1, 1]
+    corners_local = [
+        np.array([x * he[0], y * he[1], z * he[2]])
+        for x in sx for y in sx for z in sx
+    ]
+    corners_world = [pos + R @ c for c in corners_local]
+
+    contacts: list[ContactPoint] = []
+    for corner in corners_world:
+        x, y = float(corner[0]), float(corner[1])
+        h = hf.height_at(x, y)
+        depth = h - float(corner[2])
+        if depth > 0:
+            normal = hf.normal_at(x, y)
+            contacts.append(
+                ContactPoint(
+                    body_a_idx=box_idx,
+                    body_b_idx=-1,
+                    pos=np.array([x, y, h]),
+                    normal=normal,
+                    depth=depth,
+                )
+            )
+
+    # Return up to 4 deepest contacts
+    if len(contacts) > 4:
+        contacts.sort(key=lambda c: -c.depth)
+        contacts = contacts[:4]
+    return contacts