Forward collision group and shape flags in Kamino's collision pipeline (#4089)

Author: chschuma-disney

newton/_src/solvers/kamino/_src/core/builder.py

@@ -1272,7 +1272,13 @@ def collect_geometry_model_data():
                 geoms_params.append(shape.paramsvec)
                 geoms_offset.append(geom.offset)
                 geoms_material.append(geom.mid)
-                geoms_group.append(geom.group)
+                # `GeometriesModel.group` feeds Newton's broad-phase group test, which uses a
+                # single integer per shape (positive groups mutually collide, zero never
+                # collides). Kamino's own group/collides bitmask is strictly more expressive and
+                # is already fully resolved into `collidable_pairs`/`excluded_pairs` above, so
+                # here we only need to preserve collidability, collapsing every collidable group
+                # onto the same value.
+                geoms_group.append(1 if geom.group > 0 else 0)
                 geoms_collides.append(geom.collides)
                 geoms_gap.append(geom.gap)
                 geoms_margin.append(geom.margin)

newton/_src/solvers/kamino/_src/core/geometry.py

@@ -89,13 +89,23 @@ class GeometryDescriptor(Descriptor):
 
     group: int = 1
     """
-    The collision group assigned to the collision geometry.
+    The collision group assigned to the collision geometry. Together with this
+    geometry's `collides` value, this determines whether the geometry collides
+    with another geometry with a given `group` and `collides`:
+
+    ``can_collide = ((geom1.group & geom2.collides) != 0) and ((geom2.group & geom1.collides) != 0)``
+
     Defaults to the default group with value `1`.
     """
 
     collides: int = 1
     """
-    The collision groups with which the collision geometry can collide.
+    The collision groups with which the collision geometry can collide. Together
+    with this geometry's `group` value, this determines whether the geometry
+    collides with another geometry with a given `group` and `collides`:
+
+    ``can_collide = ((geom1.group & geom2.collides) != 0) and ((geom2.group & geom1.collides) != 0)``
+
     Defaults to enabling collisions with the default group with value `1`.
     """
 
@@ -310,7 +320,15 @@ class GeometriesModel:
 
     group: wp.array[wp.int32] | None = None
     """
-    Collision group assigned to each collision geometry.
+    Collision group assigned to each collision geometry. These groups are based
+    on Newton's collision group semantics, not the group/collides semantics used
+    by `ModelBuilderKamino`.
+
+    Group `0` will not collide with anything. Any positive group N will collide
+    with the same group as well as any negative group. Any negative group -M
+    will collide with all groups except -M. See docs/concepts/collisions.rst
+    for details.
+
     Shape of ``(num_geoms,)``.
     """
 

newton/_src/solvers/kamino/_src/geometry/unified.py

@@ -19,7 +19,6 @@
 from .....geometry.broad_phase_sap import BroadPhaseSAP
 from .....geometry.collision_core import compute_tight_aabb_from_support
 from .....geometry.contact_data import ContactData
-from .....geometry.flags import ShapeFlags
 from .....geometry.narrow_phase import NarrowPhase
 from .....geometry.sdf_texture import TextureSDFData
 from .....geometry.support_function import GenericShapeData, SupportMapDataProvider, pack_mesh_ptr
@@ -29,9 +28,6 @@
 from ..core.data import DataKamino
 from ..core.materials import DEFAULT_FRICTION, DEFAULT_RESTITUTION, make_get_material_pair_properties
 from ..core.model import ModelKamino
