unity A*寻路 （三）A*算法

using System.Collections.Generic;
using Polygon;

namespace AStar
{
    public class AStarTools
    {
        /// <summary>
        /// 判断点在哪个三角形中
        /// </summary>
        /// <param name="point"></param>
        /// <param name="allAStarTriangle"></param>
        /// <returns></returns>
        public static Triangle GetAStarTriangleByPoint(Ponit point, List<Triangle> allAStarTriangle)
        {
            if (allAStarTriangle == null)
            {
                return new Triangle();
            }

            Point Point = new Point(point.x, point.z);

            for (int i = 0; i < allAStarTriangle.Count; i++)
            {
                Triangle AStarTriangle = allAStarTriangle[i];

                List<Point> pointList = new List<Point>();
                pointList.Add(new Point(AStarTriangle.a.x, AStarTriangle.a.z));
                pointList.Add(new Point(AStarTriangle.b.x, AStarTriangle.b.z));
                pointList.Add(new Point(AStarTriangle.c.x, AStarTriangle.c.z));

                if (Utility.CheckPointInPolygon(Point, new Polygon.Polygon(pointList)))
                {
                    return AStarTriangle;
                }
            }
            return new Triangle();
        }
    }
}

using UnityEngine;

namespace AStar
{
    public class AStarData
    {
        /// <summary>
        /// 开始.
        /// </summary>
        public Ponit start;
        /// <summary>
        /// 终点.
        /// </summary>
        public Ponit end;
        /// <summary>
        /// 导航网格信息
        /// </summary>
        public NavMeshInfo navMeshInfo;


        /// <summary>
        /// 起点所在的三角形
        /// </summary>
        public Triangle startPlace;
        /// <summary>
        /// 终点所在的三角形
        /// </summary>
        public Triangle endPlace;
        /// <summary>
        /// 起点新建三角形
        /// </summary>
        public Triangle startAStarTriangle;
        /// <summary>
        /// 终点新建三角形
        /// </summary>
        public Triangle endAStarTriangle;

        public bool Init(Ponit start, Ponit end, NavMeshInfo navMeshInfo)
        {
            this.start = start;
            this.end = end;
            this.navMeshInfo = navMeshInfo;

            startAStarTriangle = new Triangle(start, start, start);
            endAStarTriangle = new Triangle(end, end, end);

            startPlace = AStarTools.GetAStarTriangleByPoint(start, navMeshInfo.allTriangle);
            endPlace = AStarTools.GetAStarTriangleByPoint(end, navMeshInfo.allTriangle);

            if (startPlace == new Triangle())
            {
                Debug.Log("起点不在导航网格信息中");
                return false;
            }

            if (endPlace == new Triangle())
            {
                Debug.Log("终点不在导航网格信息中");
                return false;
            }

            return true;
        }
    }
}

using System.Collections.Generic;
using System.Linq;

namespace AStar
{
    public delegate void AStarCallback(List<Ponit> list);

    public class Pathfinding
    {
        /// <summary>
        /// 数据
        /// </summary>
        AStarData aStarData;


        /// <summary>
        /// 待计算列表.
        /// </summary>
        List<Triangle> openList;
        /// <summary>
        /// 关闭列表.
        /// </summary>
        List<Triangle> closeList;

        /// <summary>
        /// 父级索引.
        /// </summary>
        Dictionary<Triangle, Triangle> parentIndexes;

        /// <summary>
        /// G值.
        /// </summary>
        Dictionary<Triangle, float> triangle_G;
        /// <summary>
        /// H值.
        /// </summary>
        Dictionary<Triangle, float> triangle_H;
        /// <summary>
        /// F值.
        /// </summary>
        Dictionary<Triangle, float> triangle_F;

        /// <summary>
        /// 回调.
        /// </summary>
        AStarCallback callback;

        public void Start(Ponit start, Ponit end, NavMeshInfo navMeshInfo, AStarCallback callback)
        {
            if (callback == null)
            {
                return;
            }

            aStarData = new AStarData();

            if (aStarData.Init(start, end, navMeshInfo) == false)
            {
                return;
            }


            this.callback = callback;


            openList = new List<Triangle>();
            closeList = new List<Triangle>();
            parentIndexes = new Dictionary<Triangle, Triangle>();
            triangle_G = new Dictionary<Triangle, float>();
            triangle_H = new Dictionary<Triangle, float>();
            triangle_F = new Dictionary<Triangle, float>();

            Core();
        }

        /// <summary>
        /// 核心
        /// </summary>
        void Core()
        {
            openList.Add(aStarData.startAStarTriangle);

            //开始寻路 寻路到终点结束
            while (!openList.Contains(aStarData.endAStarTriangle) && openList.Count != 0)
            {
                Triangle AStarTriangle = GetOpenListMin();

                openList.Remove(AStarTriangle);

                closeList.Add(AStarTriangle);

                Explore(AStarTriangle);
            }

            List<Triangle> list = new List<Triangle>();

            Triangle T = aStarData.endAStarTriangle;
            while (parentIndexes.ContainsKey(T) && parentIndexes[T] != null)
            {
                list.Add(T);

                T = parentIndexes[T];
            }

            Callback(list);
        }

