Esempio n. 1
0
        public List <PathFinderNode> FindPath(IVec3 start, IVec3 end)
        {
            lock (this)
            {
                HighResolutionTime.Start();

                // Is faster if we don't clear the matrix, just assign different values for open and close and ignore the rest
                // I could have user Array.Clear() but using unsafe code is faster, no much but it is.
                //fixed (PathFinderNodeFast* pGrid = tmpGrid)
                //    ZeroMemory((byte*) pGrid, sizeof(PathFinderNodeFast) * 1000000);

                mFound            = false;
                mStop             = false;
                mStopped          = false;
                mCloseNodeCounter = 0;
                mOpenNodeValue   += 2;
                mCloseNodeValue  += 2;
                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

                mLocation                   = (start.y << mGridYLog2) + start.x;
                mEndLocation                = (end.y << mGridYLog2) + end.x;
                mCalcGrid[mLocation].G      = 0;
                mCalcGrid[mLocation].F      = mHEstimate;
                mCalcGrid[mLocation].PX     = (ushort)start.x;
                mCalcGrid[mLocation].PY     = (ushort)start.y;
                mCalcGrid[mLocation].Status = mOpenNodeValue;

                mOpen.Push(mLocation);
                while (mOpen.Count > 0 && !mStop)
                {
                    mLocation = mOpen.Pop();

                    //Is it in closed list? means this node was already processed
                    if (mCalcGrid[mLocation].Status == mCloseNodeValue)
                    {
                        continue;
                    }

                    mLocationX = (ushort)(mLocation & mGridXMinus1);
                    mLocationY = (ushort)(mLocation >> mGridYLog2);

                    #if DEBUGON
                    if (mDebugProgress && PathFinderDebug != null)
                    {
                        PathFinderDebug(0, 0, mLocation & mGridXMinus1, mLocation >> mGridYLog2, PathFinderNodeType.Current, -1, -1);
                    }
                    #endif

                    if (mLocation == mEndLocation)
                    {
                        mCalcGrid[mLocation].Status = mCloseNodeValue;
                        mFound = true;
                        break;
                    }

                    if (mCloseNodeCounter > mSearchLimit)
                    {
                        mStopped       = true;
                        mCompletedTime = HighResolutionTime.GetTime();
                        return(null);
                    }

                    if (mPunishChangeDirection)
                    {
                        mHoriz = (mLocationX - mCalcGrid[mLocation].PX);
                    }

                    //Lets calculate each successors
                    for (int i = 0; i < (mDiagonals ? 8 : 4); i++)
                    {
                        mNewLocationX = (ushort)(mLocationX + mDirection[i, 0]);
                        mNewLocationY = (ushort)(mLocationY + mDirection[i, 1]);
                        mNewLocation  = (mNewLocationY << mGridYLog2) + mNewLocationX;

                        if (mNewLocationX >= mGridX || mNewLocationY >= mGridY)
                        {
                            continue;
                        }

                        // Unbreakeable?
                        if (mGrid[mNewLocationX, mNewLocationY] == 0)
                        {
                            continue;
                        }

                        if (mHeavyDiagonals && i > 3)
                        {
                            mNewG = mCalcGrid[mLocation].G + (int)(mGrid[mNewLocationX, mNewLocationY] * 2.41);
                        }
                        else
                        {
                            mNewG = mCalcGrid[mLocation].G + mGrid[mNewLocationX, mNewLocationY];
                        }

                        if (mPunishChangeDirection)
                        {
                            if ((mNewLocationX - mLocationX) != 0)
                            {
                                if (mHoriz == 0)
                                {
                                    mNewG += Math.Abs(mNewLocationX - end.x) + Math.Abs(mNewLocationY - end.y);
                                }
                            }
                            if ((mNewLocationY - mLocationY) != 0)
                            {
                                if (mHoriz != 0)
                                {
                                    mNewG += Math.Abs(mNewLocationX - end.x) + Math.Abs(mNewLocationY - end.y);
                                }
                            }
                        }

                        //Is it open or closed?
                        if (mCalcGrid[mNewLocation].Status == mOpenNodeValue || mCalcGrid[mNewLocation].Status == mCloseNodeValue)
                        {
                            // The current node has less code than the previous? then skip this node
                            if (mCalcGrid[mNewLocation].G <= mNewG)
                            {
                                continue;
                            }
                        }

                        mCalcGrid[mNewLocation].PX = mLocationX;
                        mCalcGrid[mNewLocation].PY = mLocationY;
                        mCalcGrid[mNewLocation].G  = mNewG;

                        switch (mFormula)
                        {
                        default:
                        case HeuristicFormula.Manhattan:
                            mH = mHEstimate * (Math.Abs(mNewLocationX - end.x) + Math.Abs(mNewLocationY - end.y));
                            break;

                        case HeuristicFormula.MaxDXDY:
                            mH = mHEstimate * (Math.Max(Math.Abs(mNewLocationX - end.x), Math.Abs(mNewLocationY - end.y)));
                            break;

                        case HeuristicFormula.DiagonalShortCut:
                            int h_diagonal = Math.Min(Math.Abs(mNewLocationX - end.x), Math.Abs(mNewLocationY - end.y));
                            int h_straight = (Math.Abs(mNewLocationX - end.x) + Math.Abs(mNewLocationY - end.y));
                            mH = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal);
                            break;

                        case HeuristicFormula.Euclidean:
                            mH = (int)(mHEstimate * Math.Sqrt(Math.Pow((mNewLocationY - end.x), 2) + Math.Pow((mNewLocationY - end.y), 2)));
                            break;

                        case HeuristicFormula.EuclideanNoSQR:
                            mH = (int)(mHEstimate * (Math.Pow((mNewLocationX - end.x), 2) + Math.Pow((mNewLocationY - end.y), 2)));
                            break;

                        case HeuristicFormula.Custom1:
                            IVec3 dxy = new IVec3();
                            dxy.Set(Math.Abs(end.x - mNewLocationX), Math.Abs(end.y - mNewLocationY), 0);     //### fix z parameter
                            int Orthogonal = Math.Abs(dxy.x - dxy.y);
                            int Diagonal   = Math.Abs(((dxy.x + dxy.y) - Orthogonal) / 2);
                            mH = mHEstimate * (Diagonal + Orthogonal + dxy.x + dxy.y);
                            break;
                        }
                        if (mTieBreaker)
                        {
                            int dx1   = mLocationX - end.x;
                            int dy1   = mLocationY - end.y;
                            int dx2   = start.x - end.x;
                            int dy2   = start.y - end.y;
                            int cross = Math.Abs(dx1 * dy2 - dx2 * dy1);
                            mH = (int)(mH + cross * 0.001);
                        }
                        mCalcGrid[mNewLocation].F = mNewG + mH;

                        #if DEBUGON
                        if (mDebugProgress && PathFinderDebug != null)
                        {
                            PathFinderDebug(mLocationX, mLocationY, mNewLocationX, mNewLocationY, PathFinderNodeType.Open, mCalcGrid[mNewLocation].F, mCalcGrid[mNewLocation].G);
                        }
                        #endif

                        //It is faster if we leave the open node in the priority queue
                        //When it is removed, it will be already closed, it will be ignored automatically
                        //if (tmpGrid[newLocation].Status == 1)
                        //{
                        //    //int removeX   = newLocation & gridXMinus1;
                        //    //int removeY   = newLocation >> gridYLog2;
                        //    mOpen.RemoveLocation(newLocation);
                        //}

                        //if (tmpGrid[newLocation].Status != 1)
                        //{
                        mOpen.Push(mNewLocation);
                        //}
                        mCalcGrid[mNewLocation].Status = mOpenNodeValue;
                    }

                    mCloseNodeCounter++;
                    mCalcGrid[mLocation].Status = mCloseNodeValue;

                    #if DEBUGON
                    if (mDebugProgress && PathFinderDebug != null)
                    {
                        PathFinderDebug(0, 0, mLocationX, mLocationY, PathFinderNodeType.Close, mCalcGrid[mLocation].F, mCalcGrid[mLocation].G);
                    }
                    #endif
                }

                mCompletedTime = HighResolutionTime.GetTime();
                if (mFound)
                {
                    mClose.Clear();
                    int posX = end.x;
                    int posY = end.y;
                    int posZ = end.z;

                    PathFinderNodeFast fNodeTmp = mCalcGrid[(end.y << mGridYLog2) + end.x];
                    PathFinderNode     fNode;
                    fNode.F  = fNodeTmp.F;
                    fNode.G  = fNodeTmp.G;
                    fNode.H  = 0;
                    fNode.PX = fNodeTmp.PX;
                    fNode.PY = fNodeTmp.PY;
                    fNode.PZ = fNodeTmp.PZ;
                    fNode.X  = end.x;
                    fNode.Y  = end.y;
                    fNode.Z  = end.z;

                    while (fNode.X != fNode.PX || fNode.Y != fNode.PY)
                    {
                        mClose.Add(fNode);
                        #if DEBUGON
                        if (mDebugFoundPath && PathFinderDebug != null)
                        {
                            PathFinderDebug(fNode.PX, fNode.PY, fNode.X, fNode.Y, PathFinderNodeType.Path, fNode.F, fNode.G);
                        }
                        #endif
                        posX     = fNode.PX;
                        posY     = fNode.PY;
                        posZ     = fNode.PZ;
                        fNodeTmp = mCalcGrid[(posY << mGridYLog2) + posX];
                        fNode.F  = fNodeTmp.F;
                        fNode.G  = fNodeTmp.G;
                        fNode.H  = 0;
                        fNode.PX = fNodeTmp.PX;
                        fNode.PY = fNodeTmp.PY;
                        fNode.PZ = fNodeTmp.PZ;
                        fNode.X  = posX;
                        fNode.Y  = posY;
                        fNode.Z  = posZ;     //### fix for pathfinder to work in Z
                    }

                    mClose.Add(fNode);
                    #if DEBUGON
                    if (mDebugFoundPath && PathFinderDebug != null)
                    {
                        PathFinderDebug(fNode.PX, fNode.PY, fNode.X, fNode.Y, PathFinderNodeType.Path, fNode.F, fNode.G);
                    }
                    #endif

                    mStopped = true;
                    return(mClose);
                }
                mStopped = true;
                return(null);
            }
        }
