Some renames

Author: gasnica

genesis/engine/solvers/rigid/constraint_solver_decomp_island.py

@@ -8,7 +8,7 @@
 
 import genesis.utils.array_class as array_class
 
-from .rigid_solver_decomp_util import func_wakeup_entity_and_its_island
+from .rigid_solver_decomp_util import func_wakeup_entity_and_its_temp_island
 from .contact_island import ContactIsland
 from genesis.engine.solvers.rigid.rigid_debug import Debug
 
@@ -127,7 +127,7 @@ def resolve(self):
                 if ti.static(self._solver._use_hibernation):
                     is_active = not self.contact_island.island_hibernated[i_island, i_b]
                 if is_active:
-                    self.add_collision_constraints(i_island, i_b)
+                    self.add_collision_constraints__and_wakeup_entities(i_island, i_b)
                     self.add_joint_limit_constraints(i_island, i_b)
                     self._func_init_solver(i_island, i_b)
                     self._func_solve(i_island, i_b)
@@ -139,7 +139,7 @@ def handle_constraints(self):
         self.resolve()
 
     @ti.func
-    def add_collision_constraints(self, i_island: int, i_b: int):
+    def add_collision_constraints__and_wakeup_entities(self, i_island: int, i_b: int):
         self.n_constraints[i_b] = 0
         for i_island_col in range(self.contact_island.island_col[i_island, i_b].n):
             i_col_ = self.contact_island.island_col[i_island, i_b].start + i_island_col
@@ -233,9 +233,9 @@ def add_collision_constraints(self, i_island: int, i_b: int):
                     # wake up entities
                     any_hibernated_entity_idx = entity_idx_a if is_entity_a_hibernated else entity_idx_b
                     temp_island_idx = self.contact_island.entity_island[any_hibernated_entity_idx, i_b]
-                    Debug.assertf(0x0ad00004, temp_island_idx == i_island)  # Temp island indices don't match
+                    Debug.assertf(0x7ad00004, temp_island_idx == i_island)  # Temp island indices don't match
 
