A星寻路算法(五):权重与并发

Number of views 57

所谓的权重,即是多条可选线路中选择更优路线,如下图A与B两点,三条可选线路用黑线表示。其中直线代表该条线路需要过沼泽,稍微弯一些的曲线表示这条线路经过草地。最后一条曲线经过马路。按常识来讲,沼泽地很难通过,草地次之,马路最优。所以当通过沼泽时,尽管它是直线,线路可能通过时间最慢。这个就是权重的意义,因为我们需要选取最优线路。

image1747098608972.png

理论上直线的距离最短,但是结合现实,沼泽地最难穿越,那最难穿越在代码中应该怎么表示?我们可以通过移动惩罚 (或叫地形成本)来表示,虽然直线的距离最短,但是除了最短距离外还需要附加 地形成本来对比三条线路的最优路径。

由于之前寻路的逻辑代码只适合单点对单点的寻路逻辑,所以我们需要重新改造下,让其可以在多个角色情况下,同时对单个或多个目标进行寻路并发。

using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using System;

/// <summary>
/// 路径请求管理器:负责接收路径请求并按顺序处理
/// 使用队列确保路径请求按顺序处理,防止并发处理冲突
/// </summary>
public class PathRequestManager : MonoBehaviour {

    // 路径请求队列
    Queue<PathRequest> pathRequestQueue = new Queue<PathRequest>();
    // 当前正在处理的路径请求
    PathRequest currentPathRequest;

    // 单例实例(静态访问)
    static PathRequestManager instance;
    // 路径查找组件
    Pathfinding pathfinding;

    // 是否正在处理路径
    bool isProcessingPath;

    // 初始化方法(Unity生命周期)
    void Awake() {
        // 设置单例实例
        instance = this;
        // 获取路径查找组件
        pathfinding = GetComponent<Pathfinding>();
    }

    /// <summary>
    /// 请求路径查找
    /// </summary>
    /// <param name="pathStart">起点坐标</param>
    /// <param name="pathEnd">终点坐标</param>
    /// <param name="callback">处理完成后的回调函数</param>
    public static void RequestPath(Vector3 pathStart, Vector3 pathEnd, Action<Vector3[], bool> callback) {
        // 创建新的路径请求
        PathRequest newRequest = new PathRequest(pathStart,pathEnd,callback);
        // 将请求加入队列
        instance.pathRequestQueue.Enqueue(newRequest);
        // 尝试开始处理下一个请求
        instance.TryProcessNext();
    }

    /// <summary>
    /// 尝试处理队列中的下一个路径请求
    /// </summary>
    void TryProcessNext() {
        // 如果当前未处理路径且队列不为空
        if (!isProcessingPath && pathRequestQueue.Count > 0) {
            // 取出队列中的第一个请求
            currentPathRequest = pathRequestQueue.Dequeue();
            // 标记正在处理路径
            isProcessingPath = true;
            // 开始路径查找(调用Pathfinding组件的方法)
            pathfinding.StartFindPath(currentPathRequest.pathStart, currentPathRequest.pathEnd);
        }
    }

    /// <summary>
    /// 路径处理完成回调
    /// </summary>
    /// <param name="path">找到的路径点数组</param>
    /// <param name="success">是否成功找到路径</param>
    public void FinishedProcessingPath(Vector3[] path, bool success) {
        // 调用原始请求的回调函数
        currentPathRequest.callback(path,success);
        // 标记路径处理完成
        isProcessingPath = false;
        // 继续处理下一个请求
        TryProcessNext();
    }

    /// <summary>
    /// 路径请求数据结构
    /// 保存路径请求的起点、终点和回调函数
    /// </summary>
    struct PathRequest {
        public Vector3 pathStart;   // 起点坐标
        public Vector3 pathEnd;     // 终点坐标
        public Action<Vector3[], bool> callback;  // 回调函数

        public PathRequest(Vector3 _start, Vector3 _end, Action<Vector3[], bool> _callback) {
            pathStart = _start;
            pathEnd = _end;
            callback = _callback;
        }
    }
}

