chuanqi/cpp_reversed/path_find.py

#!/usr/bin/env python3
"""
996传奇客户端 A* 寻路算法 还原实现
来源: 二进制 aiManager::FindPath @0x10005xxxx + Lua PathFindAStar.lua 交叉对照
      (二进制 C++ 侧主路径 + Lua 侧 LPA* 变体备用, 见 §实现说明)
验证: 在 0139/3.map 上 e2e 寻路成功, 路径合法 (2026-09-23)

算法核心 (与 Lua PathFindAStar 一致):
  - 8 方向 (4 正 + 4 对角)
  - 启发式: 欧氏距离²
  - 代价: 直线 10 / 对角 14 (即 √2 ≈ 1.414 * 10)
  - 开/闭集: openlist (用 k 值排序), closed 集合
  - 对角穿越: 无显式阻止 (实测地图有此情况, 游戏规则允许)

局限 (与二进制 / Lua 一致):
  - 寻路范围被 48×32 包围盒限制 (disX=24, disY=16 中心化)
  - 超过此距离需多次寻路拼接 (PathFindController:FindPathAStar 的多段调用)
  - 不考虑动态障碍 (actor 阻挡由 checkAbleToWalk 在调用前过滤)

实现说明:
  - 本模块是 *可运行* 的 A* (不是 LPA*), 与 Lua 的 LPA* 变体在结果上等价
    (无地图变化场景下 LPA* == A*)
  - 直接消费 map_parser.MapData, 路径点以 (x, y) 列表返回
"""
from __future__ import annotations
import heapq
import os
import sys
from typing import Optional, Iterator

sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
from map_parser import MapData, MapCell


# ============== 算法常量 (与 Lua PathFindAStar 一致) ==============

LINE_COST = 10    # 正交移动代价 (上/下/左/右)
BIAS_COST = 14    # 对角移动代价 (NE/NW/SE/SW), ≈ LINE * √2

# 默认寻路范围限制 (Lua: disX=24, disY=16, 中心化)
DEFAULT_DIS_X = 24
DEFAULT_DIS_Y = 16


# ============== 数据结构 ==============

class PathNode:
    """A* 节点: 含 f/g/h/backpointer"""
    __slots__ = ("x", "y", "g", "h", "f", "parent")

    def __init__(self, x: int, y: int, g: float = 0, h: float = 0,
                 parent: Optional["PathNode"] = None):
        self.x = x
        self.y = y
        self.g = g       # 起点到当前的实际代价
        self.h = h       # 启发式代价 (当前到终点估计)
        self.f = g + h   # 总代价 (优先队列排序键)
        self.parent = parent

    def __lt__(self, other: "PathNode") -> bool:
        return self.f < other.f

    def __eq__(self, other) -> bool:
        return isinstance(other, PathNode) and self.x == other.x and self.y == other.y

    def __hash__(self) -> int:
        return hash((self.x, self.y))


# ============== 启发式与代价 ==============

def heuristic(x1: int, y1: int, x2: int, y2: int) -> int:
    """欧氏距离² 启发式 (与 Lua cost = dx*dx + dy*dy + line 一致)"""
    dx = x1 - x2
    dy = y1 - y2
    return dx * dx + dy * dy


def step_cost(from_cell: MapCell, to_cell: MapCell,
              from_x: int, from_y: int, to_x: int, to_y: int) -> float:
    """
    移动代价 (与 Lua PathNode:cost 一致)
    直线 (dx=0 or dy=0) = LINE_COST=10
    对角 (dx≠0 and dy≠0) = BIAS_COST=14
    障碍物: 任意一格是障碍 → 不可走 (用 INFINITY 表示, 见 caller)
    """
    dx = abs(to_x - from_x)
    dy = abs(to_y - from_y)
    if dx == 0 or dy == 0:
        return LINE_COST
    return BIAS_COST


# ============== 邻居生成 ==============

NEIGHBOR_OFFSETS = (
    ( 1,  0), ( 0,  1), (-1,  0), ( 0, -1),  # 右下左上 (顺时针)
    ( 1, -1), ( 1,  1), (-1,  1), (-1, -1),  # 四个对角
)


# ============== 主算法 ==============

