using System.Collections.Generic;

//优先队列(二叉堆)，这里实现的是"小堆"，即堆顶一直为最小值，用于AStar中对OpenList的维护
//下标为i的节点其左子节点下标是(i*2+1)，右子节点下标是(i*2+2)，父节点的下标是((i-1)/2)
public class BinaryHeap
{
#pragma warning disable 0414
    private List<stARoadPoint> _HeapArray = new List<stARoadPoint>();
    private Dictionary<int, int> _KeyDict = new Dictionary<int, int>();

    public int MapWidth = 0;
    public int MapHeight = 0;

    public void Clear()
    {
        _HeapArray.Clear();
        _KeyDict.Clear();
    }

    public int Count { get { return _HeapArray.Count; } }

    //添加到堆尾
    public void Push(stARoadPoint item)
    {
        item.Idex = item.px * MapHeight + item.py;

        _KeyDict.Add(item.Idex, 0);

        _HeapArray.Add(item);
        PercolateUp(_HeapArray.Count - 1);
    }

    //只能从堆顶开始移除
    public stARoadPoint Pop()
    {
        if (_HeapArray.Count == 0)
        {
            return null;
        }

        int lastIndex = _HeapArray.Count - 1;
        var first = _HeapArray[0];
        _HeapArray[0] = _HeapArray[lastIndex];
        _HeapArray.RemoveAt(lastIndex);

        if (_HeapArray.Count > 0)
        {
            PercolateDown(0);
        }
        
        _KeyDict.Remove(first.Idex);

        return first;
    }

    //更新堆
    public void Update(stARoadPoint node)
    {
        if (node == null || _HeapArray.Count == 0)
        {
            return;
        }
        int index = _HeapArray.IndexOf(node);
        if (index >= 0)
        {
            PercolateUp(index);
        }
    }

    public bool ContainsKey(int key)
    {
        return _KeyDict.ContainsKey(key);
    }

    //向上调整
    private void PercolateUp(int index)
    {
        //新节点在堆中的初始下标
        int curIdx = index;

        //新节点的信息
        stARoadPoint item = _HeapArray[curIdx];

        //父节点在堆中的下标
        int parentIdx = (curIdx - 1) / 2;

        while (curIdx > 0)
        {
            int tmp = _HeapArray[parentIdx].F.CompareTo(item.F);
            if (tmp <= 0)
            {
                break;
            }
            else
            {
                _HeapArray[curIdx] = _HeapArray[parentIdx];
                curIdx = parentIdx;
                parentIdx = (parentIdx - 1) / 2;
            }
        }
        _HeapArray[curIdx] = item;
    }

    //向下调整
    private void PercolateDown(int index)
    {
        //从堆中对比排序的起始元素下标
        int curIdx = index;

        //结束下标
        int endIdx = _HeapArray.Count - 1;

        //左子节点的下标
        int leftIdx = 2 * curIdx + 1;

        //起始节点的信息
        stARoadPoint item = _HeapArray[curIdx];

        while (leftIdx < endIdx)
        {
            int tmp = _HeapArray[leftIdx].F.CompareTo(_HeapArray[leftIdx + 1].F);
            if (leftIdx < endIdx && tmp > 0)
            {
                leftIdx++;
            }

            tmp = item.F.CompareTo(_HeapArray[leftIdx].F);

            if (tmp < 0)
            {
                break;
            }
            else
            {
                _HeapArray[curIdx] = _HeapArray[leftIdx];
                curIdx = leftIdx;
                leftIdx = leftIdx * 2 + 1;
            }
        }
        _HeapArray[curIdx] = item;
    }
#pragma warning restore 0414
}

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

#pragma warning disable 0414
/ <summary>
/ shenjun
/ AStar寻路(二叉堆优化)
/ </summary>

public static class Cost
{
    //直线移动代价
    public static int StraightCost = 10;
    //对角线移动代价
    public static int DiagCost = 14;
}

public class stARoadPoint
{
    public int px = 0;
    public int py = 0;
    public stARoadPoint father = null;

    /**网格权重*/
    public int weight = 0;
    public int G = 0;
    public int F;
    public int H = 0;
    public int go = 0;
    public ushort chflag = 0; // 源地图的值

    public int Idex = 0;

    /**是否需要跳跃*/
    public bool jump = false;

    public stARoadPoint()
    {

    }

    public void Normalize()
    {
        G = 0;
        H = 0;
        F = 0;
        father = null;
    }

    public stARoadPoint(int nx, int ny)
    {
        px = nx;
        py = ny;
    }

    //计算G值
    private void CalcG(stARoadPoint currPoint)
    {
        //如果两个格子的坐标差1，说明是上、下、左、右关系，否则认为是左上、左下、右上、右下关系
        int moveG = (Math.Abs(px - currPoint.px) + Math.Abs(py - currPoint.py)) == 1 ? Cost.StraightCost : Cost.DiagCost;
        int fatherG = father != null ? father.G : 0;
        G = moveG + fatherG + weight;
    }

    //估算H值
    private void CalcH(stARoadPoint end)
    {
        H = (Mathf.Abs(end.px - px) + Mathf.Abs(end.py - py)) * Cost.StraightCost;//曼哈顿估算法
    }

    //计算F值
    private void CalcF()
    {
        F = G + H;
    }

    //更新GF值
    public void UpdateGF(int newG, stARoadPoint newFather)
    {
        father = newFather;
        G = newG + weight;
        CalcF();
    }

    //计算GHF值
    public void CalcGHF(stARoadPoint currPoint, stARoadPoint endPoint)
    {
        father = currPoint;
        //计算GHF
        CalcG(currPoint);
        CalcH(endPoint);
        CalcF();
    }
}

public class AStarPath
{
    //寻路终点
    private stARoadPoint _End;
    private stARoadPoint _Start;
    //地图信息
    private stARoadPoint[] _MapArray;
    //地图的横向节点数QKMapInfo中的usWidth
    private int _Width;
    //地图的纵向节点数QKMapInfo中的usHeight
    private int _Height;
    //开启列表
    private BinaryHeap _BinaryOpen = new BinaryHeap();
    //关闭列表(保存已经处理过的节点)
    private Dictionary<int, stARoadPoint> _CloseList = new Dictionary<int, stARoadPoint>();
    
