Astar PathFinding设置墙壁

时间:2018-07-03 10:04:18

标签: c# path-finding

正在学习Astar算法,试图从书籍中应用伪代码,目前尚无法理解-如何设置墙?我在矩阵中的对象的值小于-1,自由单元格的值为-1,边界为int.MaxValue。在检查当前Point周围的邻居时是否应该检查?还是有一种更简单的方法来使用这种算法?

public List<PathFinderNode> FindPath(Point start, Point end)
    {
        PathFinderNode parentNode;
        bool found  = false;
        int  gridX  = mGrid[0].Count;
        int  gridY  = mGrid.Count;
        mStop       = false;
        mStopped    = false;
        mOpen.Clear();
        mClose.Clear();

        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 (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.Y][newNode.X] * 2.41);
                else
                    newG = parentNode.G + mGrid[newNode.Y][newNode.X];


                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;
                newNode.H       = mHEstimate * (Math.Abs(newNode.X - end.X) + Math.Abs(newNode.Y - end.Y));

                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;

                    mOpen.Push(newNode);
            }

            mClose.Add(parentNode);

        }

        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)
                {
                    fNode = mClose[i];
                }
                else
                    mClose.RemoveAt(i);
            }
            mStopped = true;
            return mClose;
        }
        mStopped = true;
        return null;
    }

0 个答案:

没有答案