1141 lines
42 KiB
Python
1141 lines
42 KiB
Python
"""
|
|
地形路径和路网系统
|
|
提供完整的路径创建、编辑、导航、寻路算法和可视化功能
|
|
"""
|
|
|
|
from panda3d.core import NodePath, Vec3, Point3, BitMask32
|
|
from panda3d.core import CardMaker, LineSegs
|
|
from panda3d.core import Texture, TextureStage
|
|
from panda3d.ai import AIPathFinder, AIPath, AICharacter
|
|
import math
|
|
import json
|
|
import os
|
|
import heapq
|
|
|
|
class PathSystem:
|
|
"""
|
|
地形路径和路网系统类
|
|
提供路径创建、编辑、导航、寻路算法和可视化功能
|
|
"""
|
|
|
|
def __init__(self, world):
|
|
self.world = world
|
|
self.paths = [] # 存储路径
|
|
self.waypoints = [] # 存储路径点
|
|
self.roads = [] # 存储道路
|
|
self.navigation_mesh = None # 导航网格
|
|
self.pathfinding_enabled = True
|
|
self.graph = {} # 寻路图
|
|
self.traffic_system = {} # 交通系统
|
|
self.pathfinding_algorithm = 'a*' # 寻路算法 ('a*', 'dijkstra', 'rrt')
|
|
|
|
def create_path(self, name, points=None):
|
|
"""
|
|
创建路径
|
|
name: 路径名称
|
|
points: 路径点列表
|
|
"""
|
|
try:
|
|
# 创建路径节点
|
|
path_node = self.world.render.attachNewNode(f"path_{name}")
|
|
|
|
# 创建路径信息
|
|
path_info = {
|
|
'name': name,
|
|
'node': path_node,
|
|
'points': points if points else [],
|
|
'waypoints': [], # 路径点对象
|
|
'segments': [], # 路段信息
|
|
'properties': {
|
|
'width': 2.0,
|
|
'type': 'generic', # generic, road, trail, river, etc.
|
|
'speed_limit': 50.0, # km/h
|
|
'one_way': False,
|
|
'lanes': 1,
|
|
'difficulty': 1.0, # 通行难度系数
|
|
'traffic_density': 0.0 # 交通密度
|
|
}
|
|
}
|
|
|
|
# 如果提供了点，创建路径点
|
|
if points:
|
|
self._create_waypoints(path_info, points)
|
|
|
|
# 设置标签
|
|
path_node.setTag("path_name", name)
|
|
path_node.setTag("is_scene_element", "1")
|
|
path_node.setTag("tree_item_type", "PATH_NODE")
|
|
|
|
# 添加到路径列表
|
|
self.paths.append(path_info)
|
|
|
|
# 更新寻路图
|
|
self._update_pathfinding_graph()
|
|
|
|
print(f"创建路径: {name}")
|
|
return path_info
|
|
|
|
except Exception as e:
|
|
print(f"创建路径时出错: {e}")
|
|
return None
|
|
|
|
def _create_waypoints(self, path_info, points):
|
|
"""
|
|
创建路径点
|
|
"""
|
|
try:
|
|
waypoints = []
|
|
|
|
for i, point in enumerate(points):
|
|
# 创建路径点节点
|
|
waypoint_node = path_info['node'].attachNewNode(f"waypoint_{i}")
|
|
waypoint_node.setPos(point)
|
|
|
|
# 创建路径点信息
|
|
waypoint_info = {
|
|
'node': waypoint_node,
|
|
'position': point,
|
|
'index': i,
|
|
'connections': [], # 连接的其他路径点
|
|
'properties': {
|
|
'speed_limit': path_info['properties']['speed_limit'],
|
|
'stop_sign': False,
|
|
'traffic_light': False,
|
|
'priority': 1.0, # 路径点优先级
|
|
'wait_time': 0.0 # 等待时间
|
|
}
|
|
}
|
|
|
|
waypoints.append(waypoint_info)
|
|
self.waypoints.append(waypoint_info)
|
|
|
|
path_info['waypoints'] = waypoints
|
|
|
|
# 创建路径段
|
|
self._create_path_segments(path_info)
|
|
|
|
# 可视化路径
|
|
self._visualize_path(path_info)
|
|
|
|
except Exception as e:
|
|
print(f"创建路径点时出错: {e}")
|
|
|
|
def _create_path_segments(self, path_info):
|
|
"""
|
|
创建路径段
|
|
"""
|
|
try:
|
|
waypoints = path_info['waypoints']
|
|
segments = []
|
|
|
|
for i in range(len(waypoints) - 1):
|
|
start_waypoint = waypoints[i]
|
|
end_waypoint = waypoints[i + 1]
|
|
|
|
# 计算路段信息
|
|
segment_length = (end_waypoint['position'] - start_waypoint['position']).length()
|
|
segment_direction = (end_waypoint['position'] - start_waypoint['position']).normalized()
|
|
segment_slope = self._calculate_slope(start_waypoint['position'], end_waypoint['position'])
|
|
|
|
segment_info = {
|
|
'start': start_waypoint,
|
|
'end': end_waypoint,
|
|
'length': segment_length,
|
|
'direction': segment_direction,
|
|
'slope': segment_slope,
|
|
'difficulty': 1.0 + abs(segment_slope) * 0.1, # 坡度影响通行难度
|
|
'traffic_flow': 0.0, # 交通流量
|
|
'last_update': self.world.globalClock.getFrameTime()
|
|
}
|
|
|
|
segments.append(segment_info)
|
|
|
|
# 建立连接关系
|
|
start_waypoint['connections'].append(end_waypoint)
|
|
if not path_info['properties']['one_way']:
|
|
end_waypoint['connections'].append(start_waypoint)
|
|
|
|
path_info['segments'] = segments
|
|
|
|
except Exception as e:
|
|
print(f"创建路径段时出错: {e}")
|
|
|
|
def _calculate_slope(self, start_pos, end_pos):
|
|
"""
|
|
计算两点间的坡度
|
|
"""
|
|
try:
|
|
horizontal_distance = Vec3(end_pos.getX() - start_pos.getX(),
|
|
end_pos.getY() - start_pos.getY(), 0).length()
|
|
vertical_distance = end_pos.getZ() - start_pos.getZ()
|
|
|
|
if horizontal_distance > 0:
|
|
slope = math.degrees(math.atan(vertical_distance / horizontal_distance))
|
|
return slope
|
|
else:
|
|
return 0.0
|
|
|
|
except:
|
|
return 0.0
|
|
|
|
def _visualize_path(self, path_info):
|
|
"""
|
|
可视化路径
|
|
"""
|
|
try:
|
|
# 移除现有的可视化节点
|
|
if 'visualization' in path_info:
|
|
if path_info['visualization'] and not path_info['visualization'].isEmpty():
|
|
path_info['visualization'].removeNode()
|
|
|
|
# 创建线条段
|
|
ls = LineSegs()
|
|
ls.setThickness(3.0)
|
|
|
|
# 根据路径类型和交通状况设置颜色
|
|
path_type = path_info['properties']['type']
|
|
traffic_density = path_info['properties']['traffic_density']
|
|
|
|
# 基础颜色
|
|
if path_type == 'road':
|
|
base_color = Vec3(0.3, 0.3, 0.3) # 灰色
|
|
elif path_type == 'trail':
|
|
base_color = Vec3(0.5, 0.3, 0.1) # 棕色
|
|
elif path_type == 'river':
|
|
base_color = Vec3(0.0, 0.0, 1.0) # 蓝色
|
|
else:
|
|
base_color = Vec3(1.0, 1.0, 0.0) # 黄色
|
|
|
|
# 交通密度影响颜色
|
|
if traffic_density > 0.8:
|
|
color = Vec3(1.0, 0.0, 0.0) # 红色 - 拥堵
|
|
elif traffic_density > 0.5:
|
|
color = Vec3(1.0, 0.5, 0.0) # 橙色 - 繁忙
|
|
else:
|
|
color = base_color # 正常
|
|
|
|
ls.setColor(color[0], color[1], color[2], 1.0)
|
|
|
|
# 添加线条点
|
|
for waypoint in path_info['waypoints']:
|
|
ls.moveTo(waypoint['position'])
|
|
|
|
# 创建可视化节点
|
|
visualization_node = self.world.render.attachNewNode(ls.create())
|
|
visualization_node.reparentTo(path_info['node'])
|
|
|
|
# 保存可视化节点
|
|
path_info['visualization'] = visualization_node
|
|
|
|
except Exception as e:
|
|
print(f"可视化路径时出错: {e}")
|
|
|
|
def add_waypoint(self, path_info, position, index=None):
|
|
"""
|
|
添加路径点
|
|
"""
|
|
try:
|
|
waypoints = path_info['waypoints']
|
|
|
|
# 确定插入位置
|
|
if index is None:
|
|
index = len(waypoints)
|
|
|
|
# 创建路径点节点
|
|
waypoint_node = path_info['node'].attachNewNode(f"waypoint_{index}")
|
|
waypoint_node.setPos(position)
|
|
|
|
# 创建路径点信息
|
|
waypoint_info = {
|
|
'node': waypoint_node,
|
|
'position': position,
|
|
'index': index,
|
|
'connections': [],
|
|
'properties': {
|
|
'speed_limit': path_info['properties']['speed_limit'],
|
|
'stop_sign': False,
|
|
'traffic_light': False,
|
|
'priority': 1.0,
|
|
'wait_time': 0.0
|
|
}
|
|
}
|
|
|
|
# 插入到路径点列表
|
|
waypoints.insert(index, waypoint_info)
|
|
self.waypoints.append(waypoint_info)
|
|
|
|
# 更新索引
|
|
for i, wp in enumerate(waypoints):
|
|
wp['index'] = i
|
|
|
|
# 重新创建路径段
|
|
self._create_path_segments(path_info)
|
|
|
|
# 重新可视化路径
|
|
self._visualize_path(path_info)
|
|
|
|
# 更新寻路图
|
|
self._update_pathfinding_graph()
|
|
|
|
print(f"添加路径点到路径 {path_info['name']}")
|
|
return waypoint_info
|
|
|
|
except Exception as e:
|
|
print(f"添加路径点时出错: {e}")
|
|
return None
|
|
|
|
def remove_waypoint(self, path_info, waypoint_info):
|
|
"""
|
|
移除路径点
|
|
"""
|
|
try:
|
|
# 从路径中移除
|
|
if waypoint_info in path_info['waypoints']:
|
|
path_info['waypoints'].remove(waypoint_info)
|
|
|
|
# 从全局列表中移除
|
|
if waypoint_info in self.waypoints:
|
|
self.waypoints.remove(waypoint_info)
|
|
|
|
# 移除节点
|
|
if waypoint_info['node'] and not waypoint_info['node'].isEmpty():
|
|
waypoint_info['node'].removeNode()
|
|
|
|
# 重新创建路径段
|
|
self._create_path_segments(path_info)
|
|
|
|
# 重新可视化路径
|
|
self._visualize_path(path_info)
|
|
|
|
# 更新寻路图
|
|
self._update_pathfinding_graph()
|
|
|
|
print(f"从路径 {path_info['name']} 移除路径点")
|
|
return True
|
|
|
|
except Exception as e:
|
|
print(f"移除路径点时出错: {e}")
|
|
return False
|
|
|
|
def create_road(self, name, start_point, end_point, width=5.0, lanes=2):
|
|
"""
|
|
创建道路
|
|
"""
|
|
try:
|
|
# 创建道路节点
|
|
road_node = self.world.render.attachNewNode(f"road_{name}")
|
|
|
|
# 计算道路方向和长度
|
|
direction = (end_point - start_point).normalized()
|
|
length = (end_point - start_point).length()
|
|
|
|
# 创建道路几何体
|
|
cm = CardMaker('road_surface')
|
|
cm.setFrame(0, length, -width/2, width/2)
|
|
road_surface = road_node.attachNewNode(cm.generate())
|
|
|
|
# 设置道路方向和位置
|
|
road_node.setPos(start_point)
|
|
road_node.lookAt(end_point)
|
|
|
|
# 创建道路信息
|
|
road_info = {
|
|
'name': name,
|
|
'node': road_node,
|
|
'surface': road_surface,
|
|
'start_point': start_point,
|
|
'end_point': end_point,
|
|
'direction': direction,
|
|
'length': length,
|
|
'width': width,
|
|
'lanes': lanes,
|
|
'properties': {
|
|
'surface_type': 'asphalt', # asphalt, concrete, dirt, etc.
|
|
'speed_limit': 50.0,
|
|
'one_way': False,
|
|
'traffic_lights': [], # 交通灯列表
|
|
'road_markings': True # 道路标记
|
|
}
|
|
}
|
|
|
|
# 应用道路材质
|
|
self._setup_road_material(road_info)
|
|
|
|
# 设置标签
|
|
road_node.setTag("road_name", name)
|
|
road_node.setTag("is_scene_element", "1")
|
|
road_node.setTag("tree_item_type", "ROAD_NODE")
|
|
|
|
# 添加到道路列表
|
|
self.roads.append(road_info)
|
|
|
|
# 创建对应路径
|
|
path_points = [start_point, end_point]
|
|
path_info = self.create_path(f"path_for_{name}", path_points)
|
|
if path_info:
|
|
path_info['properties']['type'] = 'road'
|
|
path_info['properties']['width'] = width
|
|
path_info['properties']['lanes'] = lanes
|
|
road_info['path'] = path_info
|
|
|
|
print(f"创建道路: {name}")
|
|
return road_info
|
|
|
|
except Exception as e:
|
|
print(f"创建道路时出错: {e}")
|
|
return None
|
|
|
|
def _setup_road_material(self, road_info):
|
|
"""
|
|
设置道路材质
|
|
"""
|
|
try:
|
|
surface = road_info['surface']
|
|
|
|
# 创建道路材质
|
|
from panda3d.core import Material
|
|
road_material = Material()
|
|
road_material.setAmbient((0.3, 0.3, 0.3, 1.0))
|
|
road_material.setDiffuse((0.4, 0.4, 0.4, 1.0))
|
|
road_material.setSpecular((0.1, 0.1, 0.1, 1.0))
|
|
road_material.setShininess(10.0)
|
|
|
|
surface.setMaterial(road_material)
|
|
|
|
# 如果有纹理系统，应用道路纹理
|
|
# 这里可以集成纹理系统
|
|
|
|
except Exception as e:
|
|
print(f"设置道路材质时出错: {e}")
|
|
|
|
def generate_navigation_mesh(self, terrain_info):
|
|
"""
|
|
生成导航网格
|
|
"""
|
|
try:
|
|
# 基于地形生成导航网格
|
|
# 包括可行走区域、障碍物识别等
|
|
|
|
# 获取地形信息
|
|
heightfield = terrain_info['heightfield']
|
|
if not heightfield:
|
|
print("无法获取地形高度图数据")
|
|
return None
|
|
|
|
# 创建导航网格数据结构
|
|
nav_mesh = {
|
|
'triangles': [], # 三角形网格
|
|
'vertices': [], # 顶点
|
|
'obstacles': [], # 障碍物
|
|
'walkable_areas': [], # 可行走区域
|
|
'terrain_info': terrain_info
|
|
}
|
|
|
|
# 生成三角形网格（简化实现）
|
|
width = heightfield.getXSize()
|
|
height = heightfield.getYSize()
|
|
|
|
# 创建顶点
|
|
for y in range(0, height, 4): # 降低分辨率
|
|
for x in range(0, width, 4):
|
|
# 获取高度值
|
|
height_value = heightfield.getRed(x, y)
|
|
|
|
# 计算世界坐标
|
|
terrain_node = terrain_info['node']
|
|
terrain_pos = terrain_node.getPos()
|
|
terrain_scale = terrain_node.getScale()
|
|
center_offset = (width - 1) / 2
|
|
|
|
world_x = (x - center_offset) * terrain_scale.getX() + terrain_pos.getX()
|
|
world_y = (y - center_offset) * terrain_scale.getY() + terrain_pos.getY()
|
|
world_z = height_value * terrain_scale.getZ() + terrain_pos.getZ()
|
|
|
|
nav_mesh['vertices'].append(Point3(world_x, world_y, world_z))
|
|
|
|
# 创建三角形（简化实现）
|
|
vertex_count = len(nav_mesh['vertices'])
|
|
for i in range(0, vertex_count - 1, 2):
|
|
if i + 2 < vertex_count:
|
|
nav_mesh['triangles'].append([i, i + 1, i + 2])
|
|
|
|
# 识别可行走区域（基于坡度）
|
|
for i, vertex in enumerate(nav_mesh['vertices']):
|
|
# 检查周围点的坡度
|
|
if i + 1 < len(nav_mesh['vertices']):
|
|
next_vertex = nav_mesh['vertices'][i + 1]
|
|
slope = self._calculate_slope(vertex, next_vertex)
|
|
if slope < 30.0: # 坡度小于30度认为可行走
|
|
nav_mesh['walkable_areas'].append(i)
|
|
|
|
print(f"生成导航网格: {vertex_count} 个顶点, {len(nav_mesh['triangles'])} 个三角形")
|
|
self.navigation_mesh = nav_mesh
|
|
return nav_mesh
|
|
|
|
except Exception as e:
|
|
print(f"生成导航网格时出错: {e}")
|
|
return None
|
|
|
|
def find_path(self, start_pos, end_pos, algorithm=None):
|
|
"""
|
|
寻找路径
|
|
"""
|
|
try:
|
|
if not self.pathfinding_enabled:
|
|
print("路径寻找已禁用")
|
|
return None
|
|
|
|
# 选择寻路算法
|
|
if algorithm is None:
|
|
algorithm = self.pathfinding_algorithm
|
|
|
|
# 执行寻路
|
|
if algorithm == 'a*':
|
|
path = self._a_star_pathfinding(start_pos, end_pos)
|
|
elif algorithm == 'dijkstra':
|
|
path = self._dijkstra_pathfinding(start_pos, end_pos)
|
|
elif algorithm == 'rrt':
|
|
path = self._rrt_pathfinding(start_pos, end_pos)
|
|
else:
|
|
# 默认使用A*
|
|
path = self._a_star_pathfinding(start_pos, end_pos)
|
|
|
|
if path:
|
|
print(f"找到路径: {start_pos} -> {end_pos} (长度: {len(path)} 个点)")
|
|
return path
|
|
else:
|
|
print(f"无法找到路径: {start_pos} -> {end_pos}")
|
|
return None
|
|
|
|
except Exception as e:
|
|
print(f"寻找路径时出错: {e}")
|
|
return None
|
|
|
|
def _a_star_pathfinding(self, start_pos, end_pos):
|
|
"""
|
|
A*寻路算法
|
|
"""
|
|
try:
|
|
# 如果没有寻路图，使用路径点
|
|
if not self.graph or len(self.graph) == 0:
|
|
return self._simple_a_star(start_pos, end_pos)
|
|
|
|
# A*算法实现
|
|
open_set = []
|
|
closed_set = set()
|
|
came_from = {}
|
|
g_score = {start_pos: 0}
|
|
f_score = {start_pos: self._heuristic(start_pos, end_pos)}
|
|
|
|
heapq.heappush(open_set, (f_score[start_pos], start_pos))
|
|
|
|
while open_set:
|
|
current = heapq.heappop(open_set)[1]
|
|
|
|
if self._is_close(current, end_pos, 1.0):
|
|
# 重构路径
|
|
path = []
|
|
while current in came_from:
|
|
path.append(current)
|
|
current = came_from[current]
|
|
path.append(start_pos)
|
|
path.reverse()
|
|
return path
|
|
|
|
closed_set.add(current)
|
|
|
|
# 检查邻居
|
|
for neighbor in self._get_neighbors(current):
|
|
if neighbor in closed_set:
|
|
continue
|
|
|
|
tentative_g_score = g_score[current] + (neighbor - current).length()
|
|
|
|
if neighbor not in g_score or tentative_g_score < g_score[neighbor]:
|
|
came_from[neighbor] = current
|
|
g_score[neighbor] = tentative_g_score
|
|
f_score[neighbor] = g_score[neighbor] + self._heuristic(neighbor, end_pos)
|
|
|
|
if neighbor not in [i[1] for i in open_set]:
|
|
heapq.heappush(open_set, (f_score[neighbor], neighbor))
|
|
|
|
return None # 未找到路径
|
|
|
|
except Exception as e:
|
|
print(f"A*寻路时出错: {e}")
|
|
return None
|
|
|
|
def _dijkstra_pathfinding(self, start_pos, end_pos):
|
|
"""
|
|
Dijkstra寻路算法
|
|
"""
|
|
try:
|
|
# Dijkstra算法实现
|
|
distances = {start_pos: 0}
|
|
previous = {}
|
|
unvisited = set()
|
|
|
|
# 初始化
|
|
for waypoint in self.waypoints:
|
|
pos = waypoint['position']
|
|
distances[pos] = float('inf')
|
|
unvisited.add(pos)
|
|
distances[start_pos] = 0
|
|
unvisited.add(start_pos)
|
|
|
|
while unvisited:
|
|
# 找到距离最小的节点
|
|
current = min(unvisited, key=lambda x: distances[x])
|
|
unvisited.remove(current)
|
|
|
|
if self._is_close(current, end_pos, 1.0):
|
|
# 重构路径
|
|
path = []
|
|
while current in previous:
|
|
path.append(current)
|
|
current = previous[current]
|
|
path.append(start_pos)
|
|
path.reverse()
|
|
return path
|
|
|
|
# 更新邻居距离
|
|
for neighbor in self._get_neighbors(current):
|
|
if neighbor in unvisited:
|
|
alt = distances[current] + (neighbor - current).length()
|
|
if alt < distances[neighbor]:
|
|
distances[neighbor] = alt
|
|
previous[neighbor] = current
|
|
|
|
return None # 未找到路径
|
|
|
|
except Exception as e:
|
|
print(f"Dijkstra寻路时出错: {e}")
|
|
return None
|
|
|
|
def _rrt_pathfinding(self, start_pos, end_pos):
|
|
"""
|
|
RRT (快速随机树) 寻路算法
|
|
"""
|
|
try:
|
|
# RRT算法简化实现
|
|
nodes = [start_pos]
|
|
parent = {start_pos: None}
|
|
max_iterations = 1000
|
|
step_size = 5.0
|
|
|
|
for i in range(max_iterations):
|
|
# 随机采样或偏向目标
|
|
if random.random() < 0.1:
|
|
random_point = end_pos
|
|
else:
|
|
# 随机点
|
|
random_point = Point3(
|
|
start_pos.getX() + random.uniform(-50, 50),
|
|
start_pos.getY() + random.uniform(-50, 50),
|
|
start_pos.getZ()
|
|
)
|
|
|
|
# 找到最近的节点
|
|
nearest_node = min(nodes, key=lambda x: (x - random_point).length())
|
|
|
|
# 向随机点延伸
|
|
direction = (random_point - nearest_node).normalized()
|
|
new_point = nearest_node + direction * step_size
|
|
|
|
# 检查是否与障碍物碰撞（简化检查）
|
|
if self._is_valid_point(new_point):
|
|
nodes.append(new_point)
|
|
parent[new_point] = nearest_node
|
|
|
|
# 检查是否到达目标附近
|
|
if (new_point - end_pos).length() < step_size:
|
|
# 重构路径
|
|
path = [end_pos]
|
|
current = new_point
|
|
while current is not None:
|
|
path.append(current)
|
|
current = parent.get(current)
|
|
path.reverse()
|
|
return path
|
|
|
|
return None # 未找到路径
|
|
|
|
except Exception as e:
|
|
print(f"RRT寻路时出错: {e}")
|
|
return None
|
|
|
|
def _simple_a_star(self, start_pos, end_pos):
|
|
"""
|
|
简化版A*（基于路径点）
|
|
"""
|
|
try:
|
|
if not self.waypoints:
|
|
return [start_pos, end_pos]
|
|
|
|
# 找到最近的路径点作为起点和终点
|
|
start_waypoint = min(self.waypoints, key=lambda x: (x['position'] - start_pos).length())
|
|
end_waypoint = min(self.waypoints, key=lambda x: (x['position'] - end_pos).length())
|
|
|
|
# 在路径点间寻路
|
|
waypoint_path = self._waypoint_a_star(start_waypoint, end_waypoint)
|
|
if not waypoint_path:
|
|
return None
|
|
|
|
# 构造完整路径
|
|
path = [start_pos]
|
|
for waypoint in waypoint_path:
|
|
path.append(waypoint['position'])
|
|
path.append(end_pos)
|
|
|
|
return path
|
|
|
|
except Exception as e:
|
|
print(f"简化A*寻路时出错: {e}")
|
|
return None
|
|
|
|
def _waypoint_a_star(self, start_waypoint, end_waypoint):
|
|
"""
|
|
路径点间的A*寻路
|
|
"""
|
|
try:
|
|
open_set = []
|
|
closed_set = set()
|
|
came_from = {}
|
|
g_score = {start_waypoint: 0}
|
|
f_score = {start_waypoint: self._waypoint_heuristic(start_waypoint, end_waypoint)}
|
|
|
|
heapq.heappush(open_set, (f_score[start_waypoint], id(start_waypoint), start_waypoint))
|
|
|
|
while open_set:
|
|
current = heapq.heappop(open_set)[2]
|
|
|
|
if current == end_waypoint:
|
|
# 重构路径
|
|
path = []
|
|
while current in came_from:
|
|
path.append(current)
|
|
current = came_from[current]
|
|
path.reverse()
|
|
return path
|
|
|
|
closed_set.add(current)
|
|
|
|
# 检查连接的路径点
|
|
for neighbor in current['connections']:
|
|
if neighbor in closed_set:
|
|
continue
|
|
|
|
# 计算通行成本
|
|
distance = (neighbor['position'] - current['position']).length()
|
|
difficulty = 1.0 # 可以从路径属性获取
|
|
tentative_g_score = g_score[current] + distance * difficulty
|
|
|
|
if neighbor not in g_score or tentative_g_score < g_score[neighbor]:
|
|
came_from[neighbor] = current
|
|
g_score[neighbor] = tentative_g_score
|
|
f_score[neighbor] = g_score[neighbor] + self._waypoint_heuristic(neighbor, end_waypoint)
|
|
|
|
if neighbor not in [item[2] for item in open_set]:
|
|
heapq.heappush(open_set, (f_score[neighbor], id(neighbor), neighbor))
|
|
|
|
return None # 未找到路径
|
|
|
|
except Exception as e:
|
|
print(f"路径点A*寻路时出错: {e}")
|
|
return None
|
|
|
|
def _heuristic(self, pos1, pos2):
|
|
"""
|
|
启发式函数（欧几里得距离）
|
|
"""
|
|
return (pos1 - pos2).length()
|
|
|
|
def _waypoint_heuristic(self, waypoint1, waypoint2):
|
|
"""
|
|
路径点启发式函数
|
|
"""
|
|
return (waypoint1['position'] - waypoint2['position']).length()
|
|
|
|
def _is_close(self, pos1, pos2, threshold):
|
|
"""
|
|
检查两点是否接近
|
|
"""
|
|
return (pos1 - pos2).length() <= threshold
|
|
|
|
def _get_neighbors(self, pos):
|
|
"""
|
|
获取位置的邻居点
|
|
"""
|
|
# 简化实现：返回附近的路径点
|
|
neighbors = []
|
|
for waypoint in self.waypoints:
|
|
distance = (waypoint['position'] - pos).length()
|
|
if 0.1 < distance < 10.0: # 距离阈值
|
|
neighbors.append(waypoint['position'])
|
|
return neighbors
|
|
|
|
def _is_valid_point(self, point):
|
|
"""
|
|
检查点是否有效（不与障碍物碰撞）
|
|
"""
|
|
# 简化实现：检查是否在合理高度范围内
|
|
return -10.0 <= point.getZ() <= 100.0
|
|
|
|
def create_ai_character(self, name, position, path=None):
|
|
"""
|
|
创建AI角色
|
|
"""
|
|
try:
|
|
# 创建AI角色
|
|
ai_char = AICharacter(name, position, 50, 0.05, 5)
|
|
|
|
# 如果提供了路径，设置路径跟随
|
|
if path:
|
|
ai_path = AIPath()
|
|
for point in path:
|
|
ai_path.addPoint(point)
|
|
ai_char.setAiPath(ai_path)
|
|
|
|
print(f"创建AI角色: {name}")
|
|
return ai_char
|
|
|
|
except Exception as e:
|
|
print(f"创建AI角色时出错: {e}")
|
|
return None
|
|
|
|
def _update_pathfinding_graph(self):
|
|
"""
|
|
更新寻路图
|
|
"""
|
|
try:
|
|
# 基于路径点构建寻路图
|
|
self.graph = {}
|
|
|
|
for waypoint in self.waypoints:
|
|
pos = waypoint['position']
|
|
self.graph[pos] = []
|
|
|
|
# 添加连接的邻居
|
|
for connection in waypoint['connections']:
|
|
neighbor_pos = connection['position']
|
|
self.graph[pos].append(neighbor_pos)
|
|
|
|
print(f"更新寻路图: {len(self.graph)} 个节点")
|
|
|
|
except Exception as e:
|
|
print(f"更新寻路图时出错: {e}")
|
|
|
|
def update_path_system(self, time_delta):
|
|
"""
|
|
更新路径系统
|
|
"""
|
|
try:
|
|
# 更新交通系统
|
|
self._update_traffic_system(time_delta)
|
|
|
|
# 更新路径可视化
|
|
self._update_path_visualization()
|
|
|
|
# 更新AI角色
|
|
# 这里可以更新所有AI角色的状态
|
|
|
|
except Exception as e:
|
|
print(f"更新路径系统时出错: {e}")
|
|
|
|
def _update_traffic_system(self, time_delta):
|
|
"""
|
|
更新交通系统
|
|
"""
|
|
try:
|
|
current_time = self.world.globalClock.getFrameTime()
|
|
|
|
# 更新每条路径的交通密度
|
|
for path_info in self.paths:
|
|
# 模拟交通流量变化
|
|
if 'traffic_density' in path_info['properties']:
|
|
# 简单的交通流量模拟
|
|
base_density = 0.3
|
|
time_factor = (math.sin(current_time * 0.1) + 1.0) * 0.5
|
|
path_info['properties']['traffic_density'] = base_density + time_factor * 0.4
|
|
|
|
except Exception as e:
|
|
print(f"更新交通系统时出错: {e}")
|
|
|
|
def _update_path_visualization(self):
|
|
"""
|
|
更新路径可视化
|
|
"""
|
|
try:
|
|
# 重新可视化所有路径以反映交通状况
|
|
for path_info in self.paths:
|
|
self._visualize_path(path_info)
|
|
|
|
except Exception as e:
|
|
print(f"更新路径可视化时出错: {e}")
|
|
|
|
def set_path_properties(self, path_info, properties):
|
|
"""
|
|
设置路径属性
|
|
"""
|
|
try:
|
|
# 更新路径属性
|
|
path_info['properties'].update(properties)
|
|
|
|
# 重新可视化路径
|
|
self._visualize_path(path_info)
|
|
|
|
# 更新寻路图（如果影响通行难度）
|
|
if 'difficulty' in properties or 'speed_limit' in properties:
|
|
self._update_pathfinding_graph()
|
|
|
|
print(f"更新路径 {path_info['name']} 属性")
|
|
return True
|
|
|
|
except Exception as e:
|
|
print(f"设置路径属性时出错: {e}")
|
|
return False
|
|
|
|
def get_path_stats(self):
|
|
"""
|
|
获取路径统计信息
|
|
"""
|
|
stats = {
|
|
'total_paths': len(self.paths),
|
|
'total_waypoints': len(self.waypoints),
|
|
'total_roads': len(self.roads),
|
|
'path_types': {},
|
|
'total_length': 0,
|
|
'algorithm': self.pathfinding_algorithm,
|
|
'traffic_stats': {
|
|
'total_density': 0.0,
|
|
'avg_density': 0.0
|
|
}
|
|
}
|
|
|
|
# 统计路径类型和总长度
|
|
total_density = 0.0
|
|
for path_info in self.paths:
|
|
path_type = path_info['properties']['type']
|
|
if path_type in stats['path_types']:
|
|
stats['path_types'][path_type] += 1
|
|
else:
|
|
stats['path_types'][path_type] = 1
|
|
|
|
# 计算路径长度
|
|
for segment in path_info['segments']:
|
|
stats['total_length'] += segment['length']
|
|
|
|
# 累计交通密度
|
|
total_density += path_info['properties'].get('traffic_density', 0.0)
|
|
|
|
# 计算平均交通密度
|
|
if len(self.paths) > 0:
|
|
stats['traffic_stats']['avg_density'] = total_density / len(self.paths)
|
|
stats['traffic_stats']['total_density'] = total_density
|
|
|
|
return stats
|
|
|
|
def save_path_data(self, output_path):
|
|
"""
|
|
保存路径数据
|
|
"""
|
|
try:
|
|
# 收集路径数据
|
|
path_data = {
|
|
'paths': [],
|
|
'roads': [],
|
|
'settings': {
|
|
'pathfinding_algorithm': self.pathfinding_algorithm,
|
|
'pathfinding_enabled': self.pathfinding_enabled
|
|
}
|
|
}
|
|
|
|
# 保存路径信息
|
|
for path_info in self.paths:
|
|
path_dict = {
|
|
'name': path_info['name'],
|
|
'points': [],
|
|
'properties': path_info['properties']
|
|
}
|
|
|
|
# 保存路径点
|
|
for waypoint in path_info['waypoints']:
|
|
point_dict = {
|
|
'position': [waypoint['position'].x,
|
|
waypoint['position'].y,
|
|
waypoint['position'].z],
|
|
'properties': waypoint['properties']
|
|
}
|
|
path_dict['points'].append(point_dict)
|
|
|
|
path_data['paths'].append(path_dict)
|
|
|
|
# 保存道路信息
|
|
for road_info in self.roads:
|
|
road_dict = {
|
|
'name': road_info['name'],
|
|
'start_point': [road_info['start_point'].x,
|
|
road_info['start_point'].y,
|
|
road_info['start_point'].z],
|
|
'end_point': [road_info['end_point'].x,
|
|
road_info['end_point'].y,
|
|
road_info['end_point'].z],
|
|
'width': road_info['width'],
|
|
'lanes': road_info['lanes'],
|
|
'properties': road_info['properties']
|
|
}
|
|
path_data['roads'].append(road_dict)
|
|
|
|
# 确保输出目录存在
|
|
output_dir = os.path.dirname(output_path)
|
|
if not os.path.exists(output_dir):
|
|
os.makedirs(output_dir)
|
|
|
|
# 写入JSON文件
|
|
with open(output_path, 'w', encoding='utf-8') as f:
|
|
json.dump(path_data, f, indent=2, ensure_ascii=False)
|
|
|
|
print(f"路径数据保存成功: {output_path}")
|
|
return True
|
|
|
|
except Exception as e:
|
|
print(f"保存路径数据时出错: {e}")
|
|
return False
|
|
|
|
def load_path_data(self, input_path):
|
|
"""
|
|
加载路径数据
|
|
"""
|
|
try:
|
|
if not os.path.exists(input_path):
|
|
print(f"路径数据文件不存在: {input_path}")
|
|
return False
|
|
|
|
# 读取JSON文件
|
|
with open(input_path, 'r', encoding='utf-8') as f:
|
|
path_data = json.load(f)
|
|
|
|
# 清除现有路径
|
|
self.clear_all_paths()
|
|
|
|
# 恢复设置
|
|
if 'settings' in path_data:
|
|
settings = path_data['settings']
|
|
self.pathfinding_algorithm = settings.get('pathfinding_algorithm', 'a*')
|
|
self.pathfinding_enabled = settings.get('pathfinding_enabled', True)
|
|
|
|
# 创建路径
|
|
for path_dict in path_data['paths']:
|
|
points = []
|
|
for point_dict in path_dict['points']:
|
|
position = Point3(point_dict['position'][0],
|
|
point_dict['position'][1],
|
|
point_dict['position'][2])
|
|
points.append(position)
|
|
|
|
path_info = self.create_path(path_dict['name'], points)
|
|
if path_info:
|
|
self.set_path_properties(path_info, path_dict['properties'])
|
|
|
|
# 创建道路
|
|
for road_dict in path_data['roads']:
|
|
start_point = Point3(road_dict['start_point'][0],
|
|
road_dict['start_point'][1],
|
|
road_dict['start_point'][2])
|
|
end_point = Point3(road_dict['end_point'][0],
|
|
road_dict['end_point'][1],
|
|
road_dict['end_point'][2])
|
|
|
|
self.create_road(road_dict['name'], start_point, end_point,
|
|
road_dict['width'], road_dict['lanes'])
|
|
|
|
print(f"路径数据加载成功: {input_path}")
|
|
return True
|
|
|
|
except Exception as e:
|
|
print(f"加载路径数据时出错: {e}")
|
|
return False
|
|
|
|
def clear_all_paths(self):
|
|
"""
|
|
清除所有路径
|
|
"""
|
|
try:
|
|
# 移除所有路径节点
|
|
for path_info in self.paths:
|
|
if path_info['node'] and not path_info['node'].isEmpty():
|
|
path_info['node'].removeNode()
|
|
|
|
# 移除所有道路节点
|
|
for road_info in self.roads:
|
|
if road_info['node'] and not road_info['node'].isEmpty():
|
|
road_info['node'].removeNode()
|
|
|
|
# 清空数据结构
|
|
self.paths = []
|
|
self.waypoints = []
|
|
self.roads = []
|
|
self.graph = {}
|
|
|
|
print("清除所有路径")
|
|
return True
|
|
|
|
except Exception as e:
|
|
print(f"清除所有路径时出错: {e}")
|
|
return False
|
|
|
|
def paint_path(self, start_point, end_point, path_type='generic', width=2.0):
|
|
"""
|
|
绘制路径（笔刷工具）
|
|
"""
|
|
try:
|
|
# 创建路径名称
|
|
path_name = f"painted_path_{len(self.paths)}"
|
|
|
|
# 创建路径点
|
|
points = [start_point, end_point]
|
|
|
|
# 创建路径
|
|
path_info = self.create_path(path_name, points)
|
|
if path_info:
|
|
# 设置路径属性
|
|
properties = {
|
|
'type': path_type,
|
|
'width': width
|
|
}
|
|
self.set_path_properties(path_info, properties)
|
|
|
|
print(f"绘制路径: {path_name}")
|
|
return path_info
|
|
|
|
except Exception as e:
|
|
print(f"绘制路径时出错: {e}")
|
|
return None
|
|
|
|
def set_pathfinding_algorithm(self, algorithm):
|
|
"""
|
|
设置寻路算法
|
|
"""
|
|
valid_algorithms = ['a*', 'dijkstra', 'rrt']
|
|
if algorithm in valid_algorithms:
|
|
self.pathfinding_algorithm = algorithm
|
|
print(f"寻路算法设置为: {algorithm}")
|
|
return True
|
|
else:
|
|
print(f"无效的寻路算法: {algorithm}")
|
|
return False
|
|
|
|
def get_navigation_mesh_stats(self):
|
|
"""
|
|
获取导航网格统计信息
|
|
"""
|
|
if not self.navigation_mesh:
|
|
return {'generated': False}
|
|
|
|
return {
|
|
'generated': True,
|
|
'vertices': len(self.navigation_mesh['vertices']),
|
|
'triangles': len(self.navigation_mesh['triangles']),
|
|
'obstacles': len(self.navigation_mesh['obstacles']),
|
|
'walkable_areas': len(self.navigation_mesh['walkable_areas'])
|
|
} |