From 87b8bd83a543ed5e63e5055ef749b0cc0ed97016 Mon Sep 17 00:00:00 2001
From: qbp758 <qbp758@futharkhpa03fl.unicph.domain>
Date: Wed, 4 Oct 2023 09:07:53 +0200
Subject: [PATCH 1/7] create aabb and spatial hashing class

---
 python/rainbow/geometry/aabb.py            |  62 ++++++++
 python/rainbow/geometry/spatial_hashing.py | 174 +++++++++++++++++++++
 2 files changed, 236 insertions(+)
 create mode 100644 python/rainbow/geometry/aabb.py
 create mode 100644 python/rainbow/geometry/spatial_hashing.py

diff --git a/python/rainbow/geometry/aabb.py b/python/rainbow/geometry/aabb.py
new file mode 100644
index 0000000..221d28f
--- /dev/null
+++ b/python/rainbow/geometry/aabb.py
@@ -0,0 +1,62 @@
+import numpy as np
+from typing import List
+
+class AABB:
+    """
+    Axis-Aligned Bounding Box (AABB) for 3D spatial objects.
+
+    An AABB is a box that encloses a 3D object, aligned with the coordinate axes. 
+    It is defined by two points: the minimum point (`min_point`) and the maximum point (`max_point`),
+    which represent opposite corners of the box.
+
+    Attributes:
+        min_point (List[float]): The smallest x, y, z coordinates from the bounding box.
+        max_point (List[float]): The largest x, y, z coordinates from the bounding box.
+
+    Methods:
+        create_from_vertices(vertices: List[List[float]]) -> 'AABB'
+            Class method to create an AABB instance from a list of vertices.
+
+        is_overlap(aabb1: 'AABB', aabb2: 'AABB') -> bool
+            Class method to determine if two AABB instances overlap.
+
+    Example:
+        >>> aabb1 = AABB([0, 0, 0], [1, 1, 1])
+        >>> vertices = [[0, 0, 0], [1, 1, 1], [1, 0, 0]]
+        >>> aabb2 = AABB.create_from_vertices(vertices)
+        >>> AABB.is_overlap(aabb1, aabb2)
+        True
+    """
+    
+    def __init__(self, min_point: List[float], max_point: List[float]) -> None:
+        self.min_point = min_point
+        self.max_point = max_point
+        
+    @classmethod
+    def create_from_vertices(cls, vertices: List[List[float]]) -> 'AABB':
+        """ Create AABB instance from vertices, such as triangle vertices
+
+        Args:
+            vertices (List[List[float]]): A list of vertices, each vertex is a list of 3 elements
+
+        Returns:
+            AABB: a new AABB instance
+        """
+        max_point = np.max(vertices, axis=0)
+        min_point = np.min(vertices, axis=0)
+        return cls(min_point, max_point)
+    
+    @classmethod
+    def is_overlap(cls, aabb1: 'AABB', aabb2: 'AABB') -> bool:
+        """ Test two aabb instance are overlap or not
+
+        Args:
+            aabb1 (AABB): _description_
+            aabb2 (AABB): _description_
+
+        Returns:
+            bool: Return True if both of aabb instances are overlap, otherwise return False
+        """
+        return not (aabb1.max_point[0] < aabb2.min_point[0] or aabb1.min_point[0] > aabb2.max_point[0] or
+                    aabb1.max_point[1] < aabb2.min_point[1] or aabb1.min_point[1] > aabb2.max_point[1] or
+                    aabb1.max_point[2] < aabb2.min_point[2] or aabb1.min_point[2] > aabb2.max_point[2])
\ No newline at end of file
diff --git a/python/rainbow/geometry/spatial_hashing.py b/python/rainbow/geometry/spatial_hashing.py
new file mode 100644
index 0000000..9f99cb4
--- /dev/null
+++ b/python/rainbow/geometry/spatial_hashing.py
@@ -0,0 +1,174 @@
+import numpy as np
+from typing import Any, List, Tuple
+from rainbow.geometry.aabb import AABB
+
+
+class HashCell:
+    """
+    A class representing a hash cell in spatial hashing for quick lookup and 
+    managing spatial-related objects, such as triangles in a 3D mesh.
+
+    The `HashCell` class allows for efficient management of objects (such as 
+    triangles in a mesh) within a spatial hashing grid. It uses a "lazy clear" 
+    mechanism, resetting the cell only when a new object is added with a more 
+    recent timestamp, to optimize object management in dynamic simulations.
+    
+    Attributes:
+        time_stamp (int): A marker representing the last moment when the 
+            cell was accessed or modified.
+        size (int): The number of objects currently stored in the cell.
+        object_list (List[Any]): A list holding the objects stored in the cell.
+
+    Methods:
+        add(object: Any, time_stamp: int)
+            Adds an object to the cell and updates the time stamp, 
+            performing a lazy clear if needed.
+
+    Example:
+        >>> cell = HashCell()
+        >>> cell.add(("triangle", "body_name", "aabb"), 1)
+        >>> cell.size
+        1
+    
+    Note:
+        The objects stored can be of any type (`Any`), but for applications 
+        like collision detection, it is recommended to store relevant spatial 
+        data, such as a tuple containing (triangle index, body name, triangle AABB).
+    """
+    def __init__(self, time_stamp: int=0) -> None:
+        self.time_stamp = time_stamp
+        self.size = 0
+        self.object_list = []
+
+    def add(self, object: Any, time_stamp: int):
+        """ Add an object to the cell
+
+        Args:
+            object (Any): This object can be a triangle index, a body name, or a triangle AABB, it depends on the context. In the context of collision detection, it is a tuple of (triangle index, body name, triangle AABB)
+            time_stamp (int): The time stamp of the simulation program
+        """
+        # Lazy Clear: If the time stamp is older than the current time stamp, reset the cell
+        if self.time_stamp < time_stamp:
+            self.time_stamp = time_stamp
+            self.size = 0
+            self.object_list = []
+        
+        self.object_list.append(object)
+        self.size += 1
+
+
+class HashGird:
+    """
+    A class representing a 3D spatial hash grid for efficient spatial 
+    querying and management of objects, such as triangles in a 3D mesh.
+
+    The `HashGrid` uses a hash function to map spatial cells into a 1D 
+    hash table, which allows for an efficient query of neighboring objects 
+    in a spatial domain, commonly used in collision detection and other 
+    physical simulations.
+
+    Attributes:
+        hash_table_size (int): Size of the hash table, dictating how many 
+            possible hashed keys/values pairs it can manage.
+        hash_table (dict): Dictionary acting as the hash table, 
+            storing objects in the spatial grid.
+        cell_size (np.array): 1D numpy array containing the 3D dimensions 
+            of a cell in the grid (x, y, z).
+
+    Methods:
+        set_hash_table_size(hash_table_size: int, update: bool = True)
+            Sets the size of the hash table and optionally updates the 
+            current hash table size.
+        set_cell_size(cell_size_x: float, cell_size_y: float, cell_size_z: float)
+            Sets the 3D dimensions of a cell in the grid.
+        get_hash_value(i: int, j: int, k: int) -> int
+            Computes and returns the hash value for a spatial cell given 
+            its 3D grid indices (i, j, k).
+        insert(i: int, j: int, k: int, tri_idx: int, body_name: str, 
+               tri_aabb: AABB, time_stamp: int) -> list
+            Inserts a triangle into the hash grid and returns a list of 
+            objects in the cell, performing collision checks.
+
+    Example:
+        >>> hash_grid = HashGrid()
+        >>> hash_grid.set_cell_size(1.0, 1.0, 1.0)
+        >>> hash_grid.insert(1, 2, 3, 0, "body1", aabb, 1)
+
+    Note:
+        The objects inserted into the `HashGrid` are typically related 
+        to spatial entities (such as triangles in a 3D mesh) and include 
+        details like an index, body name, and an axis-aligned bounding box (AABB).
+    """
+
+    def __init__(self) -> None:
+        self.hash_table_size = 199999
+        self.hash_tbale = dict()
+        self.cell_size = np.array([1.0, 1.0, 1.0])
+        
+        # Prefect Hashing Setup, refer to https://dl.acm.org/doi/10.1145/1141911.1141926
+        self.offset_table_size = 100
+        self.M0 = np.eye(3, dtype=int)
+        self.M1 = np.eye(3, dtype=int)
+        self.Phi = np.random.randint(self.hash_table_size, size=(self.offset_table_size,) * 3)
+    
+    def set_hash_table_size(self, hash_table_size: int, update = True):
+        """ Set the size of the hash table, if update is True, the hash table size will be updated, otherwise the hash table size will be added by the given hash table size
+
+        Args:
+            hash_table_size (int32): The size of the hash table
+            update (bool, optional): If it is true the hash table size will be updated, otherwise the hash table size will be added by the given hash table size. Defaults to True.
+        """
+        self.hash_table_size = hash_table_size if update else self.hash_table_size + hash_table_size
+
+    def set_cell_size(self, cell_size_x: float, cell_size_y: float, cell_size_z: float):
+        """ Set the x, y, z axis length of a cell
+
+        Args:
+            cell_size_x (float): the cell size of X aixs
+            cell_size_y (float): the cell size of Y axis
+            cell_size_z (float): the cell size of Z axis
+        """
+        self.cell_size = [cell_size_x, cell_size_y, cell_size_z]
+    
+    def get_prefect_hash_value(self, i: int, j: int, k: int) -> int:
+        """ Get the prefect hash value of the cell, 
+            regarding the prefect hash, refer to https://dl.acm.org/doi/10.1145/1141911.1141926
+
+        Args:
+            i (int): The i index of the cell of X axis
+            j (int): The j index of the cell of Y axis
+            k (int): The k index of the cell of Z axis
+
+        Returns:
+            int: The prefect hash value of the cell
+        """
+        p = np.array([i, j, k])
+        h0 = np.dot(p, self.M0) % self.hash_table_size
+        h1 = np.dot(p, self.M1) % self.offset_table_size
+        hv = (h0 + self.Phi[tuple(h1)]) % self.hash_table_size
+        return int(''.join(map(str, hv.flatten())))
+
+    def insert(self, i: int, j: int, k: int, tri_idx: int, body_name: str, tri_aabb: 'AABB', time_stamp: int) -> list:
+        """ Insert a triangle into the hash table, and return the list of object of the cell
+
+        Args:
+            i (int): The i index of the cell of X axis
+            j (int): The j index of the cell of Y axis
+            k (int): The k index of the cell of Z axis
+            tri_idx (int): The index of the triangle of the body
+            body_name (str): The name of the body
+            tri_aabb (AABB): The AABB of the triangle
+            time_stamp (int): The time stamp of the simulation program
+
+        Returns:
+            list: The list of object of the cell
+        """
+        overlaps = []
+        hv = self.get_prefect_hash_value(i, j, k)
+        if hv not in self.hash_tbale:
+            self.hash_tbale[hv] = HashCell()
+            self.hash_tbale[hv].add((tri_idx, body_name, tri_aabb), time_stamp)
+        else:
+            overlaps = self.hash_tbale[hv].object_list
+            self.hash_tbale[hv].add((tri_idx, body_name, tri_aabb), time_stamp)
+        return overlaps
\ No newline at end of file

