C#实现Astar 算法以及导航系统

C#实现Astar 算法以及导航系统

首先来说一下 Astar 是什么，它是寻求两个点之间一条最优路径 。但可能并不是最短路径.
要找地图上的一个点你就需要一个地图，而且把地图分成一个个格子，每个格子都有自己的信息

每个地图格子身上的类

//格子类型
public enum E_Type_Node
{
    //可行走区域
    walk,
    //不可走区域
    stop
}


//格子类
public class AstarNode
{
    //格子坐标
    public int x, y;

    public float f;//寻路消耗
    
    public float g;//起点距离消耗
    
    public float h;//终点距离消耗

    public AstarNode father;//父对象

    public E_Type_Node type;//格子类型

    public AstarNode(int x,int y,E_Type_Node type)
    {
        this. x = x;
        this.y = y;
        this.type = type;
    }
}

AstarMgr

这里 实现了 A星 找到了路径点

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


public class Singletion<T> where T : class, new()
{
    static T ins;
    public static T Ins
    {
        get
        {
            if (ins == null)
            {
                ins = new T();
            }

            return ins;
        }
    }
}


// A星寻路管理器
public class AstarMgr : Singletion<AstarMgr>
{
    //所有格子对象
    public AstarNode[,] nodes;
    //打开
    private List<AstarNode> OpenList=new List<AstarNode>();
    //关闭
    private List<AstarNode> CloseList= new List<AstarNode>();

    //地图的 x,y 宽高
    private int Mapw;
    private int Maph;
    public void InitMapInfo(int w, int h)
    {
        this.Mapw = w;
        this.Maph = h;

        //声明容器 的长度
        nodes = new AstarNode[w, h];

        AstarNode node;
        for (int i = 0; i < w; i++)
        {
            for (int j = 0; j < h; j++)
            {
                //只是为了测试
                node = new AstarNode(i, j, Random.Range(0, 100) < 20 ? E_Type_Node.stop : E_Type_Node.walk);
                nodes[i, j] = node;
            }
        }
    }

    /// <summary>
    /// 寻找路径
    /// </summary>
    /// <param name="startPos">开始坐标</param>
    /// <param name="endPos">结束坐标 </param>
    /// <returns></returns>
    public List<AstarNode> FindPath(Vector2 startPos, Vector2 endPos)
    {
        //1.是否在地图范围内
        if (startPos.x < 0 || startPos.x >= Mapw || startPos.y < 0 || startPos.y >= Maph || endPos.x < 0 || endPos.x >= Mapw || endPos.y < 0 || endPos.y >= Maph)
        {
            Debug.Log("不在地图范围内");
            return null;
        }

        AstarNode start = nodes[(int)startPos.x, (int)startPos.y];
        AstarNode end = nodes[(int)endPos.x, (int)endPos.y];
        //2.判断格子是否阻挡
        if (start.type == E_Type_Node.stop || end.type == E_Type_Node.stop)
        {
            Debug.Log("开始或结束点是阻挡");
            return null;
        }

        //清空上一次寻路数据
        // 清空关闭和寻路列表
        OpenList.Clear();
        CloseList.Clear();

        // 把开始点放入关闭列表中
        start.father = null;
        start.f = 0;
        start.g = 0;
        start.h = 0;
        CloseList.Add(start);

        while (true)
        {
            //从起点开始找周围的点并放入开启列表中
            ///左上x -1 y - 1
            FindNearNodeToOpenList(start.x - 1, start.y - 1, 1.4f, start, end);
            //上 x y-1
            FindNearNodeToOpenList(start.x, start.y - 1, 1, start, end);
            //右上 x+1,y-11
            FindNearNodeToOpenList(start.x + 1, start.y - 1, 1.4f, start, end);
            //左 x-1,y
            FindNearNodeToOpenList(start.x - 1, start.y, 1f, start, end);
            //右 x,y-1;
            FindNearNodeToOpenList(start.x + 1, start.y, 1f, start, end);
            //左下 x-1,y+1
            FindNearNodeToOpenList(start.x - 1, start.y + 1, 1.4f, start, end);
            //下 x,y+1
            FindNearNodeToOpenList(start.x, start.y + 1, 1, start, end);
            //右下 x+1,y+1
            FindNearNodeToOpenList(start.x + 1, start.y + 1, 1.4f, start, end);

            //死路判断 开启列表为空 就是死路
            if (OpenList.Count == 0)
            {
                Debug.Log("死路");
                return null;
            }

            // 选择 寻路消耗最小的点
            OpenList.Sort(SotrOpenList);

            //放入关闭列表中
            CloseList.Add(OpenList[0]);
            start = OpenList[0];
            OpenList.RemoveAt(0);

            if (start == end)
            {
                //找到重点
                List<AstarNode> path = new List<AstarNode>();
                path.Add(end);
                while (end.father!=null)
                {
                    path.Add(end.father);
                    end = end.father;
                }

                //反转
                path.Reverse();
                return path;
            }
          
        }


    }

    private int SotrOpenList(AstarNode a, AstarNode b)
    {
        if (a.f > b.f)
        {
            return 1;
        }
        else
         return -1; 
    }


    /// <summary>
    /// 计算临近点 是否可以放入Open List
    /// </summary>
    /// <param name="x"></param>
    /// <param name="y"></param>
    /// <param name="g"></param>
    /// <param name="father"></param>
    private void FindNearNodeToOpenList(int x, int y, float g, AstarNode father, AstarNode end)
    {
        //判断边界
        if (x < 0 || x >= Mapw || y < 0 || y >= Maph)
        {
            Debug.Log("不在地图范围内");
            return;
        }

        //在范围内 取点
        AstarNode node = nodes[x, y];
        if (node == null || node.type == E_Type_Node.stop||CloseList.Contains(node)||OpenList.Contains(node))
        {
            return;
        }
        //计算寻路消耗


        //记录父对象
        node.father = father;

        //计算g 我的开始距离= 我父亲的距离 +我的距离
        node.g = father.g = g;

        node.h = Mathf.Abs(end.x - node.x) + Mathf.Abs(end.y - node.y);
        node.f = node.g + node.h;
        //通过了上面 的验证，存入开启列表
        OpenList.Add(node);
    }
}

NavAgent( 导航实现类)

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

public class NavAgent : MonoBehaviour
{
    //速度
    private float speed;
    //路径的集合
    private Vector3[] roads;
    // 回掉
    private UnityAction endFunc;
    //当前路径点下标
    private int nextIndex = 0;


    private float totalTime;
    private float passedTime;
    private float vx, vy, vz;
    private bool isWalking;


    private Quaternion dstRot;
    public void Init() {
        
    }

    public void NavOnRoad(float speed, Vector3[] roads, UnityAction endFunc) {
        // step1: 准备数据，验证数据的合法性;
        this.speed = speed;
        this.roads = roads;
        this.endFunc = endFunc;

        if (this.roads.Length < 2) { 
            return;
        }

        // step1: 将我们的角色放到起点，调整好初始方向
        this.transform.position = this.roads[0];
        this.transform.LookAt(this.roads[1]);
        // end

        // step2: 下一个点数据相关
        this.nextIndex = 1;
        this.WalkToNext();
        // end
    }

    // 走向nextIndex
    private void WalkToNext(){
        if (this.nextIndex >= this.roads.Length) { // 走到了尽头;
            this.isWalking = false;
            if (this.endFunc != null) {
                this.endFunc();
            }
            return;
        }

        // 从当前点走到下一个点
        Vector3 src = this.transform.position;
        Vector3 dst = this.roads[this.nextIndex];
        Vector3 dir = dst - src;
        float len = dir.magnitude;
        if (len <= 0) {
            this.nextIndex++;
            this.WalkToNext();
            return;
        }

        this.totalTime = len / this.speed;
        this.passedTime = 0;
        // end

        // 分解速度
        this.vx = this.speed * dir.x / len;
        this.vy = this.speed * dir.y / len;
        this.vz = this.speed * dir.z / len;
        // end

        // 调整角色的朝向
        Quaternion old = this.transform.rotation;
        this.transform.LookAt(dst);
        this.dstRot = this.transform.rotation;
        this.transform.rotation = old;
        // end

        this.isWalking = true;
    }
    public void StopNav() { 
    }

    public void GoAheadOnRoad() { 
    }

    private void Update() {
        if (this.isWalking == false) {
            return;
        }

        // 更新移动的时间
        float dt = Time.deltaTime;
        this.passedTime += dt;
        if(this.passedTime > this.totalTime) {
            dt -= (this.passedTime - this.totalTime);
        }
        // end

        // 每次Update更新位置
        Vector3 pos = this.transform.position;
        pos.x += this.vx * dt;
        pos.y += this.vy * dt;
        pos.z += this.vz * dt;
        this.transform.position = pos;

        // 转向插值
        this.transform.rotation = Quaternion.Slerp(this.transform.rotation, this.dstRot, 8.0f * dt);
        // end

        if (this.passedTime >= this.totalTime) {
            this.nextIndex++;
            this.WalkToNext();
        }
    }
}