        /// <summary>
        /// 获取待计算列表中F值最小的一个.
        /// </summary>
        /// <returns></returns>
        Triangle GetOpenListMin()
        {
            Triangle AStarTriangle = openList[0];
            for (int i = 0; i < openList.Count; i++)
            {
                if (GetDictionaryValue(triangle_F, AStarTriangle) > GetDictionaryValue(triangle_F, openList[i]))
                {
                    AStarTriangle = openList[i];
                }
            }

            return AStarTriangle;
        }

        /// <summary>
        /// 获取当前三角形周围的三角形.
        /// </summary>
        /// <param name="current"></param>
        /// <param name="end"></param>
        /// <param name="map"></param>
        void Explore(Triangle current)
        {
            //获取当前三角形所有的邻边三角形
            List<Triangle> list = GetRuoundAStarTriangle(current, aStarData);


            for (int i = 0; i < list.Count; i++)
            {
                Triangle AStarTriangle = list[i];

                //去掉当前三角形
                if (AStarTriangle == current)
                {
                    continue;
                }

                //去掉已经关闭的三角形
                if (closeList.Contains(AStarTriangle))
                {
                    continue;
                }

                //如果不在待检测的集合  则加入待检测集合
                if (!openList.Contains(AStarTriangle))
                {
                    SetParentIndexes(AStarTriangle, current);

                    SetDictionaryValue(triangle_G, AStarTriangle, GetG(AStarTriangle, current));
                    SetDictionaryValue(triangle_H, AStarTriangle, GetH(AStarTriangle));

                    openList.Add(AStarTriangle);
                }
                //如果在待检测的集合  则判断是否修改父级
                else
                {
                    float G = GetDictionaryValue(triangle_G, AStarTriangle);
                    float H = GetDictionaryValue(triangle_H, AStarTriangle);
                    float F = GetG(AStarTriangle, current) + GetH(AStarTriangle);

                    if (G + H > F)
                    {
                        SetParentIndexes(AStarTriangle, current);
                        SetDictionaryValue(triangle_G, AStarTriangle, GetG(AStarTriangle, current));
                        SetDictionaryValue(triangle_H, AStarTriangle, GetH(AStarTriangle));

                        openList.Remove(AStarTriangle);
                    }
                }
            }
        }





        /// <summary>
        /// 获取G值.
        /// </summary>
        /// <param name="grid"></param>
        /// <param name="parent"></param>
        /// <returns></returns>
        float GetG(Triangle grid, Triangle parent)
        {
            float distance = Ponit.Distance(grid.centroid, parent.centroid);

            distance += GetDictionaryValue(triangle_G, parent);

            return distance;
        }

        /// <summary>
        /// 获取H值.
        /// </summary>
        /// <param name="grid"></param>
        /// <param name="end"></param>
        /// <returns></returns>
        float GetH(Triangle grid)
        {
            float distance = Ponit.Distance(grid.centroid, aStarData.end);

            return distance;
        }




        /// <summary>
        /// 添加父级索引.
        /// </summary>
        /// <param name="current"></param>
        /// <param name="parent"></param>
        void SetParentIndexes(Triangle current, Triangle parent)
        {
            if (parentIndexes.ContainsKey(current))
            {
                parentIndexes[current] = parent;
            }
            else
            {
                parentIndexes.Add(current, parent);
            }
        }




        /// <summary>
        /// 回调
        /// </summary>
        /// <param name="listAStarTriangle"></param>
        void Callback(List<Triangle> listAStarTriangle)
        {
            if (callback == null || listAStarTriangle == null)
            {
                return;
            }


            listAStarTriangle.Reverse();

            List<Ponit> list = new List<Ponit>();

            //进距离移动 不超过一个三角形
            if (listAStarTriangle.Count == 1)
            {
                list.Add(aStarData.end);
                callback(list);
                return;
            }

            for (int i = 0; i < listAStarTriangle.Count; i++)
            {
                list.Add(listAStarTriangle[i].centroid);
            }

            callback(list);

        }



        /// <summary>
        /// 获取周围的三角形
        /// </summary>
        /// <param name="current"></param>
        /// <returns></returns>
        public static List<Triangle> GetRuoundAStarTriangle(Triangle current, AStarData aStarData)
        {
            //该点为开始点 则获取该点三角重心来取值
            if (current.a == aStarData.start && current.b == aStarData.start)
            {
                current = aStarData.startPlace;
            }

            //获取三角形所有的邻边三角形
            List<Triangle> list = new List<Triangle>();

            list.AddRange(GetListAStarTriangle(current.a, aStarData.navMeshInfo.pointIndexes));

            list.AddRange(GetListAStarTriangle(current.b, aStarData.navMeshInfo.pointIndexes));

            list.AddRange(GetListAStarTriangle(current.c, aStarData.navMeshInfo.pointIndexes));

            //去掉重复的三角形
            list = list.Distinct().ToList();

            if (list.Contains(aStarData.endPlace))
            {
                list.Add(aStarData.endAStarTriangle);
            }

            return list;
        }

        /// <summary>
        /// 获取顶点对应的三角形列表.
        /// </summary>
        /// <param name="ponit</param>
        /// <param name="map"></param>
        /// <returns></returns>
        public static List<Triangle> GetListAStarTriangle(Ponit ponit, Dictionary<Ponit, List<Triangle>> pointIndexes)
        {
            List<Triangle> list = null;

            if (pointIndexes == null)
            {
                return list;
            }

            if (pointIndexes.ContainsKey(ponit))
            {
                list = pointIndexes[ponit];
            }

            return list;
        }

        /// <summary>
        /// 设置字典的值.
        /// </summary>
        /// <param name="data"></param>
        /// <param name="key"></param>
        /// <param name="value"></param>
        public static void SetDictionaryValue(Dictionary<Triangle, float> data, Triangle key, float value)
        {
            if (data == null)
            {
                return;
            }

            if (data.ContainsKey(key))
            {
                data[key] = value;
            }
            else
            {
                data.Add(key, value);
            }
        }

        /// <summary>
        /// 获取字典的值.
        /// </summary>
        /// <param name="data"></param>
        /// <param name="key"></param>
        /// <returns></returns>
        public static float GetDictionaryValue(Dictionary<Triangle, float> data, Triangle key)
        {
            if (data == null)
            {
                return 0;
            }

            if (!data.ContainsKey(key))
            {
                data.Add(key, 0);
            }
            return data[key];
        }
    }
}

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

public delegate void CharacterMoveCallback();

/// <summary>
/// 角色移动
/// </summary>
public class CharacterMove : MonoBehaviour
{
    /// <summary>
    /// 目标
    /// </summary>
    public Vector3 target;
    /// <summary>
    /// 速度
    /// </summary>
    public float speed  =  2;
    /// <summary>
    /// 目标距离
    /// </summary>
    public float distance = 0.05f;



    /// <summary>
    /// 移动经过的点
    /// </summary>
    List<Vector3> points;
    /// <summary>
    /// 回调
    /// </summary>
    CharacterMoveCallback callback;

    private void Awake()
    {
        target = transform.position;
    }


    public void Move(Ponit vector3, CharacterMoveCallback callback = null)
    {
        target = Ponit.Convert(vector3);
        this.callback = callback;
    }

    public void Move(List<Ponit> points, CharacterMoveCallback callback = null)
    {
        this.points = new List<Vector3>();

        for (int i = 0; i < points.Count; i++)
        {
            this.points.Add(Ponit.Convert(points[i]));
        }
        this.callback = callback;

        GetTarget();
    }

    void GetTarget()
    {
        if (points != null && points.Count >= 1)
        {
            target = points[0];
            points.Remove(points[0]);

            Debug.Log("前进点 ：" + target);
        }
    }

    void Callback()
    {
        if (callback != null)
        {
            callback();
            callback = null;
        }
    }

    void Update()
    {
        //当前位置与目标距离小于间距 移动停止
        if (Vector3.Distance(target, transform.position) < distance)
        {
            Callback();
            return;
        }

        //获取移动向量;
        Vector3 vector = target - transform.position;
        vector = vector.normalized;

        //移动
        transform.position += vector * Time.deltaTime * speed;

        //如果到达目标点则替换下一个目标
        if (Vector3.Distance(target, transform.position) < distance)
        {
            GetTarget();
        }

    }
}

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

public class test : MonoBehaviour
{
    NavMeshInfo navMeshInfo;

    void Start()
    {
        NavMeshLoad navMeshLoad = new NavMeshLoad();

        navMeshInfo = navMeshLoad.Load(Application.dataPath + "/AStar/obj/test.obj");
    }

    private void Update()
    {
        if (Input.GetMouseButtonUp(0))
        {
            Ray ray = Camera.main.ScreenPointToRay(Input.mousePosition);

            RaycastHit hit;//
            if (Physics.Raycast(ray, out hit))//函数是对射线碰撞的检测
            {
                Vector3 Point = hit.point;//得到碰撞点的坐标

                Debug.Log("开始点： " + transform.position);
                Debug.Log("目标点： " + Point);
                Debug.Log("目标点对象： " + hit.transform.name);

                Vector3 start = transform.position;
                Vector3 end = Point;

                NavMeshHit tmpNavMeshHit;

                if (NavMesh.SamplePosition(start, out tmpNavMeshHit, 5.0f, NavMesh.AllAreas))
                {
                    start = tmpNavMeshHit.position;
                }

                Debug.Log("修改后 开始点： " + start);

                StartNavigation(start, end, navMeshInfo);
            }
        }
    }


    public void StartNavigation(Vector3 start, Vector3 end, NavMeshInfo navMeshInfo)
    {
        if (navMeshInfo == null)
        {
            return;
        }

        Pathfinding pathfinding = new Pathfinding();

        Ponit startPonit = new Ponit(start);
        Ponit endPonit = new Ponit(end);


        pathfinding.Start(startPonit, endPonit, navMeshInfo, (List<Ponit> list) =>
       {
           CharacterMove controller = GetComponent<CharacterMove>();

           controller.Move(list, () =>
           {
               Debug.Log("寻路结束");
           });

       });
    }


}