V1
这个提交包含在:
2
.gitignore
vendored
普通文件
2
.gitignore
vendored
普通文件
@ -0,0 +1,2 @@
|
||||
__pycache__
|
||||
.venv
|
||||
660
demo.py
普通文件
660
demo.py
普通文件
@ -0,0 +1,660 @@
|
||||
import pygame
|
||||
import sys
|
||||
from pathfinder import PathFinder, Position, Direction
|
||||
import math
|
||||
|
||||
class DiagonalReasoningVisualizer:
|
||||
def __init__(self):
|
||||
pygame.init()
|
||||
pygame.display.init()
|
||||
|
||||
# 窗口设置
|
||||
self.WINDOW_WIDTH = 1600
|
||||
self.WINDOW_HEIGHT = 1000
|
||||
self.GRID_SIZE = 60 # 每个格子的大小
|
||||
|
||||
self.display = pygame.display.set_mode((self.WINDOW_WIDTH, self.WINDOW_HEIGHT))
|
||||
pygame.display.set_caption('D* Algorithm - Diagonal Movement Reasoning')
|
||||
|
||||
# 字体
|
||||
|
||||
font = "/usr/share/fonts/adobe-source-han-serif/SourceHanSerifCN-Light.otf"
|
||||
|
||||
self.font = pygame.font.Font(font, 36)
|
||||
self.medium_font = pygame.font.Font(font, 28)
|
||||
self.small_font = pygame.font.Font(font, 22)
|
||||
self.code_font = pygame.font.Font(font, 24)
|
||||
|
||||
# 颜色定义
|
||||
self.COLORS = {
|
||||
'background': (240, 242, 245),
|
||||
'grid': (200, 210, 220),
|
||||
'current': (255, 165, 0), # 橙色 - 当前位置
|
||||
'open': (100, 200, 100), # 绿色 - 可通行
|
||||
'obstacle': (200, 100, 100), # 红色 - 障碍物
|
||||
'unknown': (150, 150, 150), # 灰色 - 未知
|
||||
'diagonal': (255, 200, 50), # 黄色 - 斜向方向
|
||||
'text': (40, 40, 50), # 深灰色 - 文字
|
||||
'highlight': (50, 150, 255), # 蓝色 - 高亮
|
||||
'formula': (80, 80, 120), # 紫色 - 公式
|
||||
'info_bg': (250, 250, 255), # 浅色 - 信息背景
|
||||
}
|
||||
|
||||
# 寻路器设置
|
||||
self.map_width = 9 # 小地图,方便展示
|
||||
self.map_height = 9
|
||||
self.allow_diagonal = True
|
||||
|
||||
# 创建寻路器
|
||||
self.pathfinder = PathFinder(self.map_width, self.map_height, self.allow_diagonal)
|
||||
|
||||
# 设置起点和终点
|
||||
self.start_pos = Position(4, 4) # 中心位置
|
||||
self.target_pos = Position(8, 8)
|
||||
|
||||
# 初始化寻路器
|
||||
self.pathfinder.position = self.start_pos
|
||||
self.pathfinder.setTarget(self.target_pos)
|
||||
|
||||
# 障碍物模式
|
||||
self.obstacle_patterns = [
|
||||
"none", # 无障碍物
|
||||
"north", # 北方向有障碍
|
||||
"north_east", # 北和东都有障碍
|
||||
"corner", # 角落障碍
|
||||
"all_north", # 所有北向都有障碍
|
||||
]
|
||||
self.current_pattern = 0
|
||||
|
||||
# 解释文本
|
||||
self.explanations = [
|
||||
"场景1: 无障碍物情况",
|
||||
"场景2: 北方向有障碍物",
|
||||
"场景3: 北方向和东方向都有障碍物",
|
||||
"场景4: 东北角落障碍物",
|
||||
"场景5: 所有北向方向都有障碍物"
|
||||
]
|
||||
|
||||
# 逻辑规则
|
||||
self.rules = [
|
||||
"规则1: 东北方向可通行 = (北可通行 AND 东可通行)",
|
||||
"规则2: 西北方向可通行 = (北可通行 AND 西可通行)",
|
||||
"规则3: 东南方向可通行 = (南可通行 AND 东可通行)",
|
||||
"规则4: 西南方向可通行 = (南可通行 AND 西可通行)",
|
||||
"规则5: 斜向移动代价 = √2 ≈ 1.414",
|
||||
"规则6: 直向移动代价 = 1"
|
||||
]
|
||||
|
||||
# 状态
|
||||
self.show_logic = True
|
||||
self.show_arrows = True
|
||||
self.show_cost = True
|
||||
|
||||
# 初始化检测结果
|
||||
self.detection_results = {}
|
||||
|
||||
# 应用初始障碍物模式
|
||||
self.apply_obstacle_pattern(self.current_pattern)
|
||||
|
||||
def apply_obstacle_pattern(self, pattern_index):
|
||||
"""应用障碍物模式"""
|
||||
# 清空所有障碍物
|
||||
for x in range(self.map_width):
|
||||
for y in range(self.map_height):
|
||||
self.pathfinder.Map[Position(x, y)] = False
|
||||
|
||||
pattern = self.obstacle_patterns[pattern_index]
|
||||
current = self.start_pos
|
||||
|
||||
if pattern == "north":
|
||||
# 北方向有障碍
|
||||
north_pos = current + Direction.North
|
||||
if self.pathfinder.Map.is_within_bounds(north_pos):
|
||||
self.pathfinder.Map[north_pos] = True
|
||||
|
||||
elif pattern == "north_east":
|
||||
# 北和东都有障碍
|
||||
north_pos = current + Direction.North
|
||||
east_pos = current + Direction.East
|
||||
if self.pathfinder.Map.is_within_bounds(north_pos):
|
||||
self.pathfinder.Map[north_pos] = True
|
||||
if self.pathfinder.Map.is_within_bounds(east_pos):
|
||||
self.pathfinder.Map[east_pos] = True
|
||||
|
||||
elif pattern == "corner":
|
||||
# 东北角有障碍
|
||||
ne_pos = current + Direction.NorthEast
|
||||
if self.pathfinder.Map.is_within_bounds(ne_pos):
|
||||
self.pathfinder.Map[ne_pos] = True
|
||||
|
||||
elif pattern == "all_north":
|
||||
# 所有北向都有障碍
|
||||
directions = [Direction.North, Direction.NorthEast, Direction.NorthWest]
|
||||
for direction in directions:
|
||||
pos = current + direction
|
||||
if self.pathfinder.Map.is_within_bounds(pos):
|
||||
self.pathfinder.Map[pos] = True
|
||||
|
||||
# 重新检测环境
|
||||
self.update_detection()
|
||||
|
||||
def update_detection(self):
|
||||
"""更新环境检测"""
|
||||
# 获取基础4个方向的信息
|
||||
directions_4 = Direction.get_4_directions()
|
||||
info_4 = []
|
||||
|
||||
for direction in directions_4:
|
||||
check_pos = self.pathfinder.position + direction
|
||||
is_obstacle = False
|
||||
if self.pathfinder.Map.is_within_bounds(check_pos):
|
||||
is_obstacle = self.pathfinder.Map[check_pos]
|
||||
info_4.append(is_obstacle)
|
||||
|
||||
# 转换为8方向信息
|
||||
info_8 = list(info_4)
|
||||
|
||||
# 计算斜向方向的可通行性
|
||||
# 东北 = 北 and 东
|
||||
ne_passable = not (info_4[0] or info_4[3]) # 北和东都可通行时,东北才可通行
|
||||
|
||||
# 西北 = 北 and 西
|
||||
nw_passable = not (info_4[0] or info_4[2]) # 北和西都可通行时,西北才可通行
|
||||
|
||||
# 东南 = 南 and 东
|
||||
se_passable = not (info_4[1] or info_4[3]) # 南和东都可通行时,东南才可通行
|
||||
|
||||
# 西南 = 南 and 西
|
||||
sw_passable = not (info_4[1] or info_4[2]) # 南和西都可通行时,西南才可通行
|
||||
|
||||
info_8.extend([
|
||||
not ne_passable, # 东北 (True表示障碍)
|
||||
not nw_passable, # 西北
|
||||
not se_passable, # 东南
|
||||
not sw_passable # 西南
|
||||
])
|
||||
|
||||
# 传递给寻路器
|
||||
self.pathfinder.detect(tuple(info_8))
|
||||
|
||||
# 保存检测结果用于显示(使用正确的键名)
|
||||
self.detection_results = {
|
||||
'north': info_4[0],
|
||||
'south': info_4[1],
|
||||
'west': info_4[2],
|
||||
'east': info_4[3],
|
||||
'northeast': info_8[4],
|
||||
'northwest': info_8[5],
|
||||
'southeast': info_8[6],
|
||||
'southwest': info_8[7],
|
||||
'ne_passable': ne_passable,
|
||||
'nw_passable': nw_passable,
|
||||
'se_passable': se_passable,
|
||||
'sw_passable': sw_passable
|
||||
}
|
||||
|
||||
def grid_to_screen(self, grid_pos):
|
||||
"""将网格坐标转换为屏幕坐标"""
|
||||
x, y = grid_pos
|
||||
screen_x = 100 + x * self.GRID_SIZE + self.GRID_SIZE // 2
|
||||
screen_y = 100 + y * self.GRID_SIZE + self.GRID_SIZE // 2
|
||||
return screen_x, screen_y
|
||||
|
||||
def grid_to_screen_rect(self, grid_pos):
|
||||
"""将网格坐标转换为屏幕矩形"""
|
||||
x, y = grid_pos
|
||||
screen_x = 100 + x * self.GRID_SIZE
|
||||
screen_y = 100 + y * self.GRID_SIZE
|
||||
return (screen_x, screen_y, self.GRID_SIZE, self.GRID_SIZE)
|
||||
|
||||
def draw_main_grid(self):
|
||||
"""绘制主网格"""
|
||||
# 绘制网格背景
|
||||
grid_rect = (100, 100,
|
||||
self.map_width * self.GRID_SIZE,
|
||||
self.map_height * self.GRID_SIZE)
|
||||
pygame.draw.rect(self.display, (255, 255, 255), grid_rect)
|
||||
|
||||
# 绘制网格线
|
||||
for x in range(self.map_width + 1):
|
||||
pygame.draw.line(self.display, self.COLORS['grid'],
|
||||
(100 + x * self.GRID_SIZE, 100),
|
||||
(100 + x * self.GRID_SIZE, 100 + self.map_height * self.GRID_SIZE),
|
||||
2)
|
||||
|
||||
for y in range(self.map_height + 1):
|
||||
pygame.draw.line(self.display, self.COLORS['grid'],
|
||||
(100, 100 + y * self.GRID_SIZE),
|
||||
(100 + self.map_width * self.GRID_SIZE, 100 + y * self.GRID_SIZE),
|
||||
2)
|
||||
|
||||
# 绘制坐标标签
|
||||
for x in range(self.map_width):
|
||||
text = self.small_font.render(str(x), True, self.COLORS['text'])
|
||||
pos_x, _ = self.grid_to_screen((x, -0.5))
|
||||
self.display.blit(text, (pos_x - text.get_width() // 2, 80))
|
||||
|
||||
for y in range(self.map_height):
|
||||
text = self.small_font.render(str(y), True, self.COLORS['text'])
|
||||
_, pos_y = self.grid_to_screen((-0.5, y))
|
||||
self.display.blit(text, (75, pos_y - text.get_height() // 2))
|
||||
|
||||
def draw_cells(self):
|
||||
"""绘制所有单元格"""
|
||||
current = self.pathfinder.position
|
||||
|
||||
# 绘制所有单元格
|
||||
for x in range(self.map_width):
|
||||
for y in range(self.map_height):
|
||||
pos = Position(x, y)
|
||||
rect = self.grid_to_screen_rect((x, y))
|
||||
|
||||
# 检查是否障碍物
|
||||
is_obstacle = self.pathfinder.Map[pos]
|
||||
|
||||
# 绘制单元格
|
||||
if pos == current:
|
||||
# 当前位置
|
||||
pygame.draw.rect(self.display, self.COLORS['current'],
|
||||
(rect[0] + 5, rect[1] + 5, rect[2] - 10, rect[3] - 10))
|
||||
# 添加"C"标记
|
||||
text = self.medium_font.render("C", True, (255, 255, 255))
|
||||
text_rect = text.get_rect(center=(rect[0] + rect[2] // 2, rect[1] + rect[3] // 2))
|
||||
self.display.blit(text, text_rect)
|
||||
elif is_obstacle:
|
||||
# 障碍物
|
||||
pygame.draw.rect(self.display, self.COLORS['obstacle'],
|
||||
(rect[0] + 3, rect[1] + 3, rect[2] - 6, rect[3] - 6))
|
||||
else:
|
||||
# 可通行区域
|
||||
pygame.draw.rect(self.display, self.COLORS['open'],
|
||||
(rect[0] + 3, rect[1] + 3, rect[2] - 6, rect[3] - 6))
|
||||
|
||||
# 绘制格子边框
|
||||
pygame.draw.rect(self.display, self.COLORS['grid'],
|
||||
(rect[0] + 1, rect[1] + 1, rect[2] - 2, rect[3] - 2), 1)
|
||||
|
||||
# 绘制目标位置
|
||||
if self.target_pos:
|
||||
rect = self.grid_to_screen_rect((self.target_pos.X, self.target_pos.Y))
|
||||
pygame.draw.rect(self.display, (50, 200, 50),
|
||||
(rect[0] + 8, rect[1] + 8, rect[2] - 16, rect[3] - 16))
|
||||
# 添加"T"标记
|
||||
text = self.medium_font.render("T", True, (255, 255, 255))
|
||||
text_rect = text.get_rect(center=(rect[0] + rect[2] // 2, rect[1] + rect[3] // 2))
|
||||
self.display.blit(text, text_rect)
|
||||
|
||||
def draw_directions(self):
|
||||
"""绘制方向检测"""
|
||||
if not hasattr(self, 'detection_results') or not self.detection_results:
|
||||
return
|
||||
|
||||
current = self.pathfinder.position
|
||||
directions_8 = Direction.get_8_directions()
|
||||
|
||||
# 主方向(北南西东)
|
||||
main_directions = directions_8[:4]
|
||||
main_names = ['north', 'south', 'west', 'east']
|
||||
|
||||
# 斜向方向
|
||||
diag_directions = directions_8[4:]
|
||||
diag_names = ['northeast', 'northwest', 'southeast', 'southwest']
|
||||
|
||||
# 绘制主方向检测
|
||||
for i, (direction, name) in enumerate(zip(main_directions, main_names)):
|
||||
check_pos = current + direction
|
||||
if self.pathfinder.Map.is_within_bounds(check_pos):
|
||||
# 安全获取检测结果
|
||||
is_obstacle = self.detection_results.get(name, False)
|
||||
screen_x, screen_y = self.grid_to_screen((check_pos.X, check_pos.Y))
|
||||
|
||||
# 绘制方向指示器
|
||||
center_x, center_y = self.grid_to_screen((current.X, current.Y))
|
||||
pygame.draw.line(self.display, (100, 100, 200),
|
||||
(center_x, center_y),
|
||||
(screen_x, screen_y), 3)
|
||||
|
||||
# 绘制检测结果
|
||||
color = self.COLORS['obstacle'] if is_obstacle else self.COLORS['open']
|
||||
radius = 15
|
||||
pygame.draw.circle(self.display, color, (int(screen_x), int(screen_y)), radius)
|
||||
|
||||
# 绘制文字
|
||||
status = "障碍" if is_obstacle else "可通行"
|
||||
text_color = (255, 255, 255) if is_obstacle else (0, 0, 0)
|
||||
display_name = name.capitalize()
|
||||
text = self.small_font.render(f"{display_name}:{status}", True, text_color)
|
||||
text_rect = text.get_rect(center=(screen_x, screen_y))
|
||||
self.display.blit(text, text_rect)
|
||||
|
||||
# 绘制斜向方向推理
|
||||
for i, (direction, name) in enumerate(zip(diag_directions, diag_names)):
|
||||
check_pos = current + direction
|
||||
if self.pathfinder.Map.is_within_bounds(check_pos):
|
||||
# 安全获取检测结果
|
||||
is_obstacle = self.detection_results.get(name, False)
|
||||
screen_x, screen_y = self.grid_to_screen((check_pos.X, check_pos.Y))
|
||||
|
||||
# 计算斜向方向的可通行性
|
||||
calculated_passable = False
|
||||
formula = ""
|
||||
|
||||
if name == 'northeast':
|
||||
calculated_passable = self.detection_results.get('ne_passable', False)
|
||||
formula = "北 AND 东"
|
||||
elif name == 'northwest':
|
||||
calculated_passable = self.detection_results.get('nw_passable', False)
|
||||
formula = "北 AND 西"
|
||||
elif name == 'southeast':
|
||||
calculated_passable = self.detection_results.get('se_passable', False)
|
||||
formula = "南 AND 东"
|
||||
elif name == 'southwest':
|
||||
calculated_passable = self.detection_results.get('sw_passable', False)
|
||||
formula = "南 AND 西"
|
||||
|
||||
# 绘制斜向方向指示器(虚线)
|
||||
center_x, center_y = self.grid_to_screen((current.X, current.Y))
|
||||
pygame.draw.line(self.display, self.COLORS['diagonal'],
|
||||
(center_x, center_y),
|
||||
(screen_x, screen_y), 2)
|
||||
|
||||
# 绘制斜向方向格子
|
||||
color = self.COLORS['obstacle'] if is_obstacle else self.COLORS['open']
|
||||
pygame.draw.rect(self.display, color,
|
||||
(screen_x - 20, screen_y - 20, 40, 40), 3)
|
||||
|
||||
# 绘制推理结果
|
||||
result_text = "可通行" if calculated_passable else "障碍"
|
||||
match = is_obstacle == (not calculated_passable)
|
||||
status_color = (50, 200, 50) if match else (255, 50, 50)
|
||||
|
||||
# 绘制公式和结果
|
||||
display_name = name.upper()
|
||||
formula_text = self.small_font.render(f"{display_name}: {formula}", True, self.COLORS['formula'])
|
||||
result_display = self.small_font.render(f"推理: {result_text}", True, status_color)
|
||||
actual_display = self.small_font.render(f"实际: {'障碍' if is_obstacle else '可通行'}",
|
||||
True, color)
|
||||
|
||||
# 计算文字位置
|
||||
if name in ['northeast', 'southeast']:
|
||||
text_x = screen_x + 30
|
||||
else:
|
||||
text_x = screen_x - 130
|
||||
|
||||
if name in ['northeast', 'northwest']:
|
||||
text_y = screen_y - 50
|
||||
else:
|
||||
text_y = screen_y + 30
|
||||
|
||||
self.display.blit(formula_text, (text_x, text_y))
|
||||
self.display.blit(result_display, (text_x, text_y + 25))
|
||||
self.display.blit(actual_display, (text_x, text_y + 50))
|
||||
|
||||
def draw_logic_panel(self):
|
||||
"""绘制逻辑推理面板"""
|
||||
panel_x = self.map_width * self.GRID_SIZE + 150
|
||||
panel_y = 100
|
||||
panel_width = 400
|
||||
panel_height = 500
|
||||
|
||||
# 绘制面板背景
|
||||
pygame.draw.rect(self.display, self.COLORS['info_bg'],
|
||||
(panel_x, panel_y, panel_width, panel_height), border_radius=10)
|
||||
pygame.draw.rect(self.display, (180, 190, 200),
|
||||
(panel_x, panel_y, panel_width, panel_height), 3, border_radius=10)
|
||||
|
||||
# 绘制标题
|
||||
title = self.medium_font.render("斜向移动推理逻辑", True, self.COLORS['text'])
|
||||
self.display.blit(title, (panel_x + panel_width // 2 - title.get_width() // 2, panel_y + 20))
|
||||
|
||||
# 绘制当前场景说明
|
||||
explanation = self.explanations[self.current_pattern]
|
||||
scene_text = self.small_font.render(f"当前场景: {explanation}", True, (80, 100, 150))
|
||||
self.display.blit(scene_text, (panel_x + 20, panel_y + 60))
|
||||
|
||||
# 绘制推理规则
|
||||
rules_y = panel_y + 100
|
||||
rule_title = self.small_font.render("推理规则:", True, self.COLORS['formula'])
|
||||
self.display.blit(rule_title, (panel_x + 20, rules_y))
|
||||
|
||||
for i, rule in enumerate(self.rules):
|
||||
rule_text = self.code_font.render(rule, True, (60, 70, 90))
|
||||
self.display.blit(rule_text, (panel_x + 40, rules_y + 30 + i * 30))
|
||||
|
||||
# 绘制代码示例
|
||||
code_y = rules_y + len(self.rules) * 30 + 60
|
||||
code_title = self.small_font.render("代码逻辑:", True, self.COLORS['formula'])
|
||||
self.display.blit(code_title, (panel_x + 20, code_y))
|
||||
|
||||
code_lines = [
|
||||
"# 检测四个主方向",
|
||||
"north = detect(North)",
|
||||
"east = detect(East)",
|
||||
"# 推理东北方向",
|
||||
"northeast_passable = not (north or east)",
|
||||
"# 设置斜向障碍",
|
||||
"map[Northeast] = not northeast_passable"
|
||||
]
|
||||
|
||||
for i, line in enumerate(code_lines):
|
||||
code_text = self.code_font.render(line, True, (40, 50, 80))
|
||||
self.display.blit(code_text, (panel_x + 40, code_y + 30 + i * 25))
|
||||
|
||||
def draw_cost_panel(self):
|
||||
"""绘制移动代价面板"""
|
||||
panel_x = self.map_width * self.GRID_SIZE + 150
|
||||
panel_y = 620
|
||||
panel_width = 400
|
||||
panel_height = 150
|
||||
|
||||
# 绘制面板背景
|
||||
pygame.draw.rect(self.display, self.COLORS['info_bg'],
|
||||
(panel_x, panel_y, panel_width, panel_height), border_radius=10)
|
||||
pygame.draw.rect(self.display, (180, 190, 200),
|
||||
(panel_x, panel_y, panel_width, panel_height), 3, border_radius=10)
|
||||
|
||||
# 绘制标题
|
||||
title = self.medium_font.render("移动代价计算", True, self.COLORS['text'])
|
||||
self.display.blit(title, (panel_x + panel_width // 2 - title.get_width() // 2, panel_y + 20))
|
||||
|
||||
# 绘制代价说明
|
||||
cost_texts = [
|
||||
"直向移动(北/南/西/东):",
|
||||
" • 代价 = 1.0",
|
||||
"斜向移动(东北/西北/东南/西南):",
|
||||
" • 代价 = √2 ≈ 1.414",
|
||||
"启发函数(估算剩余距离):",
|
||||
" • 欧几里得距离 = √(dx² + dy²)"
|
||||
]
|
||||
|
||||
for i, text in enumerate(cost_texts):
|
||||
color = self.COLORS['text'] if i % 2 == 0 else (80, 90, 110)
|
||||
cost_text = self.small_font.render(text, True, color)
|
||||
self.display.blit(cost_text, (panel_x + 30, panel_y + 50 + i * 22))
|
||||
|
||||
def draw_control_panel(self):
|
||||
"""绘制控制面板"""
|
||||
panel_x = 100
|
||||
panel_y = self.map_height * self.GRID_SIZE + 130
|
||||
panel_width = self.map_width * self.GRID_SIZE
|
||||
panel_height = 120
|
||||
|
||||
# 绘制面板背景
|
||||
pygame.draw.rect(self.display, (245, 247, 250),
|
||||
(panel_x, panel_y, panel_width, panel_height), border_radius=8)
|
||||
pygame.draw.rect(self.display, (200, 210, 220),
|
||||
(panel_x, panel_y, panel_width, panel_height), 2, border_radius=8)
|
||||
|
||||
# 绘制控制按钮
|
||||
button_width = 180
|
||||
button_height = 40
|
||||
button_margin = 20
|
||||
button_y = panel_y + 20
|
||||
|
||||
buttons = [
|
||||
{"rect": pygame.Rect(panel_x + 20, button_y, button_width, button_height),
|
||||
"text": "上一个场景", "action": "prev_pattern"},
|
||||
{"rect": pygame.Rect(panel_x + 220, button_y, button_width, button_height),
|
||||
"text": "下一个场景", "action": "next_pattern"},
|
||||
{"rect": pygame.Rect(panel_x + 420, button_y, button_width, button_height),
|
||||
"text": f"显示逻辑: {'ON' if self.show_logic else 'OFF'}",
|
||||
"action": "toggle_logic"},
|
||||
{"rect": pygame.Rect(panel_x + 620, button_y, button_width, button_height),
|
||||
"text": f"显示箭头: {'ON' if self.show_arrows else 'OFF'}",
|
||||
"action": "toggle_arrows"},
|
||||
]
|
||||
|
||||
mouse_pos = pygame.mouse.get_pos()
|
||||
|
||||
for button in buttons:
|
||||
# 检查鼠标是否在按钮上
|
||||
is_hover = button["rect"].collidepoint(mouse_pos)
|
||||
color = (120, 170, 220) if is_hover else (80, 140, 200)
|
||||
|
||||
# 绘制按钮
|
||||
pygame.draw.rect(self.display, color, button["rect"], border_radius=5)
|
||||
pygame.draw.rect(self.display, (60, 100, 160), button["rect"], 2, border_radius=5)
|
||||
|
||||
# 绘制按钮文字
|
||||
text = self.small_font.render(button["text"], True, (255, 255, 255))
|
||||
text_rect = text.get_rect(center=button["rect"].center)
|
||||
self.display.blit(text, text_rect)
|
||||
|
||||
button["hover"] = is_hover
|
||||
|
||||
self.control_buttons = buttons
|
||||
|
||||
# 绘制说明文字
|
||||
instructions = [
|
||||
"按左右方向键切换场景,按L键切换逻辑显示,按A键切换箭头显示",
|
||||
"每个场景展示不同的障碍物配置对斜向移动推理的影响"
|
||||
]
|
||||
|
||||
for i, instruction in enumerate(instructions):
|
||||
inst_text = self.small_font.render(instruction, True, self.COLORS['text'])
|
||||
self.display.blit(inst_text, (panel_x + 20, panel_y + 70 + i * 25))
|
||||
|
||||
def draw_header(self):
|
||||
"""绘制头部信息"""
|
||||
# 绘制标题
|
||||
title = self.font.render("D* 算法斜向移动推理演示", True, (30, 40, 60))
|
||||
subtitle = self.medium_font.render("展示如何根据四个主方向的障碍物信息推断斜向方向的可通行性",
|
||||
True, (80, 90, 120))
|
||||
|
||||
self.display.blit(title, (self.WINDOW_WIDTH // 2 - title.get_width() // 2, 20))
|
||||
self.display.blit(subtitle, (self.WINDOW_WIDTH // 2 - subtitle.get_width() // 2, 60))
|
||||
|
||||
# 绘制当前状态
|
||||
status_text = f"场景 {self.current_pattern + 1}/{len(self.obstacle_patterns)}"
|
||||
status = self.medium_font.render(status_text, True, (100, 120, 180))
|
||||
self.display.blit(status, (self.WINDOW_WIDTH - 150, 20))
|
||||
|
||||
def handle_events(self):
|
||||
"""处理事件"""
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.QUIT:
|
||||
return False
|
||||
|
||||
elif event.type == pygame.MOUSEBUTTONDOWN:
|
||||
mouse_pos = pygame.mouse.get_pos()
|
||||
|
||||
# 检查控制按钮点击
|
||||
if hasattr(self, 'control_buttons'):
|
||||
for button in self.control_buttons:
|
||||
if button["rect"].collidepoint(mouse_pos):
|
||||
self.handle_button_click(button["action"])
|
||||
return True
|
||||
|
||||
elif event.type == pygame.KEYDOWN:
|
||||
if event.key == pygame.K_LEFT:
|
||||
self.prev_pattern()
|
||||
elif event.key == pygame.K_RIGHT:
|
||||
self.next_pattern()
|
||||
elif event.key == pygame.K_l:
|
||||
self.show_logic = not self.show_logic
|
||||
elif event.key == pygame.K_a:
|
||||
self.show_arrows = not self.show_arrows
|
||||
elif event.key == pygame.K_c:
|
||||
self.show_cost = not self.show_cost
|
||||
elif event.key == pygame.K_ESCAPE:
|
||||
return False
|
||||
|
||||
return True
|
||||
|
||||
def handle_button_click(self, action):
|
||||
"""处理按钮点击"""
|
||||
if action == "prev_pattern":
|
||||
self.prev_pattern()
|
||||
elif action == "next_pattern":
|
||||
self.next_pattern()
|
||||
elif action == "toggle_logic":
|
||||
self.show_logic = not self.show_logic
|
||||
elif action == "toggle_arrows":
|
||||
self.show_arrows = not self.show_arrows
|
||||
|
||||
def prev_pattern(self):
|
||||
"""上一个场景"""
|
||||
self.current_pattern = (self.current_pattern - 1) % len(self.obstacle_patterns)
|
||||
self.apply_obstacle_pattern(self.current_pattern)
|
||||
|
||||
def next_pattern(self):
|
||||
"""下一个场景"""
|
||||
self.current_pattern = (self.current_pattern + 1) % len(self.obstacle_patterns)
|
||||
self.apply_obstacle_pattern(self.current_pattern)
|
||||
|
||||
def draw(self):
|
||||
"""绘制所有内容"""
|
||||
# 清空屏幕
|
||||
self.display.fill(self.COLORS['background'])
|
||||
|
||||
# 绘制头部
|
||||
self.draw_header()
|
||||
|
||||
# 绘制主网格
|
||||
self.draw_main_grid()
|
||||
|
||||
# 绘制单元格
|
||||
self.draw_cells()
|
||||
|
||||
# 绘制方向检测和推理
|
||||
if self.show_arrows:
|
||||
self.draw_directions()
|
||||
|
||||
# 绘制逻辑面板
|
||||
if self.show_logic:
|
||||
self.draw_logic_panel()
|
||||
|
||||
# 绘制代价面板
|
||||
if self.show_cost:
|
||||
self.draw_cost_panel()
|
||||
|
||||
# 绘制控制面板
|
||||
self.draw_control_panel()
|
||||
|
||||
# 更新显示
|
||||
pygame.display.flip()
|
||||
|
||||
def run(self):
|
||||
"""主循环"""
|
||||
clock = pygame.time.Clock()
|
||||
|
||||
while True:
|
||||
# 处理事件
|
||||
if not self.handle_events():
|
||||
break
|
||||
|
||||
# 绘制
|
||||
self.draw()
|
||||
|
||||
# 控制帧率
|
||||
clock.tick(60)
|
||||
|
||||
pygame.quit()
|
||||
sys.exit()
|
||||
|
||||
def main():
|
||||
visualizer = DiagonalReasoningVisualizer()
|
||||
visualizer.run()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
716
pathfinder.py
普通文件
716
pathfinder.py
普通文件
@ -0,0 +1,716 @@
|
||||
import numpy
|
||||
from typing import Tuple, List, Dict, Optional
|
||||
import heapq
|
||||
import math
|
||||
from collections import deque
|
||||
|
||||
class Position():
|
||||
def __init__(self,x,y) -> None:
|
||||
self.X = x
|
||||
self.Y = y
|
||||
|
||||
def __add__(self,that):
|
||||
return Position(self.X + that.X, self.Y + that.Y)
|
||||
|
||||
def __sub__(self, that):
|
||||
return Position(self.X - that.X, self.Y - that.Y)
|
||||
|
||||
def __eq__(self, that) -> bool:
|
||||
if not isinstance(that, Position):
|
||||
return False
|
||||
return self.X == that.X and self.Y == that.Y
|
||||
|
||||
def __hash__(self) -> int:
|
||||
return hash((self.X, self.Y))
|
||||
|
||||
def __lt__(self, that) -> bool:
|
||||
return (self.X, self.Y) < (that.X, that.Y)
|
||||
|
||||
def __repr__(self) -> str:
|
||||
return f"({self.X}, {self.Y})"
|
||||
|
||||
def copy(self):
|
||||
return Position(self.X, self.Y)
|
||||
|
||||
def distance_to(self, other):
|
||||
"""计算到另一个位置的距离"""
|
||||
return math.sqrt((self.X - other.X) ** 2 + (self.Y - other.Y) ** 2)
|
||||
|
||||
class Direction():
|
||||
# 基础方向
|
||||
North = Position(0,1)
|
||||
South = Position(0,-1)
|
||||
West = Position(-1,0)
|
||||
East = Position(1,0)
|
||||
|
||||
# 斜向方向
|
||||
NorthEast = Position(1,1)
|
||||
NorthWest = Position(-1,1)
|
||||
SouthEast = Position(1,-1)
|
||||
SouthWest = Position(-1,-1)
|
||||
|
||||
# 别名
|
||||
Up = Position(0,1)
|
||||
Down = Position(0,-1)
|
||||
Left = Position(-1,0)
|
||||
Right = Position(1,0)
|
||||
|
||||
@staticmethod
|
||||
def get_all_directions():
|
||||
"""获取所有8个方向"""
|
||||
return [
|
||||
Direction.North,
|
||||
Direction.South,
|
||||
Direction.West,
|
||||
Direction.East,
|
||||
Direction.NorthEast,
|
||||
Direction.NorthWest,
|
||||
Direction.SouthEast,
|
||||
Direction.SouthWest
|
||||
]
|
||||
|
||||
@staticmethod
|
||||
def get_4_directions():
|
||||
"""获取4个基础方向"""
|
||||
return [
|
||||
Direction.North,
|
||||
Direction.South,
|
||||
Direction.West,
|
||||
Direction.East
|
||||
]
|
||||
|
||||
@staticmethod
|
||||
def get_8_directions():
|
||||
"""获取8个方向(包括斜向)"""
|
||||
return Direction.get_all_directions()
|
||||
|
||||
class VMap():
|
||||
def __init__(self,w:int,h:int, allow_diagonal:bool = True) -> None:
|
||||
self.w = w
|
||||
self.h = h
|
||||
self.allow_diagonal = allow_diagonal
|
||||
self.raw = numpy.zeros((h, w)) # 修正:使用(h, w)格式
|
||||
|
||||
def shape(self) -> Tuple[int,int]:
|
||||
return (self.w, self.h)
|
||||
|
||||
def __getitem__(self, pos:Position) -> bool:
|
||||
# True表示不可通行,False表示可以通行
|
||||
if(pos.X < 0 or pos.X >= self.w or pos.Y < 0 or pos.Y >= self.h):
|
||||
return True
|
||||
return bool(self.raw[pos.Y][pos.X]) # 修正:交换索引顺序
|
||||
|
||||
def __setitem__(self, pos:Position, value:bool) -> None:
|
||||
if(pos.X < 0 or pos.X >= self.w or pos.Y < 0 or pos.Y >= self.h):
|
||||
return
|
||||
self.raw[pos.Y][pos.X] = value # 修正:交换索引顺序
|
||||
|
||||
def is_within_bounds(self, pos: Position) -> bool:
|
||||
"""检查位置是否在地图范围内"""
|
||||
return (0 <= pos.X < self.w and 0 <= pos.Y < self.h)
|
||||
|
||||
class Node:
|
||||
def __init__(self, pos: Position):
|
||||
self.pos = pos
|
||||
self.g = float('inf') # 从起点到当前节点的代价
|
||||
self.rhs = float('inf') # 基于邻居的最小g值+代价
|
||||
self.key = (float('inf'), float('inf')) # 优先级队列的key
|
||||
self.parent = None # 添加父节点指针用于回溯
|
||||
|
||||
def __lt__(self, other):
|
||||
return self.key < other.key
|
||||
|
||||
def __eq__(self, other):
|
||||
if not isinstance(other, Node):
|
||||
return False
|
||||
return self.pos == other.pos
|
||||
|
||||
def __hash__(self):
|
||||
return hash(self.pos)
|
||||
|
||||
def __repr__(self):
|
||||
return f"Node({self.pos}, g={self.g:.2f}, rhs={self.rhs:.2f})"
|
||||
|
||||
class PathFinder():
|
||||
def __init__(self, x:int, y:int, allow_diagonal:bool = True) -> None:
|
||||
self.Map = VMap(x, y, allow_diagonal)
|
||||
self.allow_diagonal = allow_diagonal
|
||||
self.target:Position = Position(0,0)
|
||||
self.position:Position = Position(0,0)
|
||||
self.U = [] # 优先级队列
|
||||
self.km = 0 # 用于处理重新规划
|
||||
self.nodes: Dict[Tuple[int, int], Node] = {}
|
||||
|
||||
# 卡顿回溯相关
|
||||
self.stuck_counter = 0
|
||||
self.max_stuck_count = 5 # 连续卡顿多少次后触发回溯
|
||||
self.last_position = Position(0, 0)
|
||||
self.visited_positions = deque(maxlen=20) # 记录最近访问的位置
|
||||
self.blocked_positions = set() # 记录已知的阻塞位置
|
||||
|
||||
# 初始化所有节点
|
||||
for i in range(x):
|
||||
for j in range(y):
|
||||
pos = Position(i, j)
|
||||
self.nodes[(i, j)] = Node(pos)
|
||||
|
||||
def setTarget(self, target:Position):
|
||||
shape = self.Map.shape()
|
||||
if (target.X >= shape[0] or target.Y >= shape[1] or target.X < 0 or target.Y < 0):
|
||||
E = ValueError(target)
|
||||
E.add_note(f"Target pos should within Map Size {shape}")
|
||||
raise E
|
||||
|
||||
self.target = target
|
||||
self.initialize()
|
||||
|
||||
def initialize(self):
|
||||
"""初始化D*算法"""
|
||||
self.U = []
|
||||
self.km = 0
|
||||
|
||||
# 重置所有节点
|
||||
for node in self.nodes.values():
|
||||
node.g = float('inf')
|
||||
node.rhs = float('inf')
|
||||
node.key = (float('inf'), float('inf'))
|
||||
node.parent = None
|
||||
|
||||
# 设置目标节点
|
||||
target_node = self.get_node(self.target)
|
||||
target_node.rhs = 0
|
||||
target_node.key = self.calculate_key(target_node)
|
||||
heapq.heappush(self.U, (target_node.key, target_node))
|
||||
|
||||
def get_node(self, pos: Position) -> Optional[Node]:
|
||||
"""安全获取节点"""
|
||||
if 0 <= pos.X < self.Map.w and 0 <= pos.Y < self.Map.h:
|
||||
return self.nodes.get((pos.X, pos.Y))
|
||||
return None
|
||||
|
||||
def calculate_key(self, node: Node) -> Tuple[float, float]:
|
||||
"""计算节点的key值"""
|
||||
if node is None:
|
||||
return (float('inf'), float('inf'))
|
||||
k1 = min(node.g, node.rhs) + self.heuristic(self.position, node.pos) + self.km
|
||||
k2 = min(node.g, node.rhs)
|
||||
return (k1, k2)
|
||||
|
||||
def heuristic(self, pos1: Position, pos2: Position) -> float:
|
||||
"""启发式函数,使用欧几里得距离"""
|
||||
if self.allow_diagonal:
|
||||
dx = abs(pos1.X - pos2.X)
|
||||
dy = abs(pos1.Y - pos2.Y)
|
||||
return math.sqrt(dx * dx + dy * dy)
|
||||
else:
|
||||
return abs(pos1.X - pos2.X) + abs(pos1.Y - pos2.Y)
|
||||
|
||||
def get_neighbors(self, pos: Position) -> List[Position]:
|
||||
"""获取所有邻居位置"""
|
||||
neighbors = []
|
||||
|
||||
if self.allow_diagonal:
|
||||
directions = Direction.get_8_directions()
|
||||
else:
|
||||
directions = Direction.get_4_directions()
|
||||
|
||||
for direction in directions:
|
||||
new_pos = pos + direction
|
||||
if self.Map.is_within_bounds(new_pos):
|
||||
neighbors.append(new_pos)
|
||||
|
||||
return neighbors
|
||||
|
||||
def cost(self, pos1: Position, pos2: Position) -> float:
|
||||
"""计算两个相邻节点之间的代价"""
|
||||
if self.Map[pos1] or self.Map[pos2]:
|
||||
return float('inf') # 不可通行
|
||||
|
||||
# 计算移动代价
|
||||
dx = abs(pos1.X - pos2.X)
|
||||
dy = abs(pos1.Y - pos2.Y)
|
||||
|
||||
if dx == 1 and dy == 1:
|
||||
# 斜向移动,代价为√2
|
||||
return math.sqrt(2)
|
||||
else:
|
||||
# 直线移动,代价为1
|
||||
return 1.0
|
||||
|
||||
def update_vertex(self, u: Node):
|
||||
"""更新顶点"""
|
||||
if u is None:
|
||||
return
|
||||
|
||||
# 如果节点不一致,需要更新
|
||||
if u.g != u.rhs:
|
||||
u.key = self.calculate_key(u)
|
||||
# 从队列中移除旧的u(如果存在)
|
||||
self.U = [(key, node) for key, node in self.U if node.pos != u.pos]
|
||||
heapq.heapify(self.U)
|
||||
heapq.heappush(self.U, (u.key, u))
|
||||
elif u in [node for _, node in self.U]:
|
||||
# 如果在队列中且一致,移除
|
||||
self.U = [(key, node) for key, node in self.U if node.pos != u.pos]
|
||||
heapq.heapify(self.U)
|
||||
|
||||
def compute_shortest_path(self):
|
||||
"""计算最短路径"""
|
||||
start_node = self.get_node(self.position)
|
||||
if start_node is None:
|
||||
return
|
||||
|
||||
while self.U and (self.U[0][0] < self.calculate_key(start_node) or start_node.rhs != start_node.g):
|
||||
k_old, u = heapq.heappop(self.U)
|
||||
k_new = self.calculate_key(u)
|
||||
|
||||
if k_old < k_new:
|
||||
# 节点需要更新
|
||||
u.key = k_new
|
||||
heapq.heappush(self.U, (u.key, u))
|
||||
elif u.g > u.rhs:
|
||||
# 节点欠一致,更新g值
|
||||
u.g = u.rhs
|
||||
# 更新所有邻居
|
||||
for neighbor_pos in self.get_neighbors(u.pos):
|
||||
s = self.get_node(neighbor_pos)
|
||||
if s is not None and s != u:
|
||||
if not self.Map[s.pos]:
|
||||
new_rhs = self.cost(s.pos, u.pos) + u.g
|
||||
if new_rhs < s.rhs:
|
||||
s.rhs = new_rhs
|
||||
s.parent = u # 设置父节点
|
||||
self.update_vertex(s)
|
||||
else:
|
||||
# 节点过一致
|
||||
g_old = u.g
|
||||
u.g = float('inf')
|
||||
# 更新所有邻居
|
||||
for neighbor_pos in self.get_neighbors(u.pos):
|
||||
s = self.get_node(neighbor_pos)
|
||||
if s is not None and s != u:
|
||||
if s.rhs == self.cost(s.pos, u.pos) + g_old:
|
||||
# 重新计算s的rhs
|
||||
min_rhs = float('inf')
|
||||
best_parent = None
|
||||
for s2_pos in self.get_neighbors(s.pos):
|
||||
s2 = self.get_node(s2_pos)
|
||||
if s2 is not None and not self.Map[s2.pos]:
|
||||
cost_val = self.cost(s.pos, s2.pos) + s2.g
|
||||
if cost_val < min_rhs:
|
||||
min_rhs = cost_val
|
||||
best_parent = s2
|
||||
s.rhs = min_rhs
|
||||
s.parent = best_parent
|
||||
self.update_vertex(s)
|
||||
self.update_vertex(u)
|
||||
|
||||
def detect(self, info: Tuple[bool, bool, bool, bool, bool, bool, bool, bool] = None):
|
||||
"""检测周围障碍物并更新地图"""
|
||||
if info is None:
|
||||
# 如果没有提供信息,检测所有8个方向
|
||||
directions = Direction.get_8_directions()
|
||||
info = []
|
||||
for direction in directions:
|
||||
check_pos = self.position + direction
|
||||
info.append(self.Map[check_pos])
|
||||
elif len(info) == 4:
|
||||
# 如果只提供了4个方向的信息,转换为8个方向
|
||||
old_info = info
|
||||
|
||||
# 计算斜向方向的可通行性
|
||||
# 东北 = 北 and 东,且两个主方向都无障碍
|
||||
ne_passable = not (old_info[0] or old_info[3])
|
||||
|
||||
# 西北 = 北 and 西
|
||||
nw_passable = not (old_info[0] or old_info[2])
|
||||
|
||||
# 东南 = 南 and 东
|
||||
se_passable = not (old_info[1] or old_info[3])
|
||||
|
||||
# 西南 = 南 and 西
|
||||
sw_passable = not (old_info[1] or old_info[2])
|
||||
|
||||
info = [
|
||||
old_info[0], # 北
|
||||
old_info[1], # 南
|
||||
old_info[2], # 西
|
||||
old_info[3], # 东
|
||||
not ne_passable, # 东北
|
||||
not nw_passable, # 西北
|
||||
not se_passable, # 东南
|
||||
not sw_passable # 西南
|
||||
]
|
||||
|
||||
directions = Direction.get_8_directions()
|
||||
for i, direction in enumerate(directions):
|
||||
if i >= len(info):
|
||||
break
|
||||
|
||||
check_pos = self.position + direction
|
||||
|
||||
# 确保位置在地图范围内
|
||||
if not self.Map.is_within_bounds(check_pos):
|
||||
continue
|
||||
|
||||
old_value = self.Map[check_pos]
|
||||
new_value = bool(info[i])
|
||||
self.Map[check_pos] = new_value
|
||||
|
||||
# 如果地图状态改变,更新相关节点
|
||||
if old_value != new_value:
|
||||
node = self.get_node(check_pos)
|
||||
if node is not None:
|
||||
if new_value: # 变为障碍
|
||||
self.blocked_positions.add(check_pos)
|
||||
node.g = float('inf')
|
||||
node.rhs = float('inf')
|
||||
else: # 变为可通行
|
||||
if check_pos in self.blocked_positions:
|
||||
self.blocked_positions.remove(check_pos)
|
||||
|
||||
# 更新邻居节点
|
||||
for neighbor_pos in self.get_neighbors(check_pos):
|
||||
s = self.get_node(neighbor_pos)
|
||||
if s is not None:
|
||||
# 重新计算s的rhs
|
||||
min_rhs = float('inf')
|
||||
best_parent = None
|
||||
for s2_pos in self.get_neighbors(s.pos):
|
||||
s2 = self.get_node(s2_pos)
|
||||
if s2 is not None and not self.Map[s2.pos]:
|
||||
cost_val = self.cost(s.pos, s2.pos) + s2.g
|
||||
if cost_val < min_rhs:
|
||||
min_rhs = cost_val
|
||||
best_parent = s2
|
||||
s.rhs = min_rhs
|
||||
s.parent = best_parent
|
||||
self.update_vertex(s)
|
||||
|
||||
# 重新计算路径
|
||||
self.km += self.heuristic(self.position, self.target)
|
||||
self.compute_shortest_path()
|
||||
|
||||
def check_stuck(self) -> bool:
|
||||
"""检查是否卡顿,返回True表示需要回溯"""
|
||||
# 记录当前位置
|
||||
self.visited_positions.append(self.position.copy())
|
||||
|
||||
# 检查是否在同一个位置停留太久
|
||||
if self.position == self.last_position:
|
||||
self.stuck_counter += 1
|
||||
else:
|
||||
self.stuck_counter = 0
|
||||
self.last_position = self.position.copy()
|
||||
|
||||
# 如果卡顿次数超过阈值,需要回溯
|
||||
if self.stuck_counter >= self.max_stuck_count:
|
||||
print(f"检测到卡顿,当前位置: {self.position}")
|
||||
return True
|
||||
|
||||
# 检查是否在循环
|
||||
if len(self.visited_positions) >= 10:
|
||||
recent_positions = list(self.visited_positions)
|
||||
if len(set(recent_positions)) < len(recent_positions) * 0.5:
|
||||
print("检测到循环移动")
|
||||
return True
|
||||
|
||||
return False
|
||||
|
||||
def backtrack(self):
|
||||
"""回溯到之前的可通行位置"""
|
||||
print("开始回溯...")
|
||||
|
||||
# 尝试回溯到最近访问过的位置(但不是当前位置)
|
||||
backtrack_successful = False
|
||||
|
||||
# 检查历史位置,找到最近的可行位置
|
||||
for pos in reversed(list(self.visited_positions)):
|
||||
if pos == self.position:
|
||||
continue
|
||||
|
||||
# 检查该位置是否可通行
|
||||
if not self.Map[pos] and pos not in self.blocked_positions:
|
||||
# 尝试找到从当前位置到该位置的路径
|
||||
temp_path = self.find_path_to_position(pos)
|
||||
if temp_path and len(temp_path) > 1:
|
||||
# 找到可行路径,回溯到该位置
|
||||
print(f"回溯到位置: {pos}")
|
||||
self.position = pos
|
||||
self.stuck_counter = 0
|
||||
# 将当前位置周围的阻塞位置临时标记为可通行
|
||||
self.temporarily_clear_blocks(pos)
|
||||
backtrack_successful = True
|
||||
break
|
||||
|
||||
if not backtrack_successful:
|
||||
# 如果找不到可回溯的位置,尝试随机移动
|
||||
print("尝试随机移动以摆脱卡顿")
|
||||
random_dir = self.get_random_direction()
|
||||
if random_dir:
|
||||
new_pos = self.position + random_dir
|
||||
if self.Map.is_within_bounds(new_pos) and not self.Map[new_pos]:
|
||||
self.position = new_pos
|
||||
self.stuck_counter = 0
|
||||
|
||||
# 重新初始化算法
|
||||
self.initialize()
|
||||
|
||||
def find_path_to_position(self, target_pos: Position) -> List[Position]:
|
||||
"""使用A*算法找到到指定位置的路径(用于回溯)"""
|
||||
open_set = []
|
||||
closed_set = set()
|
||||
|
||||
start_node = self.get_node(self.position)
|
||||
target_node = self.get_node(target_pos)
|
||||
|
||||
if not start_node or not target_node:
|
||||
return []
|
||||
|
||||
# 重置g值和父节点
|
||||
for node in self.nodes.values():
|
||||
node.g = float('inf')
|
||||
node.parent = None
|
||||
|
||||
start_node.g = 0
|
||||
start_node.h = self.heuristic(start_node.pos, target_pos)
|
||||
heapq.heappush(open_set, (start_node.g + start_node.h, start_node))
|
||||
|
||||
while open_set:
|
||||
_, current = heapq.heappop(open_set)
|
||||
|
||||
if current.pos == target_pos:
|
||||
# 重构路径
|
||||
path = []
|
||||
while current:
|
||||
path.append(current.pos)
|
||||
current = current.parent
|
||||
return list(reversed(path))
|
||||
|
||||
closed_set.add(current.pos)
|
||||
|
||||
for neighbor_pos in self.get_neighbors(current.pos):
|
||||
if self.Map[neighbor_pos] or neighbor_pos in self.blocked_positions:
|
||||
continue
|
||||
|
||||
neighbor_node = self.get_node(neighbor_pos)
|
||||
if neighbor_node.pos in closed_set:
|
||||
continue
|
||||
|
||||
tentative_g = current.g + self.cost(current.pos, neighbor_pos)
|
||||
|
||||
if tentative_g < neighbor_node.g:
|
||||
neighbor_node.parent = current
|
||||
neighbor_node.g = tentative_g
|
||||
neighbor_node.h = self.heuristic(neighbor_pos, target_pos)
|
||||
|
||||
if neighbor_node not in [node for _, node in open_set]:
|
||||
heapq.heappush(open_set, (neighbor_node.g + neighbor_node.h, neighbor_node))
|
||||
|
||||
return []
|
||||
|
||||
def temporarily_clear_blocks(self, pos: Position):
|
||||
"""临时清除指定位置周围的阻塞标记"""
|
||||
for neighbor_pos in self.get_neighbors(pos):
|
||||
if neighbor_pos in self.blocked_positions:
|
||||
print(f"临时清除阻塞标记: {neighbor_pos}")
|
||||
self.blocked_positions.remove(neighbor_pos)
|
||||
# 更新地图
|
||||
self.Map[neighbor_pos] = False
|
||||
|
||||
def get_random_direction(self) -> Optional[Position]:
|
||||
"""获取一个随机的可行方向"""
|
||||
directions = Direction.get_8_directions() if self.allow_diagonal else Direction.get_4_directions()
|
||||
import random
|
||||
random.shuffle(directions)
|
||||
|
||||
for direction in directions:
|
||||
new_pos = self.position + direction
|
||||
if (self.Map.is_within_bounds(new_pos) and
|
||||
not self.Map[new_pos] and
|
||||
new_pos not in self.blocked_positions):
|
||||
return direction
|
||||
|
||||
return None
|
||||
|
||||
def get_next_direction(self) -> Optional[Position]:
|
||||
"""获取下一步的方向"""
|
||||
# 检查是否卡顿
|
||||
if self.check_stuck():
|
||||
self.backtrack()
|
||||
# 重新计算路径
|
||||
self.compute_shortest_path()
|
||||
|
||||
current_node = self.get_node(self.position)
|
||||
|
||||
# 如果当前位置就是目标,返回None
|
||||
if self.position == self.target:
|
||||
return Position(0, 0)
|
||||
|
||||
# 如果无路可走,返回None
|
||||
if current_node is None or current_node.g == float('inf'):
|
||||
return Position(0, 0)
|
||||
|
||||
# 选择g值最小的邻居作为下一步
|
||||
best_pos = None
|
||||
min_g = float('inf')
|
||||
|
||||
for neighbor_pos in self.get_neighbors(self.position):
|
||||
if (self.Map[neighbor_pos] or
|
||||
neighbor_pos in self.blocked_positions):
|
||||
continue
|
||||
|
||||
neighbor_node = self.get_node(neighbor_pos)
|
||||
if neighbor_node and neighbor_node.g < min_g:
|
||||
min_g = neighbor_node.g
|
||||
best_pos = neighbor_pos
|
||||
|
||||
if best_pos is None:
|
||||
# 如果没有找到可行方向,尝试随机移动
|
||||
random_dir = self.get_random_direction()
|
||||
if random_dir:
|
||||
return random_dir
|
||||
return Position(0, 0)
|
||||
|
||||
# 计算方向向量
|
||||
diff = best_pos - self.position
|
||||
return diff
|
||||
|
||||
def exec(self) -> Position:
|
||||
"""
|
||||
执行D*算法,返回下一步的移动方向向量
|
||||
返回值为一个Position对象,表示移动方向
|
||||
"""
|
||||
# 如果目标是当前位置,不需要移动
|
||||
if self.position == self.target:
|
||||
return Position(0, 0)
|
||||
|
||||
# 计算最短路径
|
||||
self.compute_shortest_path()
|
||||
|
||||
# 获取下一步方向
|
||||
next_dir = self.get_next_direction()
|
||||
|
||||
# 如果找到方向,移动到下一个位置
|
||||
if next_dir != Position(0, 0):
|
||||
new_position = self.position + next_dir
|
||||
# 确保新位置在地图范围内且不是障碍物
|
||||
if (self.Map.is_within_bounds(new_position) and
|
||||
not self.Map[new_position] and
|
||||
new_position not in self.blocked_positions):
|
||||
self.position = new_position
|
||||
|
||||
return next_dir
|
||||
|
||||
def get_path_to_target(self) -> List[Position]:
|
||||
"""获取从当前位置到目标位置的完整路径"""
|
||||
path = []
|
||||
current = self.position
|
||||
|
||||
# 如果当前位置就是目标,返回空路径
|
||||
if current == self.target:
|
||||
return [current]
|
||||
|
||||
# 计算最短路径
|
||||
self.compute_shortest_path()
|
||||
|
||||
# 从当前位置开始,跟随最小g值的邻居直到目标
|
||||
while current != self.target:
|
||||
path.append(current.copy())
|
||||
|
||||
current_node = self.get_node(current)
|
||||
if current_node is None or current_node.g == float('inf'):
|
||||
break
|
||||
|
||||
# 找到最小g值的邻居
|
||||
best_pos = None
|
||||
min_g = float('inf')
|
||||
|
||||
for neighbor_pos in self.get_neighbors(current):
|
||||
if self.Map[neighbor_pos] or neighbor_pos in self.blocked_positions:
|
||||
continue
|
||||
|
||||
neighbor_node = self.get_node(neighbor_pos)
|
||||
if neighbor_node and neighbor_node.g < min_g:
|
||||
min_g = neighbor_node.g
|
||||
best_pos = neighbor_pos
|
||||
|
||||
if best_pos is None:
|
||||
break
|
||||
|
||||
current = best_pos
|
||||
|
||||
if current == self.target:
|
||||
path.append(self.target.copy())
|
||||
|
||||
return path
|
||||
|
||||
def create_test_map_with_obstacles():
|
||||
"""创建一个带有障碍物的测试地图"""
|
||||
finder = PathFinder(10, 10, allow_diagonal=True)
|
||||
|
||||
# 设置目标
|
||||
finder.setTarget(Position(9, 9))
|
||||
finder.position = Position(0, 0)
|
||||
|
||||
# 添加一些障碍物
|
||||
for i in range(3, 7):
|
||||
finder.Map[Position(i, 5)] = True # 水平障碍墙
|
||||
finder.Map[Position(5, i)] = True # 垂直障碍墙
|
||||
|
||||
# 创建一个"陷阱"区域
|
||||
for i in range(1, 4):
|
||||
for j in range(1, 4):
|
||||
finder.Map[Position(i, j)] = True
|
||||
finder.Map[Position(2, 2)] = False # 只留一个入口
|
||||
|
||||
return finder
|
||||
|
||||
if __name__ == "__main__":
|
||||
# 测试示例
|
||||
print("测试斜向移动的D*算法")
|
||||
|
||||
# 创建测试地图
|
||||
k = create_test_map_with_obstacles()
|
||||
|
||||
print(f"起始位置: {k.position}")
|
||||
print(f"目标位置: {k.target}")
|
||||
|
||||
# 获取初始路径
|
||||
path = k.get_path_to_target()
|
||||
print(f"初始路径长度: {len(path)}")
|
||||
print(f"初始路径: {path}")
|
||||
|
||||
# 模拟移动过程
|
||||
max_steps = 50
|
||||
for step in range(max_steps):
|
||||
if k.position == k.target:
|
||||
print(f"到达目标位置!总步数: {step}")
|
||||
break
|
||||
|
||||
direction = k.exec()
|
||||
|
||||
# 模拟检测环境
|
||||
if step % 3 == 0:
|
||||
# 随机添加一些障碍
|
||||
import random
|
||||
if random.random() < 0.2:
|
||||
x = random.randint(0, 9)
|
||||
y = random.randint(0, 9)
|
||||
if k.Map.is_within_bounds(Position(x, y)) and not (x == k.position.X and y == k.position.Y):
|
||||
k.Map[Position(x, y)] = True
|
||||
print(f"步数 {step}: 在位置 ({x}, {y}) 发现新障碍")
|
||||
|
||||
print(f"步数 {step}: 当前位置 {k.position}, 移动方向 {direction}")
|
||||
|
||||
# 每10步显示一次路径
|
||||
if step % 10 == 0:
|
||||
current_path = k.get_path_to_target()
|
||||
if current_path:
|
||||
print(f"当前路径长度: {len(current_path)}")
|
||||
|
||||
if k.position != k.target:
|
||||
print(f"未能在{max_steps}步内到达目标")
|
||||
print(f"最终位置: {k.position}")
|
||||
final_path = k.get_path_to_target()
|
||||
print(f"最终路径: {final_path}")
|
||||
803
playground.py
普通文件
803
playground.py
普通文件
@ -0,0 +1,803 @@
|
||||
import pygame
|
||||
import sys
|
||||
from pathfinder import PathFinder, Position, Direction
|
||||
import math
|
||||
from enum import Enum
|
||||
|
||||
class ToolMode(Enum):
|
||||
START = "start"
|
||||
TARGET = "target"
|
||||
OBSTACLE = "obstacle"
|
||||
ERASER = "eraser"
|
||||
|
||||
class PathFinderPlayground:
|
||||
def __init__(self):
|
||||
pygame.init()
|
||||
pygame.display.init()
|
||||
|
||||
# 窗口设置
|
||||
self.WINDOW_WIDTH = 1400
|
||||
self.WINDOW_HEIGHT = 900
|
||||
self.GRID_SIZE = 40 # 每个格子的大小
|
||||
|
||||
self.display = pygame.display.set_mode((self.WINDOW_WIDTH, self.WINDOW_HEIGHT))
|
||||
pygame.display.set_caption('PathFinder Playground - Interactive D* Algorithm')
|
||||
|
||||
font = "/usr/share/fonts/adobe-source-han-serif/SourceHanSerifCN-Light.otf"
|
||||
|
||||
# 字体
|
||||
self.font = pygame.font.Font(font, 32)
|
||||
self.medium_font = pygame.font.Font(font, 26)
|
||||
self.small_font = pygame.font.Font(font, 22)
|
||||
|
||||
# 颜色定义
|
||||
self.COLORS = {
|
||||
'background': (245, 248, 250),
|
||||
'grid': (200, 210, 220),
|
||||
'start': (50, 200, 50), # 绿色 - 起点
|
||||
'target': (220, 50, 50), # 红色 - 终点
|
||||
'obstacle': (120, 120, 140), # 灰色 - 障碍物
|
||||
'path': (50, 150, 255), # 蓝色 - 路径
|
||||
'current': (255, 165, 0), # 橙色 - 当前
|
||||
'visited': (180, 210, 240), # 浅蓝 - 已访问
|
||||
'text': (40, 40, 50), # 深灰 - 文字
|
||||
'button': (80, 140, 200), # 蓝色 - 按钮
|
||||
'button_hover': (110, 170, 230),
|
||||
'button_active': (140, 200, 255),
|
||||
'panel': (230, 235, 245), # 面板背景
|
||||
'info': (70, 100, 140), # 信息文字
|
||||
'success': (70, 180, 70), # 成功
|
||||
'warning': (220, 170, 50), # 警告
|
||||
'error': (220, 80, 80), # 错误
|
||||
}
|
||||
|
||||
# 地图设置
|
||||
self.map_width = 20
|
||||
self.map_height = 15
|
||||
self.allow_diagonal = True
|
||||
|
||||
# 创建寻路器
|
||||
self.reset_pathfinder()
|
||||
|
||||
# 工具和状态
|
||||
self.tool_mode = ToolMode.OBSTACLE
|
||||
self.is_drawing = False
|
||||
self.is_running = False
|
||||
self.auto_run = False
|
||||
self.speed = 0.1
|
||||
self.last_step_time = 0
|
||||
|
||||
# 路径相关
|
||||
self.full_path = []
|
||||
self.path_index = 0
|
||||
self.steps = 0
|
||||
self.is_path_found = False
|
||||
self.is_path_complete = False
|
||||
|
||||
# UI布局
|
||||
self.grid_offset_x = 50
|
||||
self.grid_offset_y = 50
|
||||
self.grid_width = self.map_width * self.GRID_SIZE
|
||||
self.grid_height = self.map_height * self.GRID_SIZE
|
||||
|
||||
self.tool_panel_x = self.grid_offset_x + self.grid_width + 50
|
||||
self.tool_panel_y = 50
|
||||
self.tool_panel_width = 300
|
||||
self.tool_panel_height = 500
|
||||
|
||||
self.info_panel_y = self.tool_panel_y + self.tool_panel_height + 20
|
||||
|
||||
# 保存初始状态用于重置
|
||||
self.save_state()
|
||||
|
||||
def save_state(self):
|
||||
"""保存当前地图状态"""
|
||||
self.saved_obstacles = []
|
||||
for x in range(self.map_width):
|
||||
for y in range(self.map_height):
|
||||
pos = Position(x, y)
|
||||
if self.pathfinder.Map[pos]:
|
||||
self.saved_obstacles.append(pos.copy())
|
||||
|
||||
self.saved_start = self.pathfinder.position.copy()
|
||||
self.saved_target = self.pathfinder.target.copy()
|
||||
|
||||
def load_state(self):
|
||||
"""加载保存的地图状态"""
|
||||
self.reset_pathfinder()
|
||||
|
||||
# 设置障碍物
|
||||
for obstacle in self.saved_obstacles:
|
||||
if 0 <= obstacle.X < self.map_width and 0 <= obstacle.Y < self.map_height:
|
||||
self.pathfinder.Map[obstacle] = True
|
||||
|
||||
# 设置起点和终点
|
||||
self.pathfinder.position = Position(self.saved_start.X, self.saved_start.Y)
|
||||
self.pathfinder.setTarget(Position(self.saved_target.X, self.saved_target.Y))
|
||||
|
||||
# 初始检测
|
||||
self.detect_around_current()
|
||||
|
||||
def reset_pathfinder(self):
|
||||
"""重置寻路器"""
|
||||
self.pathfinder = PathFinder(self.map_width, self.map_height, self.allow_diagonal)
|
||||
|
||||
# 默认起点和终点
|
||||
self.pathfinder.position = Position(0, 0)
|
||||
self.pathfinder.setTarget(Position(self.map_width - 1, self.map_height - 1))
|
||||
|
||||
# 重置状态
|
||||
self.full_path = []
|
||||
self.path_index = 0
|
||||
self.steps = 0
|
||||
self.is_path_found = False
|
||||
self.is_path_complete = False
|
||||
self.is_running = False
|
||||
self.auto_run = False
|
||||
|
||||
def grid_to_screen(self, grid_pos):
|
||||
"""网格坐标转屏幕坐标"""
|
||||
x, y = grid_pos
|
||||
screen_x = self.grid_offset_x + x * self.GRID_SIZE + self.GRID_SIZE // 2
|
||||
screen_y = self.grid_offset_y + y * self.GRID_SIZE + self.GRID_SIZE // 2
|
||||
return screen_x, screen_y
|
||||
|
||||
def grid_to_screen_rect(self, grid_pos):
|
||||
"""网格坐标转屏幕矩形"""
|
||||
x, y = grid_pos
|
||||
screen_x = self.grid_offset_x + x * self.GRID_SIZE
|
||||
screen_y = self.grid_offset_y + y * self.GRID_SIZE
|
||||
return (screen_x, screen_y, self.GRID_SIZE, self.GRID_SIZE)
|
||||
|
||||
def screen_to_grid(self, screen_pos):
|
||||
"""屏幕坐标转网格坐标"""
|
||||
x, y = screen_pos
|
||||
grid_x = (x - self.grid_offset_x) // self.GRID_SIZE
|
||||
grid_y = (y - self.grid_offset_y) // self.GRID_SIZE
|
||||
return grid_x, grid_y
|
||||
|
||||
def is_in_grid(self, screen_pos):
|
||||
"""检查点是否在网格内"""
|
||||
x, y = screen_pos
|
||||
return (self.grid_offset_x <= x < self.grid_offset_x + self.grid_width and
|
||||
self.grid_offset_y <= y < self.grid_offset_y + self.grid_height)
|
||||
|
||||
def detect_around_current(self):
|
||||
"""检测当前位置周围环境"""
|
||||
# 获取基础4个方向的信息
|
||||
directions_4 = Direction.get_4_directions()
|
||||
info_4 = []
|
||||
|
||||
for direction in directions_4:
|
||||
check_pos = self.pathfinder.position + direction
|
||||
is_obstacle = False
|
||||
if self.pathfinder.Map.is_within_bounds(check_pos):
|
||||
is_obstacle = self.pathfinder.Map[check_pos]
|
||||
info_4.append(is_obstacle)
|
||||
|
||||
# 转换为8方向信息
|
||||
info_8 = list(info_4)
|
||||
|
||||
# 计算斜向方向的可通行性
|
||||
ne_passable = not (info_4[0] or info_4[3]) # 东北
|
||||
nw_passable = not (info_4[0] or info_4[2]) # 西北
|
||||
se_passable = not (info_4[1] or info_4[3]) # 东南
|
||||
sw_passable = not (info_4[1] or info_4[2]) # 西南
|
||||
|
||||
info_8.extend([
|
||||
not ne_passable,
|
||||
not nw_passable,
|
||||
not se_passable,
|
||||
not sw_passable
|
||||
])
|
||||
|
||||
# 传递给寻路器
|
||||
self.pathfinder.detect(tuple(info_8))
|
||||
|
||||
def find_path(self):
|
||||
"""寻找完整路径"""
|
||||
try:
|
||||
self.full_path = self.pathfinder.get_path_to_target()
|
||||
self.path_index = 0
|
||||
self.is_path_found = len(self.full_path) > 0
|
||||
|
||||
if self.is_path_found:
|
||||
print(f"找到路径,长度: {len(self.full_path)}")
|
||||
for i, pos in enumerate(self.full_path):
|
||||
print(f" 步骤{i}: {pos}")
|
||||
else:
|
||||
print("未找到路径")
|
||||
|
||||
return self.is_path_found
|
||||
except Exception as e:
|
||||
print(f"寻路错误: {e}")
|
||||
self.full_path = []
|
||||
return False
|
||||
|
||||
def step_forward(self):
|
||||
"""向前执行一步"""
|
||||
if self.is_path_complete:
|
||||
return False
|
||||
|
||||
if not self.full_path:
|
||||
if not self.find_path():
|
||||
return False
|
||||
|
||||
if self.path_index < len(self.full_path):
|
||||
# 移动到下一个路径点
|
||||
next_pos = self.full_path[self.path_index]
|
||||
|
||||
# 检查是否可通行
|
||||
if self.pathfinder.Map[next_pos]:
|
||||
print(f"警告: 路径点 {next_pos} 被障碍物阻挡")
|
||||
self.find_path() # 重新寻路
|
||||
return True
|
||||
|
||||
# 移动
|
||||
self.pathfinder.position = Position(next_pos.X, next_pos.Y)
|
||||
self.path_index += 1
|
||||
self.steps += 1
|
||||
|
||||
# 检测环境
|
||||
self.detect_around_current()
|
||||
|
||||
# 检查是否到达终点
|
||||
if self.pathfinder.position == self.pathfinder.target:
|
||||
self.is_path_complete = True
|
||||
print("到达目标!")
|
||||
return False
|
||||
|
||||
return True
|
||||
else:
|
||||
self.is_path_complete = True
|
||||
return False
|
||||
|
||||
def run_to_completion(self):
|
||||
"""运行到完成"""
|
||||
self.is_running = True
|
||||
self.auto_run = True
|
||||
self.find_path()
|
||||
|
||||
def draw_grid(self):
|
||||
"""绘制网格"""
|
||||
# 绘制网格背景
|
||||
grid_rect = (self.grid_offset_x, self.grid_offset_y,
|
||||
self.grid_width, self.grid_height)
|
||||
pygame.draw.rect(self.display, (255, 255, 255), grid_rect)
|
||||
|
||||
# 绘制网格线
|
||||
for x in range(self.map_width + 1):
|
||||
pygame.draw.line(self.display, self.COLORS['grid'],
|
||||
(self.grid_offset_x + x * self.GRID_SIZE, self.grid_offset_y),
|
||||
(self.grid_offset_x + x * self.GRID_SIZE,
|
||||
self.grid_offset_y + self.grid_height), 1)
|
||||
|
||||
for y in range(self.map_height + 1):
|
||||
pygame.draw.line(self.display, self.COLORS['grid'],
|
||||
(self.grid_offset_x, self.grid_offset_y + y * self.GRID_SIZE),
|
||||
(self.grid_offset_x + self.grid_width,
|
||||
self.grid_offset_y + y * self.GRID_SIZE), 1)
|
||||
|
||||
# 绘制坐标标签
|
||||
for x in range(self.map_width):
|
||||
text = self.small_font.render(str(x), True, self.COLORS['text'])
|
||||
pos_x, _ = self.grid_to_screen((x, -0.5))
|
||||
self.display.blit(text, (pos_x - text.get_width() // 2, self.grid_offset_y - 20))
|
||||
|
||||
for y in range(self.map_height):
|
||||
text = self.small_font.render(str(y), True, self.COLORS['text'])
|
||||
_, pos_y = self.grid_to_screen((-0.5, y))
|
||||
self.display.blit(text, (self.grid_offset_x - 25, pos_y - text.get_height() // 2))
|
||||
|
||||
def draw_cells(self):
|
||||
"""绘制单元格"""
|
||||
# 绘制障碍物
|
||||
for x in range(self.map_width):
|
||||
for y in range(self.map_height):
|
||||
pos = Position(x, y)
|
||||
if self.pathfinder.Map[pos]:
|
||||
rect = self.grid_to_screen_rect((x, y))
|
||||
pygame.draw.rect(self.display, self.COLORS['obstacle'],
|
||||
(rect[0] + 2, rect[1] + 2, rect[2] - 4, rect[3] - 4))
|
||||
|
||||
# 绘制完整路径(如果已找到)
|
||||
if self.full_path and len(self.full_path) > 1:
|
||||
# 绘制路径线
|
||||
points = []
|
||||
for pos in self.full_path:
|
||||
x, y = self.grid_to_screen((pos.X, pos.Y))
|
||||
points.append((x, y))
|
||||
|
||||
if len(points) > 1:
|
||||
pygame.draw.lines(self.display, self.COLORS['path'], False, points, 3)
|
||||
|
||||
# 绘制路径点
|
||||
for i, pos in enumerate(self.full_path):
|
||||
if i <= self.path_index: # 只绘制已走过的路径点
|
||||
x, y = self.grid_to_screen((pos.X, pos.Y))
|
||||
radius = max(3, 6 - i * 0.1)
|
||||
alpha = 255 - min(100, i * 10)
|
||||
color = (self.COLORS['path'][0],
|
||||
self.COLORS['path'][1],
|
||||
self.COLORS['path'][2],
|
||||
alpha)
|
||||
|
||||
# 创建一个surface来绘制带透明度的圆
|
||||
if alpha < 255:
|
||||
circle_surf = pygame.Surface((radius*2, radius*2), pygame.SRCALPHA)
|
||||
pygame.draw.circle(circle_surf, color, (radius, radius), radius)
|
||||
self.display.blit(circle_surf, (x - radius, y - radius))
|
||||
else:
|
||||
pygame.draw.circle(self.display, color, (int(x), int(y)), int(radius))
|
||||
|
||||
# 绘制起点
|
||||
start_rect = self.grid_to_screen_rect((self.pathfinder.position.X,
|
||||
self.pathfinder.position.Y))
|
||||
pygame.draw.rect(self.display, self.COLORS['start'],
|
||||
(start_rect[0] + 5, start_rect[1] + 5,
|
||||
start_rect[2] - 10, start_rect[3] - 10), border_radius=3)
|
||||
start_text = self.medium_font.render("S", True, (255, 255, 255))
|
||||
start_text_rect = start_text.get_rect(center=(start_rect[0] + start_rect[2] // 2,
|
||||
start_rect[1] + start_rect[3] // 2))
|
||||
self.display.blit(start_text, start_text_rect)
|
||||
|
||||
# 绘制终点
|
||||
target_rect = self.grid_to_screen_rect((self.pathfinder.target.X,
|
||||
self.pathfinder.target.Y))
|
||||
pygame.draw.rect(self.display, self.COLORS['target'],
|
||||
(target_rect[0] + 5, target_rect[1] + 5,
|
||||
target_rect[2] - 10, target_rect[3] - 10), border_radius=3)
|
||||
target_text = self.medium_font.render("T", True, (255, 255, 255))
|
||||
target_text_rect = target_text.get_rect(center=(target_rect[0] + target_rect[2] // 2,
|
||||
target_rect[1] + target_rect[3] // 2))
|
||||
self.display.blit(target_text, target_text_rect)
|
||||
|
||||
# 绘制当前位置(如果正在移动)
|
||||
if self.is_running and not self.is_path_complete:
|
||||
current_rect = self.grid_to_screen_rect((self.pathfinder.position.X,
|
||||
self.pathfinder.position.Y))
|
||||
pygame.draw.rect(self.display, self.COLORS['current'],
|
||||
(current_rect[0] + 8, current_rect[1] + 8,
|
||||
current_rect[2] - 16, current_rect[3] - 16), border_radius=2)
|
||||
|
||||
def draw_tool_panel(self):
|
||||
"""绘制工具面板"""
|
||||
# 面板背景
|
||||
panel_rect = (self.tool_panel_x, self.tool_panel_y,
|
||||
self.tool_panel_width, self.tool_panel_height)
|
||||
pygame.draw.rect(self.display, self.COLORS['panel'], panel_rect, border_radius=10)
|
||||
pygame.draw.rect(self.display, (180, 190, 210), panel_rect, 2, border_radius=10)
|
||||
|
||||
# 面板标题
|
||||
title = self.font.render("PathFinder Playground", True, self.COLORS['text'])
|
||||
self.display.blit(title, (self.tool_panel_x + 20, self.tool_panel_y + 20))
|
||||
|
||||
# 工具选择
|
||||
tools_y = self.tool_panel_y + 60
|
||||
tool_title = self.medium_font.render("工具:", True, self.COLORS['text'])
|
||||
self.display.blit(tool_title, (self.tool_panel_x + 20, tools_y))
|
||||
|
||||
tools = [
|
||||
{"mode": ToolMode.START, "text": "设置起点 (S)", "key": pygame.K_s},
|
||||
{"mode": ToolMode.TARGET, "text": "设置终点 (T)", "key": pygame.K_t},
|
||||
{"mode": ToolMode.OBSTACLE, "text": "绘制障碍物 (O)", "key": pygame.K_o},
|
||||
{"mode": ToolMode.ERASER, "text": "擦除障碍物 (E)", "key": pygame.K_e},
|
||||
]
|
||||
|
||||
tool_button_width = 260
|
||||
tool_button_height = 40
|
||||
tool_button_margin = 10
|
||||
|
||||
mouse_pos = pygame.mouse.get_pos()
|
||||
self.tool_buttons = []
|
||||
|
||||
for i, tool in enumerate(tools):
|
||||
button_y = tools_y + 40 + i * (tool_button_height + tool_button_margin)
|
||||
button_rect = pygame.Rect(self.tool_panel_x + 20, button_y,
|
||||
tool_button_width, tool_button_height)
|
||||
|
||||
# 判断是否激活
|
||||
is_active = self.tool_mode == tool["mode"]
|
||||
is_hover = button_rect.collidepoint(mouse_pos)
|
||||
|
||||
# 选择颜色
|
||||
if is_active:
|
||||
color = self.COLORS['button_active']
|
||||
elif is_hover:
|
||||
color = self.COLORS['button_hover']
|
||||
else:
|
||||
color = self.COLORS['button']
|
||||
|
||||
# 绘制按钮
|
||||
pygame.draw.rect(self.display, color, button_rect, border_radius=5)
|
||||
pygame.draw.rect(self.display, (60, 80, 120), button_rect, 2, border_radius=5)
|
||||
|
||||
# 绘制工具文字
|
||||
tool_text = self.medium_font.render(tool["text"], True, (255, 255, 255))
|
||||
text_rect = tool_text.get_rect(center=button_rect.center)
|
||||
self.display.blit(tool_text, text_rect)
|
||||
|
||||
self.tool_buttons.append({
|
||||
"rect": button_rect,
|
||||
"mode": tool["mode"],
|
||||
"key": tool["key"]
|
||||
})
|
||||
|
||||
# 控制按钮
|
||||
controls_y = tools_y + 40 + len(tools) * (tool_button_height + tool_button_margin) + 30
|
||||
control_title = self.medium_font.render("控制:", True, self.COLORS['text'])
|
||||
self.display.blit(control_title, (self.tool_panel_x + 20, controls_y))
|
||||
|
||||
controls = [
|
||||
{"action": "find_path", "text": "寻找路径 (F)", "key": pygame.K_f},
|
||||
{"action": "step", "text": "单步执行 (Space)", "key": pygame.K_SPACE},
|
||||
{"action": "run", "text": "运行到终点 (R)", "key": pygame.K_r},
|
||||
{"action": "reset", "text": "重置 (C)", "key": pygame.K_c},
|
||||
{"action": "clear_obstacles", "text": "清空障碍", "key": pygame.K_DELETE},
|
||||
{"action": "toggle_diagonal", "text": f"斜向移动: {'ON' if self.allow_diagonal else 'OFF'}",
|
||||
"key": pygame.K_d},
|
||||
]
|
||||
|
||||
self.control_buttons = []
|
||||
|
||||
for i, control in enumerate(controls):
|
||||
button_y = controls_y + 40 + i * (tool_button_height + tool_button_margin // 2)
|
||||
button_rect = pygame.Rect(self.tool_panel_x + 20, button_y,
|
||||
tool_button_width, tool_button_height)
|
||||
|
||||
is_hover = button_rect.collidepoint(mouse_pos)
|
||||
color = self.COLORS['button_hover'] if is_hover else self.COLORS['button']
|
||||
|
||||
# 绘制按钮
|
||||
pygame.draw.rect(self.display, color, button_rect, border_radius=5)
|
||||
pygame.draw.rect(self.display, (60, 80, 120), button_rect, 2, border_radius=5)
|
||||
|
||||
# 绘制控制文字
|
||||
control_text = self.small_font.render(control["text"], True, (255, 255, 255))
|
||||
text_rect = control_text.get_rect(center=button_rect.center)
|
||||
self.display.blit(control_text, text_rect)
|
||||
|
||||
self.control_buttons.append({
|
||||
"rect": button_rect,
|
||||
"action": control["action"],
|
||||
"key": control["key"]
|
||||
})
|
||||
|
||||
def draw_info_panel(self):
|
||||
"""绘制信息面板"""
|
||||
panel_height = 180
|
||||
panel_rect = (self.tool_panel_x, self.info_panel_y,
|
||||
self.tool_panel_width, panel_height)
|
||||
|
||||
# 面板背景
|
||||
pygame.draw.rect(self.display, self.COLORS['panel'], panel_rect, border_radius=10)
|
||||
pygame.draw.rect(self.display, (180, 190, 210), panel_rect, 2, border_radius=10)
|
||||
|
||||
# 信息标题
|
||||
title = self.medium_font.render("状态信息", True, self.COLORS['text'])
|
||||
self.display.blit(title, (self.tool_panel_x + 20, self.info_panel_y + 20))
|
||||
|
||||
# 状态信息
|
||||
info_y = self.info_panel_y + 50
|
||||
|
||||
# 当前位置
|
||||
current_pos = self.pathfinder.position
|
||||
current_text = self.small_font.render(f"当前位置: ({current_pos.X}, {current_pos.Y})",
|
||||
True, self.COLORS['info'])
|
||||
self.display.blit(current_text, (self.tool_panel_x + 30, info_y))
|
||||
|
||||
# 目标位置
|
||||
target_pos = self.pathfinder.target
|
||||
target_text = self.small_font.render(f"目标位置: ({target_pos.X}, {target_pos.Y})",
|
||||
True, self.COLORS['info'])
|
||||
self.display.blit(target_text, (self.tool_panel_x + 30, info_y + 25))
|
||||
|
||||
# 路径状态
|
||||
if self.is_path_complete:
|
||||
status_text = "状态: 已到达目标"
|
||||
status_color = self.COLORS['success']
|
||||
elif self.is_path_found:
|
||||
status_text = f"状态: 路径已找到 ({len(self.full_path)}步)"
|
||||
status_color = self.COLORS['success']
|
||||
elif self.full_path:
|
||||
status_text = "状态: 正在寻路中"
|
||||
status_color = self.COLORS['warning']
|
||||
else:
|
||||
status_text = "状态: 未开始寻路"
|
||||
status_color = self.COLORS['text']
|
||||
|
||||
status_display = self.small_font.render(status_text, True, status_color)
|
||||
self.display.blit(status_display, (self.tool_panel_x + 30, info_y + 50))
|
||||
|
||||
# 步骤计数
|
||||
steps_text = self.small_font.render(f"已走步数: {self.steps}", True, self.COLORS['info'])
|
||||
self.display.blit(steps_text, (self.tool_panel_x + 30, info_y + 75))
|
||||
|
||||
# 移动模式
|
||||
mode_text = f"移动模式: {'8方向' if self.allow_diagonal else '4方向'}"
|
||||
mode_display = self.small_font.render(mode_text, True, self.COLORS['info'])
|
||||
self.display.blit(mode_display, (self.tool_panel_x + 30, info_y + 100))
|
||||
|
||||
def draw_instructions(self):
|
||||
"""绘制使用说明"""
|
||||
instructions = [
|
||||
"使用说明:",
|
||||
"1. 选择工具在网格上绘制地图",
|
||||
"2. 点击'寻找路径'按钮计算路径",
|
||||
"3. 使用'单步执行'或'运行到终点'观察移动",
|
||||
"4. 按对应快捷键快速切换工具/控制"
|
||||
]
|
||||
|
||||
inst_y = self.info_panel_y + 200
|
||||
for i, instruction in enumerate(instructions):
|
||||
color = self.COLORS['text'] if i == 0 else self.COLORS['info']
|
||||
font = self.medium_font if i == 0 else self.small_font
|
||||
inst_text = font.render(instruction, True, color)
|
||||
self.display.blit(inst_text, (self.tool_panel_x + 20, inst_y + i * 30))
|
||||
|
||||
def draw(self):
|
||||
"""绘制所有内容"""
|
||||
# 清空屏幕
|
||||
self.display.fill(self.COLORS['background'])
|
||||
|
||||
# 绘制网格和单元格
|
||||
self.draw_grid()
|
||||
self.draw_cells()
|
||||
|
||||
# 绘制UI面板
|
||||
self.draw_tool_panel()
|
||||
self.draw_info_panel()
|
||||
self.draw_instructions()
|
||||
|
||||
# 更新显示
|
||||
pygame.display.flip()
|
||||
|
||||
def handle_mouse_down(self, pos, button):
|
||||
"""处理鼠标按下"""
|
||||
if not self.is_in_grid(pos):
|
||||
return
|
||||
|
||||
grid_x, grid_y = self.screen_to_grid(pos)
|
||||
if not (0 <= grid_x < self.map_width and 0 <= grid_y < self.map_height):
|
||||
return
|
||||
|
||||
grid_pos = Position(grid_x, grid_y)
|
||||
|
||||
if button == 1: # 左键
|
||||
if self.tool_mode == ToolMode.START:
|
||||
# 设置起点
|
||||
if not self.pathfinder.Map[grid_pos]:
|
||||
self.pathfinder.position = Position(grid_x, grid_y)
|
||||
self.full_path = [] # 清除现有路径
|
||||
self.is_path_found = False
|
||||
self.is_path_complete = False
|
||||
self.detect_around_current()
|
||||
|
||||
elif self.tool_mode == ToolMode.TARGET:
|
||||
# 设置终点
|
||||
if not self.pathfinder.Map[grid_pos]:
|
||||
self.pathfinder.setTarget(Position(grid_x, grid_y))
|
||||
self.full_path = [] # 清除现有路径
|
||||
self.is_path_found = False
|
||||
self.is_path_complete = False
|
||||
|
||||
elif self.tool_mode == ToolMode.OBSTACLE:
|
||||
# 添加障碍物(避开起点和终点)
|
||||
if (grid_pos != self.pathfinder.position and
|
||||
grid_pos != self.pathfinder.target):
|
||||
self.pathfinder.Map[grid_pos] = True
|
||||
self.full_path = [] # 清除现有路径
|
||||
self.is_path_found = False
|
||||
self.is_path_complete = False
|
||||
|
||||
elif self.tool_mode == ToolMode.ERASER:
|
||||
# 擦除障碍物
|
||||
self.pathfinder.Map[grid_pos] = False
|
||||
self.full_path = [] # 清除现有路径
|
||||
self.is_path_found = False
|
||||
self.is_path_complete = False
|
||||
|
||||
self.is_drawing = True
|
||||
|
||||
elif button == 3: # 右键
|
||||
# 右键快速设置起点(按住Shift)或终点
|
||||
if not self.pathfinder.Map[grid_pos]:
|
||||
keys = pygame.key.get_pressed()
|
||||
if keys[pygame.K_LSHIFT] or keys[pygame.K_RSHIFT]:
|
||||
self.pathfinder.position = Position(grid_x, grid_y)
|
||||
else:
|
||||
self.pathfinder.setTarget(Position(grid_x, grid_y))
|
||||
|
||||
self.full_path = [] # 清除现有路径
|
||||
self.is_path_found = False
|
||||
self.is_path_complete = False
|
||||
self.detect_around_current()
|
||||
|
||||
def handle_mouse_motion(self, pos):
|
||||
"""处理鼠标移动"""
|
||||
if self.is_drawing and self.tool_mode in [ToolMode.OBSTACLE, ToolMode.ERASER]:
|
||||
if self.is_in_grid(pos):
|
||||
grid_x, grid_y = self.screen_to_grid(pos)
|
||||
if 0 <= grid_x < self.map_width and 0 <= grid_y < self.map_height:
|
||||
grid_pos = Position(grid_x, grid_y)
|
||||
|
||||
if self.tool_mode == ToolMode.OBSTACLE:
|
||||
# 绘制障碍物(避开起点和终点)
|
||||
if (grid_pos != self.pathfinder.position and
|
||||
grid_pos != self.pathfinder.target):
|
||||
self.pathfinder.Map[grid_pos] = True
|
||||
|
||||
elif self.tool_mode == ToolMode.ERASER:
|
||||
# 擦除障碍物
|
||||
self.pathfinder.Map[grid_pos] = False
|
||||
|
||||
def handle_mouse_up(self, pos, button):
|
||||
"""处理鼠标释放"""
|
||||
if button == 1: # 左键
|
||||
self.is_drawing = False
|
||||
|
||||
def handle_button_click(self, button_info):
|
||||
"""处理按钮点击"""
|
||||
action = button_info.get("action")
|
||||
|
||||
if action == "find_path":
|
||||
self.find_path()
|
||||
self.is_running = False
|
||||
self.auto_run = False
|
||||
|
||||
elif action == "step":
|
||||
if not self.is_running:
|
||||
self.find_path()
|
||||
self.is_running = True
|
||||
self.step_forward()
|
||||
|
||||
elif action == "run":
|
||||
self.run_to_completion()
|
||||
|
||||
elif action == "reset":
|
||||
self.load_state()
|
||||
self.full_path = []
|
||||
self.is_path_found = False
|
||||
self.is_path_complete = False
|
||||
self.is_running = False
|
||||
self.auto_run = False
|
||||
self.steps = 0
|
||||
self.path_index = 0
|
||||
|
||||
elif action == "clear_obstacles":
|
||||
# 清空所有障碍物
|
||||
for x in range(self.map_width):
|
||||
for y in range(self.map_height):
|
||||
pos = Position(x, y)
|
||||
self.pathfinder.Map[pos] = False
|
||||
self.full_path = []
|
||||
self.is_path_found = False
|
||||
self.is_path_complete = False
|
||||
|
||||
elif action == "toggle_diagonal":
|
||||
self.allow_diagonal = not self.allow_diagonal
|
||||
self.reset_pathfinder()
|
||||
self.load_state()
|
||||
|
||||
def handle_events(self):
|
||||
"""处理所有事件"""
|
||||
for event in pygame.event.get():
|
||||
if event.type == pygame.QUIT:
|
||||
return False
|
||||
|
||||
elif event.type == pygame.MOUSEBUTTONDOWN:
|
||||
mouse_pos = pygame.mouse.get_pos()
|
||||
|
||||
# 检查工具按钮
|
||||
for button in self.tool_buttons:
|
||||
if button["rect"].collidepoint(mouse_pos):
|
||||
self.tool_mode = button["mode"]
|
||||
break
|
||||
|
||||
# 检查控制按钮
|
||||
for button in self.control_buttons:
|
||||
if button["rect"].collidepoint(mouse_pos):
|
||||
self.handle_button_click(button)
|
||||
break
|
||||
|
||||
# 处理网格上的点击
|
||||
self.handle_mouse_down(mouse_pos, event.button)
|
||||
|
||||
elif event.type == pygame.MOUSEMOTION:
|
||||
self.handle_mouse_motion(event.pos)
|
||||
|
||||
elif event.type == pygame.MOUSEBUTTONUP:
|
||||
self.handle_mouse_up(event.pos, event.button)
|
||||
|
||||
elif event.type == pygame.KEYDOWN:
|
||||
# 快捷键
|
||||
if event.key == pygame.K_s:
|
||||
self.tool_mode = ToolMode.START
|
||||
elif event.key == pygame.K_t:
|
||||
self.tool_mode = ToolMode.TARGET
|
||||
elif event.key == pygame.K_o:
|
||||
self.tool_mode = ToolMode.OBSTACLE
|
||||
elif event.key == pygame.K_e:
|
||||
self.tool_mode = ToolMode.ERASER
|
||||
elif event.key == pygame.K_f:
|
||||
self.find_path()
|
||||
elif event.key == pygame.K_SPACE:
|
||||
if not self.is_running:
|
||||
self.find_path()
|
||||
self.is_running = True
|
||||
self.step_forward()
|
||||
elif event.key == pygame.K_r:
|
||||
self.run_to_completion()
|
||||
elif event.key == pygame.K_c:
|
||||
self.load_state()
|
||||
self.full_path = []
|
||||
self.is_path_found = False
|
||||
self.is_path_complete = False
|
||||
self.is_running = False
|
||||
self.auto_run = False
|
||||
self.steps = 0
|
||||
self.path_index = 0
|
||||
elif event.key == pygame.K_d:
|
||||
self.allow_diagonal = not self.allow_diagonal
|
||||
self.reset_pathfinder()
|
||||
self.load_state()
|
||||
elif event.key == pygame.K_DELETE:
|
||||
# 清空障碍物
|
||||
for x in range(self.map_width):
|
||||
for y in range(self.map_height):
|
||||
pos = Position(x, y)
|
||||
self.pathfinder.Map[pos] = False
|
||||
self.full_path = []
|
||||
self.is_path_found = False
|
||||
self.is_path_complete = False
|
||||
|
||||
return True
|
||||
|
||||
def update(self):
|
||||
"""更新状态"""
|
||||
import time
|
||||
|
||||
if self.auto_run and self.is_running:
|
||||
current_time = time.time()
|
||||
if current_time - self.last_step_time > self.speed:
|
||||
if not self.step_forward():
|
||||
self.auto_run = False
|
||||
self.last_step_time = current_time
|
||||
|
||||
def run(self):
|
||||
"""主循环"""
|
||||
clock = pygame.time.Clock()
|
||||
|
||||
print("=" * 50)
|
||||
print("PathFinder Playground 启动")
|
||||
print("使用说明:")
|
||||
print("1. 使用工具栏选择工具(起点、终点、障碍物、擦除)")
|
||||
print("2. 在网格上点击或拖动绘制地图")
|
||||
print("3. 点击'寻找路径'按钮计算路径")
|
||||
print("4. 使用'单步执行'或'运行到终点'观察移动")
|
||||
print("5. 快捷键: S-起点, T-终点, O-障碍, E-擦除, F-寻路, Space-单步, R-运行")
|
||||
print("=" * 50)
|
||||
|
||||
while True:
|
||||
# 处理事件
|
||||
if not self.handle_events():
|
||||
break
|
||||
|
||||
# 更新状态
|
||||
self.update()
|
||||
|
||||
# 绘制
|
||||
self.draw()
|
||||
|
||||
# 控制帧率
|
||||
clock.tick(60)
|
||||
|
||||
pygame.quit()
|
||||
sys.exit()
|
||||
|
||||
def main():
|
||||
playground = PathFinderPlayground()
|
||||
playground.run()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
2
requirements.txt
普通文件
2
requirements.txt
普通文件
@ -0,0 +1,2 @@
|
||||
numpy==2.3.5
|
||||
pygame==2.6.1
|
||||
在新工单中引用
屏蔽一个用户