From 942f4bd55237dc122b67d02a561eeb6ee2434654 Mon Sep 17 00:00:00 2001
From: chien-an-chen <chienach@amd.com>
Date: Wed, 29 Apr 2026 06:30:06 -0700
Subject: [PATCH 1/2] restore code in d543fe1b55. restore Clearing collision
 normal cache.

---
 .../solvers/rigid/collider/broadphase.py      | 36 +++++++++++++++++++
 1 file changed, 36 insertions(+)

diff --git a/genesis/engine/solvers/rigid/collider/broadphase.py b/genesis/engine/solvers/rigid/collider/broadphase.py
index 1487771faa..55aa6a6da9 100644
--- a/genesis/engine/solvers/rigid/collider/broadphase.py
+++ b/genesis/engine/solvers/rigid/collider/broadphase.py
@@ -305,6 +305,25 @@ def func_broad_phase_lds(
 
                         if (min_a0 > max_b0 or min_a1 > max_b1 or min_a2 > max_b2 or
                             max_a0 < min_b0 or max_a1 < min_b1 or max_a2 < min_b2):
+
+                            # Clear collision normal cache if not in contact
+                            if qd.static(not static_rigid_sim_config.enable_mujoco_compatibility):
+                                if func_check_collision_valid(
+                                    i_ga_c,
+                                    i_gb_c,
+                                    i_b,
+                                    links_state,
+                                    links_info,
+                                    geoms_info,
+                                    rigid_global_info,
+                                    static_rigid_sim_config,
+                                    constraint_state,
+                                    equalities_info,
+                                    collider_info,
+                                ):
+                                    i_pair = collider_info.collision_pair_idx[i_ga, i_gb]
+                                    collider_state.contact_cache.normal[i_pair, i_b] = qd.Vector.zero(gs.qd_float, 3)
+
                             continue
 
                         if not func_check_collision_valid(
@@ -611,6 +630,23 @@ def func_broad_phase_global_mem(
                         if not (min_a0 <= max_b0 and max_a0 >= min_b0 and
                                 min_a1 <= max_b1 and max_a1 >= min_b1 and
                                 min_a2 <= max_b2 and max_a2 >= min_b2):
+                            # Clear collision normal cache if not in contact
+                            if qd.static(not static_rigid_sim_config.enable_mujoco_compatibility):
+                                if func_check_collision_valid(
+                                    i_ga_c,
+                                    i_gb_c,
+                                    i_b,
+                                    links_state,
+                                    links_info,
+                                    geoms_info,
+                                    rigid_global_info,
+                                    static_rigid_sim_config,
+                                    constraint_state,
+                                    equalities_info,
+                                    collider_info,
+                                ):
+                                    i_pair = collider_info.collision_pair_idx[i_ga, i_gb]
+                                    collider_state.contact_cache.normal[i_pair, i_b] = qd.Vector.zero(gs.qd_float, 3)
                             continue
 
                         if not func_check_collision_valid(

From fd421e6f594f285de851109ed14a86df6ec3f42f Mon Sep 17 00:00:00 2001
From: chien-an-chen <chienach@amd.com>
Date: Wed, 29 Apr 2026 07:32:00 -0700
Subject: [PATCH 2/2] fix error in clearing collision normal cache

---
 genesis/engine/solvers/rigid/collider/broadphase.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/genesis/engine/solvers/rigid/collider/broadphase.py b/genesis/engine/solvers/rigid/collider/broadphase.py
index 55aa6a6da9..95c51a39a0 100644
--- a/genesis/engine/solvers/rigid/collider/broadphase.py
+++ b/genesis/engine/solvers/rigid/collider/broadphase.py
@@ -321,7 +321,7 @@ def func_broad_phase_lds(
                                     equalities_info,
                                     collider_info,
                                 ):
-                                    i_pair = collider_info.collision_pair_idx[i_ga, i_gb]
+                                    i_pair = collider_info.collision_pair_idx[i_ga_c, i_gb_c]
                                     collider_state.contact_cache.normal[i_pair, i_b] = qd.Vector.zero(gs.qd_float, 3)
 
                             continue
@@ -645,7 +645,7 @@ def func_broad_phase_global_mem(
                                     equalities_info,
                                     collider_info,
                                 ):
-                                    i_pair = collider_info.collision_pair_idx[i_ga, i_gb]
+                                    i_pair = collider_info.collision_pair_idx[i_ga_c, i_gb_c]
                                     collider_state.contact_cache.normal[i_pair, i_b] = qd.Vector.zero(gs.qd_float, 3)
                             continue