Merge pull request #434 from YilingQiao/yiling/250101_mpr_precision

[BUG FIX] modify mpr threshold
Genesis-Embodied-AI · Jan 2, 2025 · 3aaf87b · 3aaf87b
2 parents 356b18f + 5ac338c
commit 3aaf87b
Showing 1 changed file with 14 additions and 11 deletions.
diff --git a/genesis/engine/solvers/rigid/mpr_decomp.py b/genesis/engine/solvers/rigid/mpr_decomp.py
@@ -16,6 +16,11 @@ def __init__(self, rigid_solver):
         self._B = rigid_solver._B
         self._para_level = rigid_solver._para_level
 
+        if gs.ti_float == ti.f32:
+            self.CCD_EPS = 1e-6
+        else:
+            self.CCD_EPS = 1e-10
+
         self.support_field = SupportField(rigid_solver)
         self.init_support()
 
@@ -150,7 +155,6 @@ def mpr_portal_dir(self, i_ga, i_gb, i_b):
     def mpr_portal_encapsules_origin(self, direction, i_ga, i_gb, i_b):
         dot = direction.dot(self.simplex_support[i_ga, i_gb, 1, i_b].v)
 
-        CCD_EPS = 1e-5
         return dot > 0.0
         # return dot > 0 or ti.abs(dot) < CCD_EPS
 
@@ -418,7 +422,6 @@ def mpr_expand_portal(self, v, v1, v2, i_ga, i_gb, i_b):
 
     @ti.func
     def mpr_discover_portal(self, i_ga, i_gb, i_b):
-        CCD_EPS = 1e-5
         ret = 0
         self.simplex_size[i_ga, i_gb, i_b] = 0
 
@@ -436,8 +439,8 @@ def mpr_discover_portal(self, i_ga, i_gb, i_b):
         self.simplex_support[i_ga, i_gb, 0, i_b].v = center_a - center_b
         self.simplex_size[i_ga, i_gb, i_b] = 1
 
-        if self.simplex_support[i_ga, i_gb, 0, i_b].v.norm() < CCD_EPS:
-            self.simplex_support[i_ga, i_gb, 0, i_b].v += gs.ti_vec3([10.0 * CCD_EPS, 0.0, 0.0])
+        if self.simplex_support[i_ga, i_gb, 0, i_b].v.norm() < self.CCD_EPS:
+            self.simplex_support[i_ga, i_gb, 0, i_b].v += gs.ti_vec3([10.0 * self.CCD_EPS, 0.0, 0.0])
 
         direction = (self.simplex_support[i_ga, i_gb, 0, i_b].v * -1).normalized()
 
@@ -451,13 +454,13 @@ def mpr_discover_portal(self, i_ga, i_gb, i_b):
         dot = v.dot(direction)
 
         # if dot < 0 or ti.abs(dot) < CCD_EPS:
-        if dot < CCD_EPS:
+        if dot < self.CCD_EPS:
             ret = -1
         else:
             direction = self.simplex_support[i_ga, i_gb, 0, i_b].v.cross(self.simplex_support[i_ga, i_gb, 1, i_b].v)
-            if direction.norm() < CCD_EPS:
+            if direction.norm() < self.CCD_EPS:
                 # if portal.points[1].v == ccd_vec3_origin:
-                if self.simplex_support[i_ga, i_gb, 1, i_b].v.norm() < CCD_EPS:
+                if self.simplex_support[i_ga, i_gb, 1, i_b].v.norm() < self.CCD_EPS:
                     ret = 1
                 else:
                     ret = 2
@@ -466,7 +469,7 @@ def mpr_discover_portal(self, i_ga, i_gb, i_b):
                 v, v1, v2 = self.compute_support(direction, i_ga, i_gb, i_b)
                 dot = v.dot(direction)
                 # if dot < 0 or ti.abs(dot) < CCD_EPS:
-                if dot < CCD_EPS:
+                if dot < self.CCD_EPS:
                     ret = -1
                 else:
                     self.simplex_support[i_ga, i_gb, 2, i_b].v1 = v1
@@ -488,7 +491,7 @@ def mpr_discover_portal(self, i_ga, i_gb, i_b):
                         v, v1, v2 = self.compute_support(direction, i_ga, i_gb, i_b)
                         dot = v.dot(direction)
                         # if dot < 0 or ti.abs(dot) < CCD_EPS:
-                        if dot < CCD_EPS:
+                        if dot < self.CCD_EPS:
                             ret = -1
                             break
 
@@ -497,7 +500,7 @@ def mpr_discover_portal(self, i_ga, i_gb, i_b):
                         va = self.simplex_support[i_ga, i_gb, 1, i_b].v.cross(v)
                         dot = va.dot(self.simplex_support[i_ga, i_gb, 0, i_b].v)
                         # if dot < 0 or ti.abs(dot) < CCD_EPS:
-                        if dot < -CCD_EPS:
+                        if dot < -self.CCD_EPS:
                             self.simplex_support[i_ga, i_gb, 2, i_b].v1 = v1
                             self.simplex_support[i_ga, i_gb, 2, i_b].v2 = v2
                             self.simplex_support[i_ga, i_gb, 2, i_b].v = v
@@ -507,7 +510,7 @@ def mpr_discover_portal(self, i_ga, i_gb, i_b):
                             va = v.cross(self.simplex_support[i_ga, i_gb, 2, i_b].v)
                             dot = va.dot(self.simplex_support[i_ga, i_gb, 0, i_b].v)
                             # if dot < 0 or ti.abs(dot) < CCD_EPS:
-                            if dot < -CCD_EPS:
+                            if dot < -self.CCD_EPS:
                                 self.simplex_support[i_ga, i_gb, 1, i_b].v1 = v1
                                 self.simplex_support[i_ga, i_gb, 1, i_b].v2 = v2
                                 self.simplex_support[i_ga, i_gb, 1, i_b].v = v