""" 路径查找器 实现基于导航网格的寻路算法 """ import heapq import math from typing import List, Optional, Dict, Tuple from panda3d.core import Point3 class PathfinderNode: """ 寻路节点 用于A*算法中的节点表示 """ def __init__(self, polygon_id: int, position: Point3): self.polygon_id = polygon_id self.position = position self.g_cost = 0.0 # 从起点到当前节点的实际代价 self.h_cost = 0.0 # 启发式代价（到终点的估计代价） self.f_cost = 0.0 # 总代价 (g_cost + h_cost) self.parent = None # 父节点 def __lt__(self, other): """用于优先队列比较""" return self.f_cost < other.f_cost def __eq__(self, other): """用于节点比较""" return self.polygon_id == other.polygon_id class Pathfinder: """ 路径查找器 实现基于导航网格的A*寻路算法 """ def __init__(self, navmesh_manager): """ 初始化路径查找器 Args: navmesh_manager: 导航网格管理器 """ self.navmesh_manager = navmesh_manager # 寻路设置 self.heuristic_weight = 1.0 # 启发式权重 self.max_path_nodes = 10000 # 最大路径节点数 # 性能统计 self.stats = { 'paths_found': 0, 'paths_not_found': 0, 'total_search_time': 0.0, 'total_nodes_searched': 0 } print("✓ 路径查找器初始化完成") def find_path(self, start_point: Point3, end_point: Point3) -> Optional[List[Point3]]: """ 查找从起点到终点的路径 Args: start_point: 起点 end_point: 终点 Returns: 路径点列表，如果找不到路径则返回None """ try: # 检查导航网格是否可用 if not self.navmesh_manager or not self.navmesh_manager.polygons: print("⚠ 导航网格不可用") return None # 获取起点和终点所在的多边形 start_polygon = self.navmesh_manager.get_polygon_containing_point(start_point) end_polygon = self.navmesh_manager.get_polygon_containing_point(end_point) # 如果起点或终点不在导航网格上，使用最近的点 if not start_polygon: closest_start = self.navmesh_manager.get_closest_point(start_point) if not closest_start: print("⚠ 无法找到起点附近的可行走区域") return None start_point = closest_start start_polygon = self.navmesh_manager.get_polygon_containing_point(start_point) if not end_polygon: closest_end = self.navmesh_manager.get_closest_point(end_point) if not closest_end: print("⚠ 无法找到终点附近的可行走区域") return None end_point = closest_end end_polygon = self.navmesh_manager.get_polygon_containing_point(end_point) # 如果起点和终点在同一个多边形内，直接连接 if start_polygon.polygon_id == end_polygon.polygon_id: return [start_point, end_point] # 使用A*算法查找路径 path = self._a_star_search(start_polygon.polygon_id, end_polygon.polygon_id, start_point, end_point) if path: self.stats['paths_found'] += 1 print(f"✓ 找到路径，包含 {len(path)} 个节点") return path else: self.stats['paths_not_found'] += 1 print("⚠ 未找到路径") return None except Exception as e: print(f"✗ 路径查找失败: {e}") return None def _a_star_search(self, start_polygon_id: int, end_polygon_id: int, start_point: Point3, end_point: Point3) -> Optional[List[Point3]]: """ A*寻路算法实现 Args: start_polygon_id: 起始多边形ID end_polygon_id: 目标多边形ID start_point: 起点 end_point: 终点 Returns: 路径点列表，如果找不到路径则返回None """ import time start_time = time.time() # 开放列表（优先队列） open_list = [] # 关闭列表（已访问的节点） closed_list = set() # 创建起始节点 start_node = PathfinderNode(start_polygon_id, start_point) start_node.g_cost = 0 start_node.h_cost = self._calculate_heuristic(start_point, end_point) start_node.f_cost = start_node.g_cost + start_node.h_cost # 将起始节点加入开放列表 heapq.heappush(open_list, start_node) # 节点搜索计数 nodes_searched = 0 while open_list and nodes_searched < self.max_path_nodes: # 取出f_cost最小的节点 current_node = heapq.heappop(open_list) nodes_searched += 1 # 如果到达目标节点 if current_node.polygon_id == end_polygon_id: # 重构路径 path = self._reconstruct_path(current_node) # 添加终点 path.append(end_point) self.stats['total_search_time'] += time.time() - start_time self.stats['total_nodes_searched'] += nodes_searched return path # 将当前节点加入关闭列表 closed_list.add(current_node.polygon_id) # 获取当前多边形 if current_node.polygon_id not in self.navmesh_manager.polygons: continue current_polygon = self.navmesh_manager.polygons[current_node.polygon_id] # 检查所有相邻多边形 for neighbor_id in current_polygon.neighbors: # 如果已在关闭列表，跳过 if neighbor_id in closed_list: continue # 获取邻居多边形 if neighbor_id not in self.navmesh_manager.polygons: continue neighbor_polygon = self.navmesh_manager.polygons[neighbor_id] # 计算到邻居节点的代价 neighbor_position = neighbor_polygon.center tentative_g_cost = current_node.g_cost + \ self._calculate_distance(current_node.position, neighbor_position) # 检查是否需要更新邻居节点 existing_node = self._find_node_in_open_list(open_list, neighbor_id) if existing_node is None: # 创建新节点 neighbor_node = PathfinderNode(neighbor_id, neighbor_position) neighbor_node.g_cost = tentative_g_cost neighbor_node.h_cost = self._calculate_heuristic(neighbor_position, end_point) neighbor_node.f_cost = neighbor_node.g_cost + neighbor_node.h_cost neighbor_node.parent = current_node heapq.heappush(open_list, neighbor_node) elif tentative_g_cost < existing_node.g_cost: # 更新现有节点 existing_node.g_cost = tentative_g_cost existing_node.f_cost = existing_node.g_cost + existing_node.h_cost existing_node.parent = current_node # 未找到路径 self.stats['total_search_time'] += time.time() - start_time self.stats['total_nodes_searched'] += nodes_searched return None def _calculate_heuristic(self, point1: Point3, point2: Point3) -> float: """ 计算启发式代价（使用欧几里得距离） Args: point1: 起点 point2: 终点 Returns: 启发式代价 """ return (point2 - point1).length() * self.heuristic_weight def _calculate_distance(self, point1: Point3, point2: Point3) -> float: """ 计算两点间距离 Args: point1: 第一个点 point2: 第二个点 Returns: 两点间距离 """ return (point2 - point1).length() def _find_node_in_open_list(self, open_list: List[PathfinderNode], polygon_id: int) -> Optional[PathfinderNode]: """ 在开放列表中查找指定多边形ID的节点 Args: open_list: 开放列表 polygon_id: 多边形ID Returns: 找到的节点，如果未找到则返回None """ for node in open_list: if node.polygon_id == polygon_id: return node return None def _reconstruct_path(self, end_node: PathfinderNode) -> List[Point3]: """ 重构路径 Args: end_node: 终点节点 Returns: 路径点列表 """ path = [] current = end_node # 从终点回溯到起点 while current is not None: path.append(current.position) current = current.parent # 反转路径（从起点到终点） path.reverse() return path def smooth_path(self, path: List[Point3]) -> List[Point3]: """ 平滑路径（可选优化） Args: path: 原始路径 Returns: 平滑后的路径 """ if len(path) <= 2: return path smoothed_path = [path[0]] # 简单的路径平滑算法 i = 0 while i < len(path) - 1: # 尝试直接连接当前点和后面的点 j = len(path) - 1 while j > i: # 检查两点间是否可以直接通行 if self._is_line_walkable(path[i], path[j]): smoothed_path.append(path[j]) i = j break j -= 1 else: # 如果不能直接连接，添加下一个点 i += 1 if i < len(path): smoothed_path.append(path[i]) return smoothed_path def _is_line_walkable(self, start: Point3, end: Point3) -> bool: """ 检查两点间直线是否可行走 Args: start: 起点 end: 终点 Returns: 是否可行走 """ # 简化实现：检查线段上的几个点是否都在可行走区域 steps = 10 for i in range(steps + 1): t = i / steps point = start + (end - start) * t if not self.navmesh_manager.is_point_walkable(point): return False return True def get_pathfinding_stats(self) -> Dict: """ 获取寻路统计信息 Returns: 统计信息字典 """ return self.stats.copy() def reset_stats(self): """重置统计信息""" self.stats = { 'paths_found': 0, 'paths_not_found': 0, 'total_search_time': 0.0, 'total_nodes_searched': 0 } def set_heuristic_weight(self, weight: float): """ 设置启发式权重 Args: weight: 权重值 """ self.heuristic_weight = max(0.0, weight) def set_max_path_nodes(self, max_nodes: int): """ 设置最大路径节点数 Args: max_nodes: 最大节点数 """ self.max_path_nodes = max(1, max_nodes) def find_path_with_multiple_waypoints(self, waypoints: List[Point3]) -> Optional[List[Point3]]: """ 查找经过多个路点的路径 Args: waypoints: 路点列表 Returns: 完整路径，如果任何一段找不到则返回None """ if len(waypoints) < 2: return None # 查找每段路径并连接 full_path = [] for i in range(len(waypoints) - 1): segment_path = self.find_path(waypoints[i], waypoints[i + 1]) if not segment_path: return None # 如果任何一段找不到路径，则整个路径失败 # 避免重复点 if full_path: full_path.pop() # 移除上一段的终点（与这一段的起点重复） full_path.extend(segment_path) return full_path def find_random_path(self) -> Optional[List[Point3]]: """ 查找随机路径（用于测试） Returns: 随机路径 """ if not self.navmesh_manager: return None start_point = self.navmesh_manager.get_random_point() end_point = self.navmesh_manager.get_random_point() if start_point and end_point: return self.find_path(start_point, end_point) return None