jcastarpathfinder.cs

两种AstarPathFinder 一种DijkstraPathfinder BFS

using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
using System.Collections;

namespace PathFinder
{
    class JCAstarPathFinder:PathFinder
    {
        private Map currentMap;

        public JCAstarPathFinder(Map myMap)
        {
            currentMap = myMap;
        }

        public  Point[] MyPlusPoint = { new Point(1, 0), new Point(0, 1), new Point(0, -1), new Point(-1, 0) };//设为Public是为了在attct选点的时候访问

        /// <summary>
        /// Astar寻路主要方法
        /// 加入Wall和Blocker的考虑
        /// </summary>
        /// <param name="origin"></param>
        /// <param name="destination"></param>
        /// <param name="pathCost"></param>
        /// <returns></returns>
        public override Stack<Point> FindWay(Point origin, Point destination)
        {
            double cost = Math.Abs(origin.X - destination.X) + Math.Abs(origin.Y - destination.Y);
            bool IsNearBy = false;
            Stack<Point> ways = new Stack<Point>();
            JCMinQueue OpenQueue = new JCMinQueue(currentMap);
            JCMinQueue CloseList = new JCMinQueue(currentMap);
            JCAstarNode ThisNode = new JCAstarNode(origin);
            OpenQueue.MinHeapInsert(0, ThisNode);
            if (destination != Point.Empty)
            {
                while (OpenQueue.Count != 0 && ThisNode.NodeLoction != destination)
                {

                    ThisNode = OpenQueue.HeapExtractMin();
                    CloseList.Add(ThisNode);
                    for (int i = 0; i < 4; ++i)
                    {
                        JCAstarNode TempChild = new JCAstarNode(new Point(ThisNode.NodeLoction.X + MyPlusPoint[i].X, ThisNode.NodeLoction.Y + MyPlusPoint[i].Y));
                        if (currentMap.canGo(TempChild.NodeLoction))//如果可以走
                        {
                            TempChild.Pluscost = currentMap.map[TempChild.NodeLoction.X,TempChild.NodeLoction.Y];
                            if (TempChild.NodeLoction == destination) //已经搜索到
                            {
                                TempChild.GExactCost = ThisNode.GExactCost + TempChild.Pluscost;
                                TempChild.Parent = ThisNode;
                                ThisNode = TempChild;
                                break;
                            }

                            TempChild.GExactCost = ThisNode.GExactCost + TempChild.Pluscost;
                            TempChild.HeuristicCost = 1*(int)MathTools.MDistance(TempChild.NodeLoction, destination);
                            TempChild.FTotalCost = TempChild.GExactCost + TempChild.HeuristicCost;
                            TempChild.Parent = ThisNode;

                            if (CloseList.Contains(TempChild.NodeLoction))
                            {
                                if (CloseList[(CloseList.InQueue[TempChild.NodeLoction.X, TempChild.NodeLoction.Y]) - 1].GExactCost > TempChild.GExactCost)
                                {
                                    CloseList.Remove((CloseList.InQueue[TempChild.NodeLoction.X, TempChild.NodeLoction.Y]) - 1, destination);
                                    OpenQueue.MinHeapInsert(OpenQueue.Count, TempChild);
                                }
                                else
                                    continue;
                            }

                            if (OpenQueue.Contains(TempChild.NodeLoction))//在open 表中
                            {
                                if (OpenQueue[OpenQueue.InQueue[TempChild.NodeLoction.X, TempChild.NodeLoction.Y] - 1].GExactCost > TempChild.GExactCost)
                                {
                                    OpenQueue.MinHeapInsert(OpenQueue.InQueue[TempChild.NodeLoction.X, TempChild.NodeLoction.Y] - 1, TempChild);
                                }
                                else continue;
                            }
                            else
                                OpenQueue.MinHeapInsert(OpenQueue.Count, TempChild);//不在open插入open 表中                        
                        }
                        else continue;

                    }

                }
            }
            while (!ways.Contains(origin))
            {
                if (ThisNode == null)
                {
                    break;
                }
                ways.Push(ThisNode.NodeLoction);
                ThisNode = ThisNode.Parent;
            }
            ways.Pop();
            return ways;
          }
     }
}