From 5ac338c2b0c5ca143f40dcb363da109673d4c30b Mon Sep 17 00:00:00 2001 From: YilingQiao Date: Wed, 1 Jan 2025 19:00:02 -0500 Subject: [PATCH] modify mpr threshold --- genesis/engine/solvers/rigid/mpr_decomp.py | 25 ++++++++++++---------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/genesis/engine/solvers/rigid/mpr_decomp.py b/genesis/engine/solvers/rigid/mpr_decomp.py index 18c296a6..34072aa9 100644 --- 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