Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- //
- // THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY
- // KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
- // IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR
- // PURPOSE. IT CAN BE DISTRIBUTED FREE OF CHARGE AS LONG AS THIS HEADER
- // REMAINS UNCHANGED.
- //
- // Email: gustavo_franco@hotmail.com
- //
- // Copyright (C) 2006 Franco, Gustavo
- //
- //#define DEBUGON
- using System;
- using System.Drawing;
- using System.Collections.Generic;
- using System.Runtime.InteropServices;
- namespace prjMango.Game.Pathfinding
- {
- public class PathFinderFast : IPathFinder
- {
- #region Structs
- [StructLayout(LayoutKind.Sequential, Pack=1)]
- internal struct PathFinderNodeFast
- {
- #region Variables Declaration
- public int F; // f = gone + heuristic
- public int G;
- public ushort PX; // Parent
- public ushort PY;
- public byte Status;
- #endregion
- }
- #endregion
- #region Events
- public event PathFinderDebugHandler PathFinderDebug;
- #endregion
- #region Variables Declaration
- // Heap variables are initializated to default, but I like to do it anyway
- private byte[,] mGrid = null;
- private PriorityQueueB<int> mOpen = null;
- private List<PathFinderNode> mClose = new List<PathFinderNode>();
- private bool mStop = false;
- private bool mStopped = true;
- private int mHoriz = 0;
- private HeuristicFormula mFormula = HeuristicFormula.DiagonalShortCut;
- private bool mDiagonals = true;
- private int mHEstimate = 2;
- private bool mPunishChangeDirection = false;
- private bool mTieBreaker = false;
- private bool mHeavyDiagonals = true;
- private int mSearchLimit = 2000;
- private double mCompletedTime = 0;
- private bool mDebugProgress = false;
- private bool mDebugFoundPath = false;
- private PathFinderNodeFast[] mCalcGrid = null;
- private byte mOpenNodeValue = 1;
- private byte mCloseNodeValue = 2;
- //Promoted local variables to member variables to avoid recreation between calls
- private int mH = 0;
- private int mLocation = 0;
- private int mNewLocation = 0;
- private ushort mLocationX = 0;
- private ushort mLocationY = 0;
- private ushort mNewLocationX = 0;
- private ushort mNewLocationY = 0;
- private int mCloseNodeCounter = 0;
- private ushort mGridX = 0;
- private ushort mGridY = 0;
- private ushort mGridXMinus1 = 0;
- private ushort mGridYLog2 = 0;
- private bool mFound = false;
- private sbyte[,] mDirection = new sbyte[8,2]{{0,-1} , {1,0}, {0,1}, {-1,0}, {1,-1}, {1,1}, {-1,1}, {-1,-1}};
- private int mEndLocation = 0;
- private int mNewG = 0;
- #endregion
- #region Constructors
- public PathFinderFast(byte[,] grid)
- {
- if (grid == null)
- throw new Exception("Grid cannot be null");
- mGrid = grid;
- mGridX = (ushort) (mGrid.GetUpperBound(0) + 1);
- mGridY = (ushort) (mGrid.GetUpperBound(1) + 1);
- mGridXMinus1 = (ushort) (mGridX - 1);
- mGridYLog2 = (ushort) Math.Log(mGridY, 2);
- // This should be done at the constructor, for now we leave it here.
- if (Math.Log(mGridX, 2) != (int) Math.Log(mGridX, 2) ||
- Math.Log(mGridY, 2) != (int) Math.Log(mGridY, 2))
- throw new Exception("Invalid Grid, size in X and Y must be power of 2");
- if (mCalcGrid == null || mCalcGrid.Length != (mGridX * mGridY))
- mCalcGrid = new PathFinderNodeFast[mGridX * mGridY];
- mOpen = new PriorityQueueB<int>(new ComparePFNodeMatrix(mCalcGrid));
- }
- #endregion
- #region Properties
- public bool Stopped
- {
- get { return mStopped; }
- }
- public HeuristicFormula Formula
- {
- get { return mFormula; }
- set { mFormula = value; }
- }
- public bool Diagonals
- {
- get { return mDiagonals; }
- set
- {
- mDiagonals = value;
- if (mDiagonals)
- mDirection = new sbyte[8,2]{{0,-1} , {1,0}, {0,1}, {-1,0}, {1,-1}, {1,1}, {-1,1}, {-1,-1}};
- else
- mDirection = new sbyte[4,2]{{0,-1} , {1,0}, {0,1}, {-1,0}};
- }
- }
- public bool HeavyDiagonals
- {
- get { return mHeavyDiagonals; }
- set { mHeavyDiagonals = value; }
- }
- public int HeuristicEstimate
- {
- get { return mHEstimate; }
- set { mHEstimate = value; }
- }
- public bool PunishChangeDirection
- {
- get { return mPunishChangeDirection; }
- set { mPunishChangeDirection = value; }
- }
- public bool TieBreaker
- {
- get { return mTieBreaker; }
- set { mTieBreaker = value; }
- }
- public int SearchLimit
- {
- get { return mSearchLimit; }
- set { mSearchLimit = value; }
- }
- public double CompletedTime
- {
- get { return mCompletedTime; }
- set { mCompletedTime = value; }
- }
- public bool DebugProgress
- {
- get { return mDebugProgress; }
- set { mDebugProgress = value; }
- }
- public bool DebugFoundPath
- {
- get { return mDebugFoundPath; }
- set { mDebugFoundPath = value; }
- }
- #endregion
- #region Methods
- public void FindPathStop()
- {
- mStop = true;
- }
- public List<PathFinderNode> FindPath(Point start, Point end)
- {
- lock(this)
- {
- // Timer here maybe?
- // 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;
- // Get time here (maybe?)
- 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:
- Point dxy = new Point(Math.Abs(end.X - mNewLocationX), Math.Abs(end.Y - mNewLocationY));
- 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
- }
- // Get time here (maybe?)
- if (mFound)
- {
- mClose.Clear();
- int posX = end.X;
- int posY = end.Y;
- 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.X = end.X;
- fNode.Y = end.Y;
- 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;
- 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.X = posX;
- fNode.Y = posY;
- }
- 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;
- }
- }
- #endregion
- #region Inner Classes
- internal class ComparePFNodeMatrix : IComparer<int>
- {
- #region Variables Declaration
- PathFinderNodeFast[] mMatrix;
- #endregion
- #region Constructors
- public ComparePFNodeMatrix(PathFinderNodeFast[] matrix)
- {
- mMatrix = matrix;
- }
- #endregion
- #region IComparer Members
- public int Compare(int a, int b)
- {
- if (mMatrix[a].F > mMatrix[b].F)
- return 1;
- else if (mMatrix[a].F < mMatrix[b].F)
- return -1;
- return 0;
- }
- #endregion
- }
- #endregion
- }
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement