EG/plugins/user/pathfinding_algorithms/visualization/visualization_manager.py

"""
可视化管理器
负责路径规划算法的可视化显示
"""

from typing import List, Tuple, Optional, Dict, Any
from panda3d.core import NodePath, GeomNode, GeomVertexFormat, GeomVertexData
from panda3d.core import GeomVertexWriter, GeomTriangles, GeomLines, GeomPoints
from panda3d.core import LColor, Point3, Material, TransparencyAttrib
import math

class VisualizationManager:
    """
    可视化管理器
    负责路径规划算法的可视化显示
    """
    
    def __init__(self, world):
        """
        初始化可视化管理器
        
        Args:
            world: 3D世界对象
        """
        self.world = world
        self.enabled = False
        
        # 可视化节点
        self.grid_node = None
        self.path_node = None
        self.start_node = None
        self.goal_node = None
        self.visited_nodes_node = None
        self.open_list_node = None
        
        # 可视化设置
        self.settings = {
            'grid_visible': True,
            'path_visible': True,
            'start_visible': True,
            'goal_visible': True,
            'visited_nodes_visible': False,
            'open_list_visible': False,
            'grid_color': LColor(0.5, 0.5, 0.5, 1.0),
            'path_color': LColor(1.0, 0.0, 0.0, 1.0),
            'start_color': LColor(0.0, 1.0, 0.0, 1.0),
            'goal_color': LColor(0.0, 0.0, 1.0, 1.0),
            'visited_nodes_color': LColor(0.0, 0.5, 1.0, 0.7),
            'open_list_color': LColor(1.0, 1.0, 0.0, 0.7),
            'obstacle_color': LColor(0.2, 0.2, 0.2, 1.0),
            'cell_size': 1.0,
            'cell_height': 0.1,
            'path_thickness': 4.0,
            'show_coordinates': False,
            'animation_speed': 1.0
        }
        
        # 当前显示的数据
        self.current_grid = None
        self.current_path = None
        self.current_start = None
        self.current_goal = None
        self.visited_nodes = []
        self.open_list = []
        
        # 动画相关
        self.animation_enabled = False
        self.animation_progress = 0.0
        self.animation_path = []
        self.animation_timer = 0.0
        
        # 材质
        self.grid_material = None
        self.path_material = None
        self.start_material = None
        self.goal_material = None
        
    def enable(self):
        """启用可视化管理器"""
        if self.enabled:
            return
            
        self.enabled = True
        self._create_visualization_nodes()
        self._create_materials()
        print("✓ 可视化管理器已启用")
        
    def disable(self):
        """禁用可视化管理器"""
        if not self.enabled:
            return
            
        self.enabled = False
        self._cleanup_visualization_nodes()
        print("✓ 可视化管理器已禁用")
        
    def cleanup(self):
        """清理资源"""
        self.disable()
        self.current_grid = None
        self.current_path = None
        self.current_start = None
        self.current_goal = None
        self.visited_nodes = []
        self.open_list = []
        self.animation_path = []
        
    def _create_visualization_nodes(self):
        """创建可视化节点"""
        if not self.world or not self.world.render:
            return
            
        # 创建网格可视化节点
        self.grid_node = self.world.render.attachNewNode("pathfinding_grid")
        
        # 创建路径可视化节点
        self.path_node = self.world.render.attachNewNode("pathfinding_path")
        
        # 创建起点可视化节点
        self.start_node = self.world.render.attachNewNode("pathfinding_start")
        
        # 创建终点可视化节点
        self.goal_node = self.world.render.attachNewNode("pathfinding_goal")
        
        # 创建已访问节点可视化节点
        self.visited_nodes_node = self.world.render.attachNewNode("pathfinding_visited_nodes")
        
        # 创建开放列表可视化节点
        self.open_list_node = self.world.render.attachNewNode("pathfinding_open_list")
        
    def _cleanup_visualization_nodes(self):
        """清理可视化节点"""
        nodes = [
            self.grid_node, self.path_node, self.start_node, self.goal_node,
            self.visited_nodes_node, self.open_list_node
        ]
        
        for node in nodes:
            if node:
                node.removeNode()
                
        self.grid_node = None
        self.path_node = None
        self.start_node = None
        self.goal_node = None
        self.visited_nodes_node = None
        self.open_list_node = None
        
    def _create_materials(self):
        """创建材质"""
        # 网格材质
        self.grid_material = Material()
        self.grid_material.setAmbient(self.settings['grid_color'])
        self.grid_material.setDiffuse(self.settings['grid_color'])
        
        # 路径材质
        self.path_material = Material()
        self.path_material.setAmbient(self.settings['path_color'])
        self.path_material.setDiffuse(self.settings['path_color'])
        
        # 起点材质
        self.start_material = Material()
        self.start_material.setAmbient(self.settings['start_color'])
        self.start_material.setDiffuse(self.settings['start_color'])
        
        # 终点材质
        self.goal_material = Material()
        self.goal_material.setAmbient(self.settings['goal_color'])
        self.goal_material.setDiffuse(self.settings['goal_color'])
        
    def toggle_visibility(self):
        """切换可视化可见性"""
        if not self.enabled:
            return
            
        self.settings['grid_visible'] = not self.settings['grid_visible']
        self.settings['path_visible'] = not self.settings['path_visible']
        self.settings['start_visible'] = not self.settings['start_visible']
        self.settings['goal_visible'] = not self.settings['goal_visible']
        self.settings['visited_nodes_visible'] = not self.settings['visited_nodes_visible']
        self.settings['open_list_visible'] = not self.settings['open_list_visible']
        
        self._update_visibility()
        print("✓ 可视化可见性已切换")
        
    def _update_visibility(self):
        """更新可视化元素的可见性"""
        visibility_map = {
            self.grid_node: self.settings['grid_visible'],
            self.path_node: self.settings['path_visible'],
            self.start_node: self.settings['start_visible'],
            self.goal_node: self.settings['goal_visible'],
            self.visited_nodes_node: self.settings['visited_nodes_visible'],
            self.open_list_node: self.settings['open_list_visible']
        }
        
        for node, visible in visibility_map.items():
            if node:
                if visible:
                    node.show()
                else:
                    node.hide()
                    
    def visualize_grid(self, grid: List[List[int]]):
        """
        可视化网格
        
        Args:
            grid: 网格数据
        """
        if not self.enabled or not self.grid_node:
            return
            
        self.current_grid = grid
        self._create_grid_geometry()
        print(f"✓ 网格已可视化 ({len(grid)}x{len(grid[0]) if grid else 0})")
        
    def _create_grid_geometry(self):
        """创建网格几何体"""
        if not self.current_grid or not self.grid_node:
            return
            
        # 清除现有几何体
        self.grid_node.node().removeAllGeoms()
        
        rows = len(self.current_grid)
        cols = len(self.current_grid[0]) if rows > 0 else 0
        if rows == 0 or cols == 0:
            return
            
        cell_size = self.settings['cell_size']
        cell_height = self.settings['cell_height']
        grid_color = self.settings['grid_color']
        obstacle_color = self.settings['obstacle_color']
        
        # 创建顶点格式
        vertex_format = GeomVertexFormat.getV3c4()
        vertex_data = GeomVertexData("grid", vertex_format, Geom.UHStatic)
        
        # 计算总顶点数（每个格子4个顶点）
        total_vertices = rows * cols * 4
        vertex_data.setNumRows(total_vertices)
        
        # 创建顶点写入器
        vertex_writer = GeomVertexWriter(vertex_data, "vertex")
        color_writer = GeomVertexWriter(vertex_data, "color")
        
        # 创建几何体
        geom = Geom(vertex_data)
        triangles = GeomTriangles(Geom.UHStatic)
        
        vertex_index = 0
        for row in range(rows):
            for col in range(cols):
                # 计算格子位置
                x = col * cell_size - (cols * cell_size) / 2
                z = row * cell_size - (rows * cell_size) / 2
                y = 0 if self.current_grid[row][col] == 0 else cell_height
                
                # 选择颜色（可通行区域或障碍物）
                color = grid_color if self.current_grid[row][col] == 0 else obstacle_color
                
                # 创建格子的四个顶点
                vertices = [
                    Point3(x, y, z),
                    Point3(x + cell_size, y, z),
                    Point3(x + cell_size, y, z + cell_size),
                    Point3(x, y, z + cell_size)
                ]
                
                # 写入顶点数据
                for vertex in vertices:
                    vertex_writer.addData3f(vertex.x, vertex.y, vertex.z)
                    color_writer.addData4f(color)
                    
                # 添加两个三角形
                triangles.addVertex(vertex_index)
                triangles.addVertex(vertex_index + 1)
                triangles.addVertex(vertex_index + 2)
                triangles.closePrimitive()
                
                triangles.addVertex(vertex_index)
                triangles.addVertex(vertex_index + 2)
                triangles.addVertex(vertex_index + 3)
                triangles.closePrimitive()
                
                vertex_index += 4
                
        geom.addPrimitive(triangles)
        self.grid_node.node().addGeom(geom)
        
        # 应用材质
        if self.grid_material:
            self.grid_node.setMaterial(self.grid_material, 1)
            
        # 添加坐标文本（如果启用）
        if self.settings['show_coordinates']:
            self._add_coordinate_labels(rows, cols, cell_size)
            
    def _add_coordinate_labels(self, rows: int, cols: int, cell_size: float):
        """
        添加坐标标签
        
        Args:
            rows: 行数
            cols: 列数
            cell_size: 单元格大小
        """
        # 这里可以添加坐标标签的实现
        # 为了简化，我们只打印信息
        print(f"  坐标标签已启用 ({rows}x{cols})")
        
    def visualize_path(self, path: List[Tuple[int, int]], grid: Optional[List[List[int]]] = None):
        """
        可视化路径
        
        Args:
            path: 路径坐标列表
            grid: 网格数据（可选）
        """
        if not self.enabled:
            return
            
        self.current_path = path
        if grid is not None:
            self.current_grid = grid
            
        self._create_path_geometry()
        self._update_visibility()
        print(f"✓ 路径已可视化 (长度: {len(path) if path else 0})")
        
    def _create_path_geometry(self):
        """创建路径几何体"""
        if not self.current_path or not self.path_node:
            return
            
        # 清除现有几何体
        self.path_node.node().removeAllGeoms()
        
        if len(self.current_path) < 2:
            return
            
        cell_size = self.settings['cell_size']
        rows = len(self.current_grid) if self.current_grid else 0
        cols = len(self.current_grid[0]) if self.current_grid and rows > 0 else 0
        
        # 创建顶点格式
        vertex_format = GeomVertexFormat.getV3c4()
        vertex_data = GeomVertexData("path", vertex_format, Geom.UHStatic)
        vertex_data.setNumRows(len(self.current_path))
        
        # 创建顶点写入器
        vertex_writer = GeomVertexWriter(vertex_data, "vertex")
        color_writer = GeomVertexWriter(vertex_data, "color")
        
        # 写入路径点
        for i, (row, col) in enumerate(self.current_path):
            x = col * cell_size - (cols * cell_size) / 2 + cell_size / 2
            z = row * cell_size - (rows * cell_size) / 2 + cell_size / 2
            y = 0.2  # 稍微抬高以避免与网格重叠
            
            vertex_writer.addData3f(x, y, z)
            color_writer.addData4f(self.settings['path_color'])
            
        # 创建几何体
        geom = Geom(vertex_data)
        lines = GeomLines(Geom.UHStatic)
        
        # 添加线段
        for i in range(len(self.current_path) - 1):
            lines.addVertex(i)
            lines.addVertex(i + 1)
            lines.closePrimitive()
            
        geom.addPrimitive(lines)
        self.path_node.node().addGeom(geom)
        
        # 设置线宽和材质
        self.path_node.setRenderModeThickness(self.settings['path_thickness'])
        if self.path_material:
            self.path_node.setMaterial(self.path_material, 1)
            
    def visualize_visited_nodes(self, visited_nodes: List[Tuple[int, int]]):
        """
        可视化已访问节点
        
        Args:
            visited_nodes: 已访问节点列表
        """
        if not self.enabled:
            return
            
        self.visited_nodes = visited_nodes
        self._create_visited_nodes_geometry()
        print(f"✓ 已访问节点已可视化 (数量: {len(visited_nodes)})")
        
    def _create_visited_nodes_geometry(self):
        """创建已访问节点几何体"""
        if not self.visited_nodes or not self.visited_nodes_node:
            return
            
        # 清除现有几何体
        self.visited_nodes_node.node().removeAllGeoms()
        
        cell_size = self.settings['cell_size']
        rows = len(self.current_grid) if self.current_grid else 0
        cols = len(self.current_grid[0]) if self.current_grid and rows > 0 else 0
        
        # 创建顶点格式
        vertex_format = GeomVertexFormat.getV3c4()
        vertex_data = GeomVertexData("visited_nodes", vertex_format, Geom.UHStatic)
        vertex_data.setNumRows(len(self.visited_nodes))
        
        # 创建顶点写入器
        vertex_writer = GeomVertexWriter(vertex_data, "vertex")
        color_writer = GeomVertexWriter(vertex_data, "color")
        
        # 写入节点点
        for i, (row, col) in enumerate(self.visited_nodes):
            x = col * cell_size - (cols * cell_size) / 2 + cell_size / 2
            z = row * cell_size - (rows * cell_size) / 2 + cell_size / 2
            y = 0.15  # 稍微抬高以避免与网格重叠
            
            vertex_writer.addData3f(x, y, z)
            color_writer.addData4f(self.settings['visited_nodes_color'])
            
        # 创建几何体
        geom = Geom(vertex_data)
        points = GeomPoints(Geom.UHStatic)
        points.addNextVertices(len(self.visited_nodes))
        geom.addPrimitive(points)
        self.visited_nodes_node.node().addGeom(geom)
        
        # 设置点大小和透明度
        self.visited_nodes_node.setRenderModeThickness(3.0)
        self.visited_nodes_node.setTransparency(TransparencyAttrib.MAlpha)
        
    def visualize_open_list(self, open_list: List[Tuple[int, int]]):
        """
        可视化开放列表节点
        
        Args:
            open_list: 开放列表节点列表
        """
        if not self.enabled:
            return
            
        self.open_list = open_list
        self._create_open_list_geometry()
        print(f"✓ 开放列表节点已可视化 (数量: {len(open_list)})")
        
    def _create_open_list_geometry(self):
        """创建开放列表节点几何体"""
        if not self.open_list or not self.open_list_node:
            return
            
        # 清除现有几何体
        self.open_list_node.node().removeAllGeoms()
        
        cell_size = self.settings['cell_size']
        rows = len(self.current_grid) if self.current_grid else 0
        cols = len(self.current_grid[0]) if self.current_grid and rows > 0 else 0
        
        # 创建顶点格式
        vertex_format = GeomVertexFormat.getV3c4()
        vertex_data = GeomVertexData("open_list", vertex_format, Geom.UHStatic)
        vertex_data.setNumRows(len(self.open_list))
        
        # 创建顶点写入器
        vertex_writer = GeomVertexWriter(vertex_data, "vertex")
        color_writer = GeomVertexWriter(vertex_data, "color")
        
        # 写入节点点
        for i, (row, col) in enumerate(self.open_list):
            x = col * cell_size - (cols * cell_size) / 2 + cell_size / 2
            z = row * cell_size - (rows * cell_size) / 2 + cell_size / 2
            y = 0.18  # 稍微抬高以避免与网格重叠
            
            vertex_writer.addData3f(x, y, z)
            color_writer.addData4f(self.settings['open_list_color'])
            
        # 创建几何体
        geom = Geom(vertex_data)
        points = GeomPoints(Geom.UHStatic)
        points.addNextVertices(len(self.open_list))
        geom.addPrimitive(points)
        self.open_list_node.node().addGeom(geom)
        
        # 设置点大小和透明度
        self.open_list_node.setRenderModeThickness(2.0)
        self.open_list_node.setTransparency(TransparencyAttrib.MAlpha)
        
    def clear_visualization(self):
        """清除所有可视化"""
        self.clear_grid_visualization()
        self.clear_path_visualization()
        self.clear_start_visualization()
        self.clear_goal_visualization()
        self.clear_visited_nodes_visualization()
        self.clear_open_list_visualization()
        print("✓ 所有可视化已清除")
        
    def clear_grid_visualization(self):
        """清除网格可视化"""
        if self.grid_node:
            self.grid_node.node().removeAllGeoms()
        self.current_grid = None
        
    def clear_path_visualization(self):
        """清除路径可视化"""
        if self.path_node:
            self.path_node.node().removeAllGeoms()
        self.current_path = None
        
    def clear_start_visualization(self):
        """清除起点可视化"""
        if self.start_node:
            self.start_node.node().removeAllGeoms()
        self.current_start = None
        
    def clear_goal_visualization(self):
        """清除终点可视化"""
        if self.goal_node:
            self.goal_node.node().removeAllGeoms()
        self.current_goal = None
        
    def clear_visited_nodes_visualization(self):
        """清除已访问节点可视化"""
        if self.visited_nodes_node:
            self.visited_nodes_node.node().removeAllGeoms()
        self.visited_nodes = []
        
    def clear_open_list_visualization(self):
        """清除开放列表可视化"""
        if self.open_list_node:
            self.open_list_node.node().removeAllGeoms()
        self.open_list = []
        
    def set_settings(self, settings: Dict[str, Any]):
        """
        设置可视化参数
        
        Args:
            settings: 参数字典
        """
        old_settings = self.settings.copy()
        self.settings.update(settings)
        self._update_visualization()
        
        # 如果颜色设置发生变化，更新材质
        color_keys = ['grid_color', 'path_color', 'start_color', 'goal_color']
        if any(key in settings for key in color_keys):
            self._create_materials()
            
        print("✓ 可视化设置已更新")
        
    def get_settings(self) -> Dict[str, Any]:
        """
        获取可视化参数
        
        Returns:
            参数字典
        """
        return self.settings.copy()
        
    def _update_visualization(self):
        """更新可视化显示"""
        if self.current_grid:
            self._create_grid_geometry()
        if self.current_path:
            self._create_path_geometry()
        if self.visited_nodes:
            self._create_visited_nodes_geometry()
        if self.open_list:
            self._create_open_list_geometry()
        self._update_visibility()
        
    def enable_animation(self, enable: bool):
        """
        启用/禁用路径动画
        
        Args:
            enable: 是否启用动画
        """
        self.animation_enabled = enable
        if enable:
            self.animation_progress = 0.0
            self.animation_timer = 0.0
            self.animation_path = self.current_path.copy() if self.current_path else []
            print("✓ 路径动画已启用")
        else:
            print("✓ 路径动画已禁用")
            
    def update_animation(self, dt: float):
        """
        更新动画
        
        Args:
            dt: 时间增量
        """
        if not self.animation_enabled or not self.animation_path:
            return
            
        self.animation_timer += dt * self.settings['animation_speed']
        self.animation_progress = min(1.0, self.animation_timer / 2.0)  # 2秒完成动画
        
        # 更新路径显示
        path_length = len(self.animation_path)
        visible_length = int(path_length * self.animation_progress)
        
        if visible_length > 1:
            visible_path = self.animation_path[:visible_length]
            self._update_animated_path(visible_path)
            
    def _update_animated_path(self, visible_path: List[Tuple[int, int]]):
        """
        更新动画路径显示
        
        Args:
            visible_path: 可见路径部分
        """
        if not self.path_node:
            return
            
        # 清除现有几何体
        self.path_node.node().removeAllGeoms()
        
        if len(visible_path) < 2:
            return
            
        cell_size = self.settings['cell_size']
        rows = len(self.current_grid) if self.current_grid else 0
        cols = len(self.current_grid[0]) if self.current_grid and rows > 0 else 0
        
        # 创建顶点格式
        vertex_format = GeomVertexFormat.getV3c4()
        vertex_data = GeomVertexData("path", vertex_format, Geom.UHStatic)
        vertex_data.setNumRows(len(visible_path))
        
        # 创建顶点写入器
        vertex_writer = GeomVertexWriter(vertex_data, "vertex")
        color_writer = GeomVertexWriter(vertex_data, "color")
        
        # 写入路径点
        for i, (row, col) in enumerate(visible_path):
            x = col * cell_size - (cols * cell_size) / 2 + cell_size / 2
            z = row * cell_size - (rows * cell_size) / 2 + cell_size / 2
            y = 0.2  # 稍微抬高以避免与网格重叠
            
            # 根据进度调整颜色
            progress = i / len(visible_path)
            base_color = self.settings['path_color']
            color = LColor(
                base_color.x * (1 - progress) + 1.0 * progress,  # 逐渐变为白色
                base_color.y * (1 - progress),
                base_color.z * (1 - progress),
                base_color.w
            )
            
            vertex_writer.addData3f(x, y, z)
            color_writer.addData4f(color)
            
        # 创建几何体
        geom = Geom(vertex_data)
        lines = GeomLines(Geom.UHStatic)
        
        # 添加线段
        for i in range(len(visible_path) - 1):
            lines.addVertex(i)
            lines.addVertex(i + 1)
            lines.closePrimitive()
            
        geom.addPrimitive(lines)
        self.path_node.node().addGeom(geom)
        
        # 设置线宽和材质
        self.path_node.setRenderModeThickness(self.settings['path_thickness'])
        if self.path_material:
            self.path_node.setMaterial(self.path_material, 1)