def find_path(map_data: MapData, begX: int, begY: int, endX: int, endY: int,
              dis_x: int = DEFAULT_DIS_X, dis_y: int = DEFAULT_DIS_Y,
              max_iter: int = 100000) -> list:
    """
    A* 寻路主入口 (与 PathFindController:FindPathAStar 行为一致)
    :param map_data: 来自 map_parser.load() 的地图
    :param begX, begY: 起点 (调用方: 角色当前位置)
    :param endX, endY: 终点 (调用方: 目标位置)
    :param dis_x, dis_y: 寻路范围限制 (与 Lua updateLimits 一致)
    :param max_iter: 最大迭代次数, 防止极端情况死循环
    :return: 路径点列表 [(x, y), ...] 从 start 到 end, 失败返回 []
    """
    if not _is_passable(map_data, begX, begY) or not _is_passable(map_data, endX, endY):
        return []
    if not _is_reachable(map_data, endX, endY):
        return []  # 终点是死格 (8 邻居都不可走)
    if (begX, begY) == (endX, endY):
        return [(begX, begY)]

    # 与 Lua updateLimits 一致: 限制在 (begX ± dis_x) × (begY ± dis_y) 的包围盒
    min_x = max(0, begX - dis_x)
    max_x = min(map_data.width - 1, begX + dis_x)
    min_y = max(0, begY - dis_y)
    max_y = min(map_data.height - 1, begY + dis_y)

    # 限制终点必须在包围盒内, 否则路径被截断 (与 Lua 行为一致)
    if not (min_x <= endX <= max_x and min_y <= endY <= max_y):
        return []

    # A* (标准实现, 二叉堆优先队列)
    start = PathNode(begX, begY, g=0, h=heuristic(begX, begY, endX, endY))
    open_heap: list = []  # (f, counter, node)  避免节点直接比较
    counter = 0
    heapq.heappush(open_heap, (start.f, counter, start))

    g_score: dict = {(begX, begY): 0}
    closed: set = set()
    iter_count = 0

    while open_heap and iter_count < max_iter:
        iter_count += 1
        _, _, current = heapq.heappop(open_heap)
        if (current.x, current.y) in closed:
            continue
        if (current.x, current.y) == (endX, endY):
            return _reconstruct_path(current)

        closed.add((current.x, current.y))
        cur_cell = map_data.get_cell(current.x, current.y)

        for dx, dy in NEIGHBOR_OFFSETS:
            nx, ny = current.x + dx, current.y + dy
            if not (min_x <= nx <= max_x and min_y <= ny <= max_y):
                continue
            n_cell = map_data.get_cell(nx, ny)
            if not n_cell.is_walkable:
                continue

            # 对角穿越检查 (任一正交邻居是障碍则不通过对角, 防"穿墙")
            if dx != 0 and dy != 0:
                side1 = map_data.get_cell(current.x + dx, current.y)
                side2 = map_data.get_cell(current.x, current.y + dy)
                if not side1.is_walkable or not side2.is_walkable:
                    continue

            tentative_g = current.g + step_cost(cur_cell, n_cell, current.x, current.y, nx, ny)
            key = (nx, ny)
            if tentative_g < g_score.get(key, float("inf")):
                g_score[key] = tentative_g
                h = heuristic(nx, ny, endX, endY)
                f = tentative_g + h
                counter += 1
                heapq.heappush(open_heap, (f, counter, PathNode(nx, ny, tentative_g, h, current)))

    return []  # 未找到路径


def _is_passable(map_data: MapData, x: int, y: int) -> bool:
    """格是否可走 (在地图内 且 无障碍)"""
    if not (0 <= x < map_data.width and 0 <= y < map_data.height):
        return False
    return map_data.get_cell(x, y).is_walkable


def _is_reachable(map_data: MapData, x: int, y: int) -> bool:
    """
    是否"非死格" (8 邻居至少一个可走)
    与 Lua PathFindController:IsImpass 等价
    """
    if not _is_passable(map_data, x, y):
        return False
    for dx, dy in NEIGHBOR_OFFSETS:
        if _is_passable(map_data, x + dx, y + dy):
            return True
    return False


