（十一）

import laspy
import numpy as np
import open3d as o3d
from shapely.geometry import Polygon, LineString
from shapely.ops import nearest_points


def facade_follow_planner(las_path, target_z_levels, offset_dist=5.0):
    # --- 1. 加载点云 ---
    las = laspy.read(las_path)
    points = np.vstack((las.x, las.y, las.z)).transpose()

    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points)

    # --- 2. 目标建筑提取 (以欧式聚类为例) ---
    # 这一步将点云按空间距离分组，label -1 为噪声
    labels = np.array(pcd.cluster_dbscan(eps=0.5, min_points=100))
    print(labels)

    # 假设我们通过某种逻辑选定了 label 为 0 的建筑
    # (或者根据已知坐标过滤：pcd = pcd.select_by_index(np.where(...)))
    target_idx = np.where(labels == 1)[0]
    building_pcd = pcd.select_by_index(target_idx)

    # 显示被提取出的建筑物点云
    o3d.visualization.draw_geometries([building_pcd],
                                      window_name="提取出的建筑物",
                                      width=800,
                                      height=600)

    # --- 3. 立面仿地核心逻辑：切片与偏移 ---
    all_waypoints = []

    # 按照设定的高度层级进行循环
    for z in target_z_levels:
        # 切取厚度为 0.2m 的薄片
        z_min, z_max = z - 0.1, z + 0.1
        slice_idx = np.where((np.asarray(building_pcd.points)[:, 2] >= z_min) &
                             (np.asarray(building_pcd.points)[:, 2] <= z_max))[0]
        slice_pcd = building_pcd.select_by_index(slice_idx)

        if len(slice_pcd.points) < 10: continue

        # 投影到 XY 平面提取轮廓
        points_2d = np.asarray(slice_pcd.points)[:, :2]

        # 使用 Alpha Shape 提取边界线 (此处简化逻辑)
        # 实际操作中建议使用专用的 Alpha Shape 库
        # 这里假设得到了 boundary_coords: [(x1,y1), (x2,y2)...]
        line = LineString(points_2d)

        # --- 4. 核心：实现“仿地”偏移 ---
        # parallel_offset 自动处理了内角和外角的几何拓扑，非常稳健
        offset_line = line.parallel_offset(offset_dist, side='left')

        # 在偏移线上采样生成航点
        for pt in offset_line.coords:
            # 计算该点相对于原墙面的法向朝向 (Yaw)
            # 通过找到原轮廓线上最近的点来推算方向
            p_on_wall = line.interpolate(line.project(Point(pt)))
            dx = pt[0] - p_on_wall.x
            dy = pt[1] - p_on_wall.y
            yaw = np.degrees(np.arctan2(dy, dx))

            all_waypoints.append({
                'coord': (pt[0], pt[1], z),
                'yaw': yaw
            })

    return all_waypoints


if __name__ == "__main__":
    levels = np.arange(10, 50, 3)  # 从10米到50米，每3米一层
    waypoints = facade_follow_planner("input/cloud.las", levels)