Spatial Grids

1# Calculating the Grid Extents
2import math
3
4def create_grid(points, cell_size):
5    """Defines the boundaries of our voxel grid."""
6    
7    # 1. Find the Bounding Box of the entire point cloud
8    min_x = min([p.x for p in points])
9    max_x = max([p.x for p in points])
10    min_y = min([p.y for p in points])
11    max_y = max([p.y for p in points])
12    min_z = min([p.z for p in points])
13    max_z = max([p.z for p in points])
14    
15    # 2. Divide by the cell size to find out how many 
16    # rows, columns, and layers we need!
17    count_x = math.ceil((max_x - min_x) / cell_size)
18    count_y = math.ceil((max_y - min_y) / cell_size)
19    count_z = math.ceil((max_z - min_z) / cell_size)
20    
21    return min_x, min_y, min_z, count_x, count_y, count_z

1# Quantizing Floating Point to Integers
2def get_cell_coordinates(point, min_corner, cell_size):
3    """Converts a float (1.234, 4.567) into an integer cell index (1, 4)."""
4    
5    # 1. Shift the point relative to the bottom-left corner of the grid
6    local_x = point.x - min_corner.x
7    local_y = point.y - min_corner.y
8    local_z = point.z - min_corner.z
9    
10    # 2. Divide by the size of the cells
11    # 3. Floor the result! (Chop off all decimal places)
12    grid_x = math.floor(local_x / cell_size)
13    grid_y = math.floor(local_y / cell_size)
14    grid_z = math.floor(local_z / cell_size)
15    
16    return grid_x, grid_y, grid_z

1# Flat Array Indexing
2def get_hash_key(grid_x, grid_y, grid_z, count_x, count_y):
3    """Turns a 3D index into a single 1D array index."""
4    
5    # Imagine a bookshelf:
6    # 1. Skip over all the completed 'shelves' below you (grid_z)
7    # 2. Skip over all the completed 'rows' on your shelf (grid_y)
8    # 3. Add your position in the current row (grid_x)
9    
10    index = grid_x + (grid_y * count_x) + (grid_z * count_x * count_y)
11    
12    return index

1# Creating the Storage Array
2def insert_points_into_grid(points, total_cell_count):
3    """Sorts all points into their respective boxes."""
4    
5    # 1. Create a giant empty list of lists (Buckets)
6    grid_buckets = [ [] for _ in range(total_cell_count) ]
7    
8    # 2. Put every point into a bucket based on its Hash ID
9    for pt in points:
10        # Get coordinates
11        gx, gy, gz = get_cell_coordinates(pt)
12        
13        # Get ID
14        bucket_id = get_hash_key(gx, gy, gz)
15        
16        # Add the point to the bucket
17        grid_buckets[bucket_id].append(pt)
18        
19    return grid_buckets

1# Extremely Fast Searching
2def find_points_in_radius(query_pt, radius, grid_buckets, cell_size):
3    """How to search 1 million points without lagging."""
4    
5    found_points = []
6    
7    # 1. Which box is our query point in?
8    qx, qy, qz = get_cell_coordinates(query_pt)
9    
10    # 2. How many boxes do we need to check? (Radius / CellSize)
11    box_range = math.ceil(radius / cell_size)
12    
13    # 3. Only look inside the nearby boxes!
14    for x in range(qx - box_range, qx + box_range + 1):
15        for y in range(qy - box_range, qy + box_range + 1):
16            for z in range(qz - box_range, qz + box_range + 1):
17                
18                bucket_id = get_hash_key(x, y, z)
19                points_to_check = grid_buckets[bucket_id]
20                
21                # Check actual distance (Math.sqrt) only for these few points
22                for pt in points_to_check:
23                    if distance(query_pt, pt) <= radius:
24                        found_points.append(pt)
25                        
26    return found_points

1# Calculating Grid Density
2def calculate_voxel_density(grid_buckets):
3    """How full is each box?"""
4    
5    dense_voxels = []
6    
7    for bucket in grid_buckets:
8        # Check how many points fell into this box
9        point_count = len(bucket.points)
10        
11        # If it has more than 5 points, we consider it "Solid"
12        if point_count > 5:
13            dense_voxels.append(bucket)
14            
15    return dense_voxels

1# Point Cloud Voxelization
2def voxelize_bunny(bunny_points, resolution):
3    """Voxelizes a 3D scan of the Stanford Bunny."""
4    
5    # 1. Create the grid based on the Bunny's bounding box
6    grid = create_grid(bunny_points, resolution)
7    
8    # 2. Toss all the points into the buckets
9    buckets = insert_points(bunny_points, grid)
10    
11    # 3. Find the dense boxes
12    solid_voxels = calculate_voxel_density(buckets)
13    
14    return solid_voxels