From e2854a92f52479c7658347d1badd10f8551c8a2c Mon Sep 17 00:00:00 2001 From: Ruben Grandia Date: Tue, 4 Aug 2026 14:29:39 +0200 Subject: [PATCH] Add contact overflow warnings. --- .../solvers/kamino/_src/geometry/contacts.py | 16 ++ .../_src/geometry/primitive/narrowphase.py | 158 ++++++++++++++---- .../_src/geometry/primitive/pipeline.py | 18 +- .../kamino/tests/test_geometry_contacts.py | 1 + .../kamino/tests/test_geometry_primitive.py | 24 ++- 5 files changed, 184 insertions(+), 33 deletions(-) diff --git a/newton/_src/solvers/kamino/_src/geometry/contacts.py b/newton/_src/solvers/kamino/_src/geometry/contacts.py index 6166e3e5f4..6e51b46a45 100644 --- a/newton/_src/solvers/kamino/_src/geometry/contacts.py +++ b/newton/_src/solvers/kamino/_src/geometry/contacts.py @@ -238,6 +238,9 @@ def _default_num_world_max_contacts() -> list[int]: Shape of ``(num_worlds,)``. """ + contact_overflow_warning_emitted: wp.array[wp.int32] | None = None + """Internal flag to track contact-buffer overflow warning messages.""" + wid: wp.array[wp.int32] | None = None """ The world index of each active contact. @@ -805,6 +808,7 @@ def finalize( model_active_contacts=wp.zeros(shape=1, dtype=wp.int32), world_max_contacts=to_warp_int32_array(world_max_contacts), world_active_contacts=wp.zeros(shape=len(world_max_contacts), dtype=wp.int32), + contact_overflow_warning_emitted=wp.zeros(shape=1, dtype=wp.int32), wid=wp.full(value=-1, shape=(model_max_contacts,), dtype=wp.int32), cid=wp.full(value=-1, shape=(model_max_contacts,), dtype=wp.int32), gid_AB=wp.full(value=wp.vec2i(-1, -1), shape=(model_max_contacts,), dtype=wp.vec2i), @@ -874,6 +878,7 @@ def _convert_contacts_newton_to_kamino( num_worlds: wp.int32, kamino_model_max_contacts: wp.array[wp.int32], kamino_world_max_contacts: wp.array[wp.int32], + contact_overflow_warning_emitted: wp.array[wp.int32], newton_count: wp.array[wp.int32], newton_shape0: wp.array[wp.int32], newton_shape1: wp.array[wp.int32], @@ -1026,11 +1031,21 @@ def _convert_contacts_newton_to_kamino( wcid = wp.atomic_add(kamino_world_active, wid, 1) if wcid >= world_max_contacts: wp.atomic_sub(kamino_world_active, wid, 1) + if wp.atomic_exch(contact_overflow_warning_emitted, 0, 1) == 0: + wp.printf( + "Warning: Kamino per-world contact capacity exceeded while converting Newton contacts. " + "Increase Kamino contact capacity.\n" + ) return mcid = wp.atomic_add(kamino_model_active, 0, 1) if mcid >= model_max_contacts: wp.atomic_sub(kamino_model_active, 0, 1) wp.atomic_sub(kamino_world_active, wid, 1) + if wp.atomic_exch(contact_overflow_warning_emitted, 0, 1) == 0: + wp.printf( + "Warning: Kamino model contact capacity exceeded while converting Newton contacts. " + "Increase Kamino contact capacity.\n" + ) return # Store the contact data in the Kamino format if the contact is valid @@ -1383,6 +1398,7 @@ def convert_contacts_newton_to_kamino( wp.int32(model.world_count), contacts_out.model_max_contacts, contacts_out.world_max_contacts, + contacts_out._data.contact_overflow_warning_emitted, contacts_in.rigid_contact_count, contacts_in.rigid_contact_shape0, contacts_in.rigid_contact_shape1, diff --git a/newton/_src/solvers/kamino/_src/geometry/primitive/narrowphase.py b/newton/_src/solvers/kamino/_src/geometry/primitive/narrowphase.py index f4be95b6a9..daa68f63d1 100644 --- a/newton/_src/solvers/kamino/_src/geometry/primitive/narrowphase.py +++ b/newton/_src/solvers/kamino/_src/geometry/primitive/narrowphase.py @@ -74,7 +74,6 @@ List of primitive shape combinations supported by the primitive narrow-phase collider. """ - ### # Geometry helper Types ### @@ -227,6 +226,62 @@ def make_ellipsoid(pose: wp.transformf, params: wp.vec3f, gid: wp.int32, bid: wp ### +@wp.func +def _print_world_contact_capacity_warning(): + wp.printf( + "Warning: Kamino per-world contact capacity exceeded. " + "Increase CollisionDetectorConfig.max_contacts_per_world.\n" + ) + + +@wp.func +def _print_model_contact_capacity_warning(): + wp.printf("Warning: Kamino model contact capacity exceeded. Increase CollisionDetectorConfig.max_contacts.\n") + + +@wp.func +def reserve_contact_capacity( + model_max_contacts: wp.int32, + world_max_contacts: wp.int32, + wid: wp.int32, + num_contacts: wp.int32, + contact_model_num: wp.array[wp.int32], + contact_world_num: wp.array[wp.int32], + contact_overflow_warning_emitted: wp.array[wp.int32], +) -> wp.vec3i: + """Reserve contact capacity and return the model and world indices and number reserved.""" + wcio = wp.atomic_add(contact_world_num, wid, num_contacts) + if wcio >= world_max_contacts: + wp.atomic_sub(contact_world_num, wid, num_contacts) + if wp.atomic_exch(contact_overflow_warning_emitted, 0, 1) == 0: + _print_world_contact_capacity_warning() + return wp.vec3i(-1, 0, 0) + + max_num_contacts = wp.min(world_max_contacts - wcio, num_contacts) + if max_num_contacts < num_contacts: + wp.atomic_sub(contact_world_num, wid, num_contacts - max_num_contacts) + if wp.atomic_exch(contact_overflow_warning_emitted, 0, 1) == 0: + _print_world_contact_capacity_warning() + + mcio = wp.atomic_add(contact_model_num, 0, max_num_contacts) + if mcio >= model_max_contacts: + wp.atomic_sub(contact_model_num, 0, max_num_contacts) + wp.atomic_sub(contact_world_num, wid, max_num_contacts) + if wp.atomic_exch(contact_overflow_warning_emitted, 0, 1) == 0: + _print_model_contact_capacity_warning() + return wp.vec3i(-1, 0, 0) + + max_num_contacts_prev = max_num_contacts + max_num_contacts = wp.min(model_max_contacts - mcio, max_num_contacts_prev) + if max_num_contacts < max_num_contacts_prev: + wp.atomic_sub(contact_model_num, 0, max_num_contacts_prev - max_num_contacts) + wp.atomic_sub(contact_world_num, wid, max_num_contacts_prev - max_num_contacts) + if wp.atomic_exch(contact_overflow_warning_emitted, 0, 1) == 0: + _print_model_contact_capacity_warning() + + return wp.vec3i(mcio, max_num_contacts, wcio) + + @wp.func def add_single_contact( # Inputs: @@ -257,6 +312,7 @@ def add_single_contact( contact_frame: wp.array[wp.quatf], contact_material: wp.array[wp.vec2f], contact_key: wp.array[wp.uint64], + contact_overflow_warning_emitted: wp.array[wp.int32], ): # Skip if the contact distance exceeds the detection threshold if (distance - margin_plus_gap) > 0.0: @@ -266,11 +322,15 @@ def add_single_contact( wcid = wp.atomic_add(contact_world_num, wid, 1) if wcid >= world_max_contacts: wp.atomic_sub(contact_world_num, wid, 1) + if wp.atomic_exch(contact_overflow_warning_emitted, 0, 1) == 0: + _print_world_contact_capacity_warning() return mcid = wp.atomic_add(contact_model_num, 0, 1) if mcid >= model_max_contacts: wp.atomic_sub(contact_model_num, 0, 1) wp.atomic_sub(contact_world_num, wid, 1) + if wp.atomic_exch(contact_overflow_warning_emitted, 0, 1) == 0: + _print_model_contact_capacity_warning() return # Perform A/B geom and body assignment @@ -347,6 +407,7 @@ def add_multiple_contacts( contact_frame: wp.array[wp.quatf], contact_material: wp.array[wp.vec2f], contact_key: wp.array[wp.uint64], + contact_overflow_warning_emitted: wp.array[wp.int32], ): # Count valid contacts (those within the detection threshold) num_contacts = wp.int32(0) @@ -369,30 +430,20 @@ def add_multiple_contacts( gid_AB = wp.vec2i(gid_1, gid_2) bid_AB = wp.vec2i(bid_1, bid_2) - # Safely increment the per-world active contact counter (see notes in _write_contact_unified_kamino in unified.py) - wcio = wp.atomic_add(contact_world_num, wid, num_contacts) - if wcio >= world_max_contacts: - wp.atomic_sub(contact_world_num, wid, num_contacts) - return - - # Handle case where this thread saturated the counter and only partial contacts can be written - max_num_contacts = wp.min(world_max_contacts - wcio, num_contacts) - if max_num_contacts < num_contacts: - wp.atomic_sub(contact_world_num, wid, num_contacts - max_num_contacts) - - # Safely increment the model active contact counter - mcio = wp.atomic_add(contact_model_num, 0, max_num_contacts) - if mcio >= model_max_contacts: - wp.atomic_sub(contact_model_num, 0, max_num_contacts) - wp.atomic_sub(contact_world_num, wid, max_num_contacts) + reservation = reserve_contact_capacity( + model_max_contacts, + world_max_contacts, + wid, + num_contacts, + contact_model_num, + contact_world_num, + contact_overflow_warning_emitted, + ) + if reservation[0] < 0: return - - # Handle case where this thread saturated the counter and only partial contacts can be written - max_num_contacts_prev = max_num_contacts - max_num_contacts = wp.min(model_max_contacts - mcio, max_num_contacts_prev) - if max_num_contacts < max_num_contacts_prev: - wp.atomic_sub(contact_model_num, 0, max_num_contacts_prev - max_num_contacts) - wp.atomic_sub(contact_world_num, wid, max_num_contacts_prev - max_num_contacts) + mcio = reservation[0] + max_num_contacts = reservation[1] + wcio = reservation[2] # Create the common material for this contact set material = wp.vec2f(friction, restitution) @@ -486,6 +537,7 @@ def sphere_sphere( contact_frame: wp.array[wp.quatf], contact_material: wp.array[wp.vec2f], contact_key: wp.array[wp.uint64], + contact_overflow_warning_emitted: wp.array[wp.int32], ): # Run the respective collider function to detect sphere-sphere contacts distance, position, normal = collide_sphere_sphere(sphere1.pos, sphere1.radius, sphere2.pos, sphere2.radius) @@ -518,6 +570,7 @@ def sphere_sphere( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) @@ -546,6 +599,7 @@ def sphere_cylinder( contact_frame: wp.array[wp.quatf], contact_material: wp.array[wp.vec2f], contact_key: wp.array[wp.uint64], + contact_overflow_warning_emitted: wp.array[wp.int32], ): # Use the tested collision calculation from collision_primitive.py distance, position, normal = collide_sphere_cylinder( @@ -585,6 +639,7 @@ def sphere_cylinder( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) @@ -618,6 +673,7 @@ def sphere_capsule( contact_frame: wp.array[wp.quatf], contact_material: wp.array[wp.vec2f], contact_key: wp.array[wp.uint64], + contact_overflow_warning_emitted: wp.array[wp.int32], ): # Use the tested collision calculation from collision_primitive.py distance, position, normal = collide_sphere_capsule( @@ -657,6 +713,7 @@ def sphere_capsule( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) @@ -685,6 +742,7 @@ def sphere_box( contact_frame: wp.array[wp.quatf], contact_material: wp.array[wp.vec2f], contact_key: wp.array[wp.uint64], + contact_overflow_warning_emitted: wp.array[wp.int32], ): # Use the tested collision calculation from collision_primitive.py distance, position, normal = collide_sphere_box(sphere1.pos, sphere1.radius, box2.pos, box2.rot, box2.size) @@ -717,6 +775,7 @@ def sphere_box( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) @@ -795,6 +854,7 @@ def capsule_capsule( contact_frame: wp.array[wp.quatf], contact_material: wp.array[wp.vec2f], contact_key: wp.array[wp.uint64], + contact_overflow_warning_emitted: wp.array[wp.int32], ): # Use the tested collision calculation from collision_primitive.py distance, position, normal = collide_capsule_capsule( @@ -838,6 +898,7 @@ def capsule_capsule( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) @@ -866,6 +927,7 @@ def capsule_box( contact_frame: wp.array[wp.quatf], contact_material: wp.array[wp.vec2f], contact_key: wp.array[wp.uint64], + contact_overflow_warning_emitted: wp.array[wp.int32], ): # Use the tested collision calculation from collision_primitive.py distances, positions, normals = collide_capsule_box( @@ -906,6 +968,7 @@ def capsule_box( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) @@ -939,6 +1002,7 @@ def box_box( contact_frame: wp.array[wp.quatf], contact_material: wp.array[wp.vec2f], contact_key: wp.array[wp.uint64], + contact_overflow_warning_emitted: wp.array[wp.int32], ): # Use the tested collision calculation from collision_primitive.py distances, positions, normals = collide_box_box( @@ -973,6 +1037,7 @@ def box_box( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) @@ -1011,6 +1076,7 @@ def plane_sphere( contact_frame: wp.array[wp.quatf], contact_material: wp.array[wp.vec2f], contact_key: wp.array[wp.uint64], + contact_overflow_warning_emitted: wp.array[wp.int32], ): # Use the tested collision calculation from collision_primitive.py # Note: collide_plane_sphere returns (distance, position) without normal @@ -1047,6 +1113,7 @@ def plane_sphere( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) @@ -1075,6 +1142,7 @@ def plane_box( contact_frame: wp.array[wp.quatf], contact_material: wp.array[wp.vec2f], contact_key: wp.array[wp.uint64], + contact_overflow_warning_emitted: wp.array[wp.int32], ): # Use the tested collision calculation from collision_primitive.py distances, positions, normal = collide_plane_box( @@ -1109,6 +1177,7 @@ def plane_box( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) @@ -1137,6 +1206,7 @@ def plane_ellipsoid( contact_frame: wp.array[wp.quatf], contact_material: wp.array[wp.vec2f], contact_key: wp.array[wp.uint64], + contact_overflow_warning_emitted: wp.array[wp.int32], ): # Use the tested collision calculation from collision_primitive.py distance, position, normal = collide_plane_ellipsoid( @@ -1171,6 +1241,7 @@ def plane_ellipsoid( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) @@ -1199,6 +1270,7 @@ def plane_capsule( contact_frame: wp.array[wp.quatf], contact_material: wp.array[wp.vec2f], contact_key: wp.array[wp.uint64], + contact_overflow_warning_emitted: wp.array[wp.int32], ): # Use the tested collision calculation from collision_primitive.py # Note: collide_plane_capsule returns a contact frame, not individual normals @@ -1232,12 +1304,20 @@ def plane_capsule( gid_AB = wp.vec2i(plane1.gid, capsule2.gid) bid_AB = wp.vec2i(plane1.bid, capsule2.bid) - # Increment the active contact counter - mcio = wp.atomic_add(contact_model_num, 0, num_contacts) - wcio = wp.atomic_add(contact_world_num, wid, num_contacts) - - # Retrieve the maximum number of contacts that can be stored - max_num_contacts = wp.min(wp.min(model_max_contacts - mcio, world_max_contacts - wcio), num_contacts) + reservation = reserve_contact_capacity( + model_max_contacts, + world_max_contacts, + wid, + num_contacts, + contact_model_num, + contact_world_num, + contact_overflow_warning_emitted, + ) + if reservation[0] < 0: + return + mcio = reservation[0] + max_num_contacts = reservation[1] + wcio = reservation[2] # Create the common properties shared by all contacts in the current set q_frame = wp.quat_from_matrix(make_contact_frame_znorm(normal)) @@ -1309,6 +1389,7 @@ def plane_cylinder( contact_frame: wp.array[wp.quatf], contact_material: wp.array[wp.vec2f], contact_key: wp.array[wp.uint64], + contact_overflow_warning_emitted: wp.array[wp.int32], ): # Use the tested collision calculation from collision_primitive.py distances, positions, normal = collide_plane_cylinder( @@ -1343,6 +1424,7 @@ def plane_cylinder( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) @@ -1373,6 +1455,7 @@ def _primitive_narrowphase( material_pair_restitution: wp.array[wp.float32], material_pair_static_friction: wp.array[wp.float32], material_pair_dynamic_friction: wp.array[wp.float32], + contact_overflow_warning_emitted: wp.array[wp.int32], # Outputs: contact_model_num: wp.array[wp.int32], contact_world_num: wp.array[wp.int32], @@ -1466,6 +1549,7 @@ def _primitive_narrowphase( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) elif sid1 == GeoType.SPHERE and sid2 == GeoType.CYLINDER: @@ -1491,6 +1575,7 @@ def _primitive_narrowphase( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) elif sid1 == GeoType.SPHERE and sid2 == GeoType.CONE: @@ -1519,6 +1604,7 @@ def _primitive_narrowphase( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) elif sid1 == GeoType.SPHERE and sid2 == GeoType.BOX: @@ -1544,6 +1630,7 @@ def _primitive_narrowphase( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) elif sid1 == GeoType.SPHERE and sid2 == GeoType.ELLIPSOID: @@ -1599,6 +1686,7 @@ def _primitive_narrowphase( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) elif sid1 == GeoType.CAPSULE and sid2 == GeoType.BOX: @@ -1624,6 +1712,7 @@ def _primitive_narrowphase( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) elif sid1 == GeoType.CAPSULE and sid2 == GeoType.ELLIPSOID: @@ -1652,6 +1741,7 @@ def _primitive_narrowphase( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) elif sid1 == GeoType.BOX and sid2 == GeoType.ELLIPSOID: @@ -1684,6 +1774,7 @@ def _primitive_narrowphase( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) elif sid1 == GeoType.PLANE and sid2 == GeoType.BOX: @@ -1709,6 +1800,7 @@ def _primitive_narrowphase( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) elif sid1 == GeoType.PLANE and sid2 == GeoType.ELLIPSOID: @@ -1734,6 +1826,7 @@ def _primitive_narrowphase( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) elif sid1 == GeoType.PLANE and sid2 == GeoType.CAPSULE: @@ -1759,6 +1852,7 @@ def _primitive_narrowphase( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) elif sid1 == GeoType.PLANE and sid2 == GeoType.CYLINDER: @@ -1784,6 +1878,7 @@ def _primitive_narrowphase( contact_frame, contact_material, contact_key, + contact_overflow_warning_emitted, ) @@ -1797,6 +1892,7 @@ def primitive_narrowphase( data: DataKamino, candidates: CollisionCandidatesData, contacts: ContactsKaminoData, + contact_overflow_warning_emitted: wp.array[wp.int32], default_gap: float | None = None, ): """ @@ -1807,6 +1903,7 @@ def primitive_narrowphase( data: The data containing the current state of the geometries. candidates: The collision container holding collision pairs. contacts: The contacts container to store detected contacts. + contact_overflow_warning_emitted: Overflow diagnostic flag. default_gap: Default detection gap [m] applied as a floor to per-geometry gaps. If None, ``0.0`` is used. """ @@ -1838,6 +1935,7 @@ def primitive_narrowphase( model.material_pairs.restitution, model.material_pairs.static_friction, model.material_pairs.dynamic_friction, + contact_overflow_warning_emitted, ], outputs=[ contacts.model_active_contacts, diff --git a/newton/_src/solvers/kamino/_src/geometry/primitive/pipeline.py b/newton/_src/solvers/kamino/_src/geometry/primitive/pipeline.py index d10543c88b..dbec1f5bd1 100644 --- a/newton/_src/solvers/kamino/_src/geometry/primitive/pipeline.py +++ b/newton/_src/solvers/kamino/_src/geometry/primitive/pipeline.py @@ -73,6 +73,7 @@ def __init__( self._cmodel: CollisionCandidatesModel | None = None self._cdata: CollisionCandidatesData | None = None self._bvdata: BoundingVolumesData | None = None + self._contact_overflow_warning_emitted: wp.array[wp.int32] | None = None # If a builder is provided, proceed to finalize all data allocations if model is not None: @@ -159,6 +160,7 @@ def finalize( wid=wp.zeros(shape=(self._model.geoms.num_collidable_pairs,), dtype=wp.int32), geom_pair=wp.zeros_like(self._model.geoms.collidable_pairs), ) + self._contact_overflow_warning_emitted = wp.zeros(shape=(1,), dtype=wp.int32) def collide(self, data: DataKamino, state: StateKamino, contacts: ContactsKamino): """ @@ -190,7 +192,14 @@ def collide(self, data: DataKamino, state: StateKamino, contacts: ContactsKamino ) # Perform the narrow-phase collision detection to generate active contacts - primitive_narrowphase(self._model, data, self._cdata, contacts, default_gap=self._default_gap) + primitive_narrowphase( + self._model, + data, + self._cdata, + contacts, + self._contact_overflow_warning_emitted, + default_gap=self._default_gap, + ) ### # Internals @@ -203,7 +212,12 @@ def _assert_finalized(self): Raises: RuntimeError: If the pipeline has not been finalized. """ - if self._cmodel is None or self._cdata is None or self._bvdata is None: + if ( + self._cmodel is None + or self._cdata is None + or self._bvdata is None + or self._contact_overflow_warning_emitted is None + ): raise RuntimeError( "CollisionPipelinePrimitive has not been finalized. " "Please call `finalize(builder, device)` before using the pipeline." diff --git a/newton/_src/solvers/kamino/tests/test_geometry_contacts.py b/newton/_src/solvers/kamino/tests/test_geometry_contacts.py index 1cea60fb90..2a2765ad4a 100644 --- a/newton/_src/solvers/kamino/tests/test_geometry_contacts.py +++ b/newton/_src/solvers/kamino/tests/test_geometry_contacts.py @@ -1392,6 +1392,7 @@ def test_08_edge_cases(self): small_capacity, f"Active count {nc_small} must not exceed Kamino capacity {small_capacity}", ) + self.assertEqual(int(kamino_small._data.contact_overflow_warning_emitted.numpy()[0]), 1) # Geometry must be sound for whatever was kept. bid_AB = kamino_small.bid_AB.numpy()[:nc_small] for i in range(nc_small): diff --git a/newton/_src/solvers/kamino/tests/test_geometry_primitive.py b/newton/_src/solvers/kamino/tests/test_geometry_primitive.py index 1c1c984ec1..34edcb2bd0 100644 --- a/newton/_src/solvers/kamino/tests/test_geometry_primitive.py +++ b/newton/_src/solvers/kamino/tests/test_geometry_primitive.py @@ -395,7 +395,14 @@ def test_narrowphase( contacts.clear() # Execute narrowphase for primitive shapes - primitive_narrowphase(model, data, broadphase._cdata, contacts, default_gap=gap) + primitive_narrowphase( + model, + data, + broadphase._cdata, + contacts, + wp.zeros(shape=1, dtype=wp.int32, device=device), + default_gap=gap, + ) # Optional verbose output msg.debug("[%s][%s]: bodies.q_i:\n%s", case, bp_name, data.bodies.q_i) @@ -1089,6 +1096,21 @@ def test_02_make_and_collide(self): header="pipeline primitive narrow-phase", ) + def test_03_reports_contact_capacity_overflow(self): + """Report primitive contacts truncated by the final contact capacity.""" + builder = testing.make_shape_pairs_builder(shape_pairs=[("box", "box")]) + model = builder.finalize(device=self.default_device) + data = model.data() + state = model.state() + contacts = ContactsKamino(capacity=1, device=self.default_device) + pipeline = CollisionPipelinePrimitive(model=model) + + pipeline.collide(data, state, contacts) + + self.assertEqual(int(contacts.model_active_contacts.numpy()[0]), 1) + self.assertEqual(int(contacts.world_active_contacts.numpy()[0]), 1) + self.assertEqual(int(pipeline._contact_overflow_warning_emitted.numpy()[0]), 1) + ### # Test execution