Add support for speculative contacts

CHANGELOG.md

@@ -76,6 +76,8 @@
 - Export `ViewerBase` from `newton.viewer` public API
 - Add `custom_attributes` argument to `ModelBuilder.add_shape_convex_hull()`
 - Add RJ45 plug-socket insertion example with SDF contacts, latch joint, and interactive gizmo
+- Add `SpeculativeContactConfig` and speculative contact support in `CollisionPipeline` to detect contacts for fast-moving objects before they tunnel through thin geometry
+- Add `TRIANGLE_PRISM` support-function type for heightfield triangles, extruding 1 m along the heightfield's local -Z so GJK/MPR naturally resolves shapes on the back side
 
 ### Changed
 

docs/api/newton.rst

@@ -55,6 +55,7 @@ newton
    ParticleFlags
    SDF
    ShapeFlags
+   SpeculativeContactConfig
    State
    TetMesh
 

docs/concepts/collisions.rst

@@ -1101,6 +1101,48 @@ Example (mesh SDF workflow):
 
 The builder's ``rigid_gap`` (default 0.1) applies to shapes without explicit ``gap``. Alternatively, use ``builder.default_shape_cfg.gap``.
 
+.. _Speculative Contacts:
+
+Speculative Contacts
+--------------------
+
+Fast-moving objects can travel farther than the contact gap in a single time
+step, causing them to tunnel through thin geometry.  **Speculative contacts**
+widen the detection window based on per-shape velocity so that contacts which
+*will* occur within the next collision update interval are caught early.
+
+Enable speculative contacts by passing a :class:`SpeculativeContactConfig` to
+:class:`CollisionPipeline`:
+
+.. code-block:: python
+
+    config = newton.SpeculativeContactConfig(
+        max_speculative_extension=0.5,   # cap per-axis AABB growth [m]
+        collision_update_dt=1.0 / 60.0,  # expected interval between collide() calls
+    )
+    pipeline = newton.CollisionPipeline(model, speculative_config=config)
+
+At each ``collide()`` call the pipeline:
+
+1. Computes per-shape linear velocity and an angular-speed bound from
+   ``State.body_qd``.
+2. Expands each shape AABB by the clamped velocity contribution (capped by
+   ``max_speculative_extension``) so the broad phase returns candidate pairs
+   that are about to collide.
+3. In the narrow phase, recomputes the contact gap using the normal-projected
+   approach speed of the relative linear velocity **plus** per-shape
+   angular-speed bounds (clamped by ``max_speculative_extension``), so only
+   genuinely approaching pairs are accepted.
+
+The ``collision_update_dt`` can be overridden per call:
+
+.. code-block:: python
+
+    pipeline.collide(state, contacts, dt=sim_dt)
+
+When ``speculative_config`` is ``None`` (the default), all speculative code
+paths are eliminated at compile time with zero runtime overhead.
+
 .. _Common Patterns:
 
 Common Patterns

newton/__init__.py

@@ -56,6 +56,7 @@
     JointType,
     Model,
     ModelBuilder,
+    SpeculativeContactConfig,
     State,
     eval_fk,
     eval_ik,
@@ -73,6 +74,7 @@
     "JointType",
     "Model",
     "ModelBuilder",
+    "SpeculativeContactConfig",
     "State",
     "eval_fk",
     "eval_ik",

newton/_src/geometry/contact_reduction_global.py

