这个提交包含在:
2025-12-05 17:26:58 +08:00
当前提交 d87d9884f0
共有 5 个文件被更改,包括 2183 次插入 和 0 次删除

2
.gitignore vendored 普通文件
查看文件

@ -0,0 +1,2 @@
__pycache__
.venv

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 普通文件
查看文件

@ -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 普通文件
查看文件

@ -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 普通文件
查看文件

@ -0,0 +1,2 @@
numpy==2.3.5
pygame==2.6.1