Fix scale-dependent triangle MPR initialization

Author: LuyiLi

CHANGELOG.md

@@ -101,6 +101,7 @@
 
 ### Fixed
 
+- Fix MPR returning scale-dependent, excessively deep contacts for small convex shapes on large mesh triangles while preserving triangle-specific shared-edge manifold witnesses.
 - Make deterministic collision pipelines cover hydroelastic contact generation and reduction, including unique reduced-contact sort keys and overflow-safe fixed-point pressure accumulation.
 - Convert `newton:mimicCoef0` from degrees to radians when the mimic follower joint is angular. Assets authored against the old behavior need the value rescaled to degrees.
 - Complete Kamino RCM traversal for large and disconnected systems and reuse the resulting permutation by default; set `reuse_permutation=False` to recompute it for changing matrix topology.

newton/_src/geometry/mpr.py

@@ -236,24 +236,40 @@ def geometric_center(
             # point near the contact region — this dramatically improves
             # MPR convergence for large triangles.
             #
-            # Blend 1% toward the centroid so the point is strictly in the
-            # face interior.  This does NOT prevent an MPR degeneracy (MPR
-            # works fine from an edge point); it improves *manifold quality*.
+            # Nudge toward the centroid so the point moves into the face
+            # interior when possible.  This does NOT prevent an MPR
+            # degeneracy (MPR works fine from an edge point); it improves
+            # *manifold quality*.
             # When shape B projects onto a shared mesh edge, both adjacent
             # triangles get the same v0, producing MPR witness points biased
             # toward the edge.  The manifold builder (multicontact.py) uses
             # these witness points as its center for perturbed support
             # mapping, so edge-biased centers cause overlapping contact
             # polygons across the two triangles instead of distinct ones —
             # resulting in asymmetric force distribution and spurious torque.
-            # The 1% nudge gives each triangle a unique v0 pulled toward its
-            # own interior, yielding well-separated manifold centers.
+            # Keep that triangle-specific direction, but limit the tangential
+            # displacement to 1% of the normal center-to-plane distance.  A
+            # face projection therefore tilts the initial ray by at most
+            # atan(0.01), about 0.57 degrees, regardless of triangle size.  A
+            # pure barycentric blend scales with the triangle and can make the
+            # initial ray almost tangential when a small partner lies on a
+            # large terrain face, causing MPR to converge to a non-minimum
+            # portal.
             tri_a = wp.vec3(0.0, 0.0, 0.0)
             tri_b = geom_a.scale
             tri_c = geom_a.auxiliary
             proj = closest_point_on_triangle(center_b_world, tri_a, tri_b, tri_c)
             centroid = (tri_a + tri_b + tri_c) / 3.0
-            center_a = proj + 0.01 * (centroid - proj)
+            to_centroid = centroid - proj
+            distance_to_centroid = wp.length(to_centroid)
+            face_normal = wp.cross(tri_b - tri_a, tri_c - tri_a)
+            face_normal_length = wp.length(face_normal)
+            if distance_to_centroid > 1.0e-12 and face_normal_length > 1.0e-12:
+                center_to_plane = wp.abs(wp.dot(center_b_world - proj, face_normal)) / face_normal_length
+                nudge_distance = 0.01 * wp.min(distance_to_centroid, center_to_plane)
+                center_a = proj + to_centroid * (nudge_distance / distance_to_centroid)
+            else:
+                center_a = proj
 
         center.B = center_b_world
         center.BtoA = center_a - center_b_world

newton/tests/test_mpr.py

@@ -0,0 +1,168 @@
+# SPDX-FileCopyrightText: Copyright (c) 2026 The Newton Developers
+# SPDX-License-Identifier: Apache-2.0
+
+"""Regression tests for Minkowski Portal Refinement (MPR)."""
+
+import unittest
+
+import numpy as np
+import warp as wp
+
+from newton import GeoType
+from newton._src.geometry.mpr import create_solve_mpr
+from newton._src.geometry.support_function import (
+    GenericShapeData,
+    GeoTypeEx,
+    SupportMapDataProvider,
+    support_map,
+)
+
+
+@wp.kernel
+def _triangle_mpr_kernel(
+    triangle_b: wp.array[wp.vec3],
+    triangle_c: wp.array[wp.vec3],
+    shape_b_type: int,
+    shape_b_scale: wp.vec3,
+    shape_b_position: wp.array[wp.vec3],
+    shape_b_orientation: wp.array[wp.quat],
+    collision_out: wp.array[int],
+    point_a_out: wp.array[wp.vec3],
+    point_b_out: wp.array[wp.vec3],
+    normal_out: wp.array[wp.vec3],
+    penetration_out: wp.array[float],
+):
+    """Run MPR directly, with each triangle's first vertex at the origin."""
+    i = wp.tid()
+
+    shape_a = GenericShapeData()
+    shape_a.shape_type = int(GeoTypeEx.TRIANGLE)
+    shape_a.scale = triangle_b[i]
+    shape_a.auxiliary = triangle_c[i]
+
+    shape_b = GenericShapeData()
+    shape_b.shape_type = shape_b_type
+    shape_b.scale = shape_b_scale
+    shape_b.auxiliary = wp.vec3(0.0)
+
+    data_provider = SupportMapDataProvider()
+    collision, point_a, point_b, normal, penetration = wp.static(create_solve_mpr(support_map).core)(
+        shape_a,
+        shape_b,
+        shape_b_orientation[i],
+        shape_b_position[i],
+        0.0,
+        data_provider,
+    )
+
+    collision_out[i] = int(collision)
+    point_a_out[i] = point_a
+    point_b_out[i] = point_b
+    normal_out[i] = normal
+    penetration_out[i] = penetration
+
+
+def _run_triangle_mpr(triangle_b, triangle_c, shape_type, shape_scale, shape_positions, shape_orientations):
+    """Run direct triangle-vs-convex MPR cases on CPU and return NumPy outputs."""
+    device = "cpu"
+    count = len(triangle_b)
+
+    triangle_b_wp = wp.array(np.asarray(triangle_b, dtype=np.float32), dtype=wp.vec3, device=device)
+    triangle_c_wp = wp.array(np.asarray(triangle_c, dtype=np.float32), dtype=wp.vec3, device=device)
+    shape_positions_wp = wp.array(np.asarray(shape_positions, dtype=np.float32), dtype=wp.vec3, device=device)
+    shape_orientations_wp = wp.array(np.asarray(shape_orientations, dtype=np.float32), dtype=wp.quat, device=device)
+
+    collision = wp.zeros(count, dtype=int, device=device)
+    point_a = wp.zeros(count, dtype=wp.vec3, device=device)
+    point_b = wp.zeros(count, dtype=wp.vec3, device=device)
+    normal = wp.zeros(count, dtype=wp.vec3, device=device)
+    penetration = wp.zeros(count, dtype=float, device=device)
+
+    wp.launch(
+        _triangle_mpr_kernel,
+        dim=count,
+        inputs=[
+            triangle_b_wp,
+            triangle_c_wp,
+            int(shape_type),
+            wp.vec3(*shape_scale),
+            shape_positions_wp,
+            shape_orientations_wp,
+        ],
+        outputs=[collision, point_a, point_b, normal, penetration],
+        device=device,
+    )
+
+    return (
+        collision.numpy(),
+        point_a.numpy(),
+        point_b.numpy(),
+        normal.numpy(),
+        penetration.numpy(),
+    )
+
+
+class TestMPRTriangleInitialization(unittest.TestCase):
+    """Test triangle MPR initialization across disparate geometry scales."""
+
+    def test_small_cylinder_on_large_triangle(self):
+        """A small cylinder on a large triangle must resolve along the face normal.
+
+        A fixed 1% triangle-centroid blend moves the initial point by nearly
+        half a meter in this case.  With the cylinder center only 0.36 mm above
+        the face, that makes the MPR ray almost tangential and used to produce
+        a 58.8 m penetration instead of the geometric 61.7 mm penetration.
+        """
+        cylinder_position = np.array([28.87991333, 260.53430176, 0.00036136055], dtype=np.float32)
+        cylinder_orientation = np.array(
+            [-0.16498479, 0.36682746, -0.48718625, 0.77515626],
+            dtype=np.float32,
+        )
+        radius = 0.02
+        half_height = 0.07
+
+        collision, _point_a, _point_b, normals, penetrations = _run_triangle_mpr(
+            triangle_b=[[100.0, 320.0, 0.0]],
+            triangle_c=[[0.0, 320.0, 0.0]],
+            shape_type=GeoType.CYLINDER,
+            shape_scale=(radius, half_height, radius),
+            shape_positions=[cylinder_position],
+            shape_orientations=[cylinder_orientation],
+        )
+
+        # The cylinder axis is its rotated local Z axis.  Its support extent
+        # along world Z is h*|axis_z| + r*sqrt(1-axis_z^2).
+        qx, qy, _qz, _qw = cylinder_orientation
+        axis_z = 1.0 - 2.0 * (qx * qx + qy * qy)
+        vertical_extent = half_height * abs(axis_z) + radius * np.sqrt(max(0.0, 1.0 - axis_z * axis_z))
+        expected_penetration = vertical_extent - cylinder_position[2]
+
+        self.assertEqual(collision[0], 1)
+        self.assertAlmostEqual(float(penetrations[0]), float(expected_penetration), delta=2.0e-5)
+        np.testing.assert_allclose(normals[0], [0.0, 0.0, 1.0], atol=2.0e-4)
+
+    def test_shared_edge_witnesses_remain_triangle_specific(self):
+        """The bounded nudge must retain distinct witnesses across a mesh seam.
+
+        The two triangles form a square split along x=y, and the box center is
+        directly above that shared edge.  The witness on each triangle should
+        lie on its own side of the seam rather than both being edge-biased.
+        """
+        collision, points_a, _points_b, normals, penetrations = _run_triangle_mpr(
+            triangle_b=[[2.0, 0.0, 0.0], [2.0, 2.0, 0.0]],
+            triangle_c=[[2.0, 2.0, 0.0], [0.0, 2.0, 0.0]],
+            shape_type=GeoType.BOX,
+            shape_scale=(0.15, 0.12, 0.1),
+            shape_positions=[[1.0, 1.0, 0.095], [1.0, 1.0, 0.095]],
+            shape_orientations=[[0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 1.0]],
+        )
+
+        np.testing.assert_array_equal(collision, [1, 1])
+        self.assertGreater(float(points_a[0, 0] - points_a[0, 1]), 1.0e-4)
+        self.assertGreater(float(points_a[1, 1] - points_a[1, 0]), 1.0e-4)
+        np.testing.assert_allclose(normals, [[0.0, 0.0, 1.0], [0.0, 0.0, 1.0]], atol=1.0e-5)
+        np.testing.assert_allclose(penetrations, [0.005, 0.005], atol=1.0e-5)
+
+
+if __name__ == "__main__":
+    unittest.main(verbosity=2, failfast=True)