    private int Step = 0;
    private int PathLen = 0;
    private double CostTime = 0;

    private int _IgnorePointCount = 0;
    
    private bool IsEnd(stARoadPoint point)
    {
        if (point.px == _End.px && point.py == _End.py)
        {
            return true;
        }
        return false;
    }

    private void CalucNewEnd(stARoadPoint curr)
    {
        if (IsEnd(curr) && _IgnorePointCount > 0)
        {
            for (int i = -1; i <= _IgnorePointCount; i++)
            {
                for (int j = -1; j <= _IgnorePointCount; j++)
                {
                    if (i != 0 || j != 0)
                    {
                        int x = _End.px + i;
                        int y = _End.py + j;
                        
                        if (x >= 0 && x < _Width && y >= 0 && y < _Height)
                        {
                            int key = x * _Height + y;
                            stARoadPoint _point = _MapArray[key];
                            if (_point != null && _point.go == 0 && !_CloseList.ContainsKey(_point.Idex))
                            {
                                _End = _point;
                                return;
                            }
                        }
                    }
                }
            }
        }
    }

    //返回寻路路径
    public List<stARoadPoint> PathFinder(stARoadPoint start, stARoadPoint end, int ignorePoint = 0)
    {
        #region 测试
        #if UNITY_EDITOR
        System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
        sw.Start();
        Step = 0;
        PathLen = 0;
        CostTime = 0.0f;
        #endif
        #endregion

        if (start == null || end == null || start.go == 1 || end.go == 1) return null;

        Clear();

        start.father = null;
        end.father = null;
        _Start = start;
        _End = end;

        //地图中所有的节点数据
        _MapArray = QKGameMapMgr.Instance.CurMapData.mapRoadPoints;
        if (_MapArray == null) return null;

        //地图宽高
        _Width = (int)QKGameMapMgr.Instance.CurMapData.width;
        _Height = (int)QKGameMapMgr.Instance.CurMapData.height;
        _BinaryOpen.MapWidth = _Width;
        _BinaryOpen.MapHeight = _Height;
        _IgnorePointCount = ignorePoint;

        //起始点加入OpenList中
        _BinaryOpen.Push(start);

        //最终路径集合
        List<stARoadPoint> pathSet = new List<stARoadPoint>();
        
        while (_BinaryOpen.Count > 0)
        {
            //取出F值最小的节点
            var currPoint = _BinaryOpen.Pop();

            if (currPoint == null)
            {
                return null;
            }
            
            if (IsEnd(currPoint))
            {
                //生成结果
                while (currPoint != null && currPoint.father != null)
                {
                    //容错(寻路结果节点数量大于固定阀值视为错误)
                    if (pathSet.Count > 2000)
                    {
                        Debug.Log("寻路容错");
                        return null;
                    }
                    pathSet.Add(currPoint);
                    currPoint = currPoint.father;
                }

                if (pathSet.Count == 0)
                {
                    return null;
                }

                if (ignorePoint > 0 && pathSet.Count > ignorePoint)
                {
                    for (int i = 0; i < ignorePoint; i++)
                    {
                        pathSet.RemoveAt(0);
                    }
                }

                pathSet.Reverse();

                #region 测试
                #if UNITY_EDITOR
                sw.Stop();
                TimeSpan ts2 = sw.Elapsed;
                CostTime = ts2.TotalMilliseconds;
                PathLen = pathSet.Count;
                Debug.Log(PrintResult());
                #endif
                #endregion

                return pathSet;
            }

            //将节点加入CloseList
            _CloseList.Add(currPoint.Idex, currPoint);

            //搜索周围的节点
            SearchPoint(currPoint);
        }

        return null;
    }

    //搜索周围可以到达的节点
    private void SearchPoint(stARoadPoint currPoint)
    {
        bool isCheckMove = (currPoint == _Start);
        for (int i = -1; i <= 1; i++)
        {
            for (int j = -1; j <= 1; j++)
            {
                //忽略自身位置
                if (i != 0 || j != 0)
                {
                    //周围某个格子的坐标
                    int x = currPoint.px + i;
                    int y = currPoint.py + j;

                    //判断格子合法性
                    if (x >= 0 && x < _Width && y >= 0 && y < _Height)
                    {
                        int key = x * _Height + y;
                        stARoadPoint newPoint = _MapArray[key];
                        if (newPoint == null) continue;
                        bool move = true;
                        if (isCheckMove)
                        {
                            //检查动态障碍
                            QKMapData.GridIndex grid;
                            grid.xIndex = newPoint.px;
                            grid.yIndex = newPoint.py;
                            move = QKCommonHander.GridMoveChecker(grid);
                        }
                        //当前点不可行走时判断是否是终点，如果是则重新计算一个终点
                        if (!move || newPoint.go == 1)
                        {
                            CalucNewEnd(newPoint);
                        }
                        else if (newPoint.go == 0 && !_CloseList.ContainsKey(key))
                        {
                            //可以通过且不在CloseList表中
                            //不在OpenList表中，是新找到的格子
                            if (!_BinaryOpen.ContainsKey(key))
                            {
                                newPoint.CalcGHF(currPoint, _End);
                                _BinaryOpen.Push(newPoint);
                            }
                            else
                            {
                                //已经在OpenList表中，更新这个格子的GF值，并重新指定其父节点
                                int moveG = StraightOrDiagonal(currPoint, newPoint) ? Cost.StraightCost : Cost.DiagCost;
                                int tmpG = currPoint.G + moveG;
                                if (tmpG < newPoint.G)
                                {
                                    newPoint.UpdateGF(tmpG, currPoint);
                                    _BinaryOpen.Update(newPoint);
                                }
                            }
                            #region 测试
                            #if UNITY_EDITOR
                            Step++;
                            #endif
                            #endregion
                        }
                    }
                }
            }
        }
    }

