路径规划（二）从占用图到安全区域：四叉树（QuadTree）构建与凸包生成

class QuadTree:
    def __init__(self, value_matrix: np.ndarray, min_cell_size: int, num_splits:int=2):
        self.root = QuadTreeNode.init_root(value_matrix, self.max_depth, auto_split=True)
        self.leaf_nodes : list[QuadTreeNode] = []
        self._collect_free_leaf_nodes(self.root)

quadtree = QuadTree(occupancy_grid, min_cell_size=5)

hull = ConvexHull(points)

from scipy.spatial import ConvexHull
import numpy as np

def _create_convex_hulls(self):
    """Create convex hulls for leaf nodes.
    
    This method creates convex hulls for all free leaf nodes in the quadtree.
    The convex hulls are stored in the self.convex_hulls dictionary with the
    node's id_sequence as the key.
    """
    self.convex_hulls = {}  # 确保字典被初始化
    
    for leaf_node in self.leaf_nodes:
        # 使用节点的正确属性获取顶点
        vertices = np.array(leaf_node.vertices)
        
        # 检查顶点是否足够构成凸包（至少3个点）
        if len(vertices) >= 3:
            # 计算凸包
            hull = ConvexHull(vertices)
            # 存储凸包，使用节点的id_sequence作为键
            self.convex_hulls[leaf_node.id_sequence] = hull
        else:
            # 如果点不足以形成凸包，可以跳过或记录警告
            print(f"Warning: Node {leaf_node.id_sequence} has insufficient vertices for convex hull")
            

def _collect_free_leaf_nodes(self, node: "QuadTreeNode"):
    if node.is_leaf and node.state == TreeNodeState.FREE:
        self.leaf_nodes.append(node)
    else:
        for child in node.children:
            self._collect_free_leaf_nodes(child)

def update_quadtree_with_obstacle(self, obstacle_position: tuple, obstacle_radius: float):
  
        def is_node_affected(node, obstacle_position, obstacle_radius):
            # Check if the node intersects with the obstacle's influence radius
            x_min, x_max, y_min, y_max = node.get_bounds()
            x, y = obstacle_position
            return (
                x_min - obstacle_radius <= x <= x_max + obstacle_radius and
                y_min - obstacle_radius <= y <= y_max + obstacle_radius
            )

        def recursive_update(node):
            if is_node_affected(node, obstacle_position, obstacle_radius):
                if not node.is_leaf():
                    # Recursively check children
                    for child in node.children:
                        recursive_update(child)
                else:
                    # If it's a leaf node and affected, decide whether to split or mark as occupied
                    if node.size > self.min_cell_size:
                        node.split()
                    else:
                        node.mark_occupied()  # Mark the node as occupied
            elif node.is_leaf():
                # Merge unaffected nodes if previously split
                node.merge()

        recursive_update(self.root)
        self._collect_free_leaf_nodes(self.root)
     

# 邻居关系更新   
    def update_neighbors(node):
        """
        Update the neighbor relationships for a given node.
        """
        for neighbor in node.neighbors:
            if neighbor.is_leaf():
                # Update neighbor relationship based on current structure
                neighbor.neighbors = [n for n in neighbor.get_adjacent_nodes()]

def dynamic_update_and_plan(self, obstacle_positions, time_horizon)

def collect_tree_nodes_on_path(self, reference_path: list[PathNode], height_occupancy: int, threshold: float, rescale_geo_occupancy: int, rescale_geo_inflated_occupancy:float):

@property
def state(self) -> TreeNodeState:
    if np.all(self.values < 0.01):
        return TreeNodeState.FREE
    elif len(self.id_sequence) == self._max_depth:
        return TreeNodeState.FULL
    else:
        return TreeNodeState.MIX

#节点级别凸包运算，返回的凸包列表可以直接用于路径规划、安全区域描述或导航约束。
    def path_tree_nodes_convexhull(self, path_tree_nodes: dict[tuple, "QuadTreeNode"]):
        path_convex_hulls = []

        for key, tn in path_tree_nodes.items():
            vertices = np.array(tn.vertices)
            # Compute convex hull for transformed vertices
            hull = ConvexHull(tn.vertices)
            # Store convex hull along with transformed vertices in the dictionary
            path_convex_hulls.append(hull)
        return path_convex_hulls

for node in 当前的所有叶子节点:
    提取对应新地图中的这块区域 new_subgrid
    if new_subgrid 有障碍物（值是 [0,0,0]）:
        if node 可以继续分裂:
            node.split()
            替换掉原来的节点
        else:
            直接删掉这个叶子节点
    else:
        保持原样

for leaf_node in self.leaf_nodes:
    points = np.array([[y, x], [y, x+size], [y+size, x+size], [y+size, x]])
    hull = ConvexHull(points)
    self.convex_hulls.append(hull)

reference_line = LineString(reference_cells_occupancy)
for hull in self.convex_hulls:
    if Polygon(hull.points[hull.vertices]).intersects(reference_line):
        path_convex_hulls.append(hull)