1313from genesis .utils .misc import ti_field_to_torch
1414
1515from .mpr_decomp import MPR
16+ from .gjk_decomp import GJK
17+
18+ from enum import IntEnum
1619
1720if TYPE_CHECKING :
1821 from genesis .engine .solvers .rigid .rigid_solver_decomp import RigidSolver
1922
2023
24+ class CCD_ALGORITHM_CODE (IntEnum ):
25+ MPR = 0
26+ MPR_SDF = 1
27+ GJK = 2
28+
29+
2130@ti .func
2231def rotaxis (vecin , i0 , i1 , i2 , f0 , f1 , f2 ):
2332 vecres = ti .Vector ([0.0 , 0.0 , 0.0 ], dt = gs .ti_float )
@@ -42,7 +51,18 @@ def __init__(self, rigid_solver: "RigidSolver"):
4251 self ._solver = rigid_solver
4352 self ._init_verts_connectivity ()
4453 self ._init_collision_fields ()
54+
55+ # Identify the convex collision detection (ccd) algorithm
56+ if self ._solver ._options .use_gjk_collision :
57+ self .ccd_algorithm = CCD_ALGORITHM_CODE .GJK
58+ elif self ._solver ._enable_mujoco_compatibility :
59+ self .ccd_algorithm = CCD_ALGORITHM_CODE .MPR
60+ else :
61+ self .ccd_algorithm = CCD_ALGORITHM_CODE .MPR_SDF
62+
63+ # FIXME: MPR is necessary because it is used for terrain collision detection
4564 self ._mpr = MPR (rigid_solver )
65+ self ._gjk = GJK (rigid_solver ) if self .ccd_algorithm == CCD_ALGORITHM_CODE .GJK else None
4666
4767 # multi contact perturbation and tolerance
4868 if self ._solver ._enable_mujoco_compatibility :
@@ -879,7 +899,7 @@ def _func_narrow_phase_convex_vs_convex(self):
879899 and self ._solver .geoms_info [i_gb ].type == gs .GEOM_TYPE .BOX
880900 )
881901 ):
882- self ._func_mpr (i_ga , i_gb , i_b )
902+ self ._func_convex_convex_contact (i_ga , i_gb , i_b )
883903
884904 @ti .kernel
885905 def _func_narrow_phase_convex_specializations (self ):
@@ -897,7 +917,9 @@ def _func_narrow_phase_convex_specializations(self):
897917 and self ._solver .geoms_info [i_gb ].type == gs .GEOM_TYPE .BOX
898918 ):
899919 if ti .static (sys .platform == "darwin" ):
900- self ._func_mpr (i_ga , i_gb , i_b )
920+ # FIXME: It seems redundant, why don't we just call _func_plane_box_contact directly?
921+ # Anyway in this function, we will call _func_plane_box_contact.
922+ self ._func_convex_convex_contact (i_ga , i_gb , i_b )
901923 else :
902924 self ._func_plane_box_contact (i_ga , i_gb , i_b )
903925
@@ -1057,7 +1079,7 @@ def _func_plane_box_contact(self, i_ga, i_gb, i_b):
10571079 plane_dir = gu .ti_transform_by_quat (plane_dir , ga_state .quat )
10581080 normal = - plane_dir .normalized ()
10591081
1060- v1 , _ = self ._mpr .support_box (normal , i_gb , i_b )
1082+ v1 , _ = self ._mpr .support_field . _func_support_box (normal , i_gb , i_b )
10611083 penetration = normal .dot (v1 - ga_state .pos )
10621084
10631085 if penetration > 0.0 :
@@ -1173,7 +1195,7 @@ def _func_contact_orthogonals(self, i_ga, i_gb, normal, i_b):
11731195 return axis_0 , axis_1
11741196
11751197 @ti .func
1176- def _func_mpr (self , i_ga , i_gb , i_b ):
1198+ def _func_convex_convex_contact (self , i_ga , i_gb , i_b ):
11771199 if self ._solver .geoms_info [i_ga ].type > self ._solver .geoms_info [i_gb ].type :
11781200 i_ga , i_gb = i_gb , i_ga
11791201
@@ -1242,41 +1264,68 @@ def _func_mpr(self, i_ga, i_gb, i_b):
12421264 contact_pos = v1 - 0.5 * penetration * normal
12431265 is_col = penetration > 0
12441266 else :
1245- # Try using MPR before anything else
1246- is_mpr_updated = False
1247- is_mpr_guess_direction_available = True
1248- normal_ws = self .contact_cache [i_ga , i_gb , i_b ].normal
1249- for i_mpr in range (2 ):
1250- if i_mpr == 1 :
1251- # Try without warm-start if no contact was detected using it.
1252- # When penetration depth is very shallow, MPR may wrongly classify two geometries as not in
1253- # contact while they actually are. This helps to improve contact persistence without increasing
1254- # much the overall computational cost since the fallback should not be triggered very often.
1255- is_mpr_guess_direction_available = (ti .abs (normal_ws ) > gs .EPS ).any ()
1256- if (i_detection == 0 ) and not is_col and is_mpr_guess_direction_available :
1257- normal_ws = ti .Vector .zero (gs .ti_float , 3 )
1258- is_mpr_updated = False
1259-
1260- if not is_mpr_updated :
1261- is_col , normal , penetration , contact_pos = self ._mpr .func_mpr_contact (
1262- i_ga , i_gb , i_b , normal_ws
1263- )
1264- is_mpr_updated = True
1265-
1266- # Fallback on SDF if collision is detected by MPR but no collision direction was cached but the
1267- # initial penetration is already quite large, because the contact information provided by MPR
1268- # may be unreliable in such a case.
1269- # Here it is assumed that generic SDF is much slower than MPR, so it is faster in average
1270- # to first make sure that the geometries are truly colliding and only after to run SDF if
1271- # necessary. This would probably not be the case anymore if it was possible to rely on
1272- # specialized SDF implementation for convex-convex collision detection in the first place.
1273- if is_col and penetration > tolerance and not is_mpr_guess_direction_available :
1274- # Note that SDF may detect different collision points depending on geometry ordering.
1275- # Because of this, it is necessary to run it twice and take the contact information
1276- # associated with the point of deepest penetration.
1277- try_sdf = True
1278-
1279- if ti .static (not self ._solver ._enable_mujoco_compatibility ):
1267+ ### MPR, MPR + SDF
1268+ if ti .static (self .ccd_algorithm != CCD_ALGORITHM_CODE .GJK ):
1269+ # Try using MPR before anything else
1270+ is_mpr_updated = False
1271+ is_mpr_guess_direction_available = True
1272+ normal_ws = self .contact_cache [i_ga , i_gb , i_b ].normal
1273+ for i_mpr in range (2 ):
1274+ if i_mpr == 1 :
1275+ # Try without warm-start if no contact was detected using it.
1276+ # When penetration depth is very shallow, MPR may wrongly classify two geometries as not in
1277+ # contact while they actually are. This helps to improve contact persistence without increasing
1278+ # much the overall computational cost since the fallback should not be triggered very often.
1279+ is_mpr_guess_direction_available = (ti .abs (normal_ws ) > gs .EPS ).any ()
1280+ if (i_detection == 0 ) and not is_col and is_mpr_guess_direction_available :
1281+ normal_ws = ti .Vector .zero (gs .ti_float , 3 )
1282+ is_mpr_updated = False
1283+
1284+ if not is_mpr_updated :
1285+ is_col , normal , penetration , contact_pos = self ._mpr .func_mpr_contact (
1286+ i_ga , i_gb , i_b , normal_ws
1287+ )
1288+ is_mpr_updated = True
1289+
1290+ # Fallback on SDF if collision is detected by MPR but no collision direction was cached but the
1291+ # initial penetration is already quite large, because the contact information provided by MPR
1292+ # may be unreliable in such a case.
1293+ # Here it is assumed that generic SDF is much slower than MPR, so it is faster in average
1294+ # to first make sure that the geometries are truly colliding and only after to run SDF if
1295+ # necessary. This would probably not be the case anymore if it was possible to rely on
1296+ # specialized SDF implementation for convex-convex collision detection in the first place.
1297+ if is_col and penetration > tolerance and not is_mpr_guess_direction_available :
1298+ # Note that SDF may detect different collision points depending on geometry ordering.
1299+ # Because of this, it is necessary to run it twice and take the contact information
1300+ # associated with the point of deepest penetration.
1301+ try_sdf = True
1302+ ### GJK
1303+ elif ti .static (self .ccd_algorithm == CCD_ALGORITHM_CODE .GJK ):
1304+ # If it was not the first detection, only detect single contact point.
1305+ self ._gjk .func_gjk_contact (i_ga , i_gb , i_b , i_detection == 0 )
1306+
1307+ is_col = self ._gjk .is_col [i_b ] == 1
1308+ penetration = self ._gjk .penetration [i_b ]
1309+ n_contacts = self ._gjk .n_contacts [i_b ]
1310+
1311+ if is_col :
1312+ if self ._gjk .multi_contact_flag [i_b ]:
1313+ # Used MuJoCo's multi-contact algorithm to find multiple contact points. Therefore,
1314+ # add the discovered contact points and stop multi-contact search.
1315+ for i_c in range (n_contacts ):
1316+ if i_c >= self ._n_contacts_per_pair :
1317+ # Ignore contact points if the number of contacts exceeds the limit.
1318+ break
1319+ contact_pos = self ._gjk .contact_pos [i_b , i_c ]
1320+ normal = self ._gjk .normal [i_b , i_c ]
1321+ self ._func_add_contact (i_ga , i_gb , normal , contact_pos , penetration , i_b )
1322+
1323+ break
1324+ else :
1325+ contact_pos = self ._gjk .contact_pos [i_b , 0 ]
1326+ normal = self ._gjk .normal [i_b , 0 ]
1327+
1328+ if ti .static (self .ccd_algorithm == CCD_ALGORITHM_CODE .MPR_SDF ):
12801329 if try_sdf :
12811330 is_col_a = False
12821331 is_col_b = False
@@ -1350,7 +1399,7 @@ def _func_mpr(self, i_ga, i_gb, i_b):
13501399 self .contact_cache [i_ga , i_gb , i_b ].normal .fill (0.0 )
13511400
13521401 elif multi_contact and is_col_0 > 0 and is_col > 0 :
1353- if ti .static (not self ._solver . _enable_mujoco_compatibility ):
1402+ if ti .static (self .ccd_algorithm == CCD_ALGORITHM_CODE . MPR_SDF ):
13541403 # 1. Project the contact point on both geometries
13551404 # 2. Revert the effect of small rotation
13561405 # 3. Update contact point
@@ -1387,6 +1436,11 @@ def _func_mpr(self, i_ga, i_gb, i_b):
13871436 # dynamics since zero-penetration contact points should not induce any force.
13881437 penetration = normal .dot (contact_point_b - contact_point_a )
13891438
1439+ elif ti .static (self .ccd_algorithm == CCD_ALGORITHM_CODE .GJK ):
1440+ # Only change penetration to the initial one, because the normal vector could change abruptly
1441+ # under GJK-EPA as the nearest simplex is determined by discrete logic, unlike MPR.
1442+ penetration = penetration_0
1443+
13901444 # Discard contact point is repeated
13911445 repeated = False
13921446 for i_con in range (n_con ):
0 commit comments