    private void Clear()
    {
        _CloseList.Clear();
        _BinaryOpen.Clear();
        _End = null;
    }

    //计算搜索到的格子处于当前格子哪个方位
    //(ture：上、下、左、右，false：左上、左下、右上、右下)
    private bool StraightOrDiagonal(stARoadPoint currPoint, stARoadPoint point)
    {
        return (Math.Abs(point.px - currPoint.px) + Math.Abs(point.py - currPoint.py)) == 1 ? true : false;
    }

    private string PrintResult()
    {
        return string.Format("<color=yellow>★★★★★★寻路统计：共遍历节点{0}次, 最终路径长度{1}, 总耗时{2}毫秒★★★★★★</color>", Step, PathLen, CostTime);
    }
}

什么是AStar: 

       是一种静态路网中求解最短路最有效的直接搜索方法,估价值跟实例值非常接近;

启发式搜索  : 

       启发式搜索就是在状态空间中的搜索对每一个搜索的位置进行评估,得到最好的位置,在从这个位置进行搜索直到目标.这样可以省略大量无谓的搜索路径,提高了效率;

在启发式搜索中,对位置的估价是十分重要的.采用了不同的估价,可以有不同的效果;
(估价函数就一种规则,制定的规则不同,最后的效果也就不同)<比如寻路的格子不同,最后的效果也是不同的>


估价函数     : 

      从当前节点移动到 目标节点的预估费用<费用简单的说就是从起点到终点的距离>(如果放到地图上就是距离,走的路越多估价越高);这个估价就是启发式的.在寻路问题和迷宫问题中,我们通常用曼哈顿(manhattan)估价函数预估费用.

A*算法特点: 

     在理论上是时间最优的; 

     但也有缺点:它的空间增长是指数级别的;(格子越多计算的量也就越大)<优化可以使用  二叉堆 >

中心思想: 

      通过从起始点开始,检查相邻方格的方式,向外扩展,直到找到目标;

运用: 

      把一个平面分成一个个的小方格,方格分的越细,寻路就越精准;

为了更好的描述,和理解A*算法提出的两个概念(不存在于真实的场景中)


开启列表:  

      待检查方格的集合列表,寻找周围可以达到的点,加入到此列表中, 

并保存中心点为父节点

关闭列表:列表中保存不需要再次检查的方格



路径评分: 

G-当前点与起始点的距离 

H-当前点与目标点的距离

F的值是G和F的和 

F,G和H的评分被写在每个方格里. 

如图:F被打印在中间,G在左上角.H在右上角. 

<如上图> 

从一个格子到相邻格子的斜线距离(以为到斜线格子的距离为跟2,所以用1.4为单位) 

从一个格子到相格子的直线距离(因为格子为1,所以就以1为单位); 

为了方便计算查看,都乘以10作为单位;

选择路径过程中,经过哪个方格关键是:F=G+H; 



<如上图>以红色格子为例 

方块的中心位置42就是估价,也就是起始点到目标的距离 

左上角用G表示,也就是该格子到目标点的距离为14 

右上角用H表示,也就是该格子到起始点的距离为28 

红色格子右上角是62,是因为已经确定了当前红色的格子,路径是从当前红色格子到右上角的格子,所以值为62; 

如果选择的不是当前红色的格子,那么就会去改变其他格子的值,也就是说每个格子的值随着你的选择进行改变 

(改变成符合当前格子路径上的值).

例如:从A到B的一个解析过程 



开始搜索: 

1.把起始格添加到开启列表。 

从A点开始查找 

2.寻找起点周围所有可到达或者可通过的方格，把他们加入开启列表。 

遍历A所能到达的所有方格,把他们加入开启列表中 

3.从开启列表中删除点A，把它加入到一个“关闭列表”，列表中保存所有不需要再次检查的方格。 

因为A已经走过,所以把A加入关闭列表中,下次在进行遍历时把A除外 



继续搜索: 

4把当前格子从开启列表中删除，然后添加到关闭列表中。 

因为已经到达了当前格子,所以把当前格子放到关闭列表中(已经到达了,没有必要去进行遍历) 

5检查所有相邻格子。跳过那些已经在关闭列表中的或者不可通过的，把他们添加进开启列表，把选中的方格作为新的方格的父节点。 

当前中的格子周围有两个已经添加到开启列表中,上面三个为不可通过,也就是障碍物,所以只是新添加了左侧的两个格子; 

 

 



6如果某个相邻格已经在开启列表里了，检查现在的这条路径G值是否会更低一些。

如果新的G值更低，那就把相邻方格的父节点改为目前选中的方格，重新计算F和G的值。 

   一旦重新选择了一个点那么就需要重新计算一下当前的费用,上面写着只计算G和F的 值,为什么呢! 

原因: 

   每个点到目标点的距离是固定的,所以H点(又上角的点)是不需要改变的,会变化的G值仅仅是因为路经改变的原因跟起始点的距离会发生改变, 

        所以只需要计算G的值和F的值,其他的点也会相应的更新; 

<图一> 

因为选择的42,但是出现了3个相同的估价,那么我们该怎么选择呢? 

<图二> 

这里还有另外一个原则:那就是选择一个距离目标点最近的一个值,也就是H值最低的一个. 

也就是左侧的48这个方块 ,那么就把48这个点从开启列表中删除,送进关闭列表中. 

因为选择了48,新增了左侧两个点,那么跟新一下在开启列表中的当前48下面的两个点(在这里下面那两个点是没有变化的,因为42下面的也是48,所以路径的值是没有改变的); 

那么继续走 

当我们继续走的时候,发现遍历48所能达到的值,发现这条路径不是最优的. 

为什么不是最优的呢:(因为在它的开启列表中还有一个48) 

那么在他的开启列表中还有一个48的值,那么就把当前的这个48从开起列表中删除，送进关闭列表中。 

我们就回到上一步,选择42下方的48; 

同理，这个48也不是最优的，因为他的开启列表中还有一个48，那么我们继续选择42，右侧的这个48； 