-                    func_wakeup_entity_and_its_island(
+                    func_wakeup_entity_and_its_temp_island(
                         any_hibernated_entity_idx,
                         i_b,
                         self._solver.entities_state,

genesis/engine/solvers/rigid/contact_island.py

@@ -10,6 +10,7 @@
     from genesis.engine.solvers.rigid.collider_decomp import Collider
     from genesis.engine.solvers.rigid.rigid_solver_decomp import RigidSolver
 
+INVALID_NEXT_HIBERNATED_ENTITY_IDX = -1
 INVALID_HIBERNATED_ISLAND_ID = -1  # -1 is reserved for "no island", i.e. active or fixed entities
 FIRST_HIBERNATED_ISLAND_ID = 1  # start at 1, leave 0 unused, use -1 for "no island"
 
@@ -87,6 +88,7 @@ def __init__(self, collider: "Collider"):
 
         # Used to make islands persist through hibernation:
         self.unused__entity_idx_to_next_entity_idx_in_hibernated_island = ti.field(dtype=gs.ti_int, shape=self.solver._batch_shape(self.solver.n_entities))
+        self.unused__entity_idx_to_next_entity_idx_in_hibernated_island.fill(INVALID_NEXT_HIBERNATED_ENTITY_IDX)
 
         # Warning: do not mistake island_id for island_idx
         self.next_hibernated_island_id = ti.field(dtype=gs.ti_int, shape=self.solver._B)
@@ -138,9 +140,10 @@ def add_all_contact_edges(self):
 
     @ti.kernel
     def temp_hack__add_all_hibernated_island_edges(self):
+        _B = self.solver._B
         n_entities = self.solver.n_entities
         ti.loop_config(serialize=self.solver._para_level < gs.PARA_LEVEL.ALL)
-        for i_b in range(self.solver._B):
+        for i_b in range(_B):
             for i_e in range(n_entities):
                 for i_e2 in range(i_e + 1, n_entities):
                     if self.hibernated_entity_idx_to_hibernated_island_id[i_e, i_b] != INVALID_HIBERNATED_ISLAND_ID \
@@ -155,10 +158,10 @@ def construct(self):
         self.temp_hack__add_all_hibernated_island_edges()
         self.preprocess_island__map_entities_to_edges()
         self.construct_islands()
-        self.postprocess_island__assign_collisions_to_temp_islands()
+        self.postprocess_island__assign_contact_data_to_temp_islands()
 
     @ti.kernel
-    def postprocess_island__assign_collisions_to_temp_islands(self):
+    def postprocess_island__assign_contact_data_to_temp_islands(self):
         ti.loop_config(serialize=self.solver._para_level < gs.PARA_LEVEL.ALL)
         for i_b in range(self.solver._B):
             for i_col in range(self.collider._collider_state.n_contacts[i_b]):

genesis/engine/solvers/rigid/rigid_solver_decomp.py

@@ -24,7 +24,7 @@
 from .collider_decomp import Collider
 from .constraint_solver_decomp import ConstraintSolver
 from .constraint_solver_decomp_island import ConstraintSolverIsland
-from .rigid_solver_decomp_util import func_wakeup_entity_and_its_island
+from .rigid_solver_decomp_util import func_wakeup_entity_and_its_temp_island
 from ....utils.sdf_decomp import SDF
 
 if TYPE_CHECKING:
@@ -4039,7 +4039,7 @@ def kernel_step_2(
     )
 
     if ti.static(static_rigid_sim_config.use_hibernation):
-        func_mark_all_entities_for_hibernation_or_update_aabb_sort_buffer(
+        func_check_all_awake_islands_for_hibernation_or_update_aabb_sort_buffer(
             dofs_state=dofs_state,
             entities_state=entities_state,
             entities_info=entities_info,
@@ -4920,7 +4920,7 @@ def kernel_update_vgeoms(
 
 
 @ti.func
-def func_mark_all_entities_for_hibernation_or_update_aabb_sort_buffer(
+def func_check_all_awake_islands_for_hibernation_or_update_aabb_sort_buffer(
     dofs_state: array_class.DofsState,
     entities_state: array_class.EntitiesState,
     entities_info: array_class.EntitiesInfo,
@@ -4956,7 +4956,7 @@ def func_mark_all_entities_for_hibernation_or_update_aabb_sort_buffer(
                     entity_idx = ci.entity_id[entity_ref, i_b]
 
                     is_entity_fixed = entities_info.n_dofs[entity_idx] == 0
-                    Debug.assertf(0x0ad00005, not is_entity_fixed)  # Fixed entity should not belong to an island
+                    Debug.assertf(0x7ad00005, not is_entity_fixed)  # Fixed entity should not belong to an island
 
                     # we can ignore entitiy_hibernated flag -> cos it implies dofs_state.vel/acc are zero
                     is_entity_hibernated = entities_state.hibernated[entity_idx, i_b]
@@ -5006,8 +5006,8 @@ def func_mark_all_entities_for_hibernation_or_update_aabb_sort_buffer(
 
                         # store entities in the hibernated islands by daisy chaining them
                         ci.unused__entity_idx_to_next_entity_idx_in_hibernated_island[prev_entity_idx, i_b] = entity_idx
-
                         prev_entity_idx = entity_idx
+
                         ci.hibernated_entity_idx_to_hibernated_island_id[entity_idx, i_b] = hibernated_island_id
 
 
@@ -5280,7 +5280,7 @@ def func_torque_and_passive_force(
                         wakeup = True
 
         if ti.static(static_rigid_sim_config.use_hibernation) and entities_state.hibernated[i_e, i_b] and wakeup:
-            func_wakeup_entity_and_its_island(i_e, i_b, entities_state, entities_info, dofs_state, links_state, geoms_state, rigid_global_info, contact_island)
+            func_wakeup_entity_and_its_temp_island(i_e, i_b, entities_state, entities_info, dofs_state, links_state, geoms_state, rigid_global_info, contact_island)
 
     if ti.static(static_rigid_sim_config.use_hibernation):
         ti.loop_config(serialize=static_rigid_sim_config.para_level < gs.PARA_LEVEL.ALL)

genesis/engine/solvers/rigid/rigid_solver_decomp_util.py

@@ -9,7 +9,7 @@
 
 
 @ti.func
-def func_wakeup_entity_and_its_island(
+def func_wakeup_entity_and_its_temp_island(
     i_e,
     i_b,
     entities_state: array_class.EntitiesState,

genesis/engine/solvers/rigid/rigid_validate.py

@@ -23,7 +23,7 @@ def validate_entity_hibernation_state_for_all_entities_in_temp_island(
         all_okay = all_okay and entities_state.hibernated[entity_idx, i_b] == expected_hibernation_state
         if entities_state.hibernated[entity_idx, i_b] != expected_hibernation_state:
             error_count += 1
-    Debug.assertf(0x0ad0000a, all_okay)  # Entity expected to be matching the expected hibernation state
+    Debug.assertf(0x7ad0000a, all_okay)  # Entity expected to be matching the expected hibernation state
     if not all_okay:
         print(f"Entity hibernation state mismatch for temp island {temp_island_idx} of {entity_ref_range.n} entities, expected: {expected_hibernation_state}, error_count: {error_count}")
 
@@ -47,4 +47,4 @@ def validate_temp_island_contains_both_hibernated_and_awake_entities(
         all_awake = ti.u1(all_awake and not is_entity_hibernated)
         all_hibernated = ti.u1(all_hibernated and is_entity_hibernated)
 
-    Debug.assertf(0x0ad00009, not (all_hibernated or all_awake))  # Island being woken up is expected to contain both hibernated and awake entities
+    Debug.assertf(0x7ad00009, not (all_hibernated or all_awake))  # Island being woken up is expected to contain both hibernated and awake entities