Unity C#实现A*算法

Unity C#实现A*算法

  1. 算法原理

简单来说就是把地图拆成N*N个格子 以每个格子为最小移动单位。知道起始点和终点。从起点开始遍历周围的格子。计算出他们的行走代价(F=G+H);并加到待搜索列表中去。 然后不断的找待搜索列表中F代价最小的节点 找到最小F代价的节点加入到已搜索列表中。 再开始遍历他周围的格子更新代价值G,H和F。最终最后遍历到终点或者是待搜索列表没有数据 算法结束。

  1. F=G+H

F代表这个格子的总代价 这个值越小说明这个可能是当前最佳的路线格子。
G代表从起始格子到当前格子的行走消耗代价。
H代表当前格子到终点的行走消耗代价
一般计算方法为 曼哈顿距离(当前格子到终点的水平X距离 + 当前格子到终点的垂直Y的距离 即:curNode.gCost = Mathf.Abs(curNode.x - endNode.x) + Mathf.Abs(curNode.y - endNode.y))
另一种可以走对角线的计算方法为 切比雪夫距离(定义水平和垂直距离移动一格消耗10,对角线方向移动一格消耗为 14 = 即根号2取的整 乘10 避免小数运算)
int offsetX = Mathf.Abs(curNode.posX - endNode.posX);
int offsetY = Mathf.Abs(curNode.poxY - endNode.poxY);
int offsetDis = Mathf.Abs(offsetX - offsetY); //差值 为水平距离 例如 offsetX = 3 offsetY =1 就是水平走2格 斜着1格子
int maxOffset = offsetX > offsetY ? offsetX : offsetY; //找最大 最大值减去插值认为是斜边距离
curNode.gCost = offsetDis * 10+ (maxOffset - offsetDis) * 14;

在这里插入图片描述

  1. 注意在遍历周围格子时 需要判断他是否是在待搜索列表 如果在 就需要尝试计算一下当前点到这个周围格子的点的 gCost 如果比他原来记录的要小 那么就要更新一下他的gCost 和他的来源节点 赋值为当前节点。 这样就相当于更新了最佳路径

具体细节之间上代码把

先看Node.cs

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

public class Node : MonoBehaviour
{
    // G cost  代表从起点到这个节点的耗费
    public int gCost = 0;

    // H cost 代表这个节点到终点的 曼哈顿距离 就是 X+Y 
    public int hCost = 0;

    public int fCost = 0;

    public int posX;

    public int poxY;

    //F cost = G cost + H cost
    public void FCost() 
    {
        fCost = hCost + gCost;
    }

    //来源父节点
    public Node parentNode = null;

    //是否是障碍
    public bool isWall = false;


    [ContextMenu("SetWall 设置为墙体")]
    public void SetWall()
    {
        MapManager.Instance.SetNodeWall(this.gameObject);
    }

    public string toString()
    {
        return "x: " + posX + "|y: " + poxY;
    }
}

MapManger.cs

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

public class MapManager : MonoBehaviour
{
    public static MapManager _instance;


    const int ONE_PATH_COST = 10; //水平或者垂直移动一格消耗
    const int ONE_INCLIEND_PATH_COST = 14; //斜方向移动一格消耗

    [Header("地图高")]
    public const int hight = 20;
    [Header("地图宽")]
    public const int widget = 20;

    [Header("格子")]
    public GameObject nodeGameObj = null;

    [Header("墙 shader")]
    public Material wallMaterial = null;

    public Node[,] allNodes = new Node[hight, widget];

    public static MapManager Instance
    {
        get {
            return _instance;
        }
    }

    public void Awake()
    {
        _instance = this;
    }

    public void Start()
    {
        CreateMap();
    }

    public void CreateMap()
    {
        for (int i = 0; i < widget; i++)
        {
            for(int j = 0; j < hight; j++)
            {
                //创建node
                GameObject nodeObj = GameObject.Instantiate<GameObject>(nodeGameObj);
                Vector3 pos = new Vector3(i, j, 0);
                nodeObj.transform.position = pos;
                nodeObj.gameObject.name = i + "_" + j;

                Node nodeComponent = nodeObj.AddComponent<Node>();
                nodeComponent.posX = i;
                nodeComponent.poxY = j;

                allNodes[i,j] = nodeComponent;
            }
        }
    }

    public void InitMapCost()
    {
        for (int i = 0; i < widget; i++)
        {
            for (int j = 0; j < hight; j++)
            {
                //重置一下 避免二次触发死循环
                allNodes[i, j].gCost = int.MaxValue;
                allNodes[i, j].hCost = 0;
                allNodes[i, j].parentNode = null;
                allNodes[i, j].FCost();
            }
        }
    }

    public Node GetNode(int x,int y)
    {
        return allNodes[x, y];
    }

  
    //计算两个点之间的距离耗费
    public int CalculTowNodeDistance(Node startNode, Node endNode)
    {
        int offsetX = Mathf.Abs(startNode.posX - endNode.posX);
        int offsetY = Mathf.Abs(startNode.poxY - endNode.poxY);
        int offsetDis = Mathf.Abs(offsetX - offsetY);  //差值 为水平距离  例如 x:3 y:1  就是水平走2格 斜着1格子
        int maxOffset = offsetX > offsetY ? offsetX : offsetY;  //找最大  最大值减去插值认为是斜边距离
        return offsetDis * ONE_PATH_COST + (maxOffset - offsetDis) * ONE_INCLIEND_PATH_COST;
    }


    public List<Node> GetNeighborNodeList(Node node)
    {
        //拿到相邻节点
        List<Node> neighborNodeList = new List<Node>();
        int curPox = node.posX;
        int curPoy = node.poxY;

        int[,] neighborIndexs = new int[,]
        {
            {0,1 },  //上
            {0,-1}, //下
            {-1,0 }, //左
            {1,0 }, //右
            {-1,1 }, //左上
            {1,1 }, //右上
            {-1,-1 }, //左下
            {1,-1 }, //右下
        };
        
       for(int i = 0; i< 8; i++)
        {
            int offsetX = neighborIndexs[i,0];
            int offsetY = neighborIndexs[i, 1];
            int realIndexX = curPox + offsetX;
            int realIndexY = curPoy + offsetY;
            if(realIndexX < 0 || realIndexX >= widget)
            {
                continue;
            }
            if (realIndexY < 0 || realIndexY >= hight)
            {
                continue;
            }
            neighborNodeList.Add(GetNode(realIndexX, realIndexY));
        }


        return neighborNodeList;
    }

    public void SetNodeWall(GameObject obj)
    {
        Node nodeComponet = obj.GetComponent<Node>();
        nodeComponet.isWall = true;

        MeshRenderer mr = obj.GetComponent<MeshRenderer>();
        mr.material = wallMaterial;
    }
}

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

public class AStarFind : MonoBehaviour
{
    public Material startMat;
    public Material endMat;
    public Material nomarlMat;
    public Material wallMaterial;

    public GameObject startObj;
    public GameObject endObj;

    public List<Node> findPathNodeList = null;


    public List<Node> FindAStartNodePath(Node startNode, Node endNode)
    {
        List<Node> toSearchList = new List<Node>() { startNode };  //待搜索列表
        List<Node> findedList = new List<Node>();  //已经搜索过的列表

        //初始化一波地图消耗
        MapManager.Instance.InitMapCost();

         Node curNode = null;
        while(toSearchList.Count > 0)
        {
            //有待搜索的 就一直执行
            if(curNode == null)
            {
                curNode = toSearchList[0];
                toSearchList.Remove(curNode);
            }else
            {
                //找到当前待搜索列表里最小的F
                toSearchList.Sort((a, b) => 
                {
                    var cost = a.fCost - b.fCost;
                    return cost;
                });
                curNode = toSearchList[0];
                toSearchList.Remove(curNode);
            }

            if(curNode.Equals(endNode))
            {
                //到终点了 返回找过的列表
                //倒叙找回起点 就是路径了
                Debug.Log("term tips 找到路径了!!!");
                Node curParentNode = endNode.parentNode;
                List<Node> getPathNodeList = new List<Node>();
                while(curParentNode != null)
                {
                    getPathNodeList.Add(curParentNode);
                    curParentNode = curParentNode.parentNode;
                }

                //反转一下 就是路径了
                getPathNodeList.Reverse();  //反转函数
                return getPathNodeList;
            }

            findedList.Add(curNode); //加到已搜索列表

            //上下左右四个方向找一下 最佳的点
            List <Node> neighborNodeList  = MapManager.Instance.GetNeighborNodeList(curNode);
            for(int i = 0; i < neighborNodeList.Count; i++)
            {
                Node curNeighborNode = neighborNodeList[i];
                if(curNeighborNode.isWall)
                {
                    //障碍
                    continue;
                }
                if(findedList.Contains(curNeighborNode))
                {
                    //已经找过了
                    continue;
                }

                //尝试计算一下当前的点跟邻居节点的耗费
                int curGCost = curNode.gCost + MapManager.Instance.CalculTowNodeDistance(curNode, curNeighborNode);
                int curHCost = MapManager.Instance.CalculTowNodeDistance(curNeighborNode, endNode);
                int curFCost = curGCost + curHCost;
                if (!toSearchList.Contains(curNeighborNode))
                {
                    //加到待搜索列表 同时算一下当前耗费
                    curNeighborNode.gCost = curGCost;
                    curNeighborNode.hCost = curHCost;
                    curNeighborNode.FCost();
                    curNeighborNode.parentNode = curNode;
                    toSearchList.Add(curNeighborNode);
                }
                else
                {
                    //在待搜索列表里 比较一下Gcost 是不是最佳的  相当于回溯 之前的路径来源舍弃
                    if (curGCost < curNeighborNode.gCost)
                    {
                        //当前位置移动到这里更好  更新一下消耗
                        curNeighborNode.gCost = curGCost;
                        curNeighborNode.hCost = curHCost;
                        curNeighborNode.FCost();
                        curNeighborNode.parentNode = curNode;
                    }
                }
            }

        }


        Debug.Log("term tips 找不到路径了 - -!");
        return null;
    }

    // Update is called once per frame
    void Update()
    {
        if (Input.GetMouseButtonDown(0))
        {
            Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
            RaycastHit raycastHit;
            Physics.Raycast(ray, out raycastHit, 1000);

            if (raycastHit.collider)
            {
                GameObject hitObj = raycastHit.collider.gameObject;
                if (startObj == null || startObj.Equals(hitObj))
                {
                    startObj = hitObj;
                    startObj.GetComponent<MeshRenderer>().material = startMat;
                }
                else if (endObj == null)
                {
                    endObj = hitObj;
                    endObj.GetComponent<MeshRenderer>().material = endMat;

                    //开始寻路
                    Node startNode = startObj.GetComponent<Node>();
                    Node endNode = endObj.GetComponent<Node>();
                    findPathNodeList = FindAStartNodePath(startNode, endNode);
                    if (findPathNodeList != null)
                    {
                        //打印出来吧
                        foreach (var node in findPathNodeList)
                        {
                            node.GetComponent<MeshRenderer>().material = startMat;
                            Debug.Log("path :" + node.toString());
                        }
                    }
                }
            }
        }
        else if (Input.GetMouseButton(1))
        {
            //设置墙体
            Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);
            RaycastHit raycastHit;
            Physics.Raycast(ray, out raycastHit, 1000);

            if (raycastHit.collider)
            {
                GameObject hitObj = raycastHit.collider.gameObject;
                hitObj.GetComponent<Node>().isWall = true;
                hitObj.GetComponent<MeshRenderer>().material = wallMaterial; 
            }
        }

    }

    [ContextMenu("ClearStartAndEndPos 清空")]
    public void ClearStartAndEndPos()
    {
        if(startObj)
        {
            startObj.GetComponent<MeshRenderer>().material = nomarlMat;
            startObj = null;
        }

        if (endObj)
        {
            endObj.GetComponent<MeshRenderer>().material = nomarlMat;
            endObj = null;
        }

        if (findPathNodeList != null)
        {
            //打印出来吧
            foreach (var node in findPathNodeList)
            {
                node.GetComponent<MeshRenderer>().material = nomarlMat;             
            }
            findPathNodeList = null;
        }
    }
}

最终效果

找到路径了在这里插入图片描述

没有路径的情况:

这里是引用

  • 1
    点赞
  • 1
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
此为用C#写的A*算法源代码 using System; using System.Collections.Generic; using System.Text; using System.Drawing; namespace EtSoft.AStarLib { public class AStar { private StarNodeCollection openList = new StarNodeCollection(); private StarNodeCollection closeList = new StarNodeCollection(); public StarNode currentNode = null; #region 构造函数 /// <summary> /// 使用指定的地图对象、起点和终点初始化A星算法 /// </summary> /// <param name="map">地图对象</param> public AStar(Map map) { this.map = map; } /// <summary> /// /// </summary> /// <param name="map">地图对象</param> /// <param name="start">起点坐标</param> /// <param name="end">终点坐标</param> public AStar(Map map, Point start, Point end) : this(map) { this.start = new StarNode(start); this.end = new StarNode(end); openList.Add(new StarNode(start)); //AddStartNodeToOpenList(this.start); } /// <summary> /// /// </summary> /// <param name="map">地图对象</param> /// <param name="startX">起点X坐标</param> /// <param name="startY">起点Y坐标</param> /// <param name="endX">终点X坐标</param> /// <param name="endY">终点Y坐标</param> public AStar(Map map, int startX, int startY, int endX, int endY) : this(map, new Point(startX, startY), new Point(endX, endY)) { } #endregion #region 属性 protected Map map; /// <summary> /// 地图数据 /// </summary> public Map Map { get { return map; } set { map = value; } } private StarNode start = null; /// <summary> /// 起点坐标,如果没有设置起点,返回null /// </summary> public StarNode Start { get { return start; } set { start = value; openList.Clear(); openList.Add(start); //AddNodeToOpenList(start); } } private StarNode end = null; /// <summary> /// 终点坐标,如果没有设置终点,返回null /// </summary> public StarNode End { get { return end; } set { end = value; } } private StarNodeCollection path; /// <summary> /// 返回路径节点集合,没有路径则返回null /// </summary> public StarNodeCollection Path { get { return path; } } private bool allDirection = false; /// <summary> /// true,允许八方向寻路 /// </summary> public bool AllDirection { get { return allDirection; } set { allDirection = value; } } #endregion /// <summary> /// 开始寻路 /// </summary> public void StartSearchPath() { if (start == null) throw new InvalidNodeException(InvalidNodeTypes.NoStart); if (end == null) throw new InvalidNodeException(InvalidNodeTypes.NoEnd); path = null; openList.Clear(); closeList.Clear(); currentNode = start; SearchPath(currentNode); } //寻路递归,受保护的虚方法,允许在子类重写寻路算法 protected virtual void SearchPath(StarNode starNode) { //currentNode = starNode; openList.Remove(starNode); closeList.Add(starNode); AddNodeToOpenList(); if (closeList.Contains(end)) { //如果终点在关闭列表中,找到路径 StarNode node=starNode.Parent; path = new StarNodeCollection(); do { path.Add(node); node = node.Parent; } while (node != null && !node.Equals(start)); path.Reverse(); return; } else if (openList.Count == 0) { //终点不在关闭列表,开放列表已空,没有可通行的路径 return; } currentNode = GetNextNode(); //迭代过程 SearchPath(currentNode); } /// <summary> /// 获得当前节点的下一个最佳节点 /// </summary> /// <returns></returns> public StarNode GetNextNode() { openList.SortByF(); return openList[0]; } /// <summary> /// 把当前点周围可通过的点加入到开放列表中 /// </summary>
以下是一个简单的Unity C#脚本来实现A*算法: ```csharp using UnityEngine; using System.Collections.Generic; public class AStar : MonoBehaviour { public Transform seeker, target; // 寻路的起点和终点 Grid grid; // 存储地图信息的网格 void Awake() { grid = GetComponent<Grid>(); } void Update() { FindPath(seeker.position, target.position); } void FindPath(Vector3 startPos, Vector3 targetPos) { Node startNode = grid.NodeFromWorldPoint(startPos); // 起点所在的网格节点 Node targetNode = grid.NodeFromWorldPoint(targetPos); // 终点所在的网格节点 List<Node> openSet = new List<Node>(); // 未处理的节点集合 HashSet<Node> closedSet = new HashSet<Node>(); // 已处理的节点集合 openSet.Add(startNode); // 将起点加入未处理的节点集合 while (openSet.Count > 0) { Node currentNode = openSet[0]; // 当前处理的节点 for (int i = 1; i < openSet.Count; i++) { if (openSet[i].fCost < currentNode.fCost || (openSet[i].fCost == currentNode.fCost && openSet[i].hCost < currentNode.hCost)) { currentNode = openSet[i]; } } openSet.Remove(currentNode); closedSet.Add(currentNode); if (currentNode == targetNode) { // 已找到终点 RetracePath(startNode, targetNode); return; } foreach (Node neighbor in grid.GetNeighbors(currentNode)) { if (!neighbor.walkable || closedSet.Contains(neighbor)) { continue; } int newMovementCostToNeighbor = currentNode.gCost + GetDistance(currentNode, neighbor); if (newMovementCostToNeighbor < neighbor.gCost || !openSet.Contains(neighbor)) { neighbor.gCost = newMovementCostToNeighbor; neighbor.hCost = GetDistance(neighbor, targetNode); neighbor.parent = currentNode; if (!openSet.Contains(neighbor)) { openSet.Add(neighbor); } } } } } void RetracePath(Node startNode, Node endNode) { List<Node> path = new List<Node>(); Node currentNode = endNode; while (currentNode != startNode) { path.Add(currentNode); currentNode = currentNode.parent; } path.Reverse(); grid.path = path; } 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); } else { return 14 * dstX + 10 * (dstY - dstX); } } } ``` 这个脚本依赖于一个名为`Grid`的组件,它存储了地图信息的网格。 `Grid`组件的实现不在本文的讨论范围内,你可以参考其他教程或者使用你自己的实现。 在`Update()`函数中,我们不断地调用`FindPath()`函数来执行A*算法。在`FindPath()`函数中,我们首先找到起点和终点所在的网格节点,然后使用A*算法找到从起点到终点的最短路径。最后,我们使用`RetracePath()`函数来反向遍历路径链表,并将路径保存在`Grid`组件的`path`变量中。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值