结束 



起始格下方格子的父节点已 经和前面不同的。 之前它的G值是，并且指向 右上方的格子。现在它的G 值是，指向它上方的格子。 这在寻路过程中的某处发生， 当应用新路径时，G值经过 检查变得低了－于是父节点 被重新指定，G和F值被重 

新计算。

根据上述的原理得到如上图所示的路径，也就是最佳路径；

总结

1把起始格添加到开启列表。 

 2重复如下的工作：  

  寻找开启列表中F值最低的格子。我们称它为当前格。 

 把它切换到关闭列表。 

 /1/对相邻的格中的每一个–> 如果它不可通过或者已经在关闭列表中，略过它。反之如下。如果它不在开启列表中，把它添加进去。把当前格作为这一格的父节点。记录这一格的F,G,和H值。  

       /2/如果它已经在开启列表中，用G值为参考检查新的路径是否更好。更低的G值意味着更好的路径。如果是这样，就把这一格的父节点改成当前格，并且重新计算这一格的 G和F值。 

停止，当你 把目标格添加进了关闭列表，这时候路径被找到–>没有找到目标格，开启列表已经空了。这时候，路径不存在。 

3.保存路径。从目标格开始，沿着每一格的父节点移动直到回到起始格。

算法：步骤架构 

<1>开启集合  

<2>关闭集合 

<3>添加起始点到开始集合中 

<4>循环如下步骤： 当前点=开启集合中最小F_Cost的点 将当前点移出开启集合中 将当前点添加到关闭集合中 

<5>如果当前点是目标点，结束查询 

<6>遍历当前点的每个相邻点 如果相邻点不能访问或者相邻点在关闭集合中，跳过此相邻点 如果新路径到相邻点的距离更短，或者相邻点不在开启集合中 重新设置F_Cost 重新设置其父节点为当前点 

<7>如果相邻点不在开启集合中 

<8>添加相邻点到开启集合什么是AStar:    是一种静态路网中求解最短路最有效的直接搜索方法,估价值跟实例值非常接近; 

启发式搜索  :    启发式搜索就是在状态空间中的搜索对每一个搜索的位置进行评估,得到最好的位置,在从这个位置进行搜索直到目标.这样可以省略大量无谓的搜索路径,提高了效率;

在启发式搜索中,对位置的估价是十分重要的.采用了不同的估价,可以有不同的效果; 

(估价函数就一种规则,制定的规则不同,最后的效果也就不同)<比如寻路的格子不同,最后的效果也是不同的>

估价函数     :从当前节点移动到 目标节点的预估费用<费用简单的说就是从起点到终点的距离>(如果放到地图上就是距离,走的路越多估价越高);这个估价就是启发式的.在寻路问题和迷宫问题中,我们通常用曼哈顿(manhattan)估价函数预估费用.

A*算法特点:在理论上是时间最优的; 

但也有缺点:它的空间增长是指数级别的;(格子越多计算的量也就越大)<优化可以使用  二叉堆 >

中心思想:通过从起始点开始,检查相邻方格的方式,向外扩展,直到找到目标;

.运用 

把一个平面分成一个个的小方格,方格分的越细,寻路就越精准;

为了更好的描述,和理解A*算法提出的两个概念(不存在于真实的场景中) 

开启列表: 待检查方格的集合列表,寻找周围可以达到的点,加入到此列表中, 

                并保存中心点为父节点

关闭列表:列表中保存不需要再次检查的方格

路径评分: 

G-当前点与起始点的距离 

H-当前点与目标点的距离

F的值是G和F的和 

F,G和H的评分被写在每个方格里. 

如图:F被打印在中间,G在左上角.H在右上角. 

<如上图> 

从一个格子到相邻格子的斜线距离(以为到斜线格子的距离为跟2,所以用1.4为单位) 

从一个格子到相格子的直线距离(因为格子为1,所以就以1为单位); 

为了方便计算查看,都乘以10作为单位;

选择路径过程中,经过哪个方格关键是:F=G+H;

<如上图>以红色格子为例 

方块的中心位置42就是估价,也就是起始点到目标的距离 

左上角用G表示,也就是该格子到目标点的距离为14 

右上角用H表示,也就是该格子到起始点的距离为28 

红色格子右上角是62,是因为已经确定了当前红色的格子,路径是从当前红色格子到右上角的格子,所以值为62; 

如果选择的不是当前红色的格子,那么就会去改变其他格子的值,也就是说每个格子的值随着你的选择进行改变 

(改变成符合当前格子路径上的值).

例如:从A到B的一个解析过程

开始搜索: 

1.把起始格添加到开启列表。 

从A点开始查找 

2.寻找起点周围所有可到达或者可通过的方格，把他们加入开启列表。 

遍历A所能到达的所有方格,把他们加入开启列表中 

3.从开启列表中删除点A，把它加入到一个“关闭列表”，列表中保存所有不需要再次检查的方格。 

因为A已经走过,所以把A加入关闭列表中,下次在进行遍历时把A除外

继续搜索: 

4把当前格子从开启列表中删除，然后添加到关闭列表中。 

因为已经到达了当前格子,所以把当前格子放到关闭列表中(已经到达了,没有必要去进行遍历) 

5检查所有相邻格子。跳过那些已经在关闭列表中的或者不可通过的，把他们添加进开启列表，把选中的方格作为新的方格的父节点。 

当前中的格子周围有两个已经添加到开启列表中,上面三个为不可通过,也就是障碍物,所以只是新添加了左侧的两个格子;

6如果某个相邻格已经在开启列表里了，检查现在的这条路径G值是否会更低一些。 

如果新的G值更低，那就把相邻方格的父节点改为目前选中的方格，重新计算F和G的值。 

一旦重新选择了一个点那么就需要重新计算一下当前的费用,上面写着只计算G和F的 值,为什么呢! 

原因:每个点到目标点的距离是固定的,所以H点(又上角的点)是不需要改变的,会变化的G值仅仅是因为路经改变的原因跟起始点的距离会发生改变, 

        所以只需要计算G的值和F的值,其他的点也会相应的更新; 

<图一> 

因为选择的42,但是出现了3个相同的估价,那么我们该怎么选择呢? 

<图二> 

这里还有另外一个原则:那就是选择一个距离目标点最近的一个值,也就是H值最低的一个. 

也就是左侧的48这个方块 ,那么就把48这个点从开启列表中删除,送进关闭列表中. 

因为选择了48,新增了左侧两个点,那么跟新一下在开启列表中的当前48下面的两个点(在这里下面那两个点是没有变化的,因为42下面的也是48,所以路径的值是没有改变的); 

那么继续走 

当我们继续走的时候,发现遍历48所能达到的值,发现这条路径不是最优的. 

为什么不是最优的呢:(因为在它的开启列表中还有一个48) 

那么在他的开启列表中还有一个48的值,那么就把当前的这个48从开起列表中删除，送进关闭列表中。 

我们就回到上一步,选择42下方的48; 

同理，这个48也不是最优的，因为他的开启列表中还有一个48，那么我们继续选择42，右侧的这个48； 

结束

起始格下方格子的父节点已 经和前面不同的。 之前它的G值是，并且指向 右上方的格子。现在它的G 值是，指向它上方的格子。 这在寻路过程中的某处发生， 当应用新路径时，G值经过 检查变得低了－于是父节点 被重新指定，G和F值被重 

新计算。

根据上述的原理得到如上图所示的路径，也就是最佳路径；

总结

1把起始格添加到开启列表。 

 2重复如下的工作：  

  寻找开启列表中F值最低的格子。我们称它为当前格。 

 把它切换到关闭列表。 

 /1/对相邻的格中的每一个–> 如果它不可通过或者已经在关闭列表中，略过它。反之如下。如果它不在开启列表中，把它添加进去。把当前格作为这一格的父节点。记录这一格的F,G,和H值。  

       /2/如果它已经在开启列表中，用G值为参考检查新的路径是否更好。更低的G值意味着更好的路径。如果是这样，就把这一格的父节点改成当前格，并且重新计算这一格的 G和F值。 

停止，当你 把目标格添加进了关闭列表，这时候路径被找到–>没有找到目标格，开启列表已经空了。这时候，路径不存在。 

3.保存路径。从目标格开始，沿着每一格的父节点移动直到回到起始格。

算法：步骤架构 

<1>开启集合  

<2>关闭集合 

<3>添加起始点到开始集合中 

<4>循环如下步骤： 当前点=开启集合中最小F_Cost的点 将当前点移出开启集合中 将当前点添加到关闭集合中 

<5>如果当前点是目标点，结束查询 

<6>遍历当前点的每个相邻点 如果相邻点不能访问或者相邻点在关闭集合中，跳过此相邻点 如果新路径到相邻点的距离更短，或者相邻点不在开启集合中 重新设置F_Cost 重新设置其父节点为当前点 

<7>如果相邻点不在开启集合中 

<8>添加相邻点到开启集合

import burlap.behavior.singleagent.planning.deterministic.informed.astar.AStar; //導入依賴的package包/類
public void testDude(State s) {
TerminalFunction tf = new BlockDudeTF();
StateConditionTest sc = new TFGoalCondition(tf);
AStar astar = new AStar(domain, sc, new SimpleHashableStateFactory(), new NullHeuristic());
astar.toggleDebugPrinting(false);
astar.planFromState(s);
Policy p = new SDPlannerPolicy(astar);
Episode ea = PolicyUtils.rollout(p, s, domain.getModel(), 100);
State lastState = ea.stateSequence.get(ea.stateSequence.size() - 1);
Assert.assertEquals(true, tf.isTerminal(lastState));
Assert.assertEquals(true, sc.satisfies(lastState));
Assert.assertEquals(-94.0, ea.discountedReturn(1.0), 0.001);
/*
BlockDude constructor = new BlockDude();
Domain d = constructor.generateDomain();
List px = new ArrayList();
List  ph = new ArrayList();
ph.add(15);
ph.add(3);
ph.add(3);
ph.add(3);
ph.add(0);
ph.add(0);
ph.add(0);
ph.add(1);
ph.add(2);
ph.add(0);
ph.add(2);
ph.add(3);
ph.add(2);
ph.add(2);
ph.add(3);
ph.add(3);
ph.add(15);
State o = BlockDude.getCleanState(d, px, ph, 6);
o = BlockDude.setAgent(o, 9, 3, 1, 0);
o = BlockDude.setExit(o, 1, 0);
o = BlockDude.setBlock(o, 0, 5, 1);
o = BlockDude.setBlock(o, 1, 6, 1);
o = BlockDude.setBlock(o, 2, 14, 3);
o = BlockDude.setBlock(o, 3, 16, 4);
o = BlockDude.setBlock(o, 4, 17, 4);
o = BlockDude.setBlock(o, 5, 17, 5);
TerminalFunction tf = new SinglePFTF(d.getPropFunction(BlockDude.PFATEXIT));
StateConditionTest sc = new SinglePFSCT(d.getPropFunction(BlockDude.PFATEXIT));
RewardFunction rf = new UniformCostRF();
AStar astar = new AStar(d, rf, sc, new DiscreteStateHashFactory(), new NullHeuristic());
astar.toggleDebugPrinting(false);
astar.planFromState(o);
Policy p = new SDPlannerPolicy(astar);
EpisodeAnalysis ea = p.evaluateBehavior(o, rf, tf, 100);
State lastState = ea.stateSequence.get(ea.stateSequence.size() - 1);
Assert.assertEquals(true, tf.isTerminal(lastState));
Assert.assertEquals(true, sc.satisfies(lastState));
Assert.assertEquals(-94.0, ea.getDiscountedReturn(1.0), 0.001);
*/
}

JAVA实现Astar寻径算法：
此算法的主要公式：F=G+H
* G = 从起点，沿着产生的路径，移动到网格上指定方格的移动耗费。
* H = 从此点阵到结束点阵的预估移动耗费，这被称为开启式的。
* F = 等G+H的值，表示的一个权重值。
首先将开始点存入到开启列表当中(待检查的点)，检查开始点，将周围的点阵的F，G，H值算出，将周围点阵加入到开启列表当中，将周围点阵的父节点指向当前查找的节点，然后将开启列表排序，查找到下一个最小的F值进行路径查找，以此类推，只到找结束点阵为止。
当路径查找到结束点时，就通过结束点在闭合列表当中查找它指向的父节点为路径加到路径列表当中，再通过查找的节点指向的父节点为路径加到路径列表当中，以此类推以找到父节点指各的是开始节点为止，此算法就结束。以上只是大概的讲解了一下Astar算法的原则。有兴趣可以到网上去查找资料，或者与我共同讨论和学习。我的QQ：565345652。
下面是我用JAVA实现的Astar的截图
JAVA 实现Astar的核心代码：
/*
* Astar算法实现的类
* */
package com.smq.astar;
public class AStarRule implements Runnable
{
private Testastar ta=null; //拿界面对象的一个引用
private boolean astar_ok=false; //Astar算法是否成功找到
private AstarObject minao; //开启列表查找的对象引用
/*
* 构造一个Astar算法
* */
public AStarRule(Testastar ta)
{
ta.setStart(System.currentTimeMillis());
this.ta=ta;
minao=ta.getStart_ao();
setAstar(ta.getStart_ao());
}
/*
* 运行Astar算法进行查找路径
* */
public void run()
{
System.out.println("astar");
while(!astar_ok)
{
System.out.println("astar");
if(ta.getOpenlistaos().size()==0)
{
astar_ok=true;
ta.setAstar_ok(true);
ta.getGb().setString("-还原-");
ta.setEnd(System.currentTimeMillis());
System.out.println("所用时间："+(ta.getEnd()-ta.getStart())/1000.0);
}
System.out.println("open"+ta.getOpenlistaos().size());
System.out.println("close"+ta.getCloselistaos().size());
if(ta.getOpenlistaos().size()>=2)
{
Array();
for(int i=1;i<=4;i++)
{
this.LoadingOpenList(i);
minao.setOpenlist_ao(false);
minao.setCloselist_ao(true);
ta.getCloselistaos().add(minao);
ta.getOpenlistaos().remove(minao);
}
}else if(ta.getOpenlistaos().size()!=0)
{
minao=ta.getOpenlistaos().get(0);
for(int i=1;i<=4;i++)
{
this.LoadingOpenList(i);
minao.setOpenlist_ao(false);
minao.setCloselist_ao(true);
ta.getCloselistaos().add(minao);
ta.getOpenlistaos().remove(minao);
}
}
try {
Thread.sleep(ta.getSpeed());
} catch (InterruptedException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}
}
/*
* 排列开启列表
* */
public void Array()
{
AstarObject maxao;
minao=ta.getOpenlistaos().get(0);
for(int i=0;i<ta.getOpenlistaos().size()-1;i++)
{
maxao=ta.getOpenlistaos().get(i);
if(minao.getAstarF()>=maxao.getAstarF())
{
minao=maxao;
}
}
}
/*
* 加入开启列表对象
* */
public void LoadingOpenList(int dir)
{
for(int i=0;i
{
AstarObject ao=ta.getAos().get(i);
switch(dir)
{
case 1:
if(ao.getX()==minao.getX()+minao.getW()&&ao.getY()==minao.getY())
{
ao.setAstarG(minao.getAstarG()+10);
setAstar(ao);
ao.setCratelist_ao(false);
ao.setOpenlist_ao(true);
ao.setFathernode(minao);
ao.setFathernoad_x1(ao.getX()+ao.getW()/2);
ao.setFaternoad_y1(ao.getY()+ao.getH()/2);
ao.setFaternoad_x2(ao.getX()+2);
ao.setFaternoady2(ao.getY()+ao.getH()/2);
ta.getOpenlistaos().add(ao);
ta.getAos().remove(ao);
AstarObject upao=null,downao=null;
for(int n=0;n<ta.getAos().size();n++)
{
AstarObject ao1=ta.getAos().get(n);
if(ao1.getX()==minao.getX()+ao1.getW()&&ao1.getY()==minao.getY()-ao1.getH())
{
upao=ao1;
}else if(ao1.getX()==minao.getX()+ao1.getW()&&ao1.getY()==minao.getY()+ao1.getH())
{
downao=ao1;
}
if(upao!=null&&downao!=null||n==ta.getAos().size()-1)
{
break;
}
}
if(upao!=null)
{
upao.setAstarG(minao.getAstarG()+14);
setAstar(upao);
upao.setCratelist_ao(false);
upao.setOpenlist_ao(true);
upao.setFathernode(minao);
upao.setFathernoad_x1(upao.getX()+upao.getW()/2);
upao.setFaternoad_y1(upao.getY()+upao.getH()/2);
upao.setFaternoad_x2(upao.getX()+2);
upao.setFaternoady2(upao.getY()+upao.getH()-2);
ta.getOpenlistaos().add(upao);
ta.getAos().remove(upao);
}else if(ta.isPath_ok())
{
this.lookupOpenlist(minao, minao.getX()+minao.getW(), minao.getY()-minao.getH(), 14, 2);
}
if(downao!=null)
{
downao.setAstarG(minao.getAstarG()+14);
setAstar(downao);
downao.setCratelist_ao(false);
downao.setOpenlist_ao(true);
downao.setFathernode(minao);
downao.setFathernoad_x1(downao.getX()+downao.getW()/2);
downao.setFaternoad_y1(downao.getY()+downao.getH()/2);
downao.setFaternoad_x2(downao.getX()+2);
downao.setFaternoady2(downao.getY()+2);
ta.getOpenlistaos().add(downao);
ta.getAos().remove(downao);
}else if(ta.isPath_ok())
{
this.lookupOpenlist(minao, minao.getX()+minao.getW(), minao.getY()+minao.getH(), 14, 3);
}
return;
}else if(ta.getEnd_ao().getX()==minao.getX()+minao.getW()&&ta.getEnd_ao().getY()==minao.getY())
{
this.astar_ok=true;
minao.setPathlist_ao(true);
ta.getPathlistaos().add(minao);
this.loadpathing(minao);
return;
}else if(i==ta.getAos().size()-1&&ta.isPath_ok())
{
this.lookupOpenlist(minao, minao.getX()+minao.getW(), minao.getY(), 10, 1);
this.lookupOpenlist(minao, minao.getX()+minao.getW(), minao.getY()-minao.getH(), 14, 2);
this.lookupOpenlist(minao, minao.getX()+minao.getW(), minao.getY()+minao.getH(), 14, 3);
return;
}
break;
case 2:
if(ao.getX()==minao.getX()&&ao.getY()==minao.getY()-minao.getH())
{
ao.setAstarG(minao.getAstarG()+10);
setAstar(ao);
ao.setCratelist_ao(false);
ao.setOpenlist_ao(true);
ao.setFathernode(minao);
ao.setFathernoad_x1(ao.getX()+ao.getW()/2);
ao.setFaternoad_y1(ao.getY()+ao.getH()/2);
ao.setFaternoad_x2(ao.getX()+ao.getW()/2);
ao.setFaternoady2(ao.getY()+ao.getH()-2);
ta.getOpenlistaos().add(ao);
ta.getAos().remove(ao);
AstarObject upao=null;
for(int n=0;n<ta.getAos().size();n++)
{
AstarObject ao1=ta.getAos().get(n);
if(ao1.getX()==minao.getX()-ao1.getW()&&ao1.getY()==minao.getY()-ao1.getH())
{
upao=ao1;
}
if(upao!=null||n==ta.getAos().size()-1)
{
break;
}
}
if(upao!=null)
{
upao.setAstarG(minao.getAstarG()+14);
setAstar(upao);
upao.setCratelist_ao(false);
upao.setOpenlist_ao(true);
upao.setFathernode(minao);
upao.setFathernoad_x1(upao.getX()+upao.getW()/2);
upao.setFaternoad_y1(upao.getY()+upao.getH()/2);
upao.setFaternoad_x2(upao.getX()+upao.getW()-2);
upao.setFaternoady2(upao.getY()+upao.getH()-2);
ta.getOpenlistaos().add(upao);
ta.getAos().remove(upao);
}else if(ta.isPath_ok())
{
this.lookupOpenlist(minao, minao.getX()-minao.getW(), minao.getY()-minao.getH(), 14, 5);
}
return;
}else if(ta.getEnd_ao().getX()==minao.getX()&&ta.getEnd_ao().getY()==minao.getY()-minao.getH())
{
this.astar_ok=true;
minao.setPathlist_ao(true);
ta.getPathlistaos().add(minao);
this.loadpathing(minao);
return;
}else if(i==ta.getAos().size()-1&&ta.isPath_ok())
{
this.lookupOpenlist(minao, minao.getX(), minao.getY()-minao.getH(), 10, 4);
this.lookupOpenlist(minao, minao.getX()-minao.getW(), minao.getY()-minao.getH(), 14, 5);
return;
}
break;
case 3:
if(ao.getX()==minao.getX()-minao.getW()&&ao.getY()==minao.getY())
{
ao.setAstarG(minao.getAstarG()+10);
setAstar(ao);
ao.setCratelist_ao(false);
ao.setOpenlist_ao(true);
ao.setFathernode(minao);
ao.setFathernoad_x1(ao.getX()+ao.getW()/2);
ao.setFaternoad_y1(ao.getY()+ao.getH()/2);
ao.setFaternoad_x2(ao.getX()+ao.getW()-2);
ao.setFaternoady2(ao.getY()+ao.getH()/2);
ta.getOpenlistaos().add(ao);
ta.getAos().remove(ao);
AstarObject upao=null;
for(int n=0;n<ta.getAos().size();n++)
{
AstarObject ao1=ta.getAos().get(n);
if(ao1.getX()==minao.getX()-ao1.getW()&&ao1.getY()==minao.getY()+ao1.getH())
{
upao=ao1;
}
if(upao!=null||n==ta.getAos().size()-1)
{
break;
}
}
if(upao!=null)
{
upao.setAstarG(minao.getAstarG()+14);
setAstar(upao);
upao.setCratelist_ao(false);
upao.setOpenlist_ao(true);
upao.setFathernode(minao);
upao.setFathernoad_x1(upao.getX()+upao.getW()/2);
upao.setFaternoad_y1(upao.getY()+upao.getH()/2);
upao.setFaternoad_x2(upao.getX()+upao.getW()-2);
upao.setFaternoady2(upao.getY()+2);
ta.getOpenlistaos().add(upao);
ta.getAos().remove(upao);
}else if(ta.isPath_ok())
{
this.lookupOpenlist(minao, minao.getX()-minao.getW(), minao.getY()+minao.getH(), 14, 7);
}
return;
}else if(ta.getEnd_ao().getX()==minao.getX()-minao.getW()&&ta.getEnd_ao().getY()==minao.getY())
{
this.astar_ok=true;
minao.setPathlist_ao(true);
ta.getPathlistaos().add(minao);
this.loadpathing(minao);
return;
}else if(i==ta.getAos().size()-1&&ta.isPath_ok())
{
this.lookupOpenlist(minao, minao.getX()-minao.getW(), minao.getY(), 10, 6);
this.lookupOpenlist(minao, minao.getX()-minao.getW(), minao.getY()+minao.getH(), 14, 7);
return;
}
break;
case 4:
if(ao.getX()==minao.getX()&&ao.getY()==minao.getY()+minao.getH())
{
ao.setAstarG(minao.getAstarG()+10);
setAstar(ao);
ao.setCratelist_ao(false);
ao.setOpenlist_ao(true);
ao.setFathernode(minao);
ao.setFathernoad_x1(ao.getX()+ao.getW()/2);
ao.setFaternoad_y1(ao.getY()+ao.getH()/2);
ao.setFaternoad_x2(ao.getX()+ao.getW()/2);
ao.setFaternoady2(ao.getY()+2);
ta.getOpenlistaos().add(ao);
ta.getAos().remove(ao);
return;
}else if(ta.getEnd_ao().getX()==minao.getX()&&ta.getEnd_ao().getY()==minao.getY()+minao.getH())
{
this.astar_ok=true;
minao.setPathlist_ao(true);
ta.getPathlistaos().add(minao);
this.loadpathing(minao);
return;
}else if(i==ta.getAos().size()-1&&ta.isPath_ok())
{
this.lookupOpenlist(minao, minao.getX(), minao.getY()+minao.getH(), 10, 8);
return;
}
break;
}
}
}
/*
* 算出点阵的F，G，H值
* */
public void setAstar(AstarObject ao)
{
if(ao.getX()<ta.getEnd_ao().getX())
{
ao.setAstarH((ta.getEnd_ao().getX()-ao.getX())/ao.getW()*10);
}else if(ao.getX()>ta.getEnd_ao().getX())
{
ao.setAstarH((ao.getX()-ta.getEnd_ao().getX())/ao.getW()*10);
}
if(ao.getY()
{
ao.setAstarH((ta.getEnd_ao().getY()-ao.getY())/ao.getH()*10+ao.getAstarH());
}else if(ao.getY()>ta.getEnd_ao().getY())
{
ao.setAstarH((ao.getY()-ta.getEnd_ao().getY())/ao.getH()*10+ao.getAstarH());
}
ao.setAstarF(ao.getAstarG()+ao.getAstarH());
}
/*
* 加载查找的最佳路径
* */
public void loadpathing(AstarObject ao)
{
System.out.println("loadpathaos"+ta.getPathlistaos().size());
for(int i=0;i<ta.getCloselistaos().size();i++)
{
if(ao.getFathernode()==ta.getCloselistaos().get(i))
{
ta.getCloselistaos().get(i).setPathlist_ao(true);
ta.getPathlistaos().add(ta.getCloselistaos().get(i));
if(ta.getCloselistaos().get(i)!=ta.getStart_ao())
{
this.loadpathing(ta.getCloselistaos().get(i));
}else {
ta.setEnd(System.currentTimeMillis());
System.out.println("所用时间："+(ta.getEnd()-ta.getStart())/1000.0);
ta.getGb().setString("-还原-");
ta.setAstar_ok(true);
}
return;
}
}
}
/*
* 查找开启列表当中的G值大小
* */
public void lookupOpenlist(AstarObject ao,int x,int y,int G,int dir)
{
for(int i=0;i<ta.getOpenlistaos().size();i++)
{
AstarObject openlistao=ta.getOpenlistaos().get(i);
if(x==openlistao.getX()&&y==openlistao.getY())
{
if(ao.getAstarG()+G<openlistao.getAstarG())
{
System.out.println("lookupOpenList_Ok");
openlistao.setAstarG(ao.getAstarG()+G);
openlistao.setAstarF(openlistao.getAstarG()+openlistao.getAstarH());
openlistao.setFathernode(ao);
switch(dir)
{
case 1:
openlistao.setFathernoad_x1(openlistao.getX()+openlistao.getW()/2);
openlistao.setFaternoad_y1(openlistao.getY()+openlistao.getH()/2);
openlistao.setFaternoad_x2(openlistao.getX()+2);
openlistao.setFaternoady2(openlistao.getY()+openlistao.getH()/2);
break;
case 2:
openlistao.setFathernoad_x1(openlistao.getX()+openlistao.getW()/2);
openlistao.setFaternoad_y1(openlistao.getY()+openlistao.getH()/2);
openlistao.setFaternoad_x2(openlistao.getX()+2);
openlistao.setFaternoady2(openlistao.getY()+openlistao.getH()-2);
break;
case 3:
openlistao.setFathernoad_x1(openlistao.getX()+openlistao.getW()/2);
openlistao.setFaternoad_y1(openlistao.getY()+openlistao.getH()/2);
openlistao.setFaternoad_x2(openlistao.getX()+2);
openlistao.setFaternoady2(openlistao.getY()+2);
break;
case 4:
openlistao.setFathernoad_x1(openlistao.getX()+openlistao.getW()/2);
openlistao.setFaternoad_y1(openlistao.getY()+openlistao.getH()/2);
openlistao.setFaternoad_x2(openlistao.getX()+openlistao.getW()/2);
openlistao.setFaternoady2(openlistao.getY()+openlistao.getH()-2);
break;
case 5:
openlistao.setFathernoad_x1(openlistao.getX()+openlistao.getW()/2);
openlistao.setFaternoad_y1(openlistao.getY()+openlistao.getH()/2);
openlistao.setFaternoad_x2(openlistao.getX()+openlistao.getW()-2);
openlistao.setFaternoady2(openlistao.getY()+openlistao.getH()-2);
break;
case 6:
openlistao.setFathernoad_x1(openlistao.getX()+openlistao.getW()/2);
openlistao.setFaternoad_y1(openlistao.getY()+openlistao.getH()/2);
openlistao.setFaternoad_x2(openlistao.getX()+openlistao.getW()-2);
openlistao.setFaternoady2(openlistao.getY()+openlistao.getH()/2);
break;
case 7:
openlistao.setFathernoad_x1(openlistao.getX()+openlistao.getW()/2);
openlistao.setFaternoad_y1(openlistao.getY()+openlistao.getH()/2);
openlistao.setFaternoad_x2(openlistao.getX()+openlistao.getW()-2);
openlistao.setFaternoady2(openlistao.getY()+2);
break;
case 8:
openlistao.setFathernoad_x1(openlistao.getX()+openlistao.getW()/2);
openlistao.setFaternoad_y1(openlistao.getY()+openlistao.getH()/2);
openlistao.setFaternoad_x2(openlistao.getX()+openlistao.getW()/2);
openlistao.setFaternoady2(openlistao.getY()+2);
break;
}
}
return;
}
}
}
}