-from ..core.types import (
-    to_warp_int32_array,
-)
 from ..geometry.contacts import (
     DEFAULT_GEOM_PAIR_CONTACT_GAP,
     DEFAULT_GEOM_PAIR_MAX_CONTACTS,
@@ -471,27 +467,15 @@ def __init__(
                 self._max_contacts = self._model.geoms.model_minimum_contacts
 
         # Build excluded pairs for NXN/SAP broadphase filtering.
-        # Kamino uses a bitmask group/collides system that is more expressive than
-        # Newton's integer collision groups. We keep all broadphase groups at 1
-        # (same-group, all pairs pass group check) and instead supply an explicit
-        # list of excluded pairs that encodes same-body, group/collides, and
-        # neighbor-joint filtering.
-        geom_collision_group_list = [1] * self._num_geoms
         self._excluded_pairs: wp.array[wp.vec2i] | None = None
         self._num_excluded_pairs: int = 0
         if broadphase in ("nxn", "sap"):
             self._excluded_pairs = self._model.geoms.excluded_pairs
             self._num_excluded_pairs = self._model.geoms.num_excluded_pairs
 
-        # Capture a reference to per-geometry world indices already present in the model
+        # Capture a reference to per-geometry world indices and flags already present in the model
         self.geom_wid: wp.array[wp.int32] = self._model.geoms.wid
-
-        # Define default shape flags for all geometries
-        default_shape_flag: int = (
-            ShapeFlags.VISIBLE  # Mark as visible for debugging/visualization
-            | ShapeFlags.COLLIDE_SHAPES  # Enable shape-shape collision
-            | ShapeFlags.COLLIDE_PARTICLES  # Enable shape-particle collision
-        )
+        self.shape_flags: wp.array[wp.int32] = self._model.geoms.flags
 
         # Detect whether the model contains mesh, convex mesh, or heightfield shapes.
         # Keep mesh and heightfield flags separate: heightfield-only scenes should not
@@ -505,9 +489,8 @@ def __init__(
         # the Kamino model and data do not yet provide
         with wp.ScopedDevice(self._device):
             self.geom_data = wp.zeros(self._num_geoms, dtype=wp.vec4f)
-            self.geom_collision_group = to_warp_int32_array(geom_collision_group_list)
+            self.geom_collision_group = self._model.geoms.group
             self.collision_radius = wp.zeros(self._num_geoms, dtype=wp.float32)
-            self.shape_flags = wp.full(self._num_geoms, default_shape_flag, dtype=wp.int32)
             self.shape_aabb_lower = wp.zeros(self._num_geoms, dtype=wp.vec3)
             self.shape_aabb_upper = wp.zeros(self._num_geoms, dtype=wp.vec3)
             self.broad_phase_pairs = wp.zeros(self._max_shape_pairs, dtype=wp.vec2i)
@@ -539,9 +522,11 @@ def __init__(
         # Initialize the broad-phase backend depending on the selected mode
         match self._broadphase:
             case "nxn":
-                self.nxn_broadphase = BroadPhaseAllPairs(self.geom_wid, shape_flags=None, device=self._device)
+                self.nxn_broadphase = BroadPhaseAllPairs(
+                    self.geom_wid, shape_flags=self.shape_flags, device=self._device
+                )
             case "sap":
-                self.sap_broadphase = BroadPhaseSAP(self.geom_wid, shape_flags=None, device=self._device)
+                self.sap_broadphase = BroadPhaseSAP(self.geom_wid, shape_flags=self.shape_flags, device=self._device)
             case "explicit":
                 self.explicit_broadphase = BroadPhaseExplicit()
             case _:

newton/tests/kamino/test_kamino_geometry_unified.py

@@ -12,11 +12,10 @@
 import numpy as np
 import warp as wp
 
+from newton import ModelBuilder
 from newton._src.solvers.kamino._src.core.builder import ModelBuilderKamino
 from newton._src.solvers.kamino._src.core.data import DataKamino
-from newton._src.solvers.kamino._src.core.math import I_3
 from newton._src.solvers.kamino._src.core.model import ModelKamino
-from newton._src.solvers.kamino._src.core.shapes import SphereShape
 from newton._src.solvers.kamino._src.geometry.contacts import ContactsKamino
 from newton._src.solvers.kamino._src.geometry.unified import CollisionPipelineUnifiedKamino
 from newton._src.solvers.kamino._src.models.builders import basics, testing
@@ -630,7 +629,11 @@ def test_03_gap_rejects_distant_contact(self):
 
 
 class TestUnifiedPipelineNxnBroadphase(unittest.TestCase):
-    """Tests verifying NXN broadphase correctness with collision radii and filter pairs."""
+    """Tests verifying NXN broadphase correctness with collision radii and filter pairs.
+
+    These tests are Kamino-specific and ensure that the combination of model
+    creation/conversion and pipeline setup reproduce the desired behavior.
+    """
 
     def setUp(self):
         if not test_context.setup_done:
@@ -640,29 +643,66 @@ def setUp(self):
     def tearDown(self):
         self.default_device = None
 
-    def _make_two_sphere_builder(self, group_a=1, collides_a=1, group_b=1, collides_b=1, same_body=False):
+    def _make_two_sphere_builder(
+        self,
+        group_a=1,
+        group_b=1,
+        same_body=False,
+        collidable_a=True,
+        collidable_b=True,
+    ):
         """Helper: build a single-world scene with two spheres near each other."""
-        builder = ModelBuilderKamino()
-        builder.add_world()
-        bid_a = builder.add_rigid_body(
-            m_i=1.0,
-            i_I_i=I_3,
-            q_i_0=wp.transformf(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0),
-            u_i_0=wp.spatial_vectorf(0.0),
+        builder = ModelBuilder()
+        builder.begin_world()
+        bid_a = builder.add_body(
+            mass=1.0,
+            inertia=wp.mat33(np.eye(3, dtype=np.float32)),
+            xform=wp.transformf(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0),
         )
         if same_body:
             bid_b = bid_a
         else:
-            bid_b = builder.add_rigid_body(
-                m_i=1.0,
-                i_I_i=I_3,
-                q_i_0=wp.transformf(0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0),
-                u_i_0=wp.spatial_vectorf(0.0),
+            bid_b = builder.add_body(
+                mass=1.0,
+                inertia=wp.mat33(np.eye(3, dtype=np.float32)),
+                xform=wp.transformf(0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0),
             )
-        builder.add_geometry(body=bid_a, shape=SphereShape(radius=0.5), group=group_a, collides=collides_a)
-        builder.add_geometry(body=bid_b, shape=SphereShape(radius=0.5), group=group_b, collides=collides_b)
+        builder.add_shape_sphere(
+            body=bid_a,
+            radius=0.5,
+            cfg=ModelBuilder.ShapeConfig(collision_group=group_a, has_shape_collision=collidable_a),
+        )
+        builder.add_shape_sphere(
+            body=bid_b,
+            radius=0.5,
+            cfg=ModelBuilder.ShapeConfig(collision_group=group_b, has_shape_collision=collidable_b),
+        )
+        builder.end_world()
         return builder
 
+    def _run_two_sphere_pipeline(self, builder: ModelBuilder) -> int:
+        """Helper: convert a Newton builder to `ModelKamino` and run the NXN broadphase.
+
+        Returns the number of active contacts produced.
+        """
+        model = ModelKamino.from_newton(builder.finalize(self.default_device))
+        data = model.data()
+
+        pipeline = CollisionPipelineUnifiedKamino(
+            model=model,
+            broadphase="nxn",
+            default_gap=1.0,
+        )
+
+        n_geoms = builder.shape_count
+        capacity = 12 * ((n_geoms * (n_geoms - 1)) // 2)
+        contacts = ContactsKamino(capacity=max(capacity, 12), device=self.default_device)
+        contacts.clear()
+
+        pipeline.collide(data, contacts)
+
+        return int(contacts.model_active_contacts.numpy()[0])
+
     def test_00_nxn_sphere_on_plane_generates_contacts(self):
         """Sphere resting on a plane via NXN broadphase must produce contacts.
 
@@ -710,32 +750,41 @@ def test_01_nxn_box_on_plane_generates_contacts(self):
             device=self.default_device,
         )
 
-    def test_02_nxn_excludes_non_collidable_pairs(self):
-        """NXN broadphase must exclude pairs whose group/collides bitmasks do not overlap.
-
-        Creates two spheres in the same world but with non-overlapping
-        collision groups so that they should never collide.
-        """
-        builder = self._make_two_sphere_builder(group_a=0b01, collides_a=0b01, group_b=0b10, collides_b=0b10)
-
-        model = builder.finalize(self.default_device)
-        data = model.data()
-
-        pipeline = CollisionPipelineUnifiedKamino(
-            model=model,
-            broadphase="nxn",
-            default_gap=1.0,
-        )
+    def test_02_nxn_newton_collision_group_semantics(self):
+        """NXN broadphase must follow Newton's collision-group semantics.
 
-        n_geoms = builder.num_geoms
-        capacity = 12 * ((n_geoms * (n_geoms - 1)) // 2)
-        contacts = ContactsKamino(capacity=max(capacity, 12), device=self.default_device)
-        contacts.clear()
+        Mirrors `newton._src.geometry.broad_phase_common.test_group_pair`:
+          - group 0 never collides with anything, including another group 0.
+          - A positive group collides only with the same positive group, or
+            with any negative group.
+          - A negative group collides with everything except the same
+            negative group.
 
-        pipeline.collide(data, contacts)
+        Creates two touching spheres on different bodies in the same world
+        and checks every representative combination of group signs/values.
+        """
+        cases = {
+            "both_positive_equal": (1, 1, True),
+            "both_positive_different": (1, 2, False),
+            "positive_vs_zero": (1, 0, False),
+            "both_zero": (0, 0, False),
+            "positive_vs_negative_different": (1, -1, True),
+            "both_negative_equal": (-2, -2, False),
+            "both_negative_different": (-1, -2, True),
+            "negative_vs_zero": (-1, 0, False),
+        }
 
-        active = contacts.model_active_contacts.numpy()[0]
-        self.assertEqual(active, 0, "Non-collidable groups must produce zero contacts via NXN")
+        for case, (group_a, group_b, should_collide) in cases.items():
+            with self.subTest(case=case, group_a=group_a, group_b=group_b):
+                builder = self._make_two_sphere_builder(group_a=group_a, group_b=group_b)
+                active = self._run_two_sphere_pipeline(builder)
+                expected_active = 1 if should_collide else 0
+                self.assertEqual(
+                    active,
+                    expected_active,
+                    f"groups ({group_a}, {group_b}) via nxn broadphase: "
+                    f"expected {'a contact' if should_collide else 'no contacts'}",
+                )
 
     def test_03_nxn_same_body_excluded(self):
         """NXN broadphase must exclude same-body shape pairs.
@@ -744,25 +793,27 @@ def test_03_nxn_same_body_excluded(self):
         that no self-collision contacts are produced.
         """
         builder = self._make_two_sphere_builder(same_body=True)
+        active = self._run_two_sphere_pipeline(builder)
+        self.assertEqual(active, 0, "Same-body shapes must not collide via NXN broadphase")
 
-        model = builder.finalize(self.default_device)
-        data = model.data()
-
-        pipeline = CollisionPipelineUnifiedKamino(
-            model=model,
-            broadphase="nxn",
-            default_gap=1.0,
-        )
-
-        n_geoms = builder.num_geoms
-        capacity = 12 * ((n_geoms * (n_geoms - 1)) // 2)
-        contacts = ContactsKamino(capacity=max(capacity, 12), device=self.default_device)
-        contacts.clear()
+    def test_04_nxn_excludes_non_collidable_shapes(self):
+        """NXN broadphase must exclude shapes without the `COLLIDE_SHAPES` flag.
 
-        pipeline.collide(data, contacts)
+        A shape built with `ShapeConfig(has_shape_collision=False)` (e.g. a
+        sensor or visual-only shape) must never generate a contact, even
+        though its collision group would otherwise allow it to collide.
+        """
+        cases = {
+            "shape_a_non_collidable": (False, True),
+            "shape_b_non_collidable": (True, False),
+            "both_non_collidable": (False, False),
+        }
 
-        active = contacts.model_active_contacts.numpy()[0]
-        self.assertEqual(active, 0, "Same-body shapes must not collide via NXN broadphase")
+        for case, (collidable_a, collidable_b) in cases.items():
+            with self.subTest(case=case):
+                builder = self._make_two_sphere_builder(collidable_a=collidable_a, collidable_b=collidable_b)
+                active = self._run_two_sphere_pipeline(builder)
+                self.assertEqual(active, 0, "Non-collidable shapes must not collide via NXN broadphase")
 
 
 ###