在PathRequestManager类中我们新增了RequestPath函数,该函数会把所有调用操作加入队列当中,通过先后顺序取出最靠前的队列,当寻路逻辑完成后,由PathFinding类触发FinishedProcessingPath逻辑。该逻辑会再次取出调用次序靠后的队列,继续完成对应队列的寻路操作,循环往复,直到队列为空,每次调用都会触发寻路完成的回调,以便上层代码进行额外的逻辑处理。下面是完整的PathFinding类的修改:

using UnityEngine;
using System.Collections;
using System.Collections.Generic;
using System;

/// <summary>
/// 路径查找器:使用A*算法计算两点之间的最优路径
/// 通过协程实现异步寻路,避免阻塞主线程
/// </summary>
public class Pathfinding : MonoBehaviour {

	// 路径请求管理器(用于回调路径结果)
	PathRequestManager requestManager;
	// 网格数据(存储节点信息)
	Grid grid;

	// 初始化方法(Unity生命周期)
	void Awake() {
		// 获取路径请求管理器组件
		requestManager = GetComponent<PathRequestManager>();
		// 获取网格组件
		grid = GetComponent<Grid>();
	}


	/// <summary>
	/// 开始路径查找
	/// </summary>
	/// <param name="startPos">起点坐标</param>
	/// <param name="targetPos">目标坐标</param>
	public void StartFindPath(Vector3 startPos, Vector3 targetPos) {
		// 启动协程异步处理路径查找
		StartCoroutine(FindPath(startPos,targetPos));
	}

	/// <summary>
	/// A*寻路算法实现
	/// 使用开放集(优先队列)和关闭集(哈希集合)管理节点搜索
	/// </summary>
	IEnumerator FindPath(Vector3 startPos, Vector3 targetPos) {

		// 初始化返回参数
		Vector3[] waypoints = new Vector3[0];
		bool pathSuccess = false;
	
		// 获取起点和目标节点
		Node startNode = grid.NodeFromWorldPoint(startPos);
		Node targetNode = grid.NodeFromWorldPoint(targetPos);
	
	
		// 如果起点和终点都是可行走的
		if (startNode.walkable && targetNode.walkable) {
			// 开放集:使用堆结构存储待探索节点(按F值排序)
			Heap<Node> openSet = new Heap<Node>(grid.MaxSize);
			// 关闭集:存储已探索的节点
			HashSet<Node> closedSet = new HashSet<Node>();
			openSet.Add(startNode);
		
			// 主循环:探索开放集中的节点
			while (openSet.Count > 0) {
				// 取出当前F值最小的节点
				Node currentNode = openSet.RemoveFirst();
				closedSet.Add(currentNode);
			
				// 到达目标节点时结束
				if (currentNode == targetNode) {
					pathSuccess = true;
					break;
				}
			
				// 遍历当前节点的邻居
				foreach (Node neighbour in grid.GetNeighbours(currentNode)) {
					// 跳过不可行走节点和已探索节点
					if (!neighbour.walkable || closedSet.Contains(neighbour)) {
						continue;
					}
				
					// 计算新的移动代价
					int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour);
					// 如果找到更优路径或该节点未在开放集中
					if (newMovementCostToNeighbour < neighbour.gCost || !openSet.Contains(neighbour)) {
						// 更新节点代价和父节点
						neighbour.gCost = newMovementCostToNeighbour;
						neighbour.hCost = GetDistance(neighbour, targetNode);
						neighbour.parent = currentNode;
					
						// 将节点加入开放集
						if (!openSet.Contains(neighbour))
							openSet.Add(neighbour);
					}
				}
			}
		}
		// 协程等待一帧(避免阻塞主线程)
		yield return null;
		// 生成路径点并回调结果
		if (pathSuccess) {
			waypoints = RetracePath(startNode,targetNode);
		}
		requestManager.FinishedProcessingPath(waypoints,pathSuccess);
	
	}

	/// <summary>
	/// 回溯路径:从目标节点回溯到起点,收集路径节点
	/// </summary>
	Vector3[] RetracePath(Node startNode, Node endNode) {
		List<Node> path = new List<Node>();
		Node currentNode = endNode;
	
		// 通过父节点指针回溯路径
		while (currentNode != startNode) {
			path.Add(currentNode);
			currentNode = currentNode.parent;
		}
		// 简化路径并反转顺序
		Vector3[] waypoints = SimplifyPath(path);
		Array.Reverse(waypoints);
		return waypoints;
	
	}

	/// <summary>
	/// 路径简化:移除不必要的转向点
	/// </summary>
	Vector3[] SimplifyPath(List<Node> path) {
		List<Vector3> waypoints = new List<Vector3>();
		Vector2 directionOld = Vector2.zero;
	
		for (int i = 1; i < path.Count; i ++) {
			// 计算当前移动方向
			Vector2 directionNew = new Vector2(path[i-1].gridX - path[i].gridX,path[i-1].gridY - path[i].gridY);
			// 当方向改变时记录该点
			if (directionNew != directionOld) {
				waypoints.Add(path[i].worldPosition);
			}
			directionOld = directionNew;
		}
		return waypoints.ToArray();
	}

	/// <summary>
	/// 计算两个节点之间的移动代价
	/// 使用曼哈顿距离变体(14=√2*10,10=直线移动代价)
	/// </summary>
	int GetDistance(Node nodeA, Node nodeB) {
		int dstX = Mathf.Abs(nodeA.gridX - nodeB.gridX);
		int dstY = Mathf.Abs(nodeA.gridY - nodeB.gridY);
	
		// 对角线移动代价 + 直线移动代价
		if (dstX > dstY)
			return 14*dstY + 10* (dstX-dstY);
		return 14*dstX + 10 * (dstY-dstX);
	}

}

除了把FindPath函数改为协程外,还对RetracePath的返回值进行了简化,因为在寻路过程我们不需要把每个网格的位置做记录,只需要在路径点改变方向后做下记录即可。以下是新增的上层逻辑:

using UnityEngine;
using System.Collections;

/// <summary>
/// 单位控制器:控制单位沿路径移动
/// 包含寻路请求、路径追踪和可视化功能
/// </summary>
public class Unit : MonoBehaviour {


    /// <summary>
    /// 移动目标(在Unity编辑器中设置)
    /// </summary>
    public Transform target;

    /// <summary>
    /// 移动速度(单位/秒)
    /// </summary>
    float speed = 20;

    /// <summary>
    /// 存储寻路结果的路径点数组
    /// </summary>
    Vector3[] path;

    /// <summary>
    /// 当前目标路点索引
    /// </summary>
    int targetIndex;

    void Start() {
        // 请求路径查找:从当前位置到目标位置
        PathRequestManager.RequestPath(transform.position,target.position, OnPathFound);
    }

    /// <summary>
    /// 路径查找结果回调
    /// </summary>
    /// <param name="newPath">找到的新路径</param>
    /// <param name="pathSuccessful">是否成功找到路径</param>
    public void OnPathFound(Vector3[] newPath, bool pathSuccessful) {
        if (pathSuccessful) {
            // 更新路径数据
            path = newPath;
            targetIndex = 0;
            // 停止并重新启动路径追踪协程(确保路径更新生效)
            StopCoroutine("FollowPath");
            StartCoroutine("FollowPath");
        }
    }

    /// <summary>
    /// 路径追踪协程:沿着路径逐步移动到目标
    /// </summary>
    IEnumerator FollowPath() {
        Vector3 currentWaypoint = path[0];
        while (true) {
            // 到达当前路点时切换到下一个路点
            if (transform.position == currentWaypoint) {
                targetIndex ++;
                if (targetIndex >= path.Length) {
                    yield break; // 到达最终目标点时结束
                }
                currentWaypoint = path[targetIndex];
            }

            // 向当前路点移动
            transform.position = Vector3.MoveTowards(transform.position,currentWaypoint,speed * Time.deltaTime);
            yield return null; // 等待下一帧
        }
    }

    /// <summary>
    /// 场景视图路径可视化
    /// 使用Gizmos绘制路径点和连接线
    /// </summary>
    public void OnDrawGizmos() {
        if (path != null) {
            for (int i = targetIndex; i < path.Length; i ++) {
                // 绘制路径点(黑色立方体)
                Gizmos.color = Color.black;
                Gizmos.DrawCube(path[i], Vector3.one);

                // 绘制连接线
                if (i == targetIndex) {
                    // 当前单位位置到第一个路点的连接线
                    Gizmos.DrawLine(transform.position, path[i]);
                }
                else {
                    // 路点之间的连接线
                    Gizmos.DrawLine(path[i-1],path[i]);
                }
            }
        }
    }
}

在路径规划完成后,我们会把路径坐标传递出来,通过MoveTowards使角色沿着路径点移动。下图为输出结果

image1747206726213.png

现在我们新增权重的逻辑。针对权重的功能,我们为Node类新增了movementPenalty属性:

public class Node : IHeapItem<Node> {

	public bool walkable;
	public Vector3 worldPosition;
	public int gridX;
	public int gridY;
	public int movementPenalty;

	public int gCost;
	public int hCost;
	public Node parent;
	int heapIndex;

	public Node(bool _walkable, Vector3 _worldPos, int _gridX, int _gridY, int _penalty) {
		walkable = _walkable;
		worldPosition = _worldPos;
		gridX = _gridX;
		gridY = _gridY;
		movementPenalty = _penalty;
	}
        // ...
}

然后为PathFinding的移动成本逻辑添加上该值,代表每个节点还需要额外考虑地形成本。

int newMovementCostToNeighbour = currentNode.gCost + GetDistance(currentNode, neighbour) + neighbour.movementPenalty;

并且在Grid网格类,初始化节点Node类时,获取到该节点的地形成本:

using UnityEngine;
using System.Collections;
using System.Collections.Generic;

/// <summary>
/// 网格系统:管理寻路所需的基础网格数据结构
/// 包含节点创建、可行走性检测和邻居查找功能
/// </summary>
public class Grid : MonoBehaviour {

    /// <summary>
    /// 是否在编辑器中显示网格Gizmos
    /// </summary>
    public bool displayGridGizmos;
  
    /// <summary>
    /// 不可行走物体的LayerMask(用于物理检测)
    /// </summary>
    public LayerMask unwalkableMask;
  
    /// <summary>
    /// 网格世界尺寸(x,y方向)
    /// </summary>
    public Vector2 gridWorldSize;
  
    /// <summary>
    /// 节点半径(用于物理检测)
    /// </summary>
    public float nodeRadius;
  
    /// <summary>
    /// 可行走区域定义(不同地形类型及其通行代价)
    /// </summary>
    public TerrainType[] walkableRegions;
  
    /// <summary>
    /// 用于快速查询地形通行代价的字典
    /// Key: 层级索引,Value: 通行代价惩罚值
    /// </summary>
    Dictionary<int,int> walkableRegionsDictionary = new Dictionary<int, int>();
  
    /// <summary>
    /// 组合后的所有可行走区域的LayerMask
    /// </summary>
    LayerMask walkableMask;

    /// <summary>
    /// 存储所有网格节点的二维数组
    /// </summary>
    Node[,] grid;

    /// <summary>
    /// 节点直径(2*半径)
    /// </summary>
    float nodeDiameter;
  
    /// <summary>
    /// 网格横向节点数量
    /// </summary>
    int gridSizeX, gridSizeY;

    void Awake() {
        // 计算节点直径和网格尺寸
        nodeDiameter = nodeRadius*2;
        gridSizeX = Mathf.RoundToInt(gridWorldSize.x/nodeDiameter);
        gridSizeY = Mathf.RoundToInt(gridWorldSize.y/nodeDiameter);

        // 构建可行走区域的LayerMask和惩罚值字典
        foreach (TerrainType region in walkableRegions) {
            walkableMask.value |= region.terrainMask.value;
            walkableRegionsDictionary.Add((int)Mathf.Log(region.terrainMask.value,2),region.terrainPenalty);
        }

        // 创建网格
        CreateGrid();
    }

    /// <summary>
    /// 获取网格最大节点数量(用于堆内存预分配)
    /// </summary>
    public int MaxSize {
        get {
            return gridSizeX * gridSizeY;
        }
    }

    /// <summary>
    /// 创建并初始化网格节点
    /// 根据世界坐标和物理检测确定每个节点的可行走状态
    /// </summary>
    void CreateGrid() {
        grid = new Node[gridSizeX,gridSizeY];
        // 计算网格左下角的世界坐标
        Vector3 worldBottomLeft = transform.position - Vector3.right * gridWorldSize.x/2 - Vector3.forward * gridWorldSize.y/2;

        for (int x = 0; x < gridSizeX; x ++) {
            for (int y = 0; y < gridSizeY; y ++) {
                // 计算当前节点的世界坐标
                Vector3 worldPoint = worldBottomLeft + Vector3.right * (x * nodeDiameter + nodeRadius) + Vector3.forward * (y * nodeDiameter + nodeRadius);
                // 检测是否被不可行走物体阻挡
                bool walkable = !(Physics.CheckSphere(worldPoint,nodeRadius,unwalkableMask));

                int movementPenalty = 0;

                if (walkable) {
                    // 向下发射射线检测地形类型和通行代价
                    Ray ray = new Ray(worldPoint + Vector3.up * 50, Vector3.down);
                    RaycastHit hit;
                    if (Physics.Raycast(ray,out hit, 100, walkableMask)) {
                        // 通过字典获取该地形类型的通行惩罚值
                        walkableRegionsDictionary.TryGetValue(hit.collider.gameObject.layer, out movementPenalty);
                    }
                }

                // 创建并存储节点
                grid[x,y] = new Node(walkable,worldPoint, x,y, movementPenalty);
            }
        }
    }

    /// <summary>
    /// 获取指定节点的所有邻居节点
    /// 包括8个方向(包括对角线)
    /// </summary>
    public List<Node> GetNeighbours(Node node) {
        List<Node> neighbours = new List<Node>();

        for (int x = -1; x <= 1; x++) {
            for (int y = -1; y <= 1; y++) {
                if (x == 0 && y == 0)
                    continue; // 跳过当前节点本身

                int checkX = node.gridX + x;
                int checkY = node.gridY + y;

                // 检查边界条件
                if (checkX >= 0 && checkX < gridSizeX && checkY >= 0 && checkY < gridSizeY) {
                    neighbours.Add(grid[checkX,checkY]);
                }
            }
        }

        return neighbours;
    }
  

    /// <summary>
    /// 将世界坐标转换为对应的网格节点
    /// 使用百分比映射计算节点索引
    /// </summary>
    public Node NodeFromWorldPoint(Vector3 worldPosition) {
        float percentX = (worldPosition.x + gridWorldSize.x/2) / gridWorldSize.x;
        float percentY = (worldPosition.z + gridWorldSize.y/2) / gridWorldSize.y;
        percentX = Mathf.Clamp01(percentX); // 限制在0-1范围内
        percentY = Mathf.Clamp01(percentY);

        // 计算对应的网格坐标索引
        int x = Mathf.RoundToInt((gridSizeX-1) * percentX);
        int y = Mathf.RoundToInt((gridSizeY-1) * percentY);
        return grid[x,y];
    }
  
    /// <summary>
    /// 在编辑器中绘制网格可视化效果
    /// 可行走节点为白色,不可行走节点为红色
    /// </summary>
    void OnDrawGizmos() {
        Gizmos.DrawWireCube(transform.position,new Vector3(gridWorldSize.x,1,gridWorldSize.y));
        if (grid != null && displayGridGizmos) {
            foreach (Node n in grid) {
                Gizmos.color = (n.walkable)?Color.white:Color.red;
                Gizmos.DrawCube(n.worldPosition, Vector3.one * (nodeDiameter-.1f));
            }
        }
    }

    /// <summary>
    /// 可行走区域定义类(用于Unity序列化)
    /// </summary>
    [System.Serializable]
    public class TerrainType {
        /// <summary>
        /// 地形类型的LayerMask(用于物理检测)
        /// </summary>
        public LayerMask terrainMask;
      
        /// <summary>
        /// 该地形类型的通行代价惩罚值(影响寻路算法)
        /// </summary>
        public int terrainPenalty;
    }
}

在Inspector的显示如下:image1747207919724.png

0 Answers