From 7ba94699cb5952c96ff6eec66346cd96360ac9d7 Mon Sep 17 00:00:00 2001
From: qbp758 <qbp758@futharkhpa03fl.unicph.domain>
Date: Fri, 6 Oct 2023 19:53:53 +0200
Subject: [PATCH 2/7] spatial hashing update

---
 python/rainbow/geometry/spatial_hashing.py    | 45 ++++++++----
 .../simulators/prox_soft_bodies/api.py        | 10 +++
 .../prox_soft_bodies/collision_detection.py   | 69 ++++++++++++++++++-
 .../simulators/prox_soft_bodies/solver.py     |  3 +
 .../simulators/prox_soft_bodies/types.py      |  4 ++
 5 files changed, 116 insertions(+), 15 deletions(-)

diff --git a/python/rainbow/geometry/spatial_hashing.py b/python/rainbow/geometry/spatial_hashing.py
index 9f99cb4..46534fc 100644
--- a/python/rainbow/geometry/spatial_hashing.py
+++ b/python/rainbow/geometry/spatial_hashing.py
@@ -75,6 +75,11 @@ class HashGird:
         cell_size (np.array): 1D numpy array containing the 3D dimensions 
             of a cell in the grid (x, y, z).
 
+        offset_table_size(int): The size of the offset table(Phi)
+        M0(Identity Matrix): A linear transfomation matrix used to map the domain(U) to the hash tbale(H)
+        M1(Identity Matrix): A linear transfomation matrix used to map the domain(U) to the offset table(Phi)
+        Phi(List[[int, int, int]]): The offset table used to remove the collision of the hash function
+
     Methods:
         set_hash_table_size(hash_table_size: int, update: bool = True)
             Sets the size of the hash table and optionally updates the 
@@ -101,12 +106,12 @@ class HashGird:
     """
 
     def __init__(self) -> None:
-        self.hash_table_size = 199999
+        self.hash_table_size = 1000
         self.hash_tbale = dict()
-        self.cell_size = np.array([1.0, 1.0, 1.0])
+        self.cell_size = 0.0
         
-        # Prefect Hashing Setup, refer to https://dl.acm.org/doi/10.1145/1141911.1141926
-        self.offset_table_size = 100
+        # Prefect Hashing Setup, them will be used in the get_prefect_hash_value function
+        self.offset_table_size = 1000
         self.M0 = np.eye(3, dtype=int)
         self.M1 = np.eye(3, dtype=int)
         self.Phi = np.random.randint(self.hash_table_size, size=(self.offset_table_size,) * 3)
@@ -120,19 +125,24 @@ def set_hash_table_size(self, hash_table_size: int, update = True):
         """
         self.hash_table_size = hash_table_size if update else self.hash_table_size + hash_table_size
 
-    def set_cell_size(self, cell_size_x: float, cell_size_y: float, cell_size_z: float):
+    def set_cell_size(self, cell_size: float):
         """ Set the x, y, z axis length of a cell
 
         Args:
-            cell_size_x (float): the cell size of X aixs
-            cell_size_y (float): the cell size of Y axis
-            cell_size_z (float): the cell size of Z axis
+            cell_size (float): the cell size 
         """
-        self.cell_size = [cell_size_x, cell_size_y, cell_size_z]
+        self.cell_size = cell_size
     
     def get_prefect_hash_value(self, i: int, j: int, k: int) -> int:
-        """ Get the prefect hash value of the cell, 
-            regarding the prefect hash, refer to https://dl.acm.org/doi/10.1145/1141911.1141926
+        """ Get the prefect hash value of the cell.
+            The hash function h(p) is computed as follows:
+            h(p) = (h_0(p) + Phi(h_1(p))) % m
+            where:
+            h_0(p): is the primary hash function used to calculate the hash value,
+            h_1(p): is a secondary hash function used to calculate the offset,
+            Phi: is the offset table,
+            m: is the size of the hash table.
+            For more information, refer to the 3rd section of this paper: https://dl.acm.org/doi/10.1145/1141911.1141926 
 
         Args:
             i (int): The i index of the cell of X axis
@@ -171,4 +181,15 @@ def insert(self, i: int, j: int, k: int, tri_idx: int, body_name: str, tri_aabb:
         else:
             overlaps = self.hash_tbale[hv].object_list
             self.hash_tbale[hv].add((tri_idx, body_name, tri_aabb), time_stamp)
-        return overlaps
\ No newline at end of file
+        return overlaps
+    
+    @classmethod
+    def compute_optial_cell_size(cls, V, T):
+        edges = []
+        for t in T:
+            edges.append(V[t[1]] - V[t[0]])
+            edges.append(V[t[2]] - V[t[1]])
+            edges.append(V[t[0]] - V[t[2]])
+        edges = np.array(edges)
+        edge_lengths = np.linalg.norm(edges, axis=1)
+        return np.mean(edge_lengths)
\ No newline at end of file
diff --git a/python/rainbow/simulators/prox_soft_bodies/api.py b/python/rainbow/simulators/prox_soft_bodies/api.py
index 732813e..d417339 100644
--- a/python/rainbow/simulators/prox_soft_bodies/api.py
+++ b/python/rainbow/simulators/prox_soft_bodies/api.py
@@ -1,12 +1,14 @@
 from typing import List, Dict
 import rainbow.geometry.grid3 as GRID
 import rainbow.geometry.kdop_bvh as BVH
+from rainbow.geometry.spatial_hashing import HashGird
 import rainbow.math.functions as FUNC
 import rainbow.math.vector3 as V3
 import rainbow.geometry.surface_mesh as SURF_MESH
 import rainbow.geometry.volume_mesh as MESH
 import rainbow.simulators.prox_soft_bodies.solver as SOLVER
 from rainbow.simulators.prox_soft_bodies.types import *
+
 import numpy as np
 
 
@@ -85,6 +87,14 @@ def create_soft_body(engine, body_name, V, T) -> None:
     body.offset = engine.number_of_nodes
     engine.number_of_nodes += len(body.x0)
 
+    # setting up the hash grid
+    engine.hash_grid.set_hash_table_size(len(body.surface), False)
+    # the optimal cell size is 2.2 times the average edge length of the surface mesh by our experiments
+    if engine.hash_grid.cell_size == 0:
+        engine.hash_grid.cell_size = HashGird.compute_optial_cell_size(body.x0, body.surface) * 2.2
+    else :
+        engine.hash_grid.cell_size = (HashGird.compute_optial_cell_size(body.x0, body.surface)+engine.hash_grid.cell_size)/2 * 2.2
+
 
 def create_dirichlet_conditions(engine, body_name, phi) -> None:
     """
diff --git a/python/rainbow/simulators/prox_soft_bodies/collision_detection.py b/python/rainbow/simulators/prox_soft_bodies/collision_detection.py
index fc4e3b8..b14f49f 100644
--- a/python/rainbow/simulators/prox_soft_bodies/collision_detection.py
+++ b/python/rainbow/simulators/prox_soft_bodies/collision_detection.py
@@ -1,10 +1,11 @@
 import rainbow.geometry.grid3 as GRID
 import rainbow.geometry.kdop_bvh as BVH
 import rainbow.geometry.barycentric as BC
+from rainbow.geometry.aabb import AABB
 from rainbow.simulators.prox_soft_bodies.types import *
 from rainbow.util.timer import Timer
 import numpy as np
-from itertools import combinations
+from itertools import combinations, product
 
 
 def _update_bvh(engine, stats, debug_on):
@@ -38,6 +39,65 @@ def _update_bvh(engine, stats, debug_on):
     return stats
 
 
+def _spatial_hashing_narrow_phase(engine, stats, debug_on):
+    """ Use spatial hashing to find the overlapping triangles
+
+    Args:
+        engine (Engine): The current engine instance we are working with.
+        stats (dict): A dictionary where to add more profiling and timing measurements.
+        debug_on (bool): Boolean flag for toggling debug (aka profiling) info on and off.
+
+    Returns:
+        (List, dict):  A tuple with body pair overlap information and a dictionary with profiling and
+                        timing measurements.
+    """
+    narrow_phase_timer = None
+    if debug_on:
+        narrow_phase_timer = Timer("narrow_phase", 8)
+        narrow_phase_timer.start()
+    
+    cell_size = engine.hash_grid.cell_size
+    if cell_size <= 0.0:
+        raise ValueError("Cell size must be greater than zero")
+    
+    time_stamp = engine.params.time_stamp
+    results = {}
+
+    for body in engine.bodies.values():
+        tri_vertices = body.x[body.surface, :]
+        # Compute the AABB of each triangle by vectorizing the min/max operation
+        tri_aabb_min = np.min(tri_vertices, axis=1)
+        tri_aabb_max = np.max(tri_vertices, axis=1)
+        cell_min = (tri_aabb_min / cell_size).astype(int)
+        cell_max = (tri_aabb_max / cell_size).astype(int) + 1
+
+        # Traverse the cells in the AABB of each triangle and insert the triangle into the hash table
+        for tri_idx, (c_min, c_max) in enumerate(zip(cell_min, cell_max)):
+            cell_ranges = [range(cmi, cma) for cmi, cma in zip(c_min, c_max)]
+            for i, j, k in product(*cell_ranges):
+                tri_aabb = AABB(tri_aabb_min[tri_idx], tri_aabb_max[tri_idx])
+                overlaps = engine.hash_grid.insert(i, j, k, tri_idx, body.name, 
+                                                   tri_aabb, 
+                                                   time_stamp)
+                # Check all triangles in the cell to see if they overlap with the current triangle
+                if len(overlaps) > 0:
+                    for overlap in overlaps:
+                        overlap_tri_idx, overlap_body_name, overlap_tri_aabb = overlap
+                        if overlap_body_name != body.name and (AABB.is_overlap(tri_aabb, overlap_tri_aabb)):
+                            if (body, engine.bodies[overlap_body_name]) not in results:
+                                results[(body, engine.bodies[overlap_body_name])] = np.array([[tri_idx, overlap_tri_idx]], dtype=np.int32)
+                            else:
+                                results[(body, engine.bodies[overlap_body_name])] = np.vstack([results[(body, engine.bodies[overlap_body_name])], [tri_idx, overlap_tri_idx]])
+    if debug_on:
+        narrow_phase_timer.end()
+        stats["narrow_phase"] = narrow_phase_timer.elapsed
+        stats["number_of_overlaps"] = np.sum(
+            [len(result) for result in results.values()]
+        )
+
+    return results, stats
+
+
 def _narrow_phase(engine, stats, debug_on):
     """
 
@@ -344,8 +404,11 @@ def run_collision_detection(engine, stats, debug_on):
     if debug_on:
         collision_detection_timer = Timer("collision_detection")
         collision_detection_timer.start()
-    stats = _update_bvh(engine, stats, debug_on)
-    overlaps, stats = _narrow_phase(engine, stats, debug_on)
+    if engine.params.use_spatial_hashing:
+        overlaps, stats = _spatial_hashing_narrow_phase(engine, stats, debug_on)
+    else:
+        stats = _update_bvh(engine, stats, debug_on)
+        overlaps, stats = _narrow_phase(engine, stats, debug_on)
     stats = _contact_determination(overlaps, engine, stats, debug_on)
     stats = _contact_reduction(engine, stats, debug_on)
     if debug_on:
diff --git a/python/rainbow/simulators/prox_soft_bodies/solver.py b/python/rainbow/simulators/prox_soft_bodies/solver.py
index 047786a..e4874c0 100644
--- a/python/rainbow/simulators/prox_soft_bodies/solver.py
+++ b/python/rainbow/simulators/prox_soft_bodies/solver.py
@@ -1145,6 +1145,9 @@ def step(self, dt: float, engine: Engine, debug_on: bool) -> None:
                 #  stepper in its own right. This might be cool for pre-processing of simulations to make sure
                 #  no penetrations are initially present.
                 stats = apply_post_stabilization(J, WJT, engine, stats, debug_on)
+        
+        # Update time stamp
+        engine.params.time_stamp += 1
 
         if debug_on:
             timer.end()
diff --git a/python/rainbow/simulators/prox_soft_bodies/types.py b/python/rainbow/simulators/prox_soft_bodies/types.py
index b3cf91d..3bf80cc 100644
--- a/python/rainbow/simulators/prox_soft_bodies/types.py
+++ b/python/rainbow/simulators/prox_soft_bodies/types.py
@@ -1,5 +1,6 @@
 import rainbow.math.vector3 as V3
 from rainbow.simulators.prox_soft_bodies.mechanics import *
+import rainbow.geometry.spatial_hashing as SH
 
 
 class SurfacesInteraction:
@@ -252,6 +253,8 @@ def __init__(self):
             0.1  # Any geometry within this distance generates a contact point.
         )
         self.resolution = 64  # The number of grid cells along each axis in the signed distance fields.
+        self.use_spatial_hashing = True  # Boolean flag that indicates if spatial hashing should be used instead of the BVH or not.
+        self.time_stamp = 0 # The time step to use when simulating forward.
 
 
 class Engine:
@@ -284,3 +287,4 @@ def __init__(self):
         )  # All contact points in last call of collision detection system.
         self.number_of_nodes = 0  # The total number of nodes in the world.
         self.stepper = None  # A reference to the time-stepper used to simulator forward.
+        self.hash_grid = SH.HashGird()

From 5d97265a5c7c773d17f80fd4aff58369a81b724b Mon Sep 17 00:00:00 2001
From: qbp758 <qbp758@futharkhpa03fl.unicph.domain>
Date: Tue, 10 Oct 2023 23:54:54 +0200
Subject: [PATCH 3/7] reduce the smae triangle pairs in overlaps'

---
 python/rainbow/geometry/aabb.py                      | 12 +++++++++---
 .../prox_soft_bodies/collision_detection.py          | 11 ++++++-----
 2 files changed, 15 insertions(+), 8 deletions(-)

diff --git a/python/rainbow/geometry/aabb.py b/python/rainbow/geometry/aabb.py
index 221d28f..6bda52c 100644
--- a/python/rainbow/geometry/aabb.py
+++ b/python/rainbow/geometry/aabb.py
@@ -1,6 +1,7 @@
 import numpy as np
 from typing import List
 
+
 class AABB:
     """
     Axis-Aligned Bounding Box (AABB) for 3D spatial objects.
@@ -47,16 +48,21 @@ def create_from_vertices(cls, vertices: List[List[float]]) -> 'AABB':
         return cls(min_point, max_point)
     
     @classmethod
-    def is_overlap(cls, aabb1: 'AABB', aabb2: 'AABB') -> bool:
+    def is_overlap(cls, aabb1: 'AABB', aabb2: 'AABB', boundary: float = 0.0) -> bool:
         """ Test two aabb instance are overlap or not
 
         Args:
-            aabb1 (AABB): _description_
-            aabb2 (AABB): _description_
+            aabb1 (AABB): The AABB instance of one object 
+            aabb2 (AABB): The AABB instance of one object 
+            boundary (float): which is used to expand the aabb, hence we should use a positive floating point, Defaults to 0.0.
 
         Returns:
             bool: Return True if both of aabb instances are overlap, otherwise return False
         """
+        aabb1.min_point -= boundary
+        aabb1.max_point += boundary
+        aabb2.min_point -= boundary
+        aabb2.max_point += boundary
         return not (aabb1.max_point[0] < aabb2.min_point[0] or aabb1.min_point[0] > aabb2.max_point[0] or
                     aabb1.max_point[1] < aabb2.min_point[1] or aabb1.min_point[1] > aabb2.max_point[1] or
                     aabb1.max_point[2] < aabb2.min_point[2] or aabb1.min_point[2] > aabb2.max_point[2])
\ No newline at end of file
diff --git a/python/rainbow/simulators/prox_soft_bodies/collision_detection.py b/python/rainbow/simulators/prox_soft_bodies/collision_detection.py
index b14f49f..3c58022 100644
--- a/python/rainbow/simulators/prox_soft_bodies/collision_detection.py
+++ b/python/rainbow/simulators/prox_soft_bodies/collision_detection.py
@@ -6,6 +6,7 @@
 from rainbow.util.timer import Timer
 import numpy as np
 from itertools import combinations, product
+from collections import defaultdict
 
 
 def _update_bvh(engine, stats, debug_on):
@@ -61,7 +62,7 @@ def _spatial_hashing_narrow_phase(engine, stats, debug_on):
         raise ValueError("Cell size must be greater than zero")
     
     time_stamp = engine.params.time_stamp
-    results = {}
+    results = defaultdict(set)
 
     for body in engine.bodies.values():
         tri_vertices = body.x[body.surface, :]
@@ -84,10 +85,8 @@ def _spatial_hashing_narrow_phase(engine, stats, debug_on):
                     for overlap in overlaps:
                         overlap_tri_idx, overlap_body_name, overlap_tri_aabb = overlap
                         if overlap_body_name != body.name and (AABB.is_overlap(tri_aabb, overlap_tri_aabb)):
-                            if (body, engine.bodies[overlap_body_name]) not in results:
-                                results[(body, engine.bodies[overlap_body_name])] = np.array([[tri_idx, overlap_tri_idx]], dtype=np.int32)
-                            else:
-                                results[(body, engine.bodies[overlap_body_name])] = np.vstack([results[(body, engine.bodies[overlap_body_name])], [tri_idx, overlap_tri_idx]])
+                            results[(body, engine.bodies[overlap_body_name])].add((tri_idx, overlap_tri_idx))
+
     if debug_on:
         narrow_phase_timer.end()
         stats["narrow_phase"] = narrow_phase_timer.elapsed
@@ -95,6 +94,8 @@ def _spatial_hashing_narrow_phase(engine, stats, debug_on):
             [len(result) for result in results.values()]
         )
 
+    results = {key: np.array(list(value), dtype=np.int32) for key, value in results.items()}
+
     return results, stats
 
 

From 3cdaf606d0b19ba84c87224e55deea7d82caecbf Mon Sep 17 00:00:00 2001
From: qbp758 <qbp758@futharkhpa03fl.unicph.domain>
Date: Wed, 11 Oct 2023 00:08:02 +0200
Subject: [PATCH 4/7] update optimal cell size for hash grid'

---
 python/rainbow/geometry/spatial_hashing.py        | 13 ++++++++++++-
 python/rainbow/simulators/prox_soft_bodies/api.py |  6 +++---
 2 files changed, 15 insertions(+), 4 deletions(-)

diff --git a/python/rainbow/geometry/spatial_hashing.py b/python/rainbow/geometry/spatial_hashing.py
index 46534fc..ea18c34 100644
--- a/python/rainbow/geometry/spatial_hashing.py
+++ b/python/rainbow/geometry/spatial_hashing.py
@@ -185,6 +185,15 @@ def insert(self, i: int, j: int, k: int, tri_idx: int, body_name: str, tri_aabb:
     
     @classmethod
     def compute_optial_cell_size(cls, V, T):
+        """ Aim to compute the optimal cell size for the spatial hashing, which is the average edge length of the mesh
+
+        Args:
+            V (list): The vertices of the mesh
+            T (list): The triangles of the mesh
+
+        Returns:
+            float: The optimal cell size : 2.2 * average edge length
+        """
         edges = []
         for t in T:
             edges.append(V[t[1]] - V[t[0]])
@@ -192,4 +201,6 @@ def compute_optial_cell_size(cls, V, T):
             edges.append(V[t[0]] - V[t[2]])
         edges = np.array(edges)
         edge_lengths = np.linalg.norm(edges, axis=1)
-        return np.mean(edge_lengths)
\ No newline at end of file
+        
+        # the optimal cell size is 2.2 times the average edge length of the surface mesh by our experiments
+        return np.mean(edge_lengths) * 2.2
\ No newline at end of file
diff --git a/python/rainbow/simulators/prox_soft_bodies/api.py b/python/rainbow/simulators/prox_soft_bodies/api.py
index d417339..3de9ac4 100644
--- a/python/rainbow/simulators/prox_soft_bodies/api.py
+++ b/python/rainbow/simulators/prox_soft_bodies/api.py
@@ -89,11 +89,11 @@ def create_soft_body(engine, body_name, V, T) -> None:
 
     # setting up the hash grid
     engine.hash_grid.set_hash_table_size(len(body.surface), False)
-    # the optimal cell size is 2.2 times the average edge length of the surface mesh by our experiments
+ 
     if engine.hash_grid.cell_size == 0:
-        engine.hash_grid.cell_size = HashGird.compute_optial_cell_size(body.x0, body.surface) * 2.2
+        engine.hash_grid.cell_size = HashGird.compute_optial_cell_size(body.x0, body.surface)
     else :
-        engine.hash_grid.cell_size = (HashGird.compute_optial_cell_size(body.x0, body.surface)+engine.hash_grid.cell_size)/2 * 2.2
+        engine.hash_grid.cell_size = (HashGird.compute_optial_cell_size(body.x0, body.surface)+engine.hash_grid.cell_size)/2 
 
 
 def create_dirichlet_conditions(engine, body_name, phi) -> None:

From 6111f64e7cc8468c5f6fa92e3fa7b9626faf000b Mon Sep 17 00:00:00 2001
From: qbp758 <qbp758@futharkhpa03fl.unicph.domain>
Date: Mon, 16 Oct 2023 18:08:53 +0200
Subject: [PATCH 5/7] 1. Self-collision support 2. More clearly code

---
 python/rainbow/geometry/aabb.py               | 36 +++----
 python/rainbow/geometry/spatial_hashing.py    | 36 ++++---
 .../simulators/prox_soft_bodies/api.py        | 42 ++++----
 .../prox_soft_bodies/collision_detection.py   | 95 ++++++++++++++++++-
 .../simulators/prox_soft_bodies/solver.py     |  3 +-
 python/unit_tests/test_geometry_aabb.py       | 42 ++++++++
 .../test_geometry_spatial_hashing.py          | 81 ++++++++++++++++
 7 files changed, 281 insertions(+), 54 deletions(-)
 create mode 100644 python/unit_tests/test_geometry_aabb.py
 create mode 100644 python/unit_tests/test_geometry_spatial_hashing.py

diff --git a/python/rainbow/geometry/aabb.py b/python/rainbow/geometry/aabb.py
index 6bda52c..55018da 100644
--- a/python/rainbow/geometry/aabb.py
+++ b/python/rainbow/geometry/aabb.py
@@ -1,5 +1,5 @@
 import numpy as np
-from typing import List
+from numpy.typing import ArrayLike
 
 
 class AABB:
@@ -11,11 +11,11 @@ class AABB:
     which represent opposite corners of the box.
 
     Attributes:
-        min_point (List[float]): The smallest x, y, z coordinates from the bounding box.
-        max_point (List[float]): The largest x, y, z coordinates from the bounding box.
+        min_point (ArrayLike): The smallest x, y, z coordinates from the bounding box.
+        max_point (ArrayLike): The largest x, y, z coordinates from the bounding box.
 
     Methods:
-        create_from_vertices(vertices: List[List[float]]) -> 'AABB'
+        create_from_vertices(vertices: ArrayLike) -> 'AABB'
             Class method to create an AABB instance from a list of vertices.
 
         is_overlap(aabb1: 'AABB', aabb2: 'AABB') -> bool
@@ -28,13 +28,12 @@ class AABB:
         >>> AABB.is_overlap(aabb1, aabb2)
         True
     """
-    
-    def __init__(self, min_point: List[float], max_point: List[float]) -> None:
-        self.min_point = min_point
-        self.max_point = max_point
+    def __init__(self, min_point: ArrayLike, max_point: ArrayLike) -> None:
+        self.min_point = np.array(min_point, dtype=np.float64)
+        self.max_point = np.array(max_point, dtype=np.float64)
         
     @classmethod
-    def create_from_vertices(cls, vertices: List[List[float]]) -> 'AABB':
+    def create_from_vertices(cls, vertices: ArrayLike) -> 'AABB':
         """ Create AABB instance from vertices, such as triangle vertices
 
         Args:
@@ -59,10 +58,15 @@ def is_overlap(cls, aabb1: 'AABB', aabb2: 'AABB', boundary: float = 0.0) -> bool
         Returns:
             bool: Return True if both of aabb instances are overlap, otherwise return False
         """
-        aabb1.min_point -= boundary
-        aabb1.max_point += boundary
-        aabb2.min_point -= boundary
-        aabb2.max_point += boundary
-        return not (aabb1.max_point[0] < aabb2.min_point[0] or aabb1.min_point[0] > aabb2.max_point[0] or
-                    aabb1.max_point[1] < aabb2.min_point[1] or aabb1.min_point[1] > aabb2.max_point[1] or
-                    aabb1.max_point[2] < aabb2.min_point[2] or aabb1.min_point[2] > aabb2.max_point[2])
\ No newline at end of file
+        if boundary != 0.0:
+            aabb1_min_copy = np.copy(aabb1.min_point)
+            aabb1_max_copy = np.copy(aabb1.max_point)
+            aabb2_min_copy = np.copy(aabb2.min_point)
+            aabb2_max_copy = np.copy(aabb2.max_point)
+            aabb1_min_copy -= boundary
+            aabb1_max_copy += boundary
+            aabb2_min_copy -= boundary
+            aabb2_max_copy += boundary
+            return not (np.any(aabb1_max_copy < aabb2_min_copy) or np.any(aabb1_min_copy > aabb2_max_copy))
+        else:
+            return not (np.any(aabb1.max_point < aabb2.min_point) or np.any(aabb1.min_point > aabb2.max_point))
\ No newline at end of file
diff --git a/python/rainbow/geometry/spatial_hashing.py b/python/rainbow/geometry/spatial_hashing.py
index ea18c34..68df209 100644
--- a/python/rainbow/geometry/spatial_hashing.py
+++ b/python/rainbow/geometry/spatial_hashing.py
@@ -81,9 +81,10 @@ class HashGird:
         Phi(List[[int, int, int]]): The offset table used to remove the collision of the hash function
 
     Methods:
-        set_hash_table_size(hash_table_size: int, update: bool = True)
-            Sets the size of the hash table and optionally updates the 
-            current hash table size.
+        set_hash_table_size(hash_table_size: int)
+            Sets the size of the hash table
+        increment_hash_table_size(increment_size: int)
+            Increments the size of the hash table
         set_cell_size(cell_size_x: float, cell_size_y: float, cell_size_z: float)
             Sets the 3D dimensions of a cell in the grid.
         get_hash_value(i: int, j: int, k: int) -> int
@@ -110,20 +111,28 @@ def __init__(self) -> None:
         self.hash_tbale = dict()
         self.cell_size = 0.0
         
-        # Prefect Hashing Setup, them will be used in the get_prefect_hash_value function
+        # Perfect Hashing Setup: These parameters are configured and subsequently used in the get_prefect_hash_value function.
         self.offset_table_size = 1000
         self.M0 = np.eye(3, dtype=int)
         self.M1 = np.eye(3, dtype=int)
         self.Phi = np.random.randint(self.hash_table_size, size=(self.offset_table_size,) * 3)
+        self.mod_value = 1e9 + 7
     
-    def set_hash_table_size(self, hash_table_size: int, update = True):
-        """ Set the size of the hash table, if update is True, the hash table size will be updated, otherwise the hash table size will be added by the given hash table size
+    def set_hash_table_size(self, hash_table_size: int):
+        """ Set the size of the hash table
 
         Args:
             hash_table_size (int32): The size of the hash table
-            update (bool, optional): If it is true the hash table size will be updated, otherwise the hash table size will be added by the given hash table size. Defaults to True.
         """
-        self.hash_table_size = hash_table_size if update else self.hash_table_size + hash_table_size
+        self.hash_table_size = hash_table_size
+    
+    def increment_hash_table_size(self, increment_size: int):
+        """ Increment the size of the hash table
+
+        Args:
+            increment_size (int32): The size of the hash table
+        """
+        self.hash_table_size = self.hash_table_size + increment_size
 
     def set_cell_size(self, cell_size: float):
         """ Set the x, y, z axis length of a cell
@@ -156,9 +165,10 @@ def get_prefect_hash_value(self, i: int, j: int, k: int) -> int:
         h0 = np.dot(p, self.M0) % self.hash_table_size
         h1 = np.dot(p, self.M1) % self.offset_table_size
         hv = (h0 + self.Phi[tuple(h1)]) % self.hash_table_size
-        return int(''.join(map(str, hv.flatten())))
 
-    def insert(self, i: int, j: int, k: int, tri_idx: int, body_name: str, tri_aabb: 'AABB', time_stamp: int) -> list:
+        return int(np.sum(hv) % self.mod_value)
+
+    def insert(self, i: int, j: int, k: int, tri_idx: int, body_idx: int, tri_aabb: 'AABB', time_stamp: int) -> list:
         """ Insert a triangle into the hash table, and return the list of object of the cell
 
         Args:
@@ -166,7 +176,7 @@ def insert(self, i: int, j: int, k: int, tri_idx: int, body_name: str, tri_aabb:
             j (int): The j index of the cell of Y axis
             k (int): The k index of the cell of Z axis
             tri_idx (int): The index of the triangle of the body
-            body_name (str): The name of the body
+            body_idx (int): The index of the body
             tri_aabb (AABB): The AABB of the triangle
             time_stamp (int): The time stamp of the simulation program
 
@@ -177,10 +187,10 @@ def insert(self, i: int, j: int, k: int, tri_idx: int, body_name: str, tri_aabb:
         hv = self.get_prefect_hash_value(i, j, k)
         if hv not in self.hash_tbale:
             self.hash_tbale[hv] = HashCell()
-            self.hash_tbale[hv].add((tri_idx, body_name, tri_aabb), time_stamp)
+            self.hash_tbale[hv].add((tri_idx, body_idx, tri_aabb), time_stamp)
         else:
             overlaps = self.hash_tbale[hv].object_list
-            self.hash_tbale[hv].add((tri_idx, body_name, tri_aabb), time_stamp)
+            self.hash_tbale[hv].add((tri_idx, body_idx, tri_aabb), time_stamp)
         return overlaps
     
     @classmethod
diff --git a/python/rainbow/simulators/prox_soft_bodies/api.py b/python/rainbow/simulators/prox_soft_bodies/api.py
index 3de9ac4..8522e0d 100644
--- a/python/rainbow/simulators/prox_soft_bodies/api.py
+++ b/python/rainbow/simulators/prox_soft_bodies/api.py
@@ -1,7 +1,7 @@
 from typing import List, Dict
 import rainbow.geometry.grid3 as GRID
 import rainbow.geometry.kdop_bvh as BVH
-from rainbow.geometry.spatial_hashing import HashGird
+import rainbow.geometry.spatial_hashing as HASH_GRID
 import rainbow.math.functions as FUNC
 import rainbow.math.vector3 as V3
 import rainbow.geometry.surface_mesh as SURF_MESH
@@ -71,30 +71,34 @@ def create_soft_body(engine, body_name, V, T) -> None:
     body.x = np.array(mesh.V, copy=True, dtype=np.float64)
     body.u = np.zeros(V.shape, dtype=np.float64)
 
-    # Create bounding volume hierarchy data-structure (BVH), this will always be updated to live in
-    # spatial coordinates and is tested against the signed distance field (who lives in constant material space) to
-    # generate contact points.
-    body.bvh = BVH.make_bvh(
-        body.x,
-        body.surface,
-        engine.params.K,
-        engine.params.bvh_chunk_size,
-        engine.params.envelope,
-    )
+    
+    # If we use spatial hashing we need to setup the hash grid, otherwise we create a BVH
+    if engine.params.use_spatial_hashing:
+        engine.hash_grid.increment_hash_table_size(len(body.surface))
+    
+        if engine.hash_grid.cell_size == 0:
+            engine.hash_grid.cell_size = HASH_GRID.HashGird.compute_optial_cell_size(body.x0, body.surface)
+        else :
+            engine.hash_grid.cell_size = (HASH_GRID.HashGird.compute_optial_cell_size(body.x0, body.surface)+engine.hash_grid.cell_size)/2 
+    else:
+        # Create bounding volume hierarchy data-structure (BVH), this will always be updated to live in
+        # spatial coordinates and is tested against the signed distance field (who lives in constant material space) to
+        # generate contact points.
+        print("init make body bvh")
+        body.bvh = BVH.make_bvh(
+            body.x,
+            body.surface,
+            engine.params.K,
+            engine.params.bvh_chunk_size,
+            engine.params.envelope,
+        )
 
     # To have proper global indexing into assembled matrices and vectors we need to know this body nodel
     # index offset into this global space.
     body.offset = engine.number_of_nodes
     engine.number_of_nodes += len(body.x0)
 
-    # setting up the hash grid
-    engine.hash_grid.set_hash_table_size(len(body.surface), False)
- 
-    if engine.hash_grid.cell_size == 0:
-        engine.hash_grid.cell_size = HashGird.compute_optial_cell_size(body.x0, body.surface)
-    else :
-        engine.hash_grid.cell_size = (HashGird.compute_optial_cell_size(body.x0, body.surface)+engine.hash_grid.cell_size)/2 
-
+    
 
 def create_dirichlet_conditions(engine, body_name, phi) -> None:
     """