@@ -1452,106 +1452,127 @@ def export_reduced_contacts_kernel(
     return export_reduced_contacts_kernel
 
 
-@wp.kernel(enable_backward=False, module="unique")
-def mesh_triangle_contacts_to_reducer_kernel(
-    shape_types: wp.array[int],
-    shape_data: wp.array[wp.vec4],
-    shape_transform: wp.array[wp.transform],
-    shape_source: wp.array[wp.uint64],
-    shape_gap: wp.array[float],
-    shape_heightfield_index: wp.array[wp.int32],
-    heightfield_data: wp.array[HeightfieldData],
-    heightfield_elevations: wp.array[wp.float32],
-    triangle_pairs: wp.array[wp.vec3i],
-    triangle_pairs_count: wp.array[int],
-    reducer_data: GlobalContactReducerData,
-    total_num_threads: int,
-):
-    """Process mesh/heightfield-triangle contacts and store them in GlobalContactReducer.
+def create_mesh_triangle_contacts_to_reducer_kernel(speculative: bool = False):
+    """Factory for the mesh/heightfield-triangle → reducer kernel.
 
-    This kernel processes triangle pairs (mesh-or-hfield shape, convex-shape, triangle_index)
-    and computes contacts using GJK/MPR, storing results in the GlobalContactReducer for
-    subsequent reduction and export.
-
-    Uses grid stride loop over triangle pairs.
+    When *speculative* is True the kernel reads per-shape velocity arrays and
+    extends ``gap_sum`` by a scalar speculative margin.  When False the extra
+    code is eliminated at compile time via ``wp.static``.
     """
-    tid = wp.tid()
 
-    num_triangle_pairs = triangle_pairs_count[0]
+    @wp.kernel(enable_backward=False, module="unique")
+    def mesh_triangle_contacts_to_reducer_kernel(
+        shape_types: wp.array[int],
+        shape_data: wp.array[wp.vec4],
+        shape_transform: wp.array[wp.transform],
+        shape_source: wp.array[wp.uint64],
+        shape_gap: wp.array[float],
+        shape_heightfield_index: wp.array[wp.int32],
+        heightfield_data: wp.array[HeightfieldData],
+        heightfield_elevations: wp.array[wp.float32],
+        triangle_pairs: wp.array[wp.vec3i],
+        triangle_pairs_count: wp.array[int],
+        reducer_data: GlobalContactReducerData,
+        total_num_threads: int,
+        shape_lin_vel: wp.array[wp.vec3],
+        shape_ang_speed_bound: wp.array[float],
+        speculative_dt: float,
+        max_speculative_extension: float,
+    ):
+        """Process mesh/heightfield-triangle contacts and store them in GlobalContactReducer.
 
-    for i in range(tid, num_triangle_pairs, total_num_threads):
-        if i >= triangle_pairs.shape[0]:
-            break
+        This kernel processes triangle pairs (mesh-or-hfield shape, convex-shape, triangle_index)
+        and computes contacts using GJK/MPR, storing results in the GlobalContactReducer for
+        subsequent reduction and export.
 
-        triple = triangle_pairs[i]
-        shape_a = triple[0]  # Mesh or heightfield shape
-        shape_b = triple[1]  # Convex shape
-        tri_idx = triple[2]
+        Uses grid stride loop over triangle pairs.
+        """
+        tid = wp.tid()
 
-        type_a = shape_types[shape_a]
+        num_triangle_pairs = triangle_pairs_count[0]
 
-        if type_a == GeoType.HFIELD:
-            # Heightfield triangle
-            hfd = heightfield_data[shape_heightfield_index[shape_a]]
-            X_ws_a = shape_transform[shape_a]
-            shape_data_a, v0_world = get_triangle_shape_from_heightfield(hfd, heightfield_elevations, X_ws_a, tri_idx)
-        else:
-            # Mesh triangle (mesh_id already validated by midphase)
-            mesh_id_a = shape_source[shape_a]
-            scale_data_a = shape_data[shape_a]
-            mesh_scale_a = wp.vec3(scale_data_a[0], scale_data_a[1], scale_data_a[2])
-            X_ws_a = shape_transform[shape_a]
-            shape_data_a, v0_world = get_triangle_shape_from_mesh(mesh_id_a, mesh_scale_a, X_ws_a, tri_idx)
-
-        # Extract shape B data
-        pos_b, quat_b, shape_data_b, _scale_b, margin_offset_b = extract_shape_data(
-            shape_b,
-            shape_transform,
-            shape_types,
-            shape_data,
-            shape_source,
-        )
+        for i in range(tid, num_triangle_pairs, total_num_threads):
+            if i >= triangle_pairs.shape[0]:
+                break
 
-        # Triangle position is vertex A in world space.
-        # For heightfield prisms, edges are in heightfield-local space
-        # so we pass the heightfield rotation to let MPR/GJK work in
-        # that frame (where -Z is always the down axis).
-        pos_a = v0_world
-        if type_a == GeoType.HFIELD:
-            quat_a = wp.transform_get_rotation(shape_transform[shape_a])
-        else:
-            quat_a = wp.quat_identity()
-
-        # Back-face culling: skip when the convex center is behind the
-        # triangle face.  TRIANGLE_PRISM (heightfields) handles this
-        # via its extruded support function.
-        if shape_data_a.shape_type == int(GeoTypeEx.TRIANGLE):
-            face_normal = wp.cross(shape_data_a.scale, shape_data_a.auxiliary)
-            center_dist = wp.dot(face_normal, pos_b - pos_a)
-            if center_dist < 0.0:
-                continue
-
-        # Extract margin offset for shape A (signed distance padding)
-        margin_offset_a = shape_data[shape_a][3]
-
-        # Use additive per-shape contact gap for detection threshold
-        gap_a = shape_gap[shape_a]
-        gap_b = shape_gap[shape_b]
-        gap_sum = gap_a + gap_b
-
-        # Compute and write contacts using GJK/MPR
-        wp.static(create_compute_gjk_mpr_contacts(write_contact_to_reducer))(
-            shape_data_a,
-            shape_data_b,
-            quat_a,
-            quat_b,
-            pos_a,
-            pos_b,
-            gap_sum,
-            shape_a,
-            shape_b,
-            margin_offset_a,
-            margin_offset_b,
-            reducer_data,
-            (tri_idx << 1) | 1,
-        )
+            triple = triangle_pairs[i]
+            shape_a = triple[0]  # Mesh or heightfield shape
+            shape_b = triple[1]  # Convex shape
+            tri_idx = triple[2]
+
+            type_a = shape_types[shape_a]
+
+            if type_a == GeoType.HFIELD:
+                # Heightfield triangle
+                hfd = heightfield_data[shape_heightfield_index[shape_a]]
+                X_ws_a = shape_transform[shape_a]
+                shape_data_a, v0_world = get_triangle_shape_from_heightfield(
+                    hfd, heightfield_elevations, X_ws_a, tri_idx
+                )
+            else:
+                # Mesh triangle (mesh_id already validated by midphase)
+                mesh_id_a = shape_source[shape_a]
+                scale_data_a = shape_data[shape_a]
+                mesh_scale_a = wp.vec3(scale_data_a[0], scale_data_a[1], scale_data_a[2])
+                X_ws_a = shape_transform[shape_a]
+                shape_data_a, v0_world = get_triangle_shape_from_mesh(mesh_id_a, mesh_scale_a, X_ws_a, tri_idx)
+
+            # Extract shape B data
+            pos_b, quat_b, shape_data_b, _scale_b, margin_offset_b = extract_shape_data(
+                shape_b,
+                shape_transform,
+                shape_types,
+                shape_data,
+                shape_source,
+            )
+
+            # Triangle position is vertex A in world space.
+            # For heightfield prisms, edges are in heightfield-local space
+            # so we pass the heightfield rotation to let MPR/GJK work in
+            # that frame (where -Z is always the down axis).
+            pos_a = v0_world
+            if type_a == GeoType.HFIELD:
+                quat_a = wp.transform_get_rotation(shape_transform[shape_a])
+            else:
+                quat_a = wp.quat_identity()
+
+            # Back-face culling: skip when the convex center is behind the
+            # triangle face.  TRIANGLE_PRISM (heightfields) handles this
+            # via its extruded support function.
+            if shape_data_a.shape_type == int(GeoTypeEx.TRIANGLE):
+                face_normal = wp.cross(shape_data_a.scale, shape_data_a.auxiliary)
+                center_dist = wp.dot(face_normal, pos_b - pos_a)
+                if center_dist < 0.0:
+                    continue
+
+            # Extract margin offset for shape A (signed distance padding)
+            margin_offset_a = shape_data[shape_a][3]
+
+            # Use additive per-shape contact gap for detection threshold
+            gap_a = shape_gap[shape_a]
+            gap_b = shape_gap[shape_b]
+            gap_sum = gap_a + gap_b
+
+            if wp.static(speculative):
+                vel_rel = shape_lin_vel[shape_b] - shape_lin_vel[shape_a]
+                rel_speed = wp.length(vel_rel) + shape_ang_speed_bound[shape_a] + shape_ang_speed_bound[shape_b]
+                gap_sum = gap_sum + wp.min(rel_speed * speculative_dt, max_speculative_extension)
+
+            # Compute and write contacts using GJK/MPR
+            wp.static(create_compute_gjk_mpr_contacts(write_contact_to_reducer))(
+                shape_data_a,
+                shape_data_b,
+                quat_a,
+                quat_b,
+                pos_a,
+                pos_b,
+                gap_sum,
+                shape_a,
+                shape_b,
+                margin_offset_a,
+                margin_offset_b,
+                reducer_data,
+                (tri_idx << 1) | 1,
+            )
+
+    return mesh_triangle_contacts_to_reducer_kernel