示例#1
0
        public List <PathFinderNode> FindPath(Point start, Point end)
        {
            Stopwatch sw = Stopwatch.StartNew();

            PathFinderNode parentNode;
            bool           found = false;
            int            gridX = mGrid.GetUpperBound(0);
            int            gridY = mGrid.GetUpperBound(1);

            mStop    = false;
            mStopped = false;
            mOpen.Clear();
            mClose.Clear();
            #if DEBUGON
            if (mDebugProgress && PathFinderDebug != null)
            {
                PathFinderDebug(0, 0, start.X, start.Y, PathFinderNodeType.Start, -1, -1);
            }
            if (mDebugProgress && PathFinderDebug != null)
            {
                PathFinderDebug(0, 0, end.X, end.Y, PathFinderNodeType.End, -1, -1);
            }
            #endif

            sbyte[,] direction;
            if (mDiagonals)
            {
                direction = new sbyte[8, 2] {
                    { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }, { 1, -1 }, { 1, 1 }, { -1, 1 }, { -1, -1 }
                }
            }
            ;
            else
            {
                direction = new sbyte[4, 2] {
                    { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }
                }
            };

            parentNode.G  = 0;
            parentNode.H  = mHEstimate;
            parentNode.F  = parentNode.G + parentNode.H;
            parentNode.X  = start.X;
            parentNode.Y  = start.Y;
            parentNode.PX = parentNode.X;
            parentNode.PY = parentNode.Y;
            mOpen.Push(parentNode);
            while (mOpen.Count > 0 && !mStop)
            {
                parentNode = mOpen.Pop();

                #if DEBUGON
                if (mDebugProgress && PathFinderDebug != null)
                {
                    PathFinderDebug(0, 0, parentNode.X, parentNode.Y, PathFinderNodeType.Current, -1, -1);
                }
                //Console.WriteLine("{0},{1}",parentNode.X,parentNode.Y);
                #endif

                if (parentNode.X == end.X && parentNode.Y == end.Y)
                {
                    mClose.Add(parentNode);
                    found = true;
                    Console.WriteLine("Endpoint found.");
                    break;
                }

                if (mClose.Count > mSearchLimit)
                {
                    mStopped = true;
                    Console.WriteLine("Too many nodes, aborting.");
                    found      = true;
                    mLastPoint = new Point(parentNode.X, parentNode.Y);
                    break;
                }

                if (mPunishChangeDirection)
                {
                    mHoriz = (parentNode.X - parentNode.PX);
                }

                //Lets calculate each successors
                for (int i = 0; i < (mDiagonals ? 8 : 4); i++)
                {
                    PathFinderNode newNode;
                    newNode.X = parentNode.X + (direction[i, 0] * mDist);
                    newNode.Y = parentNode.Y + (direction[i, 1] * mDist);

                    if (newNode.X < 0 || newNode.Y < 0 || newNode.X >= gridX || newNode.Y >= gridY)
                    {
                        continue;
                    }

                    int newG;
                    if (mHeavyDiagonals && i > 3)
                    {
                        newG = parentNode.G + (int)(Convert.ToSingle(mGrid[newNode.X, newNode.Y]) * 512f);
                    }
                    else
                    {
                        newG = parentNode.G + (int)(Convert.ToSingle(mGrid[newNode.X, newNode.Y]) * 128f);
                    }

                    if (newG == parentNode.G)
                    {
                        //Unbrekeable
                        //Console.Write("s");
                        continue;
                    }

                    if (mClose.Count % 1000 == 0)
                    {
                        Console.CursorLeft = 0;
                        Console.Write(" Current Point is ({0},{1}) with a cost of {2}.", newNode.X, newNode.Y, newG);
                    }

                    if (mPunishChangeDirection)
                    {
                        if ((newNode.X - parentNode.X) != 0)
                        {
                            if (mHoriz == 0)
                            {
                                newG += 20;
                            }
                        }
                        if ((newNode.Y - parentNode.Y) != 0)
                        {
                            if (mHoriz != 0)
                            {
                                newG += 20;
                            }
                        }
                    }

                    int foundInOpenIndex = -1;
                    for (int j = 0; j < mOpen.Count; j++)
                    {
                        if (mOpen[j].X == newNode.X && mOpen[j].Y == newNode.Y)
                        {
                            foundInOpenIndex = j;
                            break;
                        }
                    }
                    if (foundInOpenIndex != -1 && mOpen[foundInOpenIndex].G <= newG)
                    {
                        continue;
                    }

                    int foundInCloseIndex = -1;
                    for (int j = 0; j < mClose.Count; j++)
                    {
                        if (mClose[j].X == newNode.X && mClose[j].Y == newNode.Y)
                        {
                            foundInCloseIndex = j;
                            break;
                        }
                    }
                    if (foundInCloseIndex != -1 && mClose[foundInCloseIndex].G <= newG)
                    {
                        continue;
                    }

                    newNode.PX = parentNode.X;
                    newNode.PY = parentNode.Y;
                    newNode.G  = newG;

                    switch (mFormula)
                    {
                    default:
                    case HeuristicFormula.Manhattan:
                        newNode.H = mHEstimate * (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y));
                        break;

                    case HeuristicFormula.MaxDXDY:
                        newNode.H = mHEstimate * (Math.Max(Math.Abs(newNode.X - end.X), Math.Abs(newNode.Y - end.Y)));
                        break;

                    case HeuristicFormula.DiagonalShortCut:
                        int h_diagonal = Math.Min(Math.Abs(newNode.X - end.X), Math.Abs(newNode.Y - end.Y));
                        int h_straight = (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y));
                        newNode.H = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal);
                        break;

                    case HeuristicFormula.Euclidean:
                        newNode.H = (int)(mHEstimate * Math.Sqrt(Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2)));
                        break;

                    case HeuristicFormula.EuclideanNoSQR:
                        newNode.H = (int)(mHEstimate * (Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2)));
                        break;

                    case HeuristicFormula.Custom1:
                        Point dxy        = new Point(Math.Abs(end.X - newNode.X), Math.Abs(end.Y - newNode.Y));
                        int   Orthogonal = Math.Abs(dxy.X - dxy.Y);
                        int   Diagonal   = Math.Abs(((dxy.X + dxy.Y) - Orthogonal) / 2);
                        newNode.H = mHEstimate * (Diagonal + Orthogonal + dxy.X + dxy.Y);
                        break;
                        //case HeuristicFormula.River:
                        // Need to figure this out
                        //break;
                    }
                    if (mTieBreaker)
                    {
                        int dx1   = parentNode.X - end.X;
                        int dy1   = parentNode.Y - end.Y;
                        int dx2   = start.X - end.X;
                        int dy2   = start.Y - end.Y;
                        int cross = Math.Abs(dx1 * dy2 - dx2 * dy1);
                        newNode.H = (int)(newNode.H + cross * 0.001);
                    }
                    newNode.F = newNode.G + newNode.H;

                    #if DEBUGON
                    if (mDebugProgress && PathFinderDebug != null)
                    {
                        PathFinderDebug(parentNode.X, parentNode.Y, newNode.X, newNode.Y, PathFinderNodeType.Open, newNode.F, newNode.G);
                    }
                    #endif


                    //It is faster if we leave the open node in the priority queue
                    //When it is removed, all nodes around will be closed, it will be ignored automatically
                    //if (foundInOpenIndex != -1)
                    //    mOpen.RemoveAt(foundInOpenIndex);

                    //if (foundInOpenIndex == -1)
                    mOpen.Push(newNode);
                }

                mClose.Add(parentNode);

                #if DEBUGON
                if (mDebugProgress && PathFinderDebug != null)
                {
                    PathFinderDebug(0, 0, parentNode.X, parentNode.Y, PathFinderNodeType.Close, parentNode.F, parentNode.G);
                }
                #endif
            }
            sw.Stop();
            mCompletedTime = (double)sw.Elapsed.TotalMilliseconds;
            if (found)
            {
                Console.WriteLine("Cleaning up...");
                PathFinderNode fNode = mClose[mClose.Count - 1];
                for (int i = mClose.Count - 1; i >= 0; i--)
                {
                    if (fNode.PX == mClose[i].X && fNode.PY == mClose[i].Y || i == mClose.Count - 1)
                    {
                        #if DEBUGON
                        if (mDebugFoundPath && PathFinderDebug != null)
                        {
                            PathFinderDebug(fNode.X, fNode.Y, mClose[i].X, mClose[i].Y, PathFinderNodeType.Path, mClose[i].F, mClose[i].G);
                        }
                        #endif
                        fNode = mClose[i];
                        Console.Write(".");
                    }
                    else
                    {
                        mClose.RemoveAt(i);
                        Console.Write("-");
                    }
                }
                mStopped = true;
                return(mClose);
            }
            mStopped = true;
            return(null);
        }
示例#2
0
        public List <PathFinderNode> FindPath(Vector start, Vector end)
        {
            // HighResolutionTime.Start();

            PathFinderNode parentNode;
            bool           found = false;
            int            gridX = mGrid.GetUpperBound(0);
            int            gridY = mGrid.GetUpperBound(1);

            mStop    = false;
            mStopped = false;
            mOpen.Clear();
            mClose.Clear();

            #if DEBUGON
            if (mDebugProgress && PathFinderDebug != null)
            {
                PathFinderDebug(0, 0, start.x, start.y, PathFinderNodeType.Start, -1, -1);
            }
            if (mDebugProgress && PathFinderDebug != null)
            {
                PathFinderDebug(0, 0, end.x, end.y, PathFinderNodeType.End, -1, -1);
            }
            #endif

            sbyte[,] direction;
            if (mDiagonals)
            {
                direction = new sbyte[8, 2] {
                    { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }, { 1, -1 }, { 1, 1 }, { -1, 1 }, { -1, -1 }
                }
            }
            ;
            else
            {
                direction = new sbyte[4, 2] {
                    { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }
                }
            };

            parentNode.G  = 0;
            parentNode.H  = mHEstimate;
            parentNode.F  = parentNode.G + parentNode.H;
            parentNode.X  = start.x;
            parentNode.Y  = start.y;
            parentNode.PX = parentNode.X;
            parentNode.PY = parentNode.Y;
            mOpen.Push(parentNode);
            while (mOpen.Count > 0 && !mStop)
            {
                parentNode = mOpen.Pop();

                #if DEBUGON
                if (mDebugProgress && PathFinderDebug != null)
                {
                    PathFinderDebug(0, 0, parentNode.X, parentNode.Y, PathFinderNodeType.Current, -1, -1);
                }
                #endif

                if (parentNode.X == end.x && parentNode.Y == end.y)
                {
                    mClose.Add(parentNode);
                    found = true;
                    break;
                }

                if (mClose.Count > mSearchLimit)
                {
                    mStopped = true;
                    return(null);
                }

                if (mPunishChangeDirection)
                {
                    mHoriz = (parentNode.X - parentNode.PX);
                }

                //Lets calculate each successors
                for (int i = 0; i < (mDiagonals ? 8 : 4); i++)
                {
                    PathFinderNode newNode;
                    newNode.X = parentNode.X + direction[i, 0];
                    newNode.Y = parentNode.Y + direction[i, 1];

                    if (newNode.X < 0 || newNode.Y < 0 || newNode.X > gridX || newNode.Y > gridY)
                    {
                        continue;
                    }

                    int newG;
                    if (mHeavyDiagonals && i > 3)
                    {
                        newG = parentNode.G + (int)(mGrid[newNode.X, newNode.Y] * 2.41);
                    }
                    else
                    {
                        newG = parentNode.G + mGrid[newNode.X, newNode.Y];
                    }


                    if (newG == parentNode.G)
                    {
                        //Unbrekeable
                        continue;
                    }

                    if (mPunishChangeDirection)
                    {
                        if ((newNode.X - parentNode.X) != 0)
                        {
                            if (mHoriz == 0)
                            {
                                newG += 20;
                            }
                        }
                        if ((newNode.Y - parentNode.Y) != 0)
                        {
                            if (mHoriz != 0)
                            {
                                newG += 20;
                            }
                        }
                    }

                    int foundInOpenIndex = -1;
                    for (int j = 0; j < mOpen.Count; j++)
                    {
                        if (mOpen[j].X == newNode.X && mOpen[j].Y == newNode.Y)
                        {
                            foundInOpenIndex = j;
                            break;
                        }
                    }
                    if (foundInOpenIndex != -1 && mOpen[foundInOpenIndex].G <= newG)
                    {
                        continue;
                    }

                    int foundInCloseIndex = -1;
                    for (int j = 0; j < mClose.Count; j++)
                    {
                        if (mClose[j].X == newNode.X && mClose[j].Y == newNode.Y)
                        {
                            foundInCloseIndex = j;
                            break;
                        }
                    }
                    if (foundInCloseIndex != -1 && mClose[foundInCloseIndex].G <= newG)
                    {
                        continue;
                    }

                    newNode.PX = parentNode.X;
                    newNode.PY = parentNode.Y;
                    newNode.G  = newG;

                    switch (mFormula)
                    {
                    default:
                    case HeuristicFormula.Manhattan:
                        newNode.H = mHEstimate * (Math.Abs(newNode.X - end.x) + Math.Abs(newNode.Y - end.y));
                        break;

                    case HeuristicFormula.MaxDXDY:
                        newNode.H = mHEstimate * (Math.Max(Math.Abs(newNode.X - end.x), Math.Abs(newNode.Y - end.y)));
                        break;

                    case HeuristicFormula.DiagonalShortCut:
                        int h_diagonal = Math.Min(Math.Abs(newNode.X - end.x), Math.Abs(newNode.Y - end.y));
                        int h_straight = (Math.Abs(newNode.X - end.x) + Math.Abs(newNode.Y - end.y));
                        newNode.H = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal);
                        break;

                    case HeuristicFormula.Euclidean:
                        newNode.H = (int)(mHEstimate * Math.Sqrt(Math.Pow((newNode.X - end.x), 2) + Math.Pow((newNode.Y - end.y), 2)));
                        break;

                    case HeuristicFormula.EuclideanNoSQR:
                        newNode.H = (int)(mHEstimate * (Math.Pow((newNode.X - end.x), 2) + Math.Pow((newNode.Y - end.y), 2)));
                        break;

                    case HeuristicFormula.Custom1:
                        Vector dxy        = new Vector(Math.Abs(end.x - newNode.X), Math.Abs(end.y - newNode.Y));
                        int    Orthogonal = Math.Abs(dxy.x - dxy.y);
                        int    Diagonal   = Math.Abs(((dxy.x + dxy.y) - Orthogonal) / 2);
                        newNode.H = mHEstimate * (Diagonal + Orthogonal + dxy.x + dxy.y);
                        break;
                    }
                    if (mTieBreaker)
                    {
                        int dx1   = parentNode.X - end.x;
                        int dy1   = parentNode.Y - end.y;
                        int dx2   = start.x - end.x;
                        int dy2   = start.y - end.y;
                        int cross = Math.Abs(dx1 * dy2 - dx2 * dy1);
                        newNode.H = (int)(newNode.H + cross * 0.001);
                    }
                    newNode.F = newNode.G + newNode.H;

                    #if DEBUGON
                    if (mDebugProgress && PathFinderDebug != null)
                    {
                        PathFinderDebug(parentNode.X, parentNode.Y, newNode.X, newNode.Y, PathFinderNodeType.Open, newNode.F, newNode.G);
                    }
                    #endif


                    //It is faster if we leave the open node in the priority queue
                    //When it is removed, all nodes around will be closed, it will be ignored automatically
                    //if (foundInOpenIndex != -1)
                    //    mOpen.RemoveAt(foundInOpenIndex);

                    //if (foundInOpenIndex == -1)
                    mOpen.Push(newNode);
                }

                mClose.Add(parentNode);

                #if DEBUGON
                if (mDebugProgress && PathFinderDebug != null)
                {
                    PathFinderDebug(0, 0, parentNode.X, parentNode.Y, PathFinderNodeType.Close, parentNode.F, parentNode.G);
                }
                #endif
            }

            // mCompletedTime = HighResolutionTime.GetTime();
            if (found)
            {
                PathFinderNode fNode = mClose[mClose.Count - 1];
                for (int i = mClose.Count - 1; i >= 0; i--)
                {
                    if (fNode.PX == mClose[i].X && fNode.PY == mClose[i].Y || i == mClose.Count - 1)
                    {
                        #if DEBUGON
                        if (mDebugFoundPath && PathFinderDebug != null)
                        {
                            PathFinderDebug(fNode.X, fNode.Y, mClose[i].X, mClose[i].Y, PathFinderNodeType.Path, mClose[i].F, mClose[i].G);
                        }
                        #endif
                        fNode = mClose[i];
                    }
                    else
                    {
                        mClose.RemoveAt(i);
                    }
                }
                mStopped = true;
                return(mClose);
            }
            mStopped = true;
            return(null);
        }
示例#3
0
        public List <Microsoft.Xna.Framework.Point> FindPath(Microsoft.Xna.Framework.Point start, Microsoft.Xna.Framework.Point end)
        {
            PathFinderNode parentNode;
            bool           found = false;
            int            gridX = mGrid.GetUpperBound(0);
            int            gridY = mGrid.GetUpperBound(1);

            mStop    = false;
            mStopped = false;
            mOpen.Clear();
            mClose.Clear();

#if DEBUGON
            if (mDebugProgress && PathFinderDebug != null)
            {
                PathFinderDebug(0, 0, start.X, start.Y, PathFinderNodeType.Start, -1, -1);
            }
            if (mDebugProgress && PathFinderDebug != null)
            {
                PathFinderDebug(0, 0, end.X, end.Y, PathFinderNodeType.End, -1, -1);
            }
#endif

            sbyte[,] direction;
            if (mDiagonals)
            {
                direction = new sbyte[8, 2] {
                    { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }, { 1, -1 }, { 1, 1 }, { -1, 1 }, { -1, -1 }
                }
            }
            ;
            else
            {
                direction = new sbyte[4, 2] {
                    { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }
                }
            };

            parentNode.G  = 0;
            parentNode.H  = mHEstimate;
            parentNode.F  = parentNode.G + parentNode.H;
            parentNode.X  = start.X;
            parentNode.Y  = start.Y;
            parentNode.PX = parentNode.X;
            parentNode.PY = parentNode.Y;
            mOpen.Push(parentNode);
            while (mOpen.Count > 0 && !mStop)
            {
                parentNode = mOpen.Pop();

#if DEBUGON
                if (mDebugProgress && PathFinderDebug != null)
                {
                    PathFinderDebug(0, 0, parentNode.X, parentNode.Y, PathFinderNodeType.Current, -1, -1);
                }
#endif

                if (parentNode.X == end.X && parentNode.Y == end.Y)
                {
                    mClose.Add(parentNode);
                    found = true;
                    break;
                }

                if (mClose.Count > mSearchLimit)
                {
                    mStopped = true;
                    return(null);
                }

                if (mPunishChangeDirection)
                {
                    mHoriz = (parentNode.X - parentNode.PX);
                }


                for (int i = 0; i < (mDiagonals ? 8 : 4); i++)
                {
                    PathFinderNode newNode;
                    newNode.X = parentNode.X + direction[i, 0];
                    newNode.Y = parentNode.Y + direction[i, 1];

                    if (newNode.X < 0 || newNode.Y < 0 || newNode.X >= gridX || newNode.Y >= gridY)
                    {
                        continue;
                    }

                    int newG;
                    if (mHeavyDiagonals && i > 3)
                    {
                        newG = parentNode.G + (int)(mGrid[newNode.X, newNode.Y] * 2.41);
                    }
                    else
                    {
                        newG = parentNode.G + mGrid[newNode.X, newNode.Y];
                    }


                    if (newG == parentNode.G)
                    {
                        continue;
                    }

                    if (mPunishChangeDirection)
                    {
                        if ((newNode.X - parentNode.X) != 0)
                        {
                            if (mHoriz == 0)
                            {
                                newG += 20;
                            }
                        }
                        if ((newNode.Y - parentNode.Y) != 0)
                        {
                            if (mHoriz != 0)
                            {
                                newG += 20;
                            }
                        }
                    }

                    int foundInOpenIndex = -1;
                    for (int j = 0; j < mOpen.Count; j++)
                    {
                        if (mOpen[j].X == newNode.X && mOpen[j].Y == newNode.Y)
                        {
                            foundInOpenIndex = j;
                            break;
                        }
                    }
                    if (foundInOpenIndex != -1 && mOpen[foundInOpenIndex].G <= newG)
                    {
                        continue;
                    }

                    int foundInCloseIndex = -1;
                    for (int j = 0; j < mClose.Count; j++)
                    {
                        if (mClose[j].X == newNode.X && mClose[j].Y == newNode.Y)
                        {
                            foundInCloseIndex = j;
                            break;
                        }
                    }
                    if (foundInCloseIndex != -1 && mClose[foundInCloseIndex].G <= newG)
                    {
                        continue;
                    }

                    newNode.PX = parentNode.X;
                    newNode.PY = parentNode.Y;
                    newNode.G  = newG;

                    switch (mFormula)
                    {
                    default:
                    case HeuristicFormula.Manhattan:
                        newNode.H = mHEstimate * (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y));
                        break;

                    case HeuristicFormula.MaxDXDY:
                        newNode.H = mHEstimate * (Math.Max(Math.Abs(newNode.X - end.X), Math.Abs(newNode.Y - end.Y)));
                        break;

                    case HeuristicFormula.DiagonalShortCut:
                        int h_diagonal = Math.Min(Math.Abs(newNode.X - end.X), Math.Abs(newNode.Y - end.Y));
                        int h_straight = (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y));
                        newNode.H = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal);
                        break;

                    case HeuristicFormula.Euclidean:
                        newNode.H = (int)(mHEstimate * Math.Sqrt(Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2)));
                        break;

                    case HeuristicFormula.EuclideanNoSQR:
                        newNode.H = (int)(mHEstimate * (Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2)));
                        break;

                    case HeuristicFormula.Custom1:
                        Point dxy        = new Point(Math.Abs(end.X - newNode.X), Math.Abs(end.Y - newNode.Y));
                        int   Orthogonal = Math.Abs(dxy.X - dxy.Y);
                        int   Diagonal   = Math.Abs(((dxy.X + dxy.Y) - Orthogonal) / 2);
                        newNode.H = mHEstimate * (Diagonal + Orthogonal + dxy.X + dxy.Y);
                        break;
                    }
                    if (mTieBreaker)
                    {
                        int dx1   = parentNode.X - end.X;
                        int dy1   = parentNode.Y - end.Y;
                        int dx2   = start.X - end.X;
                        int dy2   = start.Y - end.Y;
                        int cross = Math.Abs(dx1 * dy2 - dx2 * dy1);
                        newNode.H = (int)(newNode.H + cross * 0.001);
                    }
                    newNode.F = newNode.G + newNode.H;

#if DEBUGON
                    if (mDebugProgress && PathFinderDebug != null)
                    {
                        PathFinderDebug(parentNode.X, parentNode.Y, newNode.X, newNode.Y, PathFinderNodeType.Open, newNode.F, newNode.G);
                    }
#endif



                    mOpen.Push(newNode);
                }

                mClose.Add(parentNode);

#if DEBUGON
                if (mDebugProgress && PathFinderDebug != null)
                {
                    PathFinderDebug(0, 0, parentNode.X, parentNode.Y, PathFinderNodeType.Close, parentNode.F, parentNode.G);
                }
#endif
            }

            if (found)
            {
                PathFinderNode fNode = mClose[mClose.Count - 1];
                for (int i = mClose.Count - 1; i >= 0; i--)
                {
                    if (fNode.PX == mClose[i].X && fNode.PY == mClose[i].Y || i == mClose.Count - 1)
                    {
#if DEBUGON
                        if (mDebugFoundPath && PathFinderDebug != null)
                        {
                            PathFinderDebug(fNode.X, fNode.Y, mClose[i].X, mClose[i].Y, PathFinderNodeType.Path, mClose[i].F, mClose[i].G);
                        }
#endif
                        fNode = mClose[i];
                    }
                    else
                    {
                        mClose.RemoveAt(i);
                    }
                }
                mStopped = true;
                List <Microsoft.Xna.Framework.Point> copy = new List <Microsoft.Xna.Framework.Point>();
                foreach (PathFinderNode node in mClose)
                {
                    copy.Add(new Microsoft.Xna.Framework.Point(node.X, node.Y));
                }

                return(copy);
            }
            mStopped = true;
            return(null);
        }
