diff --git a/CHANGELOG.md b/CHANGELOG.md index e12a675bbc..a8fe5ae11e 100644 --- a/CHANGELOG.md +++ b/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 diff --git a/docs/api/newton.rst b/docs/api/newton.rst index 129e536b9e..8c02409c3a 100644 --- a/docs/api/newton.rst +++ b/docs/api/newton.rst @@ -55,6 +55,7 @@ newton ParticleFlags SDF ShapeFlags + SpeculativeContactConfig State TetMesh diff --git a/docs/concepts/collisions.rst b/docs/concepts/collisions.rst index 19a4dc7d52..a417621de7 100644 --- a/docs/concepts/collisions.rst +++ b/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 diff --git a/newton/__init__.py b/newton/__init__.py index 6b1517e1b1..fa3f07f7f8 100644 --- a/newton/__init__.py +++ b/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", diff --git a/newton/_src/geometry/contact_reduction_global.py b/newton/_src/geometry/contact_reduction_global.py index 3f7f4a798e..177afcbcaa 100644 --- a/newton/_src/geometry/contact_reduction_global.py +++ b/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 diff --git a/newton/_src/geometry/narrow_phase.py b/newton/_src/geometry/narrow_phase.py index 79182b8c48..4a655532c1 100644 --- a/newton/_src/geometry/narrow_phase.py +++ b/newton/_src/geometry/narrow_phase.py @@ -36,7 +36,7 @@ from ..geometry.contact_reduction_global import ( GlobalContactReducer, create_export_reduced_contacts_kernel, - mesh_triangle_contacts_to_reducer_kernel, + create_mesh_triangle_contacts_to_reducer_kernel, reduce_buffered_contacts_kernel, write_contact_to_reducer, ) @@ -133,7 +133,7 @@ def write_contact_simple( ) -def create_narrow_phase_primitive_kernel(writer_func: Any): +def create_narrow_phase_primitive_kernel(writer_func: Any, speculative: bool = False): """ Create a kernel for fast analytical collision detection of primitive shapes. @@ -144,11 +144,13 @@ def create_narrow_phase_primitive_kernel(writer_func: Any): Args: writer_func: Contact writer function (e.g., write_contact_simple) + speculative: When True, the kernel reads per-shape velocity arrays and + extends ``gap_sum`` by a scalar speculative margin. Returns: A warp kernel for primitive collision detection """ - _module = f"narrow_phase_primitive_{writer_func.__name__}" + _module = f"narrow_phase_primitive_{writer_func.__name__}_{speculative}" @wp.kernel(enable_backward=False, module=_module) def narrow_phase_primitive_kernel( @@ -162,6 +164,10 @@ def narrow_phase_primitive_kernel( shape_flags: wp.array[wp.int32], writer_data: Any, 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, # Output: pairs that need GJK/MPR processing gjk_candidate_pairs: wp.array[wp.vec2i], gjk_candidate_pairs_count: wp.array[int], @@ -242,6 +248,11 @@ def narrow_phase_primitive_kernel( 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) + # ===================================================================== # Route heightfield pairs. # Heightfield-vs-mesh and heightfield-vs-heightfield go through the @@ -525,7 +536,23 @@ def narrow_phase_primitive_kernel( contact_data.margin_b = margin_offset_b contact_data.shape_a = shape_a contact_data.shape_b = shape_b - contact_data.gap_sum = gap_sum + + # Recompute gap_sum with directed approach speed now that the + # contact normal is available (the pre-routing gap_sum used + # undirected speed as a conservative candidate-generation bound). + directed_gap_sum = gap_a + gap_b + if wp.static(speculative): + vel_rel = shape_lin_vel[shape_b] - shape_lin_vel[shape_a] + v_approach = ( + -wp.dot(vel_rel, contact_normal) + + shape_ang_speed_bound[shape_a] + + shape_ang_speed_bound[shape_b] + ) + directed_gap_sum = directed_gap_sum + wp.min( + wp.max(v_approach * speculative_dt, 0.0), + max_speculative_extension, + ) + contact_data.gap_sum = directed_gap_sum # Check margin for all possible contacts contact_0_valid = False @@ -606,7 +633,11 @@ def narrow_phase_primitive_kernel( def create_narrow_phase_kernel_gjk_mpr( - external_aabb: bool, writer_func: Any, support_func: Any = None, post_process_contact: Any = None + external_aabb: bool, + writer_func: Any, + support_func: Any = None, + post_process_contact: Any = None, + speculative: bool = False, ): """ Create a GJK/MPR narrow phase kernel for complex convex shape collisions. @@ -619,10 +650,15 @@ def create_narrow_phase_kernel_gjk_mpr( The remaining pairs are complex convex-convex (plane-box, plane-cylinder, plane-cone, box-box, cylinder-cylinder, etc.) that need GJK/MPR. + + + Args: + speculative: When True, extends ``gap_sum`` by a scalar speculative + margin derived from per-shape velocity arrays. """ _sf = support_func.__name__ if support_func is not None else "default" _ppc = post_process_contact.__name__ if post_process_contact is not None else "default" - _module = f"narrow_phase_gjk_mpr_{external_aabb}_{writer_func.__name__}_{_sf}_{_ppc}" + _module = f"narrow_phase_gjk_mpr_{external_aabb}_{writer_func.__name__}_{_sf}_{_ppc}_{speculative}" @wp.kernel(enable_backward=False, module=_module) def narrow_phase_kernel_gjk_mpr( @@ -638,6 +674,10 @@ def narrow_phase_kernel_gjk_mpr( shape_aabb_upper: wp.array[wp.vec3], writer_data: Any, 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, ): """ GJK/MPR collision detection for complex convex pairs. @@ -753,6 +793,11 @@ def narrow_phase_kernel_gjk_mpr( 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) + # Find and write contacts using GJK/MPR wp.static( create_find_contacts(writer_func, support_func=support_func, post_process_contact=post_process_contact) @@ -778,116 +823,141 @@ def narrow_phase_kernel_gjk_mpr( return narrow_phase_kernel_gjk_mpr -@wp.kernel(enable_backward=False) -def narrow_phase_find_mesh_triangle_overlaps_kernel( - shape_types: wp.array[int], - shape_transform: wp.array[wp.transform], - shape_source: wp.array[wp.uint64], - shape_gap: wp.array[float], # Per-shape contact gaps - shape_data: wp.array[wp.vec4], # Shape data (scale xyz, margin w) - shape_collision_radius: wp.array[float], - shape_heightfield_index: wp.array[wp.int32], - heightfield_data: wp.array[HeightfieldData], - shape_pairs_mesh: wp.array[wp.vec2i], - shape_pairs_mesh_count: wp.array[int], - total_num_threads: int, - # outputs - triangle_pairs: wp.array[wp.vec3i], # (shape_a, shape_b, triangle_idx) - triangle_pairs_count: wp.array[int], -): - """Find triangles that overlap with a convex shape for mesh and heightfield pairs. +def _create_find_mesh_triangle_overlaps_kernel(speculative: bool): + """Factory for the midphase kernel that finds mesh/heightfield triangle overlaps. - For mesh pairs, uses a tiled BVH query. For heightfield pairs, projects the - convex shape's bounding sphere onto the heightfield grid and emits triangle - pairs for each overlapping cell. - - Outputs triples of ``(mesh_or_hfield_shape, other_shape, triangle_idx)``. + When *speculative* is True the kernel reads per-shape velocity arrays and + expands the BVH query AABB by the speculative margin so that triangles + about to be hit are included as candidates. """ - tid, j = wp.tid() - - num_mesh_pairs = shape_pairs_mesh_count[0] - - # Strided loop over mesh pairs - for i in range(tid, num_mesh_pairs, total_num_threads): - pair = shape_pairs_mesh[i] - shape_a = pair[0] - shape_b = pair[1] - - type_a = shape_types[shape_a] - type_b = shape_types[shape_b] - - # ----------------------------------------------------------------- - # Heightfield-vs-convex midphase (grid cell lookup) - # Pairs are normalized so the heightfield is always shape_a. - # ----------------------------------------------------------------- - if type_a == GeoType.HFIELD: - # Only run on j==0; the j dimension is for tiled BVH queries (mesh only). - if j != 0: + + @wp.kernel(enable_backward=False) + def narrow_phase_find_mesh_triangle_overlaps_kernel( + shape_types: wp.array[int], + shape_transform: wp.array[wp.transform], + shape_source: wp.array[wp.uint64], + shape_gap: wp.array[float], # Per-shape contact gaps + shape_data: wp.array[wp.vec4], # Shape data (scale xyz, margin w) + shape_collision_radius: wp.array[float], + shape_heightfield_index: wp.array[wp.int32], + heightfield_data: wp.array[HeightfieldData], + shape_pairs_mesh: wp.array[wp.vec2i], + shape_pairs_mesh_count: wp.array[int], + 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, + # outputs + triangle_pairs: wp.array[wp.vec3i], # (shape_a, shape_b, triangle_idx) + triangle_pairs_count: wp.array[int], + ): + """Find triangles that overlap with a convex shape for mesh and heightfield pairs. + + For mesh pairs, uses a tiled BVH query. For heightfield pairs, projects the + convex shape's bounding sphere onto the heightfield grid and emits triangle + pairs for each overlapping cell. + + Outputs triples of ``(mesh_or_hfield_shape, other_shape, triangle_idx)``. + """ + tid, j = wp.tid() + + num_mesh_pairs = shape_pairs_mesh_count[0] + + # Strided loop over mesh pairs + for i in range(tid, num_mesh_pairs, total_num_threads): + pair = shape_pairs_mesh[i] + shape_a = pair[0] + shape_b = pair[1] + + type_a = shape_types[shape_a] + type_b = shape_types[shape_b] + + # ----------------------------------------------------------------- + # Heightfield-vs-convex midphase (grid cell lookup) + # Pairs are normalized so the heightfield is always shape_a. + # ----------------------------------------------------------------- + if type_a == GeoType.HFIELD: + # Only run on j==0; the j dimension is for tiled BVH queries (mesh only). + if j != 0: + continue + hfd = heightfield_data[shape_heightfield_index[shape_a]] + heightfield_vs_convex_midphase( + shape_a, + shape_b, + hfd, + shape_transform, + shape_collision_radius, + shape_gap, + triangle_pairs, + triangle_pairs_count, + ) continue - hfd = heightfield_data[shape_heightfield_index[shape_a]] - heightfield_vs_convex_midphase( - shape_a, - shape_b, - hfd, - shape_transform, - shape_collision_radius, - shape_gap, + + # ----------------------------------------------------------------- + # Mesh-vs-convex midphase (BVH query) + # ----------------------------------------------------------------- + mesh_shape = -1 + non_mesh_shape = -1 + + if type_a == GeoType.MESH and type_b != GeoType.MESH: + mesh_shape = shape_a + non_mesh_shape = shape_b + elif type_b == GeoType.MESH and type_a != GeoType.MESH: + mesh_shape = shape_b + non_mesh_shape = shape_a + else: + # Mesh-mesh collision not supported in this path + continue + + # Get mesh BVH ID and mesh transform + mesh_id = shape_source[mesh_shape] + if mesh_id == wp.uint64(0): + continue + + # Get mesh world transform + X_mesh_ws = shape_transform[mesh_shape] + + # Get non-mesh shape world transform + X_ws = shape_transform[non_mesh_shape] + + # Use per-shape contact gaps for consistent pairwise thresholding. + gap_non_mesh = shape_gap[non_mesh_shape] + gap_mesh = shape_gap[mesh_shape] + gap_sum = gap_non_mesh + gap_mesh + + if wp.static(speculative): + vel_rel = shape_lin_vel[non_mesh_shape] - shape_lin_vel[mesh_shape] + rel_speed = ( + wp.length(vel_rel) + shape_ang_speed_bound[mesh_shape] + shape_ang_speed_bound[non_mesh_shape] + ) + gap_sum = gap_sum + wp.min(rel_speed * speculative_dt, max_speculative_extension) + + # Call mesh_vs_convex_midphase with the shape_data and pair gap sum. + mesh_vs_convex_midphase( + j, + mesh_shape, + non_mesh_shape, + X_mesh_ws, + X_ws, + mesh_id, + shape_types, + shape_data, + shape_source, + gap_sum, triangle_pairs, triangle_pairs_count, ) - continue - - # ----------------------------------------------------------------- - # Mesh-vs-convex midphase (BVH query) - # ----------------------------------------------------------------- - mesh_shape = -1 - non_mesh_shape = -1 - - if type_a == GeoType.MESH and type_b != GeoType.MESH: - mesh_shape = shape_a - non_mesh_shape = shape_b - elif type_b == GeoType.MESH and type_a != GeoType.MESH: - mesh_shape = shape_b - non_mesh_shape = shape_a - else: - # Mesh-mesh collision not supported in this path - continue - - # Get mesh BVH ID and mesh transform - mesh_id = shape_source[mesh_shape] - if mesh_id == wp.uint64(0): - continue - - # Get mesh world transform - X_mesh_ws = shape_transform[mesh_shape] - - # Get non-mesh shape world transform - X_ws = shape_transform[non_mesh_shape] - - # Use per-shape contact gaps for consistent pairwise thresholding. - gap_non_mesh = shape_gap[non_mesh_shape] - gap_mesh = shape_gap[mesh_shape] - gap_sum = gap_non_mesh + gap_mesh - - # Call mesh_vs_convex_midphase with the shape_data and pair gap sum. - mesh_vs_convex_midphase( - j, - mesh_shape, - non_mesh_shape, - X_mesh_ws, - X_ws, - mesh_id, - shape_types, - shape_data, - shape_source, - gap_sum, - triangle_pairs, - triangle_pairs_count, - ) + return narrow_phase_find_mesh_triangle_overlaps_kernel + + +_find_mesh_triangle_overlaps = _create_find_mesh_triangle_overlaps_kernel(speculative=False) +_find_mesh_triangle_overlaps_speculative = _create_find_mesh_triangle_overlaps_kernel(speculative=True) -def create_narrow_phase_process_mesh_triangle_contacts_kernel(writer_func: Any): - _module = f"narrow_phase_mesh_tri_{writer_func.__name__}" + +def create_narrow_phase_process_mesh_triangle_contacts_kernel(writer_func: Any, speculative: bool = False): + _module = f"narrow_phase_mesh_tri_{writer_func.__name__}_{speculative}" @wp.kernel(enable_backward=False, module=_module) def narrow_phase_process_mesh_triangle_contacts_kernel( @@ -903,6 +973,10 @@ def narrow_phase_process_mesh_triangle_contacts_kernel( triangle_pairs_count: wp.array[int], writer_data: Any, 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 triangle pairs to generate contacts using GJK/MPR. @@ -974,6 +1048,11 @@ def narrow_phase_process_mesh_triangle_contacts_kernel( 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 with standard post-processing wp.static(create_compute_gjk_mpr_contacts(writer_func))( shape_data_a, @@ -1070,6 +1149,7 @@ def compute_mesh_plane_block_offsets_scan( def create_narrow_phase_process_mesh_plane_contacts_kernel( writer_func: Any, reduce_contacts: bool = False, + speculative: bool = False, ): """ Create a mesh-plane collision kernel. @@ -1077,11 +1157,13 @@ def create_narrow_phase_process_mesh_plane_contacts_kernel( Args: writer_func: Contact writer function (e.g., write_contact_simple) reduce_contacts: If True, return multi-block load-balanced variant for global reduction. + speculative: When True, extends ``gap_sum`` by a scalar speculative + margin derived from per-shape velocity arrays. Returns: A warp kernel that processes mesh-plane collisions """ - _module = f"narrow_phase_mesh_plane_{writer_func.__name__}_{reduce_contacts}" + _module = f"narrow_phase_mesh_plane_{writer_func.__name__}_{reduce_contacts}_{speculative}" @wp.kernel(enable_backward=False, module=_module) def narrow_phase_process_mesh_plane_contacts_kernel( @@ -1096,6 +1178,10 @@ def narrow_phase_process_mesh_plane_contacts_kernel( shape_pairs_mesh_plane_count: wp.array[int], writer_data: Any, total_num_blocks: int, + shape_lin_vel: wp.array[wp.vec3], + shape_ang_speed_bound: wp.array[float], + speculative_dt: float, + max_speculative_extension: float, ): """ Process mesh-plane collisions without contact reduction. @@ -1144,6 +1230,11 @@ def narrow_phase_process_mesh_plane_contacts_kernel( gap_plane = shape_gap[plane_shape] gap_sum = gap_mesh + gap_plane + if wp.static(speculative): + vel_rel = shape_lin_vel[plane_shape] - shape_lin_vel[mesh_shape] + rel_speed = wp.length(vel_rel) + shape_ang_speed_bound[mesh_shape] + shape_ang_speed_bound[plane_shape] + gap_sum = gap_sum + wp.min(rel_speed * speculative_dt, max_speculative_extension) + # Strided loop over vertices across all threads in the launch total_num_threads = total_num_blocks * wp.block_dim() for vertex_idx in range(tid, num_vertices, total_num_threads): @@ -1203,6 +1294,10 @@ def narrow_phase_process_mesh_plane_contacts_reduce_kernel( block_offsets: wp.array[wp.int32], writer_data: Any, total_num_blocks: int, + shape_lin_vel: wp.array[wp.vec3], + shape_ang_speed_bound: wp.array[float], + speculative_dt: float, + max_speculative_extension: float, ): """Process mesh-plane collisions with dynamic load balancing. @@ -1276,6 +1371,11 @@ def narrow_phase_process_mesh_plane_contacts_reduce_kernel( gap_plane = shape_gap[plane_shape] gap_sum = gap_mesh + gap_plane + if wp.static(speculative): + vel_rel = shape_lin_vel[plane_shape] - shape_lin_vel[mesh_shape] + rel_speed = wp.length(vel_rel) + shape_ang_speed_bound[mesh_shape] + shape_ang_speed_bound[plane_shape] + gap_sum = gap_sum + wp.min(rel_speed * speculative_dt, max_speculative_extension) + # Process this block's chunk of vertices — write contacts directly # to the global reducer buffer (no per-block shared memory reduction). chunk_len = vert_end - vert_start @@ -1428,6 +1528,7 @@ def __init__( has_meshes: bool = True, has_heightfields: bool = False, use_lean_gjk_mpr: bool = False, + speculative: bool = False, deterministic: bool = False, contact_max: int | None = None, ) -> None: @@ -1453,6 +1554,10 @@ def __init__( Defaults to True for safety. Set to False when constructing from a model with no meshes. has_heightfields: Whether the scene contains any heightfield shapes (GeoType.HFIELD). When True, heightfield collision buffers and kernels are allocated. Defaults to False. + speculative: Enable speculative contact support in narrow-phase kernels. + When True, kernel variants that read per-shape velocity arrays and + extend gap thresholds are compiled. When False (default), the + speculative code paths are eliminated at compile time. deterministic: Sort contacts after the narrow phase so that results are independent of GPU thread scheduling. Adds a radix sort + gather pass. Hydroelastic contacts are not yet covered. @@ -1509,9 +1614,11 @@ def __init__( self.tile_size_mesh_plane = 512 self.block_dim = 128 + self.speculative = speculative + # Create the appropriate kernel variants # Primitive kernel handles lightweight primitives and routes remaining pairs - self.primitive_kernel = create_narrow_phase_primitive_kernel(writer_func) + self.primitive_kernel = create_narrow_phase_primitive_kernel(writer_func, speculative=speculative) # GJK/MPR kernel handles remaining convex-convex pairs if use_lean_gjk_mpr: # Use lean support function (CONVEX_MESH, BOX, SPHERE only) and lean post-processing @@ -1521,14 +1628,23 @@ def __init__( writer_func, support_func=support_map_lean, post_process_contact=post_process_minkowski_only, + speculative=speculative, ) else: - self.narrow_phase_kernel = create_narrow_phase_kernel_gjk_mpr(self.external_aabb, writer_func) + self.narrow_phase_kernel = create_narrow_phase_kernel_gjk_mpr( + self.external_aabb, writer_func, speculative=speculative + ) # Create triangle contacts kernel when meshes or heightfields are present if has_meshes or has_heightfields: - self.mesh_triangle_contacts_kernel = create_narrow_phase_process_mesh_triangle_contacts_kernel(writer_func) + self.mesh_triangle_contacts_kernel = create_narrow_phase_process_mesh_triangle_contacts_kernel( + writer_func, speculative=speculative + ) + self.mesh_triangle_overlaps_kernel = ( + _find_mesh_triangle_overlaps_speculative if speculative else _find_mesh_triangle_overlaps + ) else: self.mesh_triangle_contacts_kernel = None + self.mesh_triangle_overlaps_kernel = _find_mesh_triangle_overlaps # Create mesh-specific kernels only when has_meshes=True if has_meshes: @@ -1539,10 +1655,12 @@ def __init__( self.mesh_plane_contacts_kernel = create_narrow_phase_process_mesh_plane_contacts_kernel( write_contact_to_reducer, reduce_contacts=True, + speculative=speculative, ) else: self.mesh_plane_contacts_kernel = create_narrow_phase_process_mesh_plane_contacts_kernel( writer_func, + speculative=speculative, ) # Only create mesh-mesh SDF kernel on CUDA (uses __shared__ memory via func_native) if is_gpu_device: @@ -1551,11 +1669,13 @@ def __init__( write_contact_to_reducer, enable_heightfields=has_heightfields, reduce_contacts=True, + speculative=speculative, ) else: self.mesh_mesh_contacts_kernel = create_narrow_phase_process_mesh_mesh_contacts_kernel( writer_func, enable_heightfields=has_heightfields, + speculative=speculative, ) else: self.mesh_mesh_contacts_kernel = None @@ -1568,12 +1688,16 @@ def __init__( # Global contact reducer uses hardcoded BETA_THRESHOLD (0.1mm) same as shared-memory reduction # Slot layout: NUM_SPATIAL_DIRECTIONS spatial + 1 max-depth = VALUES_PER_KEY slots per key self.export_reduced_contacts_kernel = create_export_reduced_contacts_kernel(writer_func) + self.mesh_triangle_to_reducer_kernel = create_mesh_triangle_contacts_to_reducer_kernel( + speculative=speculative + ) # Global contact reducer for all mesh contact types self.global_contact_reducer = GlobalContactReducer( max_triangle_pairs, device=device, deterministic=deterministic ) else: self.export_reduced_contacts_kernel = None + self.mesh_triangle_to_reducer_kernel = None self.global_contact_reducer = None self.hydroelastic_sdf = hydroelastic_sdf @@ -1716,6 +1840,10 @@ def launch_custom_write( shape_edge_range: wp.array[wp.vec2i] | None = None, writer_data: Any, device: Devicelike | None = None, # Device to launch on + shape_lin_vel: wp.array[wp.vec3] | None = None, + shape_ang_speed_bound: wp.array[wp.float32] | None = None, + speculative_dt: float = 0.0, + max_speculative_extension: float = 0.0, ) -> None: """ Launch narrow phase collision detection with a custom contact writer struct. @@ -1749,6 +1877,12 @@ def launch_custom_write( # Clear all counters with a single kernel launch (consolidated counter array) self._counter_array.zero_() + # Resolve speculative velocity arrays (empty when disabled) + _empty_vec3 = wp.empty(0, dtype=wp.vec3, device=device) + _empty_float = wp.empty(0, dtype=wp.float32, device=device) + _slv = shape_lin_vel if shape_lin_vel is not None else _empty_vec3 + _sasb = shape_ang_speed_bound if shape_ang_speed_bound is not None else _empty_float + # Stage 1: Launch primitive kernel for fast analytical collisions # This handles sphere-sphere, sphere-capsule, capsule-capsule, plane-sphere, plane-capsule # and routes remaining pairs to gjk_candidate_pairs and mesh buffers @@ -1766,6 +1900,10 @@ def launch_custom_write( shape_flags, writer_data, self.total_num_threads, + _slv, + _sasb, + speculative_dt, + max_speculative_extension, ], outputs=[ self.gjk_candidate_pairs, @@ -1805,6 +1943,10 @@ def launch_custom_write( self.shape_aabb_upper, writer_data, self.total_num_threads, + _slv, + _sasb, + speculative_dt, + max_speculative_extension, ], device=device, block_dim=self.block_dim, @@ -1830,6 +1972,10 @@ def launch_custom_write( self.shape_pairs_mesh_plane_count, writer_data, self.num_tile_blocks, + _slv, + _sasb, + speculative_dt, + max_speculative_extension, ], device=device, block_dim=self.block_dim, @@ -1839,7 +1985,7 @@ def launch_custom_write( # Launch midphase: finds overlapping triangles for both mesh and heightfield pairs second_dim = self.tile_size_mesh_convex if ENABLE_TILE_BVH_QUERY else 1 wp.launch( - kernel=narrow_phase_find_mesh_triangle_overlaps_kernel, + kernel=self.mesh_triangle_overlaps_kernel, dim=[self.num_tile_blocks, second_dim], inputs=[ shape_types, @@ -1853,6 +1999,10 @@ def launch_custom_write( self.shape_pairs_mesh, self.shape_pairs_mesh_count, self.num_tile_blocks, + _slv, + _sasb, + speculative_dt, + max_speculative_extension, ], outputs=[ self.triangle_pairs, @@ -1899,6 +2049,10 @@ def launch_custom_write( self.mesh_plane_block_offsets, reducer_data, self.num_mesh_plane_blocks, + _slv, + _sasb, + speculative_dt, + max_speculative_extension, ], device=device, block_dim=self.tile_size_mesh_plane, @@ -1907,7 +2061,7 @@ def launch_custom_write( # Mesh/heightfield-triangle contacts → same global reducer wp.launch( - kernel=mesh_triangle_contacts_to_reducer_kernel, + kernel=self.mesh_triangle_to_reducer_kernel, dim=self.total_num_threads, inputs=[ shape_types, @@ -1922,6 +2076,10 @@ def launch_custom_write( self.triangle_pairs_count, reducer_data, self.total_num_threads, + _slv, + _sasb, + speculative_dt, + max_speculative_extension, ], device=device, block_dim=self.block_dim, @@ -1945,6 +2103,10 @@ def launch_custom_write( self.triangle_pairs_count, writer_data, self.total_num_threads, + _slv, + _sasb, + speculative_dt, + max_speculative_extension, ], device=device, block_dim=self.block_dim, @@ -2020,6 +2182,10 @@ def launch_custom_write( self.mesh_mesh_block_offsets, reducer_data, self.num_mesh_mesh_blocks, + _slv, + _sasb, + speculative_dt, + max_speculative_extension, ], device=device, block_dim=self.tile_size_mesh_mesh, @@ -2049,6 +2215,10 @@ def launch_custom_write( shape_edge_range, writer_data, self.num_tile_blocks, + _slv, + _sasb, + speculative_dt, + max_speculative_extension, ], device=device, block_dim=self.tile_size_mesh_mesh, diff --git a/newton/_src/geometry/sdf_contact.py b/newton/_src/geometry/sdf_contact.py index 20b3883004..43d618e49e 100644 --- a/newton/_src/geometry/sdf_contact.py +++ b/newton/_src/geometry/sdf_contact.py @@ -22,6 +22,27 @@ _get_shared_memory_sdf_cache = create_shared_memory_pointer_block_dim_mul_func(1) +@wp.func +def _query_mesh_face_normal( + mesh_id: wp.uint64, + point_local: wp.vec3, + max_dist: float = 1000.0, +) -> wp.vec3: + """Return the outward face normal of the closest triangle on a mesh. + + Falls back to the point-to-center direction when the query misses. + The returned vector is in mesh-local space and is normalized. + """ + face_index = int(0) + face_u = float(0.0) + face_v = float(0.0) + sign = float(0.0) + res = wp.mesh_query_point_sign_normal(mesh_id, point_local, max_dist, sign, face_index, face_u, face_v) + if res: + return wp.mesh_eval_face_normal(mesh_id, face_index) + return wp.vec3(0.0, 1.0, 0.0) + + @wp.func def scale_sdf_result_to_world( distance: float, @@ -999,6 +1020,7 @@ def create_narrow_phase_process_mesh_mesh_contacts_kernel( writer_func: Any, enable_heightfields: bool = True, reduce_contacts: bool = False, + speculative: bool = False, ): find_interesting_edges, do_edge_sdf_collision = _create_sdf_contact_funcs(enable_heightfields) @@ -1009,7 +1031,7 @@ def create_narrow_phase_process_mesh_mesh_contacts_kernel( # compiled code, otherwise FMA-fusion or register-allocation # differences between independent JIT compilations can produce subtly # different floating-point results, breaking bit-exact reproducibility. - _module = f"sdf_contact_{writer_func.__name__}_{enable_heightfields}_{reduce_contacts}" + _module = f"sdf_contact_{writer_func.__name__}_{enable_heightfields}_{reduce_contacts}_{speculative}" @wp.kernel(enable_backward=False, module=_module) def mesh_sdf_collision_kernel( @@ -1031,6 +1053,10 @@ def mesh_sdf_collision_kernel( shape_edge_range: wp.array[wp.vec2i], writer_data: Any, total_num_blocks: int, + shape_lin_vel: wp.array[wp.vec3], + shape_ang_speed_bound: wp.array[float], + speculative_dt: float, + max_speculative_extension: float, ): """Process mesh-mesh and mesh-heightfield collisions using SDF-based detection.""" block_id, t = wp.tid() @@ -1047,7 +1073,13 @@ def mesh_sdf_collision_kernel( has_hfield = False pair = pair_encoded - gap_sum = shape_gap[pair[0]] + shape_gap[pair[1]] + base_gap_sum = shape_gap[pair[0]] + shape_gap[pair[1]] + gap_sum = base_gap_sum + + if wp.static(speculative): + vel_rel = shape_lin_vel[pair[1]] - shape_lin_vel[pair[0]] + rel_speed = wp.length(vel_rel) + shape_ang_speed_bound[pair[0]] + shape_ang_speed_bound[pair[1]] + gap_sum = gap_sum + wp.min(rel_speed * speculative_dt, max_speculative_extension) for mode in range(2): tri_shape = pair[mode] @@ -1255,6 +1287,15 @@ def mesh_sdf_collision_kernel( else: direction_world = wp.vec3(0.0, 1.0, 0.0) + if wp.static(speculative): + base_threshold = base_gap_sum + triangle_mesh_margin + sdf_mesh_margin + if dist > base_threshold and not sdf_is_hfield: + face_n = _query_mesh_face_normal(mesh_id_sdf, point) + face_n_world = wp.transform_vector(X_sdf_ws, face_n) + fn_len = wp.length(face_n_world) + if fn_len > 0.0: + direction_world = face_n_world / fn_len + contact_normal = -direction_world if mode == 0 else direction_world contact_data = ContactData() @@ -1308,6 +1349,10 @@ def mesh_sdf_collision_global_reduce_kernel( block_offsets: wp.array[wp.int32], reducer_data: GlobalContactReducerData, total_num_blocks: int, + shape_lin_vel: wp.array[wp.vec3], + shape_ang_speed_bound: wp.array[float], + speculative_dt: float, + max_speculative_extension: float, ): """Process mesh-mesh collisions with global hashtable contact reduction. @@ -1344,7 +1389,13 @@ def mesh_sdf_collision_global_reduce_kernel( has_hfield = False pair = pair_encoded - gap_sum = shape_gap[pair[0]] + shape_gap[pair[1]] + base_gap_sum = shape_gap[pair[0]] + shape_gap[pair[1]] + gap_sum = base_gap_sum + + if wp.static(speculative): + vel_rel = shape_lin_vel[pair[1]] - shape_lin_vel[pair[0]] + rel_speed = wp.length(vel_rel) + shape_ang_speed_bound[pair[0]] + shape_ang_speed_bound[pair[1]] + gap_sum = gap_sum + wp.min(rel_speed * speculative_dt, max_speculative_extension) for mode in range(2): tri_shape = pair[mode] @@ -1556,6 +1607,15 @@ def mesh_sdf_collision_global_reduce_kernel( else: direction_world = wp.vec3(0.0, 1.0, 0.0) + if wp.static(speculative): + base_threshold = base_gap_sum + triangle_mesh_margin + sdf_mesh_margin + if dist > base_threshold and not sdf_is_hfield: + face_n = _query_mesh_face_normal(mesh_id_sdf, point) + face_n_world = wp.transform_vector(X_sdf_ws, face_n) + fn_len = wp.length(face_n_world) + if fn_len > 0.0: + direction_world = face_n_world / fn_len + contact_normal = -direction_world if mode == 0 else direction_world export_and_reduce_contact_centered( diff --git a/newton/_src/sim/__init__.py b/newton/_src/sim/__init__.py index bd68e0a42f..1bca490976 100644 --- a/newton/_src/sim/__init__.py +++ b/newton/_src/sim/__init__.py @@ -3,7 +3,7 @@ from .articulation import eval_fk, eval_ik, eval_jacobian, eval_mass_matrix from .builder import ModelBuilder -from .collide import CollisionPipeline +from .collide import CollisionPipeline, SpeculativeContactConfig from .contacts import Contacts from .control import Control from .enums import ( @@ -25,6 +25,7 @@ "JointType", "Model", "ModelBuilder", + "SpeculativeContactConfig", "State", "eval_fk", "eval_ik", diff --git a/newton/_src/sim/collide.py b/newton/_src/sim/collide.py index c1f779408e..c26a057223 100644 --- a/newton/_src/sim/collide.py +++ b/newton/_src/sim/collide.py @@ -3,6 +3,7 @@ from __future__ import annotations +import dataclasses from typing import Literal import numpy as np @@ -29,6 +30,34 @@ from ..sim.state import State +def _validate_speculative_scalar(value: float, name: str) -> None: + """Raise ``ValueError`` for negative or non-finite speculative parameters.""" + if not np.isfinite(value) or value < 0.0: + raise ValueError(f"{name} must be a non-negative finite number, got {value!r}") + + +@dataclasses.dataclass +class SpeculativeContactConfig: + """Configuration for speculative contact detection. + + When passed to :class:`CollisionPipeline`, AABBs and gap thresholds are + expanded based on per-shape velocity so that contacts which will occur + within the next collision update interval are detected early. + """ + + max_speculative_extension: float = 0.1 + """Maximum speculative gap extension [m]. Clamps ``vel * dt`` to + prevent excessively large AABBs.""" + + collision_update_dt: float = 1.0 / 60.0 + """Default time interval between collision updates [s]. Can be + overridden per-call via ``CollisionPipeline.collide(dt=...)``.""" + + def __post_init__(self): + _validate_speculative_scalar(self.collision_update_dt, "collision_update_dt") + _validate_speculative_scalar(self.max_speculative_extension, "max_speculative_extension") + + @wp.struct class ContactWriterData: """Contact writer data for collide write_contact function.""" @@ -56,6 +85,11 @@ class ContactWriterData: out_damping: wp.array[float] out_friction: wp.array[float] out_sort_key: wp.array[wp.int64] + # Speculative contact fields (empty arrays / 0.0 when disabled) + shape_lin_vel: wp.array[wp.vec3] + shape_ang_speed_bound: wp.array[float] + speculative_dt: float + max_speculative_extension: float @wp.func @@ -98,6 +132,23 @@ def write_contact( gap_b = writer_data.shape_gap[contact_data.shape_b] contact_gap = gap_a + gap_b + # Directed speculative gap extension: only extend for approaching pairs. + # contact_normal_a_to_b points from A toward B, so approaching motion + # gives dot(vel_b - vel_a, n) < 0. Negate to get a positive approach speed. + if writer_data.speculative_dt > 0.0: + vel_a = writer_data.shape_lin_vel[contact_data.shape_a] + vel_b = writer_data.shape_lin_vel[contact_data.shape_b] + v_approach = ( + -wp.dot(vel_b - vel_a, contact_normal_a_to_b) + + writer_data.shape_ang_speed_bound[contact_data.shape_a] + + writer_data.shape_ang_speed_bound[contact_data.shape_b] + ) + spec_gap = wp.min( + wp.max(v_approach * writer_data.speculative_dt, 0.0), + writer_data.max_speculative_extension, + ) + contact_gap = contact_gap + spec_gap + index = output_index if index < 0: @@ -146,116 +197,200 @@ def write_contact( ) +def _create_compute_shape_aabbs(speculative: bool): + """Factory for the AABB kernel. When *speculative* is True the kernel reads + per-shape velocity arrays and applies directed linear + isotropic angular + expansion. When False the extra code is eliminated at compile time via + ``wp.static``.""" + + @wp.kernel(enable_backward=False) + def compute_shape_aabbs( + body_q: wp.array[wp.transform], + shape_transform: wp.array[wp.transform], + shape_body: wp.array[int], + shape_type: wp.array[int], + shape_scale: wp.array[wp.vec3], + shape_collision_radius: wp.array[float], + shape_source_ptr: wp.array[wp.uint64], + shape_margin: wp.array[float], + shape_gap: wp.array[float], + shape_collision_aabb_lower: wp.array[wp.vec3], + shape_collision_aabb_upper: wp.array[wp.vec3], + shape_lin_vel: wp.array[wp.vec3], + shape_ang_speed_bound: wp.array[float], + speculative_dt: float, + max_speculative_extension: float, + # outputs + aabb_lower: wp.array[wp.vec3], + aabb_upper: wp.array[wp.vec3], + geom_data: wp.array[wp.vec4], + geom_xform: wp.array[wp.transform], + ): + """Compute axis-aligned bounding boxes for each shape in world space. + + Uses support function for most shapes. Meshes and heightfields use the + pre-computed local AABB transformed to world frame. Infinite planes use + bounding sphere fallback. AABBs are enlarged by per-shape effective gap + for contact detection (``shape_margin + shape_gap``), and optionally by + speculative velocity expansion when enabled. + """ + shape_id = wp.tid() + + rigid_id = shape_body[shape_id] + geo_type = shape_type[shape_id] + + # Compute world transform + if rigid_id == -1: + X_ws = shape_transform[shape_id] + else: + X_ws = wp.transform_multiply(body_q[rigid_id], shape_transform[shape_id]) + + pos = wp.transform_get_translation(X_ws) + orientation = wp.transform_get_rotation(X_ws) + + # Enlarge AABB by per-shape effective gap for contact detection + margin = shape_margin[shape_id] + effective_gap = margin + shape_gap[shape_id] + margin_vec = wp.vec3(effective_gap, effective_gap, effective_gap) + + # Check if this is an infinite plane, mesh, or heightfield + scale = shape_scale[shape_id] + is_infinite_plane = (geo_type == GeoType.PLANE) and (scale[0] == 0.0 and scale[1] == 0.0) + is_mesh = geo_type == GeoType.MESH + is_hfield = geo_type == GeoType.HFIELD + + if is_infinite_plane: + # Bounding sphere fallback for infinite planes + radius = shape_collision_radius[shape_id] + half_extents = wp.vec3(radius, radius, radius) + aabb_lower[shape_id] = pos - half_extents - margin_vec + aabb_upper[shape_id] = pos + half_extents + margin_vec + elif is_mesh or is_hfield: + # Tight local AABB transformed to world space. + # Scale is already baked into shape_collision_aabb by the builder, + # so we only need to handle the rotation here. + local_lo = shape_collision_aabb_lower[shape_id] + local_hi = shape_collision_aabb_upper[shape_id] + + center = (local_lo + local_hi) * 0.5 + half = (local_hi - local_lo) * 0.5 + + # Rotate center to world frame + world_center = wp.quat_rotate(orientation, center) + pos + + # Rotated AABB half-extents via abs of rotation matrix columns + r0 = wp.quat_rotate(orientation, wp.vec3(1.0, 0.0, 0.0)) + r1 = wp.quat_rotate(orientation, wp.vec3(0.0, 1.0, 0.0)) + r2 = wp.quat_rotate(orientation, wp.vec3(0.0, 0.0, 1.0)) + + world_half = wp.vec3( + wp.abs(r0[0]) * half[0] + wp.abs(r1[0]) * half[1] + wp.abs(r2[0]) * half[2], + wp.abs(r0[1]) * half[0] + wp.abs(r1[1]) * half[1] + wp.abs(r2[1]) * half[2], + wp.abs(r0[2]) * half[0] + wp.abs(r1[2]) * half[1] + wp.abs(r2[2]) * half[2], + ) + + lo = world_center - world_half - margin_vec + hi = world_center + world_half + margin_vec + + if wp.static(speculative): + v = shape_lin_vel[shape_id] + vel_dt = v * speculative_dt + ang_ext = shape_ang_speed_bound[shape_id] * speculative_dt + ext_neg = wp.max(-vel_dt, wp.vec3(0.0, 0.0, 0.0)) + ext_pos = wp.max(vel_dt, wp.vec3(0.0, 0.0, 0.0)) + ang_vec = wp.vec3(ang_ext, ang_ext, ang_ext) + cap = wp.vec3(max_speculative_extension, max_speculative_extension, max_speculative_extension) + lo = lo - wp.min(ext_neg + ang_vec, cap) + hi = hi + wp.min(ext_pos + ang_vec, cap) + + aabb_lower[shape_id] = lo + aabb_upper[shape_id] = hi + else: + # Use support function to compute tight AABB + shape_data = GenericShapeData() + shape_data.shape_type = geo_type + shape_data.scale = scale + shape_data.auxiliary = wp.vec3(0.0, 0.0, 0.0) + + if geo_type == GeoType.CONVEX_MESH: + shape_data.auxiliary = pack_mesh_ptr(shape_source_ptr[shape_id]) + + data_provider = SupportMapDataProvider() + + aabb_min_world, aabb_max_world = compute_tight_aabb_from_support( + shape_data, orientation, pos, data_provider + ) + + lo = aabb_min_world - margin_vec + hi = aabb_max_world + margin_vec + + if wp.static(speculative): + v = shape_lin_vel[shape_id] + vel_dt = v * speculative_dt + ang_ext = shape_ang_speed_bound[shape_id] * speculative_dt + ext_neg = wp.max(-vel_dt, wp.vec3(0.0, 0.0, 0.0)) + ext_pos = wp.max(vel_dt, wp.vec3(0.0, 0.0, 0.0)) + ang_vec = wp.vec3(ang_ext, ang_ext, ang_ext) + cap = wp.vec3(max_speculative_extension, max_speculative_extension, max_speculative_extension) + lo = lo - wp.min(ext_neg + ang_vec, cap) + hi = hi + wp.min(ext_pos + ang_vec, cap) + + aabb_lower[shape_id] = lo + aabb_upper[shape_id] = hi + + # Narrow-phase geometry data (reuses X_ws and scale already computed above) + geom_data[shape_id] = wp.vec4(scale[0], scale[1], scale[2], margin) + geom_xform[shape_id] = X_ws + + return compute_shape_aabbs + + +_compute_shape_aabbs = _create_compute_shape_aabbs(speculative=False) +_compute_shape_aabbs_speculative = _create_compute_shape_aabbs(speculative=True) + + @wp.kernel(enable_backward=False) -def compute_shape_aabbs( - body_q: wp.array[wp.transform], - shape_transform: wp.array[wp.transform], +def compute_shape_vel( + body_qd: wp.array[wp.spatial_vector], shape_body: wp.array[int], - shape_type: wp.array[int], - shape_scale: wp.array[wp.vec3], - shape_collision_radius: wp.array[float], - shape_source_ptr: wp.array[wp.uint64], - shape_margin: wp.array[float], - shape_gap: wp.array[float], + shape_transform: wp.array[wp.transform], shape_collision_aabb_lower: wp.array[wp.vec3], shape_collision_aabb_upper: wp.array[wp.vec3], # outputs - aabb_lower: wp.array[wp.vec3], - aabb_upper: wp.array[wp.vec3], - geom_data: wp.array[wp.vec4], - geom_xform: wp.array[wp.transform], + shape_lin_vel: wp.array[wp.vec3], + shape_ang_speed_bound: wp.array[float], ): - """Compute AABBs and narrow-phase geometry data for each shape. - - Fuses AABB computation with narrow-phase data preparation so the - world transform (``body_q * shape_transform``) is computed once. + """Compute per-shape linear velocity and angular speed bound for speculative contacts. - Uses support function for most shapes. Meshes and heightfields use the - pre-computed local AABB transformed to world frame. Infinite planes use - bounding sphere fallback. AABBs are enlarged by per-shape effective gap - for contact detection. Effective expansion is ``shape_margin + shape_gap``. + Linear velocity is the body COM velocity. The angular speed bound is + ``|omega| * r_max`` where ``r_max`` is the distance from the body COM to the + farthest point of the shape, estimated as + ``|shape_offset| + local_aabb_half_diagonal``. """ shape_id = wp.tid() - rigid_id = shape_body[shape_id] - geo_type = shape_type[shape_id] - # Compute world transform if rigid_id == -1: - X_ws = shape_transform[shape_id] - else: - X_ws = wp.transform_multiply(body_q[rigid_id], shape_transform[shape_id]) - - pos = wp.transform_get_translation(X_ws) - orientation = wp.transform_get_rotation(X_ws) - - margin = shape_margin[shape_id] - - # Enlarge AABB by per-shape effective gap for contact detection - effective_gap = margin + shape_gap[shape_id] - margin_vec = wp.vec3(effective_gap, effective_gap, effective_gap) - - # Check if this is an infinite plane, mesh, or heightfield - scale = shape_scale[shape_id] - is_infinite_plane = (geo_type == GeoType.PLANE) and (scale[0] == 0.0 and scale[1] == 0.0) - is_mesh = geo_type == GeoType.MESH - is_hfield = geo_type == GeoType.HFIELD - - if is_infinite_plane: - # Bounding sphere fallback for infinite planes - radius = shape_collision_radius[shape_id] - half_extents = wp.vec3(radius, radius, radius) - aabb_lower[shape_id] = pos - half_extents - margin_vec - aabb_upper[shape_id] = pos + half_extents + margin_vec - elif is_mesh or is_hfield: - # Tight local AABB transformed to world space. - # Scale is already baked into shape_collision_aabb by the builder, - # so we only need to handle the rotation here. - local_lo = shape_collision_aabb_lower[shape_id] - local_hi = shape_collision_aabb_upper[shape_id] - - center = (local_lo + local_hi) * 0.5 - half = (local_hi - local_lo) * 0.5 + shape_lin_vel[shape_id] = wp.vec3(0.0, 0.0, 0.0) + shape_ang_speed_bound[shape_id] = 0.0 + return - # Rotate center to world frame - world_center = wp.quat_rotate(orientation, center) + pos + qd = body_qd[rigid_id] + v_lin = wp.vec3(qd[0], qd[1], qd[2]) + omega = wp.vec3(qd[3], qd[4], qd[5]) - # Rotated AABB half-extents via abs of rotation matrix columns - r0 = wp.quat_rotate(orientation, wp.vec3(1.0, 0.0, 0.0)) - r1 = wp.quat_rotate(orientation, wp.vec3(0.0, 1.0, 0.0)) - r2 = wp.quat_rotate(orientation, wp.vec3(0.0, 0.0, 1.0)) + shape_lin_vel[shape_id] = v_lin - world_half = wp.vec3( - wp.abs(r0[0]) * half[0] + wp.abs(r1[0]) * half[1] + wp.abs(r2[0]) * half[2], - wp.abs(r0[1]) * half[0] + wp.abs(r1[1]) * half[1] + wp.abs(r2[1]) * half[2], - wp.abs(r0[2]) * half[0] + wp.abs(r1[2]) * half[1] + wp.abs(r2[2]) * half[2], - ) - - aabb_lower[shape_id] = world_center - world_half - margin_vec - aabb_upper[shape_id] = world_center + world_half + margin_vec + omega_mag = wp.length(omega) + if omega_mag > 0.0: + shape_offset = wp.transform_get_translation(shape_transform[shape_id]) + local_lo = shape_collision_aabb_lower[shape_id] + local_hi = shape_collision_aabb_upper[shape_id] + half_diag = wp.length((local_hi - local_lo) * 0.5) + r_max = wp.length(shape_offset) + half_diag + shape_ang_speed_bound[shape_id] = omega_mag * r_max else: - # Use support function to compute tight AABB - # Create generic shape data - shape_data = GenericShapeData() - shape_data.shape_type = geo_type - shape_data.scale = scale - shape_data.auxiliary = wp.vec3(0.0, 0.0, 0.0) - - # For CONVEX_MESH, pack the mesh pointer - if geo_type == GeoType.CONVEX_MESH: - shape_data.auxiliary = pack_mesh_ptr(shape_source_ptr[shape_id]) - - data_provider = SupportMapDataProvider() - - # Compute tight AABB using helper function - aabb_min_world, aabb_max_world = compute_tight_aabb_from_support(shape_data, orientation, pos, data_provider) - - aabb_lower[shape_id] = aabb_min_world - margin_vec - aabb_upper[shape_id] = aabb_max_world + margin_vec - - # Narrow-phase geometry data (reuses X_ws and scale already computed above) - geom_data[shape_id] = wp.vec4(scale[0], scale[1], scale[2], margin) - geom_xform[shape_id] = X_ws + shape_ang_speed_bound[shape_id] = 0.0 def _estimate_rigid_contact_max(model: Model) -> int: @@ -460,6 +595,7 @@ def __init__( | None = None, narrow_phase: NarrowPhase | None = None, sdf_hydroelastic_config: HydroelasticSDF.Config | None = None, + speculative_config: SpeculativeContactConfig | None = None, deterministic: bool = False, ): """ @@ -492,6 +628,12 @@ def __init__( "nxn"/"sap" modes, ignored. sdf_hydroelastic_config: Configuration for hydroelastic collision handling. Defaults to None. + speculative_config: Configuration for speculative contact detection. + When provided, AABBs and gap thresholds are expanded based on + per-shape velocity (linear + angular) so that contacts expected + within the next collision update interval are detected early. + ``None`` (default) disables speculative contacts with zero + runtime overhead via compile-time elimination. deterministic: Sort contacts after the narrow phase so that results are independent of GPU thread scheduling. Adds a radix sort + gather pass. Hydroelastic contacts are not yet covered. @@ -513,6 +655,9 @@ def __init__( particle_count = model.particle_count device = model.device + self.speculative_config = speculative_config + self._speculative_enabled = speculative_config is not None + # Resolve rigid contact capacity with explicit > model > estimated precedence. if rigid_contact_max is None: model_rigid_contact_max = int(getattr(model, "rigid_contact_max", 0) or 0) @@ -669,6 +814,7 @@ def __init__( has_meshes=has_meshes, has_heightfields=has_heightfields, use_lean_gjk_mpr=use_lean_gjk_mpr, + speculative=self._speculative_enabled, deterministic=deterministic, ) self.hydroelastic_sdf = self.narrow_phase.hydroelastic_sdf @@ -713,6 +859,14 @@ def __init__( self._sort_key_array = wp.zeros(0, dtype=wp.int64, device=device) self._contact_sorter = None + if self._speculative_enabled: + with wp.ScopedDevice(device): + self._shape_lin_vel = wp.zeros(shape_count, dtype=wp.vec3, device=device) + self._shape_ang_speed_bound = wp.zeros(shape_count, dtype=wp.float32, device=device) + else: + self._shape_lin_vel = wp.empty(0, dtype=wp.vec3, device=device) + self._shape_ang_speed_bound = wp.empty(0, dtype=wp.float32, device=device) + @property def rigid_contact_max(self) -> int: """Maximum rigid contact buffer capacity used by this pipeline.""" @@ -771,6 +925,7 @@ def collide( contacts: Contacts, *, soft_contact_margin: float | None = None, + dt: float | None = None, ): """Run the collision pipeline using NarrowPhase. @@ -795,6 +950,9 @@ def collide( contacts: The contacts buffer to populate (will be cleared first). soft_contact_margin: Margin for soft contact generation. If ``None``, uses the value from construction. + dt: Override for the speculative collision update interval [s]. + When ``None``, uses ``speculative_config.collision_update_dt``. + Ignored when speculative contacts are disabled. """ contacts.clear() @@ -807,15 +965,47 @@ def collide( # update any additional parameters soft_contact_margin = soft_contact_margin if soft_contact_margin is not None else self.soft_contact_margin + # Resolve speculative dt and max extension + if self._speculative_enabled: + cfg = self.speculative_config + speculative_dt = dt if dt is not None else cfg.collision_update_dt + if dt is not None: + _validate_speculative_scalar(dt, "dt") + max_speculative_extension = cfg.max_speculative_extension + else: + speculative_dt = 0.0 + max_speculative_extension = 0.0 + # Rigid contact detection -- broad phase + narrow phase. # These kernels hardcode record_tape=False internally so they are # never captured on an active wp.Tape. The differentiable # augmentation and soft-contact kernels that follow are tape-safe # and recorded normally. + # Compute per-shape velocity for speculative AABB expansion + if self._speculative_enabled: + wp.launch( + kernel=compute_shape_vel, + dim=model.shape_count, + inputs=[ + state.body_qd, + model.shape_body, + model.shape_transform, + model.shape_collision_aabb_lower, + model.shape_collision_aabb_upper, + ], + outputs=[ + self._shape_lin_vel, + self._shape_ang_speed_bound, + ], + device=self.device, + record_tape=False, + ) + # Compute AABBs for all shapes (already expanded by per-shape effective gaps) + aabb_kernel = _compute_shape_aabbs_speculative if self._speculative_enabled else _compute_shape_aabbs wp.launch( - kernel=compute_shape_aabbs, + kernel=aabb_kernel, dim=model.shape_count, inputs=[ state.body_q, @@ -829,6 +1019,10 @@ def collide( model.shape_gap, model.shape_collision_aabb_lower, model.shape_collision_aabb_upper, + self._shape_lin_vel, + self._shape_ang_speed_bound, + speculative_dt, + max_speculative_extension, ], outputs=[ self.narrow_phase.shape_aabb_lower, @@ -912,6 +1106,11 @@ def collide( ) writer_data.out_sort_key = self._sort_key_array + writer_data.shape_lin_vel = self._shape_lin_vel + writer_data.shape_ang_speed_bound = self._shape_ang_speed_bound + writer_data.speculative_dt = speculative_dt + writer_data.max_speculative_extension = max_speculative_extension + # Run narrow phase with custom contact writer (writes directly to Contacts format) self.narrow_phase.launch_custom_write( candidate_pair=self.broad_phase_shape_pairs, @@ -935,6 +1134,10 @@ def collide( shape_edge_range=model.shape_edge_range, writer_data=writer_data, device=self.device, + shape_lin_vel=self._shape_lin_vel if self._speculative_enabled else None, + shape_ang_speed_bound=self._shape_ang_speed_bound if self._speculative_enabled else None, + speculative_dt=speculative_dt, + max_speculative_extension=max_speculative_extension, ) if self.deterministic and self._contact_sorter is not None: diff --git a/newton/_src/sim/model.py b/newton/_src/sim/model.py index 3cb783e8a7..ee59a300fc 100644 --- a/newton/_src/sim/model.py +++ b/newton/_src/sim/model.py @@ -980,6 +980,7 @@ def collide( contacts: Contacts | None = None, *, collision_pipeline: CollisionPipeline | None = None, + dt: float | None = None, ) -> Contacts: """ Generate contact points for the particles and rigid bodies in the model using the default collision @@ -990,6 +991,9 @@ def collide( contacts: The contacts buffer to populate (will be cleared first). If None, a new contacts buffer is allocated via :meth:`contacts`. collision_pipeline: Optional collision pipeline override. + dt: Override for the speculative collision update interval [s]. + Forwarded to :meth:`CollisionPipeline.collide`. Ignored when + speculative contacts are disabled. """ if collision_pipeline is not None: self._collision_pipeline = collision_pipeline @@ -999,7 +1003,7 @@ def collide( if contacts is None: contacts = self._collision_pipeline.contacts() - self._collision_pipeline.collide(state, contacts) + self._collision_pipeline.collide(state, contacts, dt=dt) return contacts def request_state_attributes(self, *attributes: str) -> None: diff --git a/newton/tests/test_speculative_contacts.py b/newton/tests/test_speculative_contacts.py new file mode 100644 index 0000000000..80bbb98cd0 --- /dev/null +++ b/newton/tests/test_speculative_contacts.py @@ -0,0 +1,534 @@ +# SPDX-FileCopyrightText: Copyright (c) 2025 The Newton Developers +# SPDX-License-Identifier: Apache-2.0 + +"""Tests for speculative contact support in the collision pipeline.""" + +import unittest + +import numpy as np +import warp as wp + +import newton +from newton.tests.unittest_utils import add_function_test, get_cuda_test_devices + + +def _build_two_sphere_model( + device, + gap: float = 3.0, + velocity: float = 0.0, + speculative_config: newton.SpeculativeContactConfig | None = None, +): + """Build a simple scene with two spheres separated along the X axis. + + Body A is at ``(-gap/2, 0, 0)`` moving with ``+velocity`` in X. + Body B is at ``(+gap/2, 0, 0)`` and is static (body index -1 via ground plane trick + is avoided; instead it's a dynamic body with zero velocity). + + Returns: + model, state, pipeline, contacts + """ + builder = newton.ModelBuilder(gravity=0.0) + builder.rigid_gap = 0.0 + + body_a = builder.add_body(xform=wp.transform(wp.vec3(-gap / 2.0, 0.0, 0.0))) + builder.add_shape_sphere(body_a, radius=0.5) + builder.body_qd[-1] = (velocity, 0.0, 0.0, 0.0, 0.0, 0.0) + + body_b = builder.add_body(xform=wp.transform(wp.vec3(gap / 2.0, 0.0, 0.0))) + builder.add_shape_sphere(body_b, radius=0.5) + + model = builder.finalize(device=device) + state = model.state() + + pipeline = newton.CollisionPipeline( + model, + broad_phase="nxn", + speculative_config=speculative_config, + ) + contacts = pipeline.contacts() + return model, state, pipeline, contacts + + +def test_speculative_disabled_no_contacts(test, device): + """Two spheres far apart with no speculative config produce zero contacts.""" + _, state, pipeline, contacts = _build_two_sphere_model(device, gap=3.0, velocity=100.0) + pipeline.collide(state, contacts) + count = contacts.rigid_contact_count.numpy()[0] + test.assertEqual(count, 0, "Non-speculative pipeline should not detect contacts for separated spheres") + + +def test_speculative_enabled_catches_fast_object(test, device): + """Two spheres far apart but approaching fast produce speculative contacts.""" + config = newton.SpeculativeContactConfig( + max_speculative_extension=10.0, + collision_update_dt=1.0 / 60.0, + ) + _, state, pipeline, contacts = _build_two_sphere_model( + device, + gap=3.0, + velocity=200.0, + speculative_config=config, + ) + pipeline.collide(state, contacts) + count = contacts.rigid_contact_count.numpy()[0] + test.assertGreater(count, 0, "Speculative pipeline should detect contacts for fast approaching spheres") + + +def test_speculative_diverging_no_contacts(test, device): + """Two spheres moving apart should not produce speculative contacts. + + Body A moves in -X (away from B). The directed gap extension should be + zero because the approach velocity is negative. + """ + config = newton.SpeculativeContactConfig( + max_speculative_extension=10.0, + collision_update_dt=1.0 / 60.0, + ) + _, state, pipeline, contacts = _build_two_sphere_model( + device, + gap=3.0, + velocity=-200.0, + speculative_config=config, + ) + pipeline.collide(state, contacts) + count = contacts.rigid_contact_count.numpy()[0] + test.assertEqual(count, 0, "Speculative pipeline should not detect contacts for diverging spheres") + + +def test_speculative_max_extension_clamp(test, device): + """Max speculative extension clamps the gap so very far objects are not detected.""" + config = newton.SpeculativeContactConfig( + max_speculative_extension=0.01, + collision_update_dt=1.0 / 60.0, + ) + _, state, pipeline, contacts = _build_two_sphere_model( + device, + gap=3.0, + velocity=200.0, + speculative_config=config, + ) + pipeline.collide(state, contacts) + count = contacts.rigid_contact_count.numpy()[0] + test.assertEqual(count, 0, "Max extension clamp should prevent detection of far-away objects") + + +def test_speculative_dt_override(test, device): + """Passing dt to collide() overrides the config default.""" + config = newton.SpeculativeContactConfig( + max_speculative_extension=10.0, + collision_update_dt=1e-6, + ) + _, state, pipeline, contacts = _build_two_sphere_model( + device, + gap=3.0, + velocity=200.0, + speculative_config=config, + ) + # With the tiny default dt, no contacts + pipeline.collide(state, contacts) + count_tiny = contacts.rigid_contact_count.numpy()[0] + test.assertEqual(count_tiny, 0, "Tiny default dt should not produce contacts") + + # Override with a large dt + contacts.clear() + pipeline.collide(state, contacts, dt=1.0 / 60.0) + count_override = contacts.rigid_contact_count.numpy()[0] + test.assertGreater(count_override, 0, "Overridden dt should produce contacts") + + +def test_speculative_angular_velocity(test, device): + """Angular velocity contributes to speculative extension. + + A spinning body should produce speculative contacts even with zero linear + velocity if the angular speed bound is large enough. + """ + builder = newton.ModelBuilder(gravity=0.0) + builder.rigid_gap = 0.0 + + body_a = builder.add_body(xform=wp.transform(wp.vec3(-0.75, 0.0, 0.0))) + builder.add_shape_box(body_a, hx=0.5, hy=0.5, hz=0.5) + builder.body_qd[-1] = (0.0, 0.0, 0.0, 0.0, 0.0, 200.0) + + body_b = builder.add_body(xform=wp.transform(wp.vec3(0.75, 0.0, 0.0))) + builder.add_shape_box(body_b, hx=0.5, hy=0.5, hz=0.5) + + model = builder.finalize(device=device) + state = model.state() + + config = newton.SpeculativeContactConfig( + max_speculative_extension=10.0, + collision_update_dt=1.0 / 60.0, + ) + pipeline = newton.CollisionPipeline(model, broad_phase="nxn", speculative_config=config) + contacts = pipeline.contacts() + pipeline.collide(state, contacts) + count = contacts.rigid_contact_count.numpy()[0] + test.assertGreater(count, 0, "Angular velocity should contribute to speculative extension") + + +def _run_sphere_vs_thin_box_sim(device, speculative_config, num_frames=5): + """Simulate a fast sphere aimed at a thin static box. + + The sphere (radius 0.25) starts at x = -2 moving at +50 m/s. + The thin box (half-thickness 0.02 in X) is centred at the origin. + With dt = 1/60 the sphere travels ~0.83 m per frame -- much more than + the box thickness (0.04 m), so without speculative contacts the sphere + tunnels straight through. + + Returns the final X position of the sphere body. + """ + builder = newton.ModelBuilder(gravity=0.0) + builder.rigid_gap = 0.0 + + sphere_body = builder.add_body(xform=wp.transform(wp.vec3(-2.0, 0.0, 0.0))) + builder.add_shape_sphere(sphere_body, radius=0.25) + builder.body_qd[-1] = (50.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + builder.add_shape_box( + body=-1, + xform=wp.transform_identity(), + hx=0.02, + hy=1.0, + hz=1.0, + ) + + model = builder.finalize(device=device) + + pipeline = newton.CollisionPipeline( + model, + broad_phase="nxn", + speculative_config=speculative_config, + ) + contacts = pipeline.contacts() + solver = newton.solvers.SolverXPBD(model) + + state_0 = model.state() + state_1 = model.state() + control = model.control() + frame_dt = 1.0 / 60.0 + + for _ in range(num_frames): + pipeline.collide(state_0, contacts, dt=frame_dt) + state_0.clear_forces() + solver.step(state_0, state_1, control, contacts, frame_dt) + state_0, state_1 = state_1, state_0 + + body_q = state_0.body_q.numpy() + sphere_x = float(body_q[0][0]) + return sphere_x + + +def test_speculative_tunneling_without(test, device): + """Without speculative contacts the sphere tunnels through the thin box.""" + final_x = _run_sphere_vs_thin_box_sim(device, speculative_config=None) + test.assertGreater( + final_x, + 0.5, + f"Sphere should tunnel through the thin box (final x={final_x:.3f})", + ) + + +def test_speculative_tunneling_with(test, device): + """With speculative contacts the sphere is stopped by the thin box.""" + config = newton.SpeculativeContactConfig( + max_speculative_extension=2.0, + collision_update_dt=1.0 / 60.0, + ) + final_x = _run_sphere_vs_thin_box_sim(device, speculative_config=config) + test.assertLess( + final_x, + -0.2, + f"Sphere should be stopped by the thin box (final x={final_x:.3f})", + ) + + +def _make_thin_wall_mesh(hx=0.02, hy=1.0, hz=1.0): + """Return a :class:`newton.Mesh` representing a thin box (wall). + + The wall is centred at the origin with half-extents ``(hx, hy, hz)``. + Winding is CCW when viewed from the +X side so that the face normals + point outward. + """ + verts = np.array( + [ + [-hx, -hy, -hz], + [hx, -hy, -hz], + [hx, hy, -hz], + [-hx, hy, -hz], + [-hx, -hy, hz], + [hx, -hy, hz], + [hx, hy, hz], + [-hx, hy, hz], + ], + dtype=np.float32, + ) + tris = np.array( + [ + # -X face + 0, + 3, + 7, + 0, + 7, + 4, + # +X face + 1, + 5, + 6, + 1, + 6, + 2, + # -Y face + 0, + 4, + 5, + 0, + 5, + 1, + # +Y face + 3, + 2, + 6, + 3, + 6, + 7, + # -Z face + 0, + 1, + 2, + 0, + 2, + 3, + # +Z face + 4, + 7, + 6, + 4, + 6, + 5, + ], + dtype=np.int32, + ) + return newton.Mesh(verts, tris, compute_inertia=False) + + +# -- Sphere vs mesh wall (mesh-triangle path) -------------------------------- + + +def _run_sphere_vs_mesh_wall_sim(device, speculative_config, num_frames=5): + """Simulate a fast sphere aimed at a thin *mesh* wall. + + Same geometry as ``_run_sphere_vs_thin_box_sim`` but the wall is a + triangle mesh instead of a primitive box, exercising the mesh-triangle + contact path. + """ + builder = newton.ModelBuilder(gravity=0.0) + builder.rigid_gap = 0.0 + + sphere_body = builder.add_body(xform=wp.transform(wp.vec3(-2.0, 0.0, 0.0))) + builder.add_shape_sphere(sphere_body, radius=0.25) + builder.body_qd[-1] = (50.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + wall_mesh = _make_thin_wall_mesh() + builder.add_shape_mesh(body=-1, mesh=wall_mesh, xform=wp.transform_identity()) + + model = builder.finalize(device=device) + + pipeline = newton.CollisionPipeline( + model, + broad_phase="nxn", + speculative_config=speculative_config, + ) + contacts = pipeline.contacts() + solver = newton.solvers.SolverXPBD(model) + + state_0 = model.state() + state_1 = model.state() + control = model.control() + frame_dt = 1.0 / 60.0 + + for _ in range(num_frames): + pipeline.collide(state_0, contacts, dt=frame_dt) + state_0.clear_forces() + solver.step(state_0, state_1, control, contacts, frame_dt) + state_0, state_1 = state_1, state_0 + + body_q = state_0.body_q.numpy() + return float(body_q[0][0]) + + +def test_speculative_tunneling_mesh_wall_without(test, device): + """Without speculative contacts the sphere tunnels through the mesh wall.""" + final_x = _run_sphere_vs_mesh_wall_sim(device, speculative_config=None) + test.assertGreater( + final_x, + 0.5, + f"Sphere should tunnel through the mesh wall (final x={final_x:.3f})", + ) + + +def test_speculative_tunneling_mesh_wall_with(test, device): + """With speculative contacts the sphere is stopped by the mesh wall.""" + config = newton.SpeculativeContactConfig( + max_speculative_extension=2.0, + collision_update_dt=1.0 / 60.0, + ) + final_x = _run_sphere_vs_mesh_wall_sim(device, speculative_config=config) + test.assertLess( + final_x, + -0.2, + f"Sphere should be stopped by the mesh wall (final x={final_x:.3f})", + ) + + +# -- Mesh box vs mesh wall (mesh-mesh SDF path) ------------------------------ + + +def _run_mesh_box_vs_mesh_wall_sim(device, speculative_config, num_frames=10): + """Simulate a fast mesh box aimed at a thin mesh wall. + + Both the projectile and the wall are triangle meshes with SDFs, + exercising the mesh-mesh SDF contact path. Same geometry as the + sphere-vs-primitive-box test (wall half-thickness 0.02 m). + + Uses ``max_resolution=256`` so the SDF has enough voxels across the + 0.04 m wall, and 8 XPBD iterations because mesh-mesh SDF contacts + have zero effective radii (unlike sphere contacts) and need more + solver work to fully resolve the large speculative gap. + """ + builder = newton.ModelBuilder(gravity=0.0) + builder.rigid_gap = 0.0 + + box_mesh = newton.Mesh.create_box(0.25, compute_normals=False, compute_uvs=False) + box_mesh.build_sdf(device=device, max_resolution=256) + + box_body = builder.add_body(xform=wp.transform(wp.vec3(-2.0, 0.0, 0.0))) + builder.add_shape_mesh(box_body, mesh=box_mesh) + builder.body_qd[-1] = (50.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + wall_mesh = _make_thin_wall_mesh() + wall_mesh.build_sdf(device=device, max_resolution=256) + builder.add_shape_mesh(body=-1, mesh=wall_mesh, xform=wp.transform_identity()) + + model = builder.finalize(device=device) + + pipeline = newton.CollisionPipeline( + model, + broad_phase="nxn", + speculative_config=speculative_config, + ) + contacts = pipeline.contacts() + solver = newton.solvers.SolverXPBD(model, iterations=8) + + state_0 = model.state() + state_1 = model.state() + control = model.control() + frame_dt = 1.0 / 60.0 + + for _ in range(num_frames): + pipeline.collide(state_0, contacts, dt=frame_dt) + state_0.clear_forces() + solver.step(state_0, state_1, control, contacts, frame_dt) + state_0, state_1 = state_1, state_0 + + body_q = state_0.body_q.numpy() + return float(body_q[0][0]) + + +def test_speculative_tunneling_mesh_mesh_without(test, device): + """Without speculative contacts the mesh box tunnels through the mesh wall.""" + final_x = _run_mesh_box_vs_mesh_wall_sim(device, speculative_config=None) + test.assertGreater( + final_x, + 0.5, + f"Mesh box should tunnel through the mesh wall (final x={final_x:.3f})", + ) + + +def test_speculative_tunneling_mesh_mesh_with(test, device): + """With speculative contacts the mesh box is stopped by the mesh wall.""" + config = newton.SpeculativeContactConfig( + max_speculative_extension=2.0, + collision_update_dt=1.0 / 60.0, + ) + final_x = _run_mesh_box_vs_mesh_wall_sim(device, speculative_config=config) + test.assertLess( + final_x, + -0.2, + f"Mesh box should be stopped by the mesh wall (final x={final_x:.3f})", + ) + + +class TestSpeculativeContacts(unittest.TestCase): + pass + + +devices = get_cuda_test_devices() +if not devices: + devices = [wp.get_device("cpu")] + +add_function_test( + TestSpeculativeContacts, + "test_speculative_disabled_no_contacts", + test_speculative_disabled_no_contacts, + devices=devices, +) +add_function_test( + TestSpeculativeContacts, + "test_speculative_enabled_catches_fast_object", + test_speculative_enabled_catches_fast_object, + devices=devices, +) +add_function_test( + TestSpeculativeContacts, + "test_speculative_diverging_no_contacts", + test_speculative_diverging_no_contacts, + devices=devices, +) +add_function_test( + TestSpeculativeContacts, + "test_speculative_max_extension_clamp", + test_speculative_max_extension_clamp, + devices=devices, +) +add_function_test( + TestSpeculativeContacts, "test_speculative_dt_override", test_speculative_dt_override, devices=devices +) +add_function_test( + TestSpeculativeContacts, "test_speculative_angular_velocity", test_speculative_angular_velocity, devices=devices +) +add_function_test( + TestSpeculativeContacts, "test_speculative_tunneling_without", test_speculative_tunneling_without, devices=devices +) +add_function_test( + TestSpeculativeContacts, "test_speculative_tunneling_with", test_speculative_tunneling_with, devices=devices +) +add_function_test( + TestSpeculativeContacts, + "test_speculative_tunneling_mesh_wall_without", + test_speculative_tunneling_mesh_wall_without, + devices=devices, +) +add_function_test( + TestSpeculativeContacts, + "test_speculative_tunneling_mesh_wall_with", + test_speculative_tunneling_mesh_wall_with, + devices=devices, +) +add_function_test( + TestSpeculativeContacts, + "test_speculative_tunneling_mesh_mesh_without", + test_speculative_tunneling_mesh_mesh_without, + devices=devices, +) +add_function_test( + TestSpeculativeContacts, + "test_speculative_tunneling_mesh_mesh_with", + test_speculative_tunneling_mesh_mesh_with, + devices=devices, +) + + +if __name__ == "__main__": + wp.clear_kernel_cache() + unittest.main(verbosity=2)