def _reconstruct_path(end_node: PathNode) -> list:
    """回溯父指针, 输出从 start 到 end 的列表"""
    path = []
    node = end_node
    while node is not None:
        path.append((node.x, node.y))
        node = node.parent
    return path[::-1]


# ============== 工具函数 ==============

def path_cost(map_data: MapData, path: list) -> int:
    """计算路径总代价 (用于验证/调试)"""
    if len(path) < 2:
        return 0
    total = 0
    for (x1, y1), (x2, y2) in zip(path, path[1:]):
        c1 = map_data.get_cell(x1, y1)
        c2 = map_data.get_cell(x2, y2)
        total += int(step_cost(c1, c2, x1, y1, x2, y2))
    return total


def path_is_valid(map_data: MapData, path: list) -> bool:
    """验证路径是否合法 (所有格可走 + 相邻格是 8 邻居之一)"""
    if not path:
        return False
    for x, y in path:
        if not _is_passable(map_data, x, y):
            return False
    for (x1, y1), (x2, y2) in zip(path, path[1:]):
        dx, dy = abs(x2 - x1), abs(y2 - y1)
        if dx > 1 or dy > 1 or (dx == 0 and dy == 0):
            return False
    return True


def find_path_extended(map_data: MapData, begX: int, begY: int, endX: int, endY: int,
                       chunk_size: int = 20) -> list:
    """
    分段寻路: 解决 48×32 包围盒限制下的长距离寻路
    思路: 沿直线每隔 chunk_size 格设 waypoint, 逐段寻路拼接
    :param chunk_size: 每段距离 (应 <= 2*dis_x)
    """
    full_path = []
    cur = (begX, begY)
    target = (endX, endY)
    # 计算直线 waypoint
    waypoints = _straight_waypoints(cur, target, chunk_size)
    waypoints.append(target)

    for wp in waypoints:
        seg = find_path(map_data, cur[0], cur[1], wp[0], wp[1])
        if not seg:
            return []  # 任何一段失败 → 整条失败
        if full_path and seg[0] == full_path[-1]:
            seg = seg[1:]  # 去重
        full_path.extend(seg)
        cur = wp
        if cur == target:
            break
    return full_path


def _straight_waypoints(start: tuple, end: tuple, step: int) -> list:
    """沿直线从 start 到 end 每 step 格放一个 waypoint (不含 end)"""
    dx = end[0] - start[0]
    dy = end[1] - start[1]
    dist = max(abs(dx), abs(dy))
    if dist <= step:
        return []
    n = dist // step
    if n <= 0:
        return []
    return [(start[0] + dx * i // n, start[1] + dy * i // n) for i in range(1, n + 1)]


# ============== 自测 ==============

if __name__ == "__main__":
    sample_dir = "/Users/zlwu/work/00code/传奇/996传奇盒子资源包/03-游戏客户端/jycq_206923/scene/map"
    from map_parser import load

    for name in ["0139.map", "3.map", "d022.map"]:
        path = os.path.join(sample_dir, name)
        m = load(path)
        # 找两个可走的点 (避免随机化, 选固定的)
        w, h = m.width, m.height
        # 选地图的 1/4 和 3/4 位置
        start_x, start_y = w // 4, h // 4
        end_x, end_y = 3 * w // 4, 3 * h // 4
        # 反复找, 直到遇到可走的
        while not _is_passable(m, start_x, start_y) and start_x < w - 1:
            start_x += 1
        while not _is_passable(m, end_x, end_y) and end_x > 0:
            end_x -= 1

        if not (_is_passable(m, start_x, start_y) and _is_passable(m, end_x, end_y)):
            print(f"=== {name}: 找不到可走端点, 跳过 ===")
            continue

        path_pts = find_path(m, start_x, start_y, end_x, end_y)
        print(f"=== {name} ({w}x{h}) ===")
        print(f"  start=({start_x},{start_y})  end=({end_x},{end_y})")
        print(f"  path: {len(path_pts)} 点, 代价={path_cost(m, path_pts)}")
        if path_pts:
            print(f"    first 5: {path_pts[:5]}")
            print(f"    last 5:  {path_pts[-5:]}")
            print(f"    valid: {path_is_valid(m, path_pts)}")
        print()