示例#4
0
        public List <PathFinderNode> FindPath(Point start, Point end)
        {
            HighResolutionTime.Start();

            PathFinderNode parentNode;
            bool           found = false;
            int            gridX = mGrid.GetUpperBound(0);
            int            gridY = mGrid.GetUpperBound(1);

            mStop    = false;
            mStopped = false;
            mOpen.Clear();
            mClose.Clear();

#if DEBUGON
            if (mDebugProgress && PathFinderDebug != null)
            {
                PathFinderDebug(0, 0, start.X, start.Y, PathFinderNodeType.Start, -1, -1);
            }
            if (mDebugProgress && PathFinderDebug != null)
            {
                PathFinderDebug(0, 0, end.X, end.Y, PathFinderNodeType.End, -1, -1);
            }
#endif

            sbyte[,] direction;
            if (mDiagonals)
            {
                direction = new sbyte[8, 2] {
                    { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }, { 1, -1 }, { 1, 1 }, { -1, 1 }, { -1, -1 }
                }
            }
            ;
            else
            {
                direction = new sbyte[4, 2] {
                    { 0, -1 }, { 1, 0 }, { 0, 1 }, { -1, 0 }
                }
            };

            parentNode.G  = 0;
            parentNode.H  = mHEstimate;
            parentNode.F  = parentNode.G + parentNode.H;
            parentNode.X  = start.X;
            parentNode.Y  = start.Y;
            parentNode.PX = parentNode.X;
            parentNode.PY = parentNode.Y;
            mOpen.Push(parentNode);
            while (mOpen.Count > 0 && !mStop)
            {
                parentNode = mOpen.Pop();

#if DEBUGON
                if (mDebugProgress && PathFinderDebug != null)
                {
                    PathFinderDebug(0, 0, parentNode.X, parentNode.Y, PathFinderNodeType.Current, -1, -1);
                }
#endif

                if (parentNode.X == end.X && parentNode.Y == end.Y) //找到end
                {
                    mClose.Add(parentNode);
                    found = true;
                    break;
                }

                if (mClose.Count > mSearchLimit) // 超出计算限制
                {
                    mStopped       = true;
                    mCompletedTime = HighResolutionTime.GetTime();
                    return(null);
                }

                if (mPunishChangeDirection) // 改变方向是否惩罚
                {
                    mHoriz = (parentNode.X - parentNode.PX);
                }

                //Lets calculate each successors
                //搜索子节点
                for (int i = 0; i < (mDiagonals ? 8 : 4); i++)
                {
                    PathFinderNode newNode;
                    newNode.X = parentNode.X + direction[i, 0];
                    newNode.Y = parentNode.Y + direction[i, 1];

                    if (newNode.X < 0 || newNode.Y < 0 || newNode.X >= gridX || newNode.Y >= gridY)
                    {
                        continue;
                    }

                    #region G
                    int newG;
                    if (mHeavyDiagonals && i > 3)
                    {
                        newG = parentNode.G + (int)(mGrid[newNode.X, newNode.Y] * 2.41);
                    }
                    else
                    {
                        newG = parentNode.G + mGrid[newNode.X, newNode.Y];
                    }


                    if (newG == parentNode.G) // when weight == X = 0
                    {
                        //Unbrekeable
                        continue;
                    }

                    if (mPunishChangeDirection)
                    {
                        if ((newNode.X - parentNode.X) != 0) // x 方向变化
                        {
                            if (mHoriz == 0)
                            {
                                newG += 20;
                            }
                        }
                        if ((newNode.Y - parentNode.Y) != 0) // y 方向变化
                        {
                            if (mHoriz != 0)
                            {
                                newG += 20;
                            }
                        }
                    }

                    // 负权路径会反复来回,形成死循环
                    // 因此 权重需大于0
                    // 因此 newG 必然大于第一次加入open或close时的 G ,避免了回溯
                    // 因此 每个节点只会被计算一次
                    int foundInOpenIndex = -1;
                    for (int j = 0; j < mOpen.Count; j++)
                    {
                        if (mOpen[j].X == newNode.X && mOpen[j].Y == newNode.Y)
                        {
                            foundInOpenIndex = j;
                            break;
                        }
                    }
                    if (foundInOpenIndex != -1 && mOpen[foundInOpenIndex].G <= newG) // 条件必然成立
                    {
                        continue;                                                    // 如果 newNode 在 open 集合并且不小于G,则跳过 newNode
                    }
                    int foundInCloseIndex = -1;
                    for (int j = 0; j < mClose.Count; j++)
                    {
                        if (mClose[j].X == newNode.X && mClose[j].Y == newNode.Y)
                        {
                            foundInCloseIndex = j;
                            break;
                        }
                    }
                    if (foundInCloseIndex != -1 && mClose[foundInCloseIndex].G <= newG) // 条件必然成立
                    {
                        continue;                                                       // 如果 newNode 在 close 集合并且不小于G,则跳过 newNode
                    }
                    // 更新 newNode 节点
                    newNode.PX = parentNode.X;
                    newNode.PY = parentNode.Y;
                    newNode.G  = newG;
                    #endregion

                    #region H
                    switch (mFormula)
                    {
                    default:
                    case HeuristicFormula.Manhattan:
                        newNode.H = mHEstimate * (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y));
                        break;

                    case HeuristicFormula.MaxDXDY:
                        newNode.H = mHEstimate * (Math.Max(Math.Abs(newNode.X - end.X), Math.Abs(newNode.Y - end.Y)));
                        break;

                    case HeuristicFormula.DiagonalShortCut:
                        int h_diagonal = Math.Min(Math.Abs(newNode.X - end.X), Math.Abs(newNode.Y - end.Y));
                        int h_straight = (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y));
                        newNode.H = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal);
                        break;

                    case HeuristicFormula.Euclidean:
                        newNode.H = (int)(mHEstimate * Math.Sqrt(Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2)));
                        break;

                    case HeuristicFormula.EuclideanNoSQR:
                        newNode.H = (int)(mHEstimate * (Math.Pow((newNode.X - end.X), 2) + Math.Pow((newNode.Y - end.Y), 2)));
                        break;

                    case HeuristicFormula.Custom1:
                        Point dxy        = new Point(Math.Abs(end.X - newNode.X), Math.Abs(end.Y - newNode.Y));
                        int   Orthogonal = Math.Abs(dxy.X - dxy.Y);
                        int   Diagonal   = Math.Abs(((dxy.X + dxy.Y) - Orthogonal) / 2);
                        newNode.H = mHEstimate * (Diagonal + Orthogonal + dxy.X + dxy.Y);
                        break;
                    }
                    if (mTieBreaker)
                    {
                        int dx1   = parentNode.X - end.X;
                        int dy1   = parentNode.Y - end.Y;
                        int dx2   = start.X - end.X;
                        int dy2   = start.Y - end.Y;
                        int cross = Math.Abs(dx1 * dy2 - dx2 * dy1);
                        newNode.H = (int)(newNode.H + cross * 0.001);
                    }
                    #endregion

                    newNode.F = newNode.G + newNode.H;