diff --git a/python/rainbow/simulators/prox_soft_bodies/collision_detection.py b/python/rainbow/simulators/prox_soft_bodies/collision_detection.py
index c735a38..49723f4 100644
--- a/python/rainbow/simulators/prox_soft_bodies/collision_detection.py
+++ b/python/rainbow/simulators/prox_soft_bodies/collision_detection.py
@@ -40,6 +40,85 @@ def _update_bvh(engine, stats, debug_on):
     return stats
 
 
+def _is_share_vertex(tri1, tri2):
+    """ Test if two triangles of a same body share a vertex.
+
+    Args:
+        tri1 (ArrayLike): coordinates of the first triangle
+        tri2 (ArrayLike): coordinates of the second triangle
+    
+    Returns:
+        bool: True if the two triangles share a vertex, False otherwise
+    """
+    return len(np.intersect1d(np.array(tri1), np.array(tri2))) > 0
+
+
+def _triangle_intersection(tri1, tri2):
+    """ Test if two triangles of a same body intersect
+        To achieve performance, this function is adapted from Moller-Trumbore intersection algorithm,
+        which is described in https://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm.
+        The idea is to check if the intersection point of the two triangles is inside both triangles.
+        Steps:
+        1. Check if the two triangles are parallel or not, if they are parallel, return False
+        2. Check if the intersection point is inside the first triangle, if not, return False
+        3. Check if the intersection point is inside the second triangle, if inside, return True, otherwise return False
+        
+    Args:
+        tri1 (ArrayLike): coordinates of the first triangle
+        tri2 (ArrayLike): coordinates of the second triangle
+
+    Returns:
+        bool: True if the two triangles intersect, False otherwise
+    """
+    v1, v2, v3 = tri1
+    u1, u2, u3 = tri2
+
+    e1 = v2 - v1
+    e2 = v3 - v1
+    normal_tri2 = np.cross(u2 - u1, u3 - u1)
+    a = np.dot(e1, normal_tri2)
+
+    # Check if the two triangles are parallel or not
+    if a > -np.finfo(float).eps and a < np.finfo(float).eps:
+        return False 
+
+    # Check the intersection point is inside the triangle(tri1)
+    f = 1.0/a
+    s = u1 - v1
+    u = f * np.dot(s, normal_tri2)
+    if u < 0.0 or u > 1.0:
+        return False
+
+    q = np.cross(s, e1)
+    v = f * np.dot(u2 - u1, q)
+    if v < 0.0 or u + v > 1.0:
+        return False
+
+    # Check the intersection point is also inside the other triangle(tri2)
+    t = f * np.dot(e2, q)
+    if t > np.finfo(float).eps:
+        return True
+
+    return False
+
+
+def _is_self_collision(tri1, tri2, tri1_aabb, tri2_aabb):
+    """ Check if two triangles of a same body are self-colliding
+
+    Args:
+        tri1 (ArrayLike): coordinates of the first triangle
+        tri2 (ArrayLike): coordinates of the second triangle
+        tri1_aabb (AABB): the AABB of the first triangle
+        tri2_aabb (AABB): the AABB of the second triangle
+
+    Returns:
+        bool: True if the two triangles are self-colliding, False otherwise
+    """
+    return (not _is_share_vertex(tri1, tri2) and
+            AABB.is_overlap(tri1_aabb, tri2_aabb) and
+            _triangle_intersection(tri1, tri2))
+
+
 def _spatial_hashing_narrow_phase(engine, stats, debug_on):
     """ Use spatial hashing to find the overlapping triangles
 
@@ -77,15 +156,21 @@ def _spatial_hashing_narrow_phase(engine, stats, debug_on):
             cell_ranges = [range(cmi, cma) for cmi, cma in zip(c_min, c_max)]
             for i, j, k in product(*cell_ranges):
                 tri_aabb = AABB(tri_aabb_min[tri_idx], tri_aabb_max[tri_idx])
-                overlaps = engine.hash_grid.insert(i, j, k, tri_idx, body.name, 
+                overlaps = engine.hash_grid.insert(i, j, k, tri_idx, body.idx, 
                                                    tri_aabb, 
                                                    time_stamp)
                 # Check all triangles in the cell to see if they overlap with the current triangle
                 if len(overlaps) > 0:
-                    for overlap in overlaps:
-                        overlap_tri_idx, overlap_body_name, overlap_tri_aabb = overlap
-                        if overlap_body_name != body.name and (AABB.is_overlap(tri_aabb, overlap_tri_aabb)):
-                            results[(body, engine.bodies[overlap_body_name])].add((tri_idx, overlap_tri_idx))
+                    for overlap_tri_idx, overlap_body_idx, overlap_tri_aabb  in overlaps:
+                        overlap_body = list(engine.bodies.values())[overlap_body_idx]
+                        if overlap_body_idx == body.idx:
+                            # Potential self-collision
+                            if _is_self_collision(body.x[body.surface[tri_idx]], overlap_body.x[overlap_body.surface[overlap_tri_idx]], tri_aabb, overlap_tri_aabb):
+                                results[(body, overlap_body)].add((tri_idx, overlap_tri_idx))
+                        else:
+                            # Potential collision with another body
+                            if AABB.is_overlap(tri_aabb, overlap_tri_aabb):
+                                results[(body, overlap_body)].add((tri_idx, overlap_tri_idx))
 
     if debug_on:
         narrow_phase_timer.end()
diff --git a/python/rainbow/simulators/prox_soft_bodies/solver.py b/python/rainbow/simulators/prox_soft_bodies/solver.py
index e4874c0..ff8f0d2 100644
--- a/python/rainbow/simulators/prox_soft_bodies/solver.py
+++ b/python/rainbow/simulators/prox_soft_bodies/solver.py
@@ -174,7 +174,8 @@ def compute_D(x, T):
             u_mi = p[3] - p[0]
             # Verify that the tetrahedron is well-defined
             if np.dot(u_mi, np.cross(u_ji, u_ki)) <= 0:
-                raise RuntimeError("compute_D(): Degenerate tetrahedron found", e)
+                # raise RuntimeError("compute_D(): Degenerate tetrahedron found", e)
+                print("compute_D(): Degenerate tetrahedron found", e)
             # Create edge-vector matrix
             D[e] = M3.make(
                 u_ji[0],
diff --git a/python/unit_tests/test_geometry_aabb.py b/python/unit_tests/test_geometry_aabb.py
new file mode 100644
index 0000000..9a46178
--- /dev/null
+++ b/python/unit_tests/test_geometry_aabb.py
@@ -0,0 +1,42 @@
+import unittest
+import os
+import sys
+import numpy as np
+import igl
+
+sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+
+from rainbow.geometry.aabb import AABB
+import rainbow.util.test_tools as TEST
+
+
+class TestAABB(unittest.TestCase):
+    def setUp(self):
+        self.p1 = np.array([0, 0, 0], dtype=np.float64)
+        self.p2 = np.array([1, 1, 1], dtype=np.float64)
+        self.p3 = np.array([2, 2, 2], dtype=np.float64)
+        self.p4 = np.array([3, 3, 3], dtype=np.float64)
+
+    def test_init(self):
+        aabb = AABB(self.p1, self.p2)
+        self.assertTrue(TEST.is_array_equal(aabb.min_point, self.p1))
+        self.assertTrue(TEST.is_array_equal(aabb.max_point, self.p2))
+
+    def test_create_from_vertices(self):
+        vertices = np.array([self.p1, self.p2, self.p3])
+        aabb = AABB.create_from_vertices(vertices)
+        self.assertTrue(TEST.is_array_equal(aabb.min_point, self.p1))
+        self.assertTrue(TEST.is_array_equal(aabb.max_point, self.p3))
+
+    def test_is_overlap(self):
+        aabb1 = AABB(self.p1, self.p2)
+        aabb2 = AABB(self.p2, self.p3)
+        self.assertTrue(AABB.is_overlap(aabb1, aabb2))
+
+        aabb3 = AABB(self.p3, self.p4)
+        self.assertFalse(AABB.is_overlap(aabb1, aabb3, boundary=0.1))
+        self.assertTrue(AABB.is_overlap(aabb1, aabb3, boundary=1.1))
+
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/python/unit_tests/test_geometry_spatial_hashing.py b/python/unit_tests/test_geometry_spatial_hashing.py
new file mode 100644
index 0000000..6ca3ada
--- /dev/null
+++ b/python/unit_tests/test_geometry_spatial_hashing.py
@@ -0,0 +1,81 @@
+import unittest
+import os
+import sys
+import numpy as np
+import igl
+
+sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
+
+from rainbow.geometry.aabb import AABB
+import rainbow.geometry.spatial_hashing as SPATIAL_HASHING
+import rainbow.util.test_tools as TEST
+
+
+class TestHashCell(unittest.TestCase):
+    def setUp(self):
+        self.triangle1 = {
+            "tri_idx": 0,
+            "body_idx": 0,
+            "aabb": AABB([0, 0, 0], [1, 1, 1])
+        }
+
+        self.triangle2 = {
+            "tri_idx": 0,
+            "body_idx": 1,
+            "aabb": AABB([1, 1, 1], [2, 2, 2])
+        }
+        self.time_stamp = 0
+        self.cell = SPATIAL_HASHING.HashCell()
+
+    def test_add(self):
+        self.cell.add((self.triangle1["tri_idx"], self.triangle1["body_idx"], self.triangle1["aabb"]), self.time_stamp)
+        self.assertEqual(self.cell.time_stamp, self.time_stamp)
+        self.assertEqual(self.cell.size, 1)
+
+    def test_lazy_clear(self):
+        self.time_stamp +=1
+        self.cell.add((self.triangle1["tri_idx"], self.triangle1["body_idx"], self.triangle1["aabb"]), self.time_stamp)
+        self.cell.add((self.triangle2["tri_idx"], self.triangle2["body_idx"], self.triangle2["aabb"]), self.time_stamp)
+        self.assertEqual(self.cell.size, 2)
+        self.assertEqual(self.cell.object_list[0][0], self.triangle1["tri_idx"])
+        self.assertEqual(self.cell.object_list[0][1], self.triangle1["body_idx"])
+        self.assertEqual(self.cell.object_list[1][0], self.triangle2["tri_idx"])
+        self.assertEqual(self.cell.object_list[1][1], self.triangle2["body_idx"])
+
+
+class TestHashGrid(unittest.TestCase):
+    def setUp(self):
+        self.grid = SPATIAL_HASHING.HashGird()
+        self.triangle1 = {
+            "tri_idx": 0,
+            "body_idx": 0,
+            "aabb": AABB([0, 0, 0], [1, 1, 1])
+        }
+        self.triangle2 = {
+            "tri_idx": 0,
+            "body_idx": 1,
+            "aabb": AABB([1, 1, 1], [2, 2, 2])
+        }
+        self.time_stamp = 0
+
+    def test_get_prefect_hash_value(self):
+        self.assertIsInstance(self.grid.get_prefect_hash_value(1, 2, 3), int)
+
+    def test_insert(self):
+        overlaps = self.grid.insert(1, 1, 1, self.triangle1["tri_idx"], self.triangle1["body_idx"], self.triangle1["aabb"], self.time_stamp)
+        self.assertEqual(len(overlaps), 0)
+
+        overlaps = self.grid.insert(1, 1, 1, self.triangle2["tri_idx"], self.triangle2["body_idx"], self.triangle2["aabb"], self.time_stamp)
+        self.assertTrue(len(overlaps)>0)
+        self.assertEqual(overlaps[0][0], self.triangle1["tri_idx"])
+        self.assertEqual(overlaps[0][1], self.triangle1["body_idx"])
+
+    def test_compute_optial_cell_size(self):
+        V = np.array([[0, 0, 0], [1, 0, 0], [0, 1, 0], [0, 0, 1]])
+        T = [[0, 1, 2], [0, 1, 3], [0, 2, 3]]
+        size = SPATIAL_HASHING.HashGird.compute_optial_cell_size(V, T)
+        self.assertIsInstance(size, float)
+
+
+if __name__ == '__main__':
+    unittest.main()
\ No newline at end of file

From 3d8bba0198c4cde12cbbe3ac4c60f3d3f65564bf Mon Sep 17 00:00:00 2001
From: qbp758 <qbp758@futharkhpa03fl.unicph.domain>
Date: Mon, 16 Oct 2023 18:48:09 +0200
Subject: [PATCH 6/7] delete test code

---
 python/rainbow/simulators/prox_soft_bodies/solver.py | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/python/rainbow/simulators/prox_soft_bodies/solver.py b/python/rainbow/simulators/prox_soft_bodies/solver.py
index ff8f0d2..e4874c0 100644
--- a/python/rainbow/simulators/prox_soft_bodies/solver.py
+++ b/python/rainbow/simulators/prox_soft_bodies/solver.py
@@ -174,8 +174,7 @@ def compute_D(x, T):
             u_mi = p[3] - p[0]
             # Verify that the tetrahedron is well-defined
             if np.dot(u_mi, np.cross(u_ji, u_ki)) <= 0:
-                # raise RuntimeError("compute_D(): Degenerate tetrahedron found", e)
-                print("compute_D(): Degenerate tetrahedron found", e)
+                raise RuntimeError("compute_D(): Degenerate tetrahedron found", e)
             # Create edge-vector matrix
             D[e] = M3.make(
                 u_ji[0],

From e1676a8b6e5389bd73ef973709dc457dc63f56bb Mon Sep 17 00:00:00 2001
From: qbp758 <qbp758@futharkhpa03fl.unicph.domain>
Date: Wed, 18 Oct 2023 10:09:25 +0200
Subject: [PATCH 7/7] delete test code

---
 python/rainbow/simulators/prox_soft_bodies/api.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/python/rainbow/simulators/prox_soft_bodies/api.py b/python/rainbow/simulators/prox_soft_bodies/api.py
index 8522e0d..9585287 100644
--- a/python/rainbow/simulators/prox_soft_bodies/api.py
+++ b/python/rainbow/simulators/prox_soft_bodies/api.py
@@ -84,7 +84,6 @@ def create_soft_body(engine, body_name, V, T) -> None:
         # Create bounding volume hierarchy data-structure (BVH), this will always be updated to live in
         # spatial coordinates and is tested against the signed distance field (who lives in constant material space) to
         # generate contact points.
-        print("init make body bvh")
         body.bvh = BVH.make_bvh(
             body.x,
             body.surface,