diff --git a/genesis/engine/solvers/rigid/collider/__init__.py b/genesis/engine/solvers/rigid/collider/__init__.py index fe3948935..4cfb3aad4 100644 --- a/genesis/engine/solvers/rigid/collider/__init__.py +++ b/genesis/engine/solvers/rigid/collider/__init__.py @@ -8,6 +8,7 @@ - broadphase: AABB and sweep-and-prune algorithms - narrowphase: SDF, convex-convex, terrain collision - box_contact: Specialized box collision (plane-box, box-box) +- capsule_contact: Specialized capsule collision (capsule-capsule analytical) - contact: Contact management utilities - gjk: Gilbert-Johnson-Keerthi algorithm - epa: Expanding Polytope Algorithm @@ -19,4 +20,5 @@ from .broadphase import * from .narrowphase import * from .box_contact import * +from .capsule_contact import * from .contact import * diff --git a/genesis/engine/solvers/rigid/collider/capsule_contact.py b/genesis/engine/solvers/rigid/collider/capsule_contact.py new file mode 100644 index 000000000..30f4caab1 --- /dev/null +++ b/genesis/engine/solvers/rigid/collider/capsule_contact.py @@ -0,0 +1,242 @@ +import gstaichi as ti + +import genesis as gs +import genesis.utils.geom as gu +import genesis.utils.array_class as array_class + + +@ti.func +def func_closest_points_on_segments( + seg_a_p1: ti.types.vector(3, gs.ti_float), + seg_a_p2: ti.types.vector(3, gs.ti_float), + seg_b_p1: ti.types.vector(3, gs.ti_float), + seg_b_p2: ti.types.vector(3, gs.ti_float), + EPS: gs.ti_float, +): + """ + Compute closest points on two line segments using analytical solution. + + References + ---------- + Real-Time Collision Detection by Christer Ericson, Chapter 5.1.9 + """ + segment_a_dir = seg_a_p2 - seg_a_p1 + segment_b_dir = seg_b_p2 - seg_b_p1 + vec_between_segment_origins = seg_a_p1 - seg_b_p1 + + a_squared_len = segment_a_dir.dot(segment_a_dir) + dot_product_dir = segment_a_dir.dot(segment_b_dir) + b_squared_len = segment_b_dir.dot(segment_b_dir) + d = segment_a_dir.dot(vec_between_segment_origins) + e = segment_b_dir.dot(vec_between_segment_origins) + + denom = a_squared_len * b_squared_len - dot_product_dir * dot_product_dir + + s = gs.ti_float(0.0) + t = gs.ti_float(0.0) + + if denom < EPS: + # Segments are parallel or one/both are degenerate + s = 0.0 + if b_squared_len > EPS: + t = ti.math.clamp(e / b_squared_len, 0.0, 1.0) + else: + t = 0.0 + else: + # General case: solve for optimal parameters + s = (dot_product_dir * e - b_squared_len * d) / denom + t = (a_squared_len * e - dot_product_dir * d) / denom + + s = ti.math.clamp(s, 0.0, 1.0) + + # Recompute t for clamped s + t = ti.math.clamp((dot_product_dir * s + e) / b_squared_len if b_squared_len > EPS else 0.0, 0.0, 1.0) + + # Recompute s for clamped t (ensures we're on segment boundaries) + s_new = ti.math.clamp((dot_product_dir * t - d) / a_squared_len if a_squared_len > EPS else 0.0, 0.0, 1.0) + + # Use refined s if it improves the solution + if a_squared_len > EPS: + s = s_new + + seg_a_closest = seg_a_p1 + s * segment_a_dir + seg_b_closest = seg_b_p1 + t * segment_b_dir + + return seg_a_closest, seg_b_closest + + +@ti.func +def func_capsule_capsule_contact( + i_ga, + i_gb, + i_b, + geoms_state: array_class.GeomsState, + geoms_info: array_class.GeomsInfo, + rigid_global_info: array_class.RigidGlobalInfo, + collider_state: array_class.ColliderState, + collider_info: array_class.ColliderInfo, + errno: array_class.V_ANNOTATION, +): + """ + Analytical capsule-capsule collision detection. + + A capsule is defined as a line segment plus a radius (swept sphere). + Collision between two capsules reduces to: + 1. Find closest points on the two line segments (analytical) + 2. Check if distance < sum of radii + 3. Compute contact point and normal + """ + EPS = rigid_global_info.EPS[None] + is_col = False + normal = ti.Vector.zero(gs.ti_float, 3) + contact_pos = ti.Vector.zero(gs.ti_float, 3) + penetration = gs.ti_float(0.0) + + # Get capsule A parameters + pos_a = geoms_state.pos[i_ga, i_b] + quat_a = geoms_state.quat[i_ga, i_b] + radius_a = geoms_info.data[i_ga][0] + halflength_a = gs.ti_float(0.5) * geoms_info.data[i_ga][1] + + # Get capsule B parameters + pos_b = geoms_state.pos[i_gb, i_b] + quat_b = geoms_state.quat[i_gb, i_b] + radius_b = geoms_info.data[i_gb][0] + halflength_b = gs.ti_float(0.5) * geoms_info.data[i_gb][1] + + # Capsules are aligned along local Z-axis by convention + local_z = ti.Vector([0.0, 0.0, 1.0], dt=gs.ti_float) + + # Get segment axes in world space + axis_a = gu.ti_transform_by_quat(local_z, quat_a) + axis_b = gu.ti_transform_by_quat(local_z, quat_b) + + # Compute segment endpoints in world space + P1 = pos_a - halflength_a * axis_a + P2 = pos_a + halflength_a * axis_a + Q1 = pos_b - halflength_b * axis_b + Q2 = pos_b + halflength_b * axis_b + + Pa, Pb = func_closest_points_on_segments(P1, P2, Q1, Q2, EPS) + + diff = Pb - Pa + dist_sq = diff.dot(diff) + combined_radius = radius_a + radius_b + combined_radius_sq = combined_radius * combined_radius + + if dist_sq < combined_radius_sq: + is_col = True + dist = ti.sqrt(dist_sq) + + # Compute contact normal (from B to A, pointing into geom A) + if dist > EPS: + # Negative because func_add_contact expects normal from B to A + normal = -diff / dist + else: + # Segments are coincident, use arbitrary perpendicular direction + # Try cross product with axis_a first + temp_normal = axis_a.cross(axis_b) + if temp_normal.dot(temp_normal) < EPS: + # Axes are parallel, use any perpendicular + if ti.abs(axis_a[0]) < 0.9: + temp_normal = ti.Vector([1.0, 0.0, 0.0], dt=gs.ti_float).cross(axis_a) + else: + temp_normal = ti.Vector([0.0, 1.0, 0.0], dt=gs.ti_float).cross(axis_a) + # For coincident case, the sign doesn't matter much, but keep consistent + normal = -gu.ti_normalize(temp_normal, EPS) + + penetration = combined_radius - dist + contact_pos = Pa - radius_a * normal + + return is_col, normal, contact_pos, penetration + + +@ti.func +def func_sphere_capsule_contact( + i_ga, + i_gb, + i_b, + geoms_state: array_class.GeomsState, + geoms_info: array_class.GeomsInfo, + rigid_global_info: array_class.RigidGlobalInfo, + collider_state: array_class.ColliderState, + collider_info: array_class.ColliderInfo, + errno: array_class.V_ANNOTATION, +): + """ + Analytical sphere-capsule collision detection. + + A sphere-capsule collision reduces to: + 1. Find closest point on the capsule's line segment to sphere center + 2. Check if distance < sum of radii + 3. Compute contact point and normal + + """ + # Ensure sphere is always i_ga and capsule is i_gb + normal_dir = 1 + if geoms_info.type[i_gb] == gs.GEOM_TYPE.SPHERE: + i_ga, i_gb = i_gb, i_ga + normal_dir = -1 + + EPS = rigid_global_info.EPS[None] + is_col = False + normal = ti.Vector.zero(gs.ti_float, 3) + contact_pos = ti.Vector.zero(gs.ti_float, 3) + penetration = gs.ti_float(0.0) + + sphere_center = geoms_state.pos[i_ga, i_b] + sphere_radius = geoms_info.data[i_ga][0] + + capsule_center = geoms_state.pos[i_gb, i_b] + capsule_quat = geoms_state.quat[i_gb, i_b] + capsule_radius = geoms_info.data[i_gb][0] + capsule_halflength = gs.ti_float(0.5) * geoms_info.data[i_gb][1] + + # Capsule is aligned along local Z-axis + local_z = ti.Vector([0.0, 0.0, 1.0], dt=gs.ti_float) + capsule_axis = gu.ti_transform_by_quat(local_z, capsule_quat) + + # Compute capsule segment endpoints + P1 = capsule_center - capsule_halflength * capsule_axis + P2 = capsule_center + capsule_halflength * capsule_axis + + # Find closest point on capsule segment to sphere center + # Using parametric form: P(t) = P1 + t*(P2-P1), t ∈ [0,1] + segment_vec = P2 - P1 + segment_length_sq = segment_vec.dot(segment_vec) + + # Project sphere center onto segment + # Default for degenerate case + t = gs.ti_float(0.5) + if segment_length_sq > EPS: + t = (sphere_center - P1).dot(segment_vec) / segment_length_sq + t = ti.math.clamp(t, 0.0, 1.0) + + closest_point = P1 + t * segment_vec + + # Compute distance from sphere center to closest point + diff = sphere_center - closest_point + dist_sq = diff.dot(diff) + combined_radius = sphere_radius + capsule_radius + combined_radius_sq = combined_radius * combined_radius + + if dist_sq < combined_radius_sq: + is_col = True + dist = ti.sqrt(dist_sq) + + # Compute contact normal (from capsule to sphere, i.e., B to A) + if dist > EPS: + normal = diff / dist + else: + # Sphere center is exactly on capsule axis + # Use any perpendicular direction to the capsule axis + if ti.abs(capsule_axis[0]) < 0.9: + normal = ti.Vector([1.0, 0.0, 0.0], dt=gs.ti_float).cross(capsule_axis) + else: + normal = ti.Vector([0.0, 1.0, 0.0], dt=gs.ti_float).cross(capsule_axis) + normal = gu.ti_normalize(normal, EPS) + + penetration = combined_radius - dist + contact_pos = sphere_center - sphere_radius * normal + + return is_col, normal * normal_dir, contact_pos, penetration diff --git a/genesis/engine/solvers/rigid/collider/narrowphase.py b/genesis/engine/solvers/rigid/collider/narrowphase.py index 0578da3d3..dd16e2d69 100644 --- a/genesis/engine/solvers/rigid/collider/narrowphase.py +++ b/genesis/engine/solvers/rigid/collider/narrowphase.py @@ -34,6 +34,11 @@ func_box_box_contact, ) +from .capsule_contact import ( + func_capsule_capsule_contact, + func_sphere_capsule_contact, +) + class CCD_ALGORITHM_CODE(IntEnum): """Convex collision detection algorithm codes.""" @@ -621,7 +626,37 @@ def func_convex_convex_contact( func_rotate_frame(i_gb, contact_pos_0, gu.ti_inv_quat(qrot), i_b, geoms_state, geoms_info) if (multi_contact and is_col_0) or (i_detection == 0): - if geoms_info.type[i_ga] == gs.GEOM_TYPE.PLANE: + if geoms_info.type[i_ga] == gs.GEOM_TYPE.CAPSULE and geoms_info.type[i_gb] == gs.GEOM_TYPE.CAPSULE: + if ti.static(__debug__): + ti.atomic_add(collider_state.debug_analytical_capsule_count[i_b], 1) + is_col, normal, contact_pos, penetration = func_capsule_capsule_contact( + i_ga, + i_gb, + i_b, + geoms_state, + geoms_info, + rigid_global_info, + collider_state, + collider_info, + errno, + ) + elif ( + geoms_info.type[i_ga] == gs.GEOM_TYPE.SPHERE and geoms_info.type[i_gb] == gs.GEOM_TYPE.CAPSULE + ) or (geoms_info.type[i_ga] == gs.GEOM_TYPE.CAPSULE and geoms_info.type[i_gb] == gs.GEOM_TYPE.SPHERE): + if ti.static(__debug__): + ti.atomic_add(collider_state.debug_analytical_sphere_capsule_count[i_b], 1) + is_col, normal, contact_pos, penetration = func_sphere_capsule_contact( + i_ga, + i_gb, + i_b, + geoms_state, + geoms_info, + rigid_global_info, + collider_state, + collider_info, + errno, + ) + elif geoms_info.type[i_ga] == gs.GEOM_TYPE.PLANE: plane_dir = ti.Vector( [geoms_info.data[i_ga][0], geoms_info.data[i_ga][1], geoms_info.data[i_ga][2]], dt=gs.ti_float ) @@ -697,6 +732,8 @@ def func_convex_convex_contact( ### GJK, MJ_GJK if ti.static(collider_static_config.ccd_algorithm != CCD_ALGORITHM_CODE.MJ_MPR): if prefer_gjk: + if ti.static(__debug__): + ti.atomic_add(collider_state.debug_gjk_count[i_b], 1) if ti.static(static_rigid_sim_config.requires_grad): diff_gjk.func_gjk_contact( links_state, @@ -840,44 +877,46 @@ def func_convex_convex_contact( # Clear collision normal cache if not in contact collider_state.contact_cache.normal[i_pair, i_b] = ti.Vector.zero(gs.ti_float, 3) elif multi_contact and is_col_0 > 0 and is_col > 0: - if ti.static(collider_static_config.ccd_algorithm in (CCD_ALGORITHM_CODE.MPR, CCD_ALGORITHM_CODE.GJK)): - # 1. Project the contact point on both geometries - # 2. Revert the effect of small rotation - # 3. Update contact point - contact_point_a = ( - gu.ti_transform_by_quat( - (contact_pos - 0.5 * penetration * normal) - contact_pos_0, - gu.ti_inv_quat(qrot), - ) - + contact_pos_0 - ) - contact_point_b = ( - gu.ti_transform_by_quat( - (contact_pos + 0.5 * penetration * normal) - contact_pos_0, - qrot, - ) - + contact_pos_0 + # For perturbed iterations (i_detection > 0), correct contact position and normal + # This applies to ALL collision methods when multi-contact is enabled + + # 1. Project the contact point on both geometries + # 2. Revert the effect of small rotation + # 3. Update contact point + contact_point_a = ( + gu.ti_transform_by_quat( + (contact_pos - 0.5 * penetration * normal) - contact_pos_0, + gu.ti_inv_quat(qrot), ) - contact_pos = 0.5 * (contact_point_a + contact_point_b) - - # First-order correction of the normal direction. - # The way the contact normal gets twisted by applying perturbation of geometry poses is - # unpredictable as it depends on the final portal discovered by MPR. Alternatively, let compute - # the minimal rotation that makes the corrected twisted normal as closed as possible to the - # original one, up to the scale of the perturbation, then apply first-order Taylor expansion of - # Rodrigues' rotation formula. - twist_rotvec = ti.math.clamp( - normal.cross(normal_0), - -collider_info.mc_perturbation[None], - collider_info.mc_perturbation[None], + + contact_pos_0 + ) + contact_point_b = ( + gu.ti_transform_by_quat( + (contact_pos + 0.5 * penetration * normal) - contact_pos_0, + qrot, ) - normal = normal + twist_rotvec.cross(normal) + + contact_pos_0 + ) + contact_pos = 0.5 * (contact_point_a + contact_point_b) + + # First-order correction of the normal direction. + # The way the contact normal gets twisted by applying perturbation of geometry poses is + # unpredictable as it depends on the final portal discovered by MPR. Alternatively, let compute + # the minimal rotation that makes the corrected twisted normal as closed as possible to the + # original one, up to the scale of the perturbation, then apply first-order Taylor expansion of + # Rodrigues' rotation formula. + twist_rotvec = ti.math.clamp( + normal.cross(normal_0), + -collider_info.mc_perturbation[None], + collider_info.mc_perturbation[None], + ) + normal = normal + twist_rotvec.cross(normal) - # Make sure that the penetration is still positive before adding contact point. - # Note that adding some negative tolerance improves physical stability by encouraging persistent - # contact points and thefore more continuous contact forces, without changing the mean-field - # dynamics since zero-penetration contact points should not induce any force. - penetration = normal.dot(contact_point_b - contact_point_a) + # Make sure that the penetration is still positive before adding contact point. + # Note that adding some negative tolerance improves physical stability by encouraging persistent + # contact points and thefore more continuous contact forces, without changing the mean-field + # dynamics since zero-penetration contact points should not induce any force. + penetration = normal.dot(contact_point_b - contact_point_a) if ti.static(collider_static_config.ccd_algorithm == CCD_ALGORITHM_CODE.MJ_GJK): # Only change penetration to the initial one, because the normal vector could change abruptly # under MuJoCo's GJK-EPA. diff --git a/genesis/utils/array_class.py b/genesis/utils/array_class.py index a51420f96..8c98884e2 100644 --- a/genesis/utils/array_class.py +++ b/genesis/utils/array_class.py @@ -544,6 +544,10 @@ class StructColliderState(metaclass=BASE_METACLASS): contact_cache: StructContactCache # Input data for differentiable contact detection used in the backward pass diff_contact_input: StructDiffContactInput + # Debug counters for collision detection path tracking + debug_analytical_capsule_count: V_ANNOTATION + debug_analytical_sphere_capsule_count: V_ANNOTATION + debug_gjk_count: V_ANNOTATION def get_collider_state( @@ -597,6 +601,9 @@ def get_collider_state( broad_collision_pairs=V_VEC(2, dtype=gs.ti_int, shape=(max(max_collision_pairs_broad, 1), _B)), contact_data=get_contact_data(solver, max_contact_pairs, requires_grad), diff_contact_input=get_diff_contact_input(solver, max(max_contact_pairs, 1), is_active=True), + debug_analytical_capsule_count=V(dtype=gs.ti_int, shape=(_B,)), + debug_analytical_sphere_capsule_count=V(dtype=gs.ti_int, shape=(_B,)), + debug_gjk_count=V(dtype=gs.ti_int, shape=(_B,)), ) diff --git a/tests/test_capsule_analytical_vs_gjk.py b/tests/test_capsule_analytical_vs_gjk.py new file mode 100644 index 000000000..a7a121118 --- /dev/null +++ b/tests/test_capsule_analytical_vs_gjk.py @@ -0,0 +1,230 @@ +""" +Unit test comparing analytical capsule-capsule contact detection with GJK. + +NOTE: These tests should ideally be run with debug mode enabled (pytest --dev) +to verify that the correct collision detection code paths are being used. + +Debug mode enables collision path tracking counters in the narrowphase kernel that +empirically verify which algorithm (analytical vs GJK) was actually executed. +""" + +import os +import tempfile +import xml.etree.ElementTree as ET + +import numpy as np +import pytest + +import genesis as gs + + +def create_capsule_mjcf(name, pos, euler, radius, half_length): + """Helper function to create an MJCF file with a single capsule.""" + mjcf = ET.Element("mujoco", model=name) + ET.SubElement(mjcf, "compiler", angle="degree") + ET.SubElement(mjcf, "option", timestep="0.01") + worldbody = ET.SubElement(mjcf, "worldbody") + body = ET.SubElement( + worldbody, + "body", + name=name, + pos=f"{pos[0]} {pos[1]} {pos[2]}", + euler=f"{euler[0]} {euler[1]} {euler[2]}", + ) + ET.SubElement(body, "geom", type="capsule", size=f"{radius} {half_length}") + ET.SubElement(body, "joint", name=f"{name}_joint", type="free") + return mjcf + + +@pytest.mark.debug(True) +@pytest.mark.parametrize("backend", [gs.cpu]) +@pytest.mark.parametrize( + "pos1,euler1,pos2,euler2,should_collide,description", + [ + # Test 1: Vertical and horizontal capsules intersecting + # Capsule 1: vertical at origin, Capsule 2: horizontal at x=0.15 + # Distance between axes: 0.15, sum of radii: 0.2 → should collide + ((0, 0, 0), (0, 0, 0), (0.15, 0, 0), (0, 90, 0), True, "perpendicular_close"), + # Test 2: Parallel capsules far apart + # Distance: 1.0, sum of radii: 0.2 → no collision + ((0, 0, 0), (0, 0, 0), (1.0, 0, 0), (0, 0, 0), False, "parallel_far"), + # Test 3: Parallel capsules exactly touching + # Distance: 0.2, sum of radii: 0.2 → edge case, may or may not detect + ((0, 0, 0), (0, 0, 0), (0.19, 0, 0), (0, 0, 0), True, "parallel_touching"), + # Test 4: Perpendicular capsules at same position (definitely colliding) + # Axes intersect at center → should collide + ((0, 0, 0), (0, 0, 0), (0, 0, 0), (90, 0, 0), True, "perpendicular_center"), + # Test 5: Capsules offset diagonally (not touching) + # Distance ~0.42, sum of radii: 0.2 → no collision + ((0, 0, 0), (0, 0, 0), (0.3, 0.3, 0), (0, 0, 0), False, "diagonal_separated"), + ], +) +def test_capsule_capsule_vs_gjk(backend, pos1, euler1, pos2, euler2, should_collide, description): + """ + Compare analytical capsule-capsule collision with GJK. + """ + radius = 0.1 + half_length = 0.25 + + # Scene 1: Using analytical capsule-capsule detection + scene_analytical = gs.Scene( + show_viewer=False, + rigid_options=gs.options.RigidOptions( + dt=0.01, + gravity=(0, 0, 0), + use_gjk_collision=False, + ), + ) + + with tempfile.TemporaryDirectory() as tmpdir: + mjcf1 = create_capsule_mjcf("capsule1", pos1, euler1, radius, half_length) + mjcf1_path = os.path.join(tmpdir, "capsule1_analytical.xml") + ET.ElementTree(mjcf1).write(mjcf1_path) + scene_analytical.add_entity(gs.morphs.MJCF(file=mjcf1_path)) + + mjcf2 = create_capsule_mjcf("capsule2", pos2, euler2, radius, half_length) + mjcf2_path = os.path.join(tmpdir, "capsule2_analytical.xml") + ET.ElementTree(mjcf2).write(mjcf2_path) + scene_analytical.add_entity(gs.morphs.MJCF(file=mjcf2_path)) + + scene_analytical.build() + + # Scene 2: Force GJK for comparison (GJK is more accurate than MPR) + scene_gjk = gs.Scene( + show_viewer=False, + rigid_options=gs.options.RigidOptions( + dt=0.01, + gravity=(0, 0, 0), + use_gjk_collision=True, + ), + ) + + with tempfile.TemporaryDirectory() as tmpdir: + # Add same capsules to GJK scene + mjcf1 = create_capsule_mjcf("capsule1", pos1, euler1, radius, half_length) + mjcf1_path = os.path.join(tmpdir, "capsule1_gjk.xml") + ET.ElementTree(mjcf1).write(mjcf1_path) + scene_gjk.add_entity(gs.morphs.MJCF(file=mjcf1_path)) + + mjcf2 = create_capsule_mjcf("capsule2", pos2, euler2, radius, half_length) + mjcf2_path = os.path.join(tmpdir, "capsule2_gjk.xml") + ET.ElementTree(mjcf2).write(mjcf2_path) + scene_gjk.add_entity(gs.morphs.MJCF(file=mjcf2_path)) + + scene_gjk.build() + + scene_analytical.step() + scene_gjk.step() + + import gstaichi as ti + + if ( + hasattr(ti.lang._template_mapper.__builtins__, "__debug__") + and ti.lang._template_mapper.__builtins__["__debug__"] + ): + analytical_capsule_count = scene_analytical.rigid_solver.collider.collider_state.debug_analytical_capsule_count[ + 0 + ] + analytical_gjk_count = scene_analytical.rigid_solver.collider.collider_state.debug_gjk_count[0] + gjk_scene_capsule_count = scene_gjk.rigid_solver.collider.collider_state.debug_analytical_capsule_count[0] + gjk_scene_gjk_count = scene_gjk.rigid_solver.collider.collider_state.debug_gjk_count[0] + + # Scene 1 (analytical) should use analytical path, NOT GJK + assert analytical_capsule_count > 0, ( + f"Scene 1 should have used analytical capsule path (count={analytical_capsule_count})" + ) + assert analytical_gjk_count == 0, f"Scene 1 should NOT have used GJK path (count={analytical_gjk_count})" + + # Scene 2 (GJK) should use GJK path, NOT analytical + assert gjk_scene_gjk_count > 0, f"Scene 2 should have used GJK path (count={gjk_scene_gjk_count})" + assert gjk_scene_capsule_count == 0, ( + f"Scene 2 should NOT have used analytical capsule path (count={gjk_scene_capsule_count})" + ) + + contacts_analytical = scene_analytical.rigid_solver.collider.get_contacts(as_tensor=False) + contacts_gjk = scene_gjk.rigid_solver.collider.get_contacts(as_tensor=False) + + has_collision_analytical = contacts_analytical is not None and len(contacts_analytical["geom_a"]) > 0 + has_collision_gjk = contacts_gjk is not None and len(contacts_gjk["geom_a"]) > 0 + + assert has_collision_analytical == has_collision_gjk, ( + f"Collision detection mismatch! Analytical: {has_collision_analytical}, GJK: {has_collision_gjk}" + ) + + if has_collision_analytical and has_collision_gjk: + # Get first contact from each (may have multiple due to multi-contact) + pen_analytical = contacts_analytical["penetration"][0] + pen_gjk = contacts_gjk["penetration"][0] + + normal_analytical = np.array(contacts_analytical["normal"][0]) + normal_gjk = np.array(contacts_gjk["normal"][0]) + + pos_analytical = np.array(contacts_analytical["position"][0]) + pos_gjk = np.array(contacts_gjk["position"][0]) + + pen_tol = max(0.01, 0.1 * max(pen_analytical, pen_gjk)) + assert abs(pen_analytical - pen_gjk) < pen_tol, ( + f"Penetration mismatch! Analytical: {pen_analytical:.6f}, GJK: {pen_gjk:.6f}, diff: {abs(pen_analytical - pen_gjk):.6f}" + ) + + normal_agreement = abs(np.dot(normal_analytical, normal_gjk)) + assert normal_agreement > 0.95, ( + f"Normal direction mismatch! Analytical: {normal_analytical}, GJK: {normal_gjk}, agreement: {normal_agreement:.4f}" + ) + + pos_diff = np.linalg.norm(pos_analytical - pos_gjk) + assert pos_diff < 0.05, f"Contact position mismatch! Diff: {pos_diff:.6f}" + + +@pytest.mark.parametrize("backend", [gs.cpu]) +def test_capsule_analytical_accuracy(backend): + """ + Test that analytical capsule-capsule gives exact results for simple cases. + """ + # Simple test case: two vertical capsules offset horizontally + # Capsule 1: center at origin, radius=0.1, half_length=0.25 + # Capsule 2: center at (0.15, 0, 0), same size + # Line segments are both vertical, closest points are at centers + # Distance between segments: 0.15 + # Sum of radii: 0.2 + # Expected penetration: 0.2 - 0.15 = 0.05 + + scene = gs.Scene( + show_viewer=False, + rigid_options=gs.options.RigidOptions( + dt=0.01, + gravity=(0, 0, 0), + ), + ) + + with tempfile.TemporaryDirectory() as tmpdir: + mjcf1 = create_capsule_mjcf("capsule1", (0, 0, 0), (0, 0, 0), 0.1, 0.25) + mjcf1_path = os.path.join(tmpdir, "capsule1.xml") + ET.ElementTree(mjcf1).write(mjcf1_path) + scene.add_entity(gs.morphs.MJCF(file=mjcf1_path)) + + mjcf2 = create_capsule_mjcf("capsule2", (0.15, 0, 0), (0, 0, 0), 0.1, 0.25) + mjcf2_path = os.path.join(tmpdir, "capsule2.xml") + ET.ElementTree(mjcf2).write(mjcf2_path) + scene.add_entity(gs.morphs.MJCF(file=mjcf2_path)) + + scene.build() + + scene.step() + + contacts = scene.rigid_solver.collider.get_contacts(as_tensor=False) + + assert contacts is not None and len(contacts["geom_a"]) > 0 + + penetration = contacts["penetration"][0] + expected_pen = 0.05 + + assert abs(penetration - expected_pen) < 1e-5, ( + f"Analytical solution not exact! Expected: {expected_pen}, Got: {penetration:.6f}" + ) + + normal = np.array(contacts["normal"][0]) + + # Check normal is along X axis + assert abs(abs(normal[0]) - 1.0) < 1e-5, f"Normal should be along X axis, got {normal}" + assert abs(normal[1]) < 1e-5 and abs(normal[2]) < 1e-5, f"Normal should be along X axis, got {normal}"