EG/plugins/user/navigation_mesh/tools/pathfinder.py

"""
路径查找器
实现基于导航网格的寻路算法
"""

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