diff --git a/python/rainbow/geometry/aabb.py b/python/rainbow/geometry/aabb.py new file mode 100644 index 0000000..55018da --- /dev/null +++ b/python/rainbow/geometry/aabb.py @@ -0,0 +1,72 @@ +import numpy as np +from numpy.typing import ArrayLike + + +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 (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: ArrayLike) -> '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: 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: ArrayLike) -> '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', boundary: float = 0.0) -> bool: + """ Test two aabb instance are overlap or not + + Args: + 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 + """ + 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 new file mode 100644 index 0000000..68df209 --- /dev/null +++ b/python/rainbow/geometry/spatial_hashing.py @@ -0,0 +1,216 @@ +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). + + 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) + 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 + 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 = 1000 + self.hash_tbale = dict() + self.cell_size = 0.0 + + # 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): + """ Set the size of the hash table + + Args: + hash_table_size (int32): The size of the hash table + """ + 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 + + Args: + cell_size (float): the cell size + """ + 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. + 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 + 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(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: + 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_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 + + 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_idx, tri_aabb), time_stamp) + else: + overlaps = self.hash_tbale[hv].object_list + self.hash_tbale[hv].add((tri_idx, body_idx, tri_aabb), time_stamp) + return overlaps + + @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]]) + 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) + + # 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 732813e..9585287 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 +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 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 @@ -69,22 +71,33 @@ 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. + 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) + 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 e5f0832..49723f4 100644 --- a/python/rainbow/simulators/prox_soft_bodies/collision_detection.py +++ b/python/rainbow/simulators/prox_soft_bodies/collision_detection.py @@ -1,10 +1,12 @@ 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 +from collections import defaultdict def _update_bvh(engine, stats, debug_on): @@ -38,6 +40,150 @@ 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 + + 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 = defaultdict(set) + + 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.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_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() + stats["narrow_phase"] = narrow_phase_timer.elapsed + stats["number_of_overlaps"] = np.sum( + [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 + + def _narrow_phase(engine, stats, debug_on): """ @@ -347,8 +493,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() 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