#if DEBUGON
                    if (mDebugProgress && PathFinderDebug != null)
                    {
                        PathFinderDebug(parentNode.X, parentNode.Y, newNode.X, newNode.Y, PathFinderNodeType.Open, newNode.F, newNode.G);
                    }
#endif


                    //It is faster if we leave the open node in the priority queue
                    //When it is removed, all nodes around will be closed, it will be ignored automatically
                    //if (foundInOpenIndex != -1)
                    //    mOpen.RemoveAt(foundInOpenIndex);

                    //if (foundInOpenIndex == -1)
                    mOpen.Push(newNode); // 添加 open 节点
                }

                mClose.Add(parentNode); // 已经搜索过的 open 节点加入 close

#if DEBUGON
                if (mDebugProgress && PathFinderDebug != null)
                {
                    PathFinderDebug(0, 0, parentNode.X, parentNode.Y, PathFinderNodeType.Close, parentNode.F, parentNode.G);
                }
#endif
            }

            mCompletedTime = HighResolutionTime.GetTime();
            if (found)
            {
                // 倒推路径节点
                PathFinderNode fNode = mClose[mClose.Count - 1];
                for (int i = mClose.Count - 1; i >= 0; i--)
                {
                    if (fNode.PX == mClose[i].X && fNode.PY == mClose[i].Y || i == mClose.Count - 1)
                    {
#if DEBUGON
                        if (mDebugFoundPath && PathFinderDebug != null)
                        {
                            PathFinderDebug(fNode.X, fNode.Y, mClose[i].X, mClose[i].Y, PathFinderNodeType.Path, mClose[i].F, mClose[i].G);
                        }
#endif
                        fNode = mClose[i];
                    }
                    else
                    {
                        mClose.RemoveAt(i);
                    }
                }

                mStopped = true;
                return(mClose);
            }

            mStopped = true;
            return(null);
        }