Esempio n. 2
0
        public List <PathFinderNode> FindPath(IVec3 start, IVec3 end)
        {
            Profiler.Start("Pathfinder.FindPath");

            HighResolutionTime.Start();

            PathFinderNode parentNode;
            bool           found = false;
            int            gridX = mGrid.GetGridWidth();
            int            gridY = mGrid.GetGridHeight();

            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.Z  = start.z;
            parentNode.PX = parentNode.X;
            parentNode.PY = parentNode.Y;
            parentNode.PZ = parentNode.Z;
            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;
                    Profiler.Stop("Pathfinder.FindPath");
                    return(null);
                }

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

                //Lets calculate each successors
                for (int i = 0; i < (mDiagonals ? 8 : 4); i++)
                {
                    PathFinderNode newNode = new PathFinderNode();
                    newNode.X = parentNode.X + direction[i, 0];
                    newNode.Y = parentNode.Y + direction[i, 1];
                    newNode.Z = 0;  //### FIX FOR PATHFINDING TO WORK IN Z

                    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.GetBlockedValue(newNode) * 2.41);
                    }
                    else
                    {
                        newG = parentNode.G + mGrid.GetBlockedValue(newNode);
                    }


                    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.PZ = parentNode.Z;
                    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:
                        IVec3 dxy = new IVec3();
                        dxy.Set(Math.Abs(end.x - newNode.X), Math.Abs(end.y - newNode.Y), 0);       //### fix z
                        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;
                mClose.RemoveAt(0); // we have duplicate path information
                PathFinderNode endPFN = new PathFinderNode();
                endPFN.PX = end.x;
                endPFN.PY = end.y;
                mClose.Add(endPFN);
                Profiler.Stop("Pathfinder.FindPath");
                return(mClose);
            }
            mStopped = true;
            Profiler.Stop("Pathfinder.FindPath");
            return(null);
        }