Merge remote-tracking branch 'origin/main' into cleanup_warnings

newton-physics · jvonmuralt · Nov 27, 2025 · Dec 1, 2025 · Dec 1, 2025 · Dec 2, 2025
commit 352fe8ef7d0b3bf92d89ab9ecd092ab67fc4382d
diff --git a/newton/_src/sim/builder.py b/newton/_src/sim/builder.py
@@ -5219,9 +5219,110 @@ def color(
            target_max_min_color_ratio=target_max_min_color_ratio,
        )

-    def finalize(
-        self, device: Devicelike | None = None, requires_grad: bool = False, verbose: bool | None = None
-    ) -> Model:
+    def _validate_world_ordering(self):
+        """Validate that world indices are monotonic, contiguous, and properly ordered.
+
+        This method checks:
+        1. World indices are monotonic (non-decreasing after first non-negative)
+        2. World indices are contiguous (no gaps in sequence)
+        3. Global entities (world -1) only appear at beginning or end of arrays
+        4. All world indices are in valid range [-1, num_worlds-1]
+
+        Raises:
+            ValueError: If any validation check fails.
+        """
+        # List of all world arrays to validate
+        world_arrays = [
+            ("particle_world", self.particle_world),
+            ("body_world", self.body_world),
+            ("shape_world", self.shape_world),
+            ("joint_world", self.joint_world),
+            ("articulation_world", self.articulation_world),
+            ("equality_constraint_world", self.equality_constraint_world),
+        ]
+
+        all_world_indices = set()
+
+        for array_name, world_array in world_arrays:
+            if not world_array:
+                continue
+
+            arr = np.array(world_array, dtype=np.int32)
+
+            # Check for invalid world indices (must be in range [-1, num_worlds-1])
+            max_valid = self.num_worlds - 1
+            invalid_indices = np.where((arr < -1) | (arr > max_valid))[0]
+            if len(invalid_indices) > 0:
+                invalid_values = arr[invalid_indices]
+                raise ValueError(
+                    f"Invalid world index in {array_name}: found value(s) {invalid_values.tolist()} "
+                    f"at indices {invalid_indices.tolist()}. Valid range is -1 to {max_valid} (num_worlds={self.num_worlds})."
+                )
+
+            # Check for global entity positioning (world -1)
+            # Find first and last occurrence of -1
+            negative_indices = np.where(arr == -1)[0]
+            if len(negative_indices) > 0:
+                # Check that all -1s form contiguous blocks at start and/or end
+                # Count -1s at the start
+                start_neg_count = 0
+                for i in range(len(arr)):
+                    if arr[i] == -1:
+                        start_neg_count += 1
+                    else:
+                        break
+
+                # Count -1s at the end (but only if they don't overlap with start)
+                end_neg_count = 0
+                if start_neg_count < len(arr):  # There are non-negative values after the start block
+                    for i in range(len(arr) - 1, -1, -1):
+                        if arr[i] == -1:
+                            end_neg_count += 1
+                        else:
+                            break
+
+                expected_neg_count = start_neg_count + end_neg_count
+                actual_neg_count = len(negative_indices)
+
+                if expected_neg_count != actual_neg_count:
+                    # There are -1s in the middle
+                    raise ValueError(
+                        f"Invalid world ordering in {array_name}: global entities (world -1) "
+                        f"must only appear at the beginning or end of the array, not in the middle. "
+                        f"Found -1 values at indices: {negative_indices.tolist()}"
+                    )
+
+            # Check monotonic ordering for non-negative values
+            non_neg_mask = arr >= 0
+            if np.any(non_neg_mask):
+                non_neg_values = arr[non_neg_mask]
+
+                # Check that non-negative values are monotonic (non-decreasing)
+                if not np.all(non_neg_values[1:] >= non_neg_values[:-1]):
+                    # Find where the order breaks
+                    for i in range(1, len(non_neg_values)):
+                        if non_neg_values[i] < non_neg_values[i - 1]:
+                            raise ValueError(
+                                f"Invalid world ordering in {array_name}: world indices must be monotonic "
+                                f"(non-decreasing). Found world {non_neg_values[i]} after world {non_neg_values[i - 1]}."
+                            )
+
+                # Collect all non-negative world indices for contiguity check
+                all_world_indices.update(non_neg_values)
+
+        # Check contiguity: all world indices should form a sequence 0, 1, 2, ..., n-1
+        if all_world_indices:
+            world_list = sorted(all_world_indices)
+            expected = list(range(world_list[-1] + 1))
+
+            if world_list != expected:
+                missing = set(expected) - set(world_list)
+                raise ValueError(
+                    f"World indices are not contiguous. Missing world(s): {sorted(missing)}. "
+                    f"Found worlds: {world_list}. Worlds must form a continuous sequence starting from 0."
+                )
+
+    def finalize(self, device: Devicelike | None = None, requires_grad: bool = False) -> Model:
        """
        Finalize the builder and create a concrete Model for simulation.