sample surface

src/dtcc_builder/polygons/surface.py

@@ -1,9 +1,12 @@
+import shapely.affinity
 from shapely.geometry import Polygon, MultiPolygon
 from shapely.validation import make_valid
 
 import numpy as np
 from scipy.spatial.transform import Rotation as R
-from dtcc_model.geometry import Surface, MultiSurface
+from dtcc_model.geometry import Surface, MultiSurface, PointCloud
+
+from shapely import minimum_rotated_rectangle
 
 from dtcc_builder.polygons.polygons import remove_slivers
 from dtcc_builder.logging import info, warning, error, debug
@@ -64,6 +67,91 @@ def clean_surface(s: Surface, tol=1e-2) -> Surface:
         return _to_surface(surface_poly, inv_trans)
 
 
+def subdivide_surface(s: Surface, longest_edge=2) -> MultiSurface:
+    """Subdivide a Surface into smaller surfaces.
+
+    Args:
+        s (Surface): The Surface to subdivide.
+        longest_edge (float): The maximum length of the edges of the new surfaces.
+
+    Returns:
+        MultiSurface: The subdivided Surface
+    """
+    trans, inv_trans = _transform_to_planar(s)
+    surface_poly = _to_polygon(s, trans)
+    mrr = minimum_rotated_rectangle(surface_poly)
+
+    mrr_coords = np.array(mrr.exterior.coords)[:-1]
+    edge_vector = mrr_coords[1] - mrr_coords[0]
+    angle = np.arctan2(edge_vector[1], edge_vector[0])
+    cetroid = mrr.centroid
+    rotated_mrr = shapely.affinity.rotate(mrr, -angle, origin=cetroid, use_radians=True)
+
+    min_x, min_y, max_x, max_y = rotated_mrr.bounds
+
+    tiles = []
+    for x in np.arange(min_x, max_x, longest_edge):
+        for y in np.arange(min_y, max_y, longest_edge):
+            tile = Polygon(
+                [
+                    (x, y),
+                    (x + longest_edge, y),
+                    (x + longest_edge, y + longest_edge),
+                    (x, y + longest_edge),
+                    (x, y),
+                ]
+            )
+            tiles.append(tile)
+    mp_tile = MultiPolygon(tiles)
+    mp_tile = shapely.affinity.rotate(mp_tile, angle, origin=cetroid, use_radians=True)
+    tiles = []
+    for t in mp_tile.geoms:
+        tiles.append(t.intersection(surface_poly))
+    mp_tile_clipped = MultiPolygon(tiles)
+
+
+def surface_sample_points(s: Surface, spacing=1.0) -> PointCloud:
+    """Sample a Surface to a PointCloud.
+
+    Args:
+        s (Surface): The Surface to sample.
+        spacing (float): spacing between the points.
+
+    Returns:
+        PointCloud: The sampled Point
+    """
+    trans, inv_trans = _transform_to_planar(s)
+    if trans is None:
+        error(f"Failed to sample surface.")
+    surface_poly = _to_polygon(s, trans)
+    mrr = minimum_rotated_rectangle(surface_poly)
+    mrr_coords = np.array(mrr.exterior.coords)[:-1]
+
+    # Calculate the angle of rotation
+    edge_vector = mrr_coords[1] - mrr_coords[0]
+    angle = np.arctan2(edge_vector[1], edge_vector[0])
+
+    # Create a rotation matrix for aligning with the main axis
+    rotation_matrix = np.array(
+        [[np.cos(-angle), -np.sin(-angle)], [np.sin(-angle), np.cos(-angle)]]
+    )
+    inverse_rotation_matrix = np.linalg.inv(rotation_matrix)
+
+    rotated_rect_coords = np.dot(mrr_coords, rotation_matrix.T)
+
+    # Determine the bounding box of the rotated minimal rectangle
+    min_x, min_y = rotated_rect_coords.min(axis=0)
+    max_x, max_y = rotated_rect_coords.max(axis=0)
+
+    # Generate a grid of points within the bounding box of the rotated minimal rectangle
+    x_range = np.arange(min_x, max_x, spacing)
+    y_range = np.arange(min_y, max_y, spacing)
+    grid_points = [np.array([x, y]) for x in x_range for y in y_range]
+
+    # Rotate the points back to the original coordinate system
+    aligned_points = [np.dot(point, inverse_rotation_matrix.T) for point in grid_points]
+
+
 def _to_polygon(s: Surface, transform) -> Polygon:
     """Convert a Surface to a Polygon."""
     transformed_vertices = np.dot(s.vertices, transform[:3, :3].T) + transform[:3, 3]