[A*寻路]

Posted 2020-11-25 cocotang

<1>效果

白色：可行走

红色：阻碍

蓝色：起点周围的格子

绿色：路径

<2>代码

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using System.Diagnostics;
using Debug = UnityEngine.Debug;

namespace JPS
{

    /// <summary>
    /// 0-1 1
    /// </summary>
    public class Grid
    {
        public short state;//0可以行走 1阻碍  2边界
        public int index;
    }

    /// <summary>
    /// JPS寻路
    /// 通过NavMesh生成网格数据
    /// JPS算法生成路径
    /// </summary>
    public class JPSTest : MonoBehaviour
    {

        public float size = 0.5f;
        private float halfSize;
        public int col = 10;
        public int row = 10;
        public Vector3 startPos;
        public List<int> lockLst = new List<int>();

        [Space]
        [Header("寻路起点")]
        public Transform p1;
        [Header("寻路终点")]
        public Transform p2;

        private List<Grid> grids = new List<Grid>();
        private List<int> pathLst = new List<int>();
        public Dictionary<int, int[]> dirMap = new Dictionary<int, int[]>
        {
            { 1 ,new int[3]{ 1,1,14} },
            { 2 ,new int[3]{ 0,1,10} },
            { 3 ,new int[3]{ -1,1,14} },
            { 4 ,new int[3]{ -1,0,10} },
            { 5 ,new int[3]{ -1,-1,14} },
            { 6 ,new int[3]{ 0,-1,10} },
            { 7 ,new int[3]{ 1,-1,14} },
            { 8 ,new int[3]{ 1,0,10} },
         };

        // Start is called before the first frame update
        void Start()
        {
            halfSize = size * 0.5f;
            initGrid();
        }

        void initGrid()
        {

            for (int i = 1; i <= row; i++)
            {
                for (int j = 1; j <= col; j++)
                {
                    Grid grid = new Grid();
                    grid.index = (i - 1) * col + j;
                    int st = lockLst.Contains(grid.index) ? 1 : 0;
                    grid.state = (short)st;
                    grids.Add(grid);
                }
            }
        }





        Vector3 getPosByIndex(int index)
        {
            Vector3 pos = Vector3.zero;
            int r = getRow(index);// Mathf.CeilToInt(index / (float)col);
            int c = getCol(index);// index % col;
            //c = c == 0 ? col : c;
            pos.y = startPos.y;
            pos.x = startPos.x + (c - 1) * size + size / 2;
            pos.z = startPos.z + (r - 1) * size + size / 2;
            return pos;
        }

        List<int> find(int start, int end)
        {
            List<int> closeLst = new List<int>();
            List<int> openLst = new List<int>();
            closeLst.Add(start);
            int count = 0;
            while (closeLst[closeLst.Count - 1] != end)
            {
                count++;
                if (count > 200)
                {
                    Debug.LogError("寻路失败");
                    break;
                }
                int index = closeLst[closeLst.Count - 1];
                int min = getMinCostIndex(index, end, closeLst);
                if (min < 0) break;
                closeLst.Add(min);
            }
            return closeLst;
        }

        List<int> find(Vector3 start, Vector3 end)
        {
            return find(getIndexByPos(start), getIndexByPos(end));
        }

        int getMinCostIndex(int org, int end, List<int> closeLst)
        {
            List<int> indexMap = new List<int>();
            int[] delta = new int[2];
            int r = getRow(org);
            int c = getCol(org);
            int endRow = getRow(end);
            int endCol = getCol(end);
            int cost = int.MaxValue;
            int minIndex = -1;
            foreach (var item in dirMap)
            {
                int[] xy = item.Value;
                delta[0] = xy[0] + r;
                delta[1] = xy[1] + c;
                int idx = (delta[0] - 1) * col + delta[1];
                if (isVaildIndex(delta) && !closeLst.Contains(idx))
                {
                    Grid idxGrid = grids[idx - 1];
                    //不是阻挡点
                    if (idxGrid.state == 0)
                    {
                        //计算cost                    
                        int G = xy[2];
                        int H = Mathf.Abs(endRow - delta[0]) + Mathf.Abs(endCol - delta[1]);
                        int F = G + H * 10;
                        if (F < cost)
                        {
                            cost = F;
                            minIndex = idx;
                        }
                    }
                }
            }
            return minIndex;
        }


        List<int> getGridAround(int index)
        {
            List<int> indexMap = new List<int>();
            int[] delta = new int[2];
            int r = getRow(index);
            int c = getCol(index);
            foreach (var item in dirMap)
            {
                int[] xy = item.Value;
                delta[0] = xy[0] + r;
                delta[1] = xy[1] + c;
                if (isVaildIndex(delta))
                {
                    indexMap.Add((delta[0] - 1) * col + delta[1]);
                }
            }
            return indexMap;
        }

        private int getRow(int index)
        {
            return Mathf.CeilToInt((index - 0.1F) / col);
        }

        private int getCol(int index)
        {
            int c = index % col;
            return c == 0 ? col : c;
        }
        //验证合法性
        private bool isVaildIndex(int[] xy)
        {
            return xy[0] > 0 && xy[0] <= row && xy[1] > 0 && xy[1] <= col;
        }


        private void OnGUI()
        {
            if (GUILayout.Button("find Path", GUILayout.Width(100), GUILayout.Height(40)))
            {
                if (p1 != null && p2 != null)
                {
                    Stopwatch stopwatch = new Stopwatch();
                    stopwatch.Start();
                    pathLst = find(p1.position, p2.position);
                    stopwatch.Stop();
                    Debug.LogError(stopwatch.ElapsedMilliseconds);
                }
            }
            if (GUILayout.Button("clear Path", GUILayout.Width(100), GUILayout.Height(40)))
            {
                pathLst.Clear();
            }

        }


        private void OnDrawGizmos()
        {
            if (grids.Count > 0)
            {
                for (int i = 0; i < grids.Count; i++)
                {
                    Grid grid = grids[i];
                    int index = grid.index;
                    Gizmos.color = grid.state == 0 ? Color.white : Color.red;
                    Vector3 pos = getPosByIndex(index);
                    //Gizmos.DrawCube(pos, cubeSize);
                    drawLine(pos);
                }
                drawAround();
                drawPath();
            }
        }

        void drawLine(Vector3 pos)
        {
            float y = pos.y;// 1f;
            float scale = 0.95f;
            float halfSize = size * 0.5f * scale;
            Vector3 p1 = new Vector3(pos.x + halfSize, y, pos.z - halfSize);
            Vector3 p2 = new Vector3(pos.x + halfSize, y, pos.z + halfSize);
            Vector3 p3 = new Vector3(pos.x - halfSize, y, pos.z + halfSize);
            Vector3 p4 = new Vector3(pos.x - halfSize, y, pos.z - halfSize);
            Gizmos.DrawLine(p1, p2);
            Gizmos.DrawLine(p2, p3);
            Gizmos.DrawLine(p3, p4);
            Gizmos.DrawLine(p4, p1);

        }

        int getIndexByPos(Vector3 pos)
        {
            float subX = pos.x - startPos.x;
            float subZ = pos.z - startPos.z;
            //10/col 
            int c = Mathf.CeilToInt(subX / size);
            int r = Mathf.CeilToInt(subZ / size);
            return (r - 1) * col + c;
        }

        void drawAround()
        {
            if (p1 != null)
            {
                int index = getIndexByPos(p1.position);
                List<int> round = getGridAround(index);
                for (int i = 0; i < round.Count; i++)
                {
                    int idx = round[i];
                    Vector3 pos = getPosByIndex(idx);
                    Gizmos.color = Color.blue;
                    drawLine(pos);
                    Gizmos.color = Color.green;
                }
            }
        }


        void drawPath()
        {
            if (pathLst != null && pathLst.Count > 0)
            {
                for (int i = 0; i < pathLst.Count; i++)
                {
                    int idx = pathLst[i];
                    Gizmos.color = Color.green;
                    drawLine(getPosByIndex(idx));
                    Gizmos.color = Color.white;
                }
            }
        }


    }


}

<3>思路 todo