public List <PathFinderNode> FindPath(PathFindingPoint start, PathFindingPoint end, out float totalCost, bool evenLayout)
{
totalCost = 0;
mFound = false;
mCloseNodeCounter = 0;
int evenLayoutValue = evenLayout ? 1 : 0;
if (mOpenNodeValue > 250)
{
Array.Clear(mCalcGrid, 0, mCalcGrid.Length);
mOpenNodeValue = 1;
mCloseNodeValue = 2;
}
else
{
mOpenNodeValue += 2;
mCloseNodeValue += 2;
}
mOpen.Clear();
mClose.Clear();
float[] sideCosts = null;
mLocation = (start.Y * mGridX) + start.X;
mEndLocation = (end.Y * mGridX) + 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)
{
mLocation = mOpen.Pop();
//Is it in closed list? means this node was already processed
if (mCalcGrid [mLocation].Status == mCloseNodeValue)
{
continue;
}
if (mLocation == mEndLocation)
{
mCalcGrid [mLocation].Status = mCloseNodeValue;
mFound = true;
break;
}
if (mCloseNodeCounter > mMaxSteps)
{
return(null);
}
mLocationX = (ushort)(mLocation % mGridX);
mLocationY = (ushort)(mLocation / mGridX);
//Lets calculate each successors
int maxi;
if (mHexagonalGrid)
{
maxi = 6;
}
else
{
maxi = mDiagonals ? 8 : 4;
}
bool hasSideCosts = false;
sideCosts = mGrid [mLocation].crossCost;
if (sideCosts != null)
{
hasSideCosts = true;
}
for (int i = 0; i < maxi; i++)
{
int cellSide = 0;
if (mHexagonalGrid)
{
if (mLocationX % 2 == evenLayoutValue)
{
mNewLocationX = (ushort)(mLocationX + mDirectionHex0 [i, 0]);
mNewLocationY = (ushort)(mLocationY + mDirectionHex0 [i, 1]);
cellSide = mCellSide0 [i];
}
else
{
mNewLocationX = (ushort)(mLocationX + mDirectionHex1 [i, 0]);
mNewLocationY = (ushort)(mLocationY + mDirectionHex1 [i, 1]);
cellSide = mCellSide1 [i];
}
}
else
{
mNewLocationX = (ushort)(mLocationX + mDirection [i, 0]);
mNewLocationY = (ushort)(mLocationY + mDirection [i, 1]);
cellSide = mCellBoxSides [i];
}
if (mNewLocationY >= mGridY)
{
continue;
}
if (mNewLocationX >= mGridX)
{
continue;
}
// Unbreakeable?
mNewLocation = (mNewLocationY * mGridX) + mNewLocationX;
if (!mGrid [mNewLocation].canCross)
{
continue;
}
float gridValue = (mGrid [mNewLocation].group & mCellGroupMask) != 0 ? 1 : 0;
if (gridValue == 0)
{
continue;
}
if (hasSideCosts)
{
gridValue = sideCosts [cellSide];
if (gridValue <= 0)
{
gridValue = 1;
}
} // Check custom validator
if (mOnCellCross != null)
{
gridValue += mOnCellCross(mNewLocation);
}
if (!mHexagonalGrid && i > 3)
{
mNewG = mCalcGrid [mLocation].G + gridValue * mHeavyDiagonalsCost;
}
else
{
mNewG = mCalcGrid [mLocation].G + gridValue;
}
if (mNewG > mMaxSearchCost)
{
continue;
}
//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;
int dist = Math.Abs(mNewLocationX - end.X);
switch (mFormula)
{
default:
case HeuristicFormula.Manhattan:
mH = mHEstimate * (dist + Math.Abs(mNewLocationY - end.Y));
break;
case HeuristicFormula.MaxDXDY:
mH = mHEstimate * (Math.Max(dist, Math.Abs(mNewLocationY - end.Y)));
break;
case HeuristicFormula.DiagonalShortCut:
int h_diagonal = Math.Min(dist, Math.Abs(mNewLocationY - end.Y));
int h_straight = (dist + Math.Abs(mNewLocationY - end.Y));
mH = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal);
break;
case HeuristicFormula.Euclidean:
mH = mHEstimate * (float)(Math.Sqrt(Math.Pow(dist, 2) + Math.Pow((mNewLocationY - end.Y), 2)));
break;
case HeuristicFormula.EuclideanNoSQR:
mH = mHEstimate * (float)(Math.Pow(dist, 2) + Math.Pow((mNewLocationY - end.Y), 2));
break;
case HeuristicFormula.Custom1:
PathFindingPoint dxy = new PathFindingPoint(dist, Math.Abs(end.Y - mNewLocationY));
float Orthogonal = Math.Abs(dxy.X - dxy.Y);
float Diagonal = Math.Abs(((dxy.X + dxy.Y) - Orthogonal) / 2);
mH = mHEstimate * (Diagonal + Orthogonal + dxy.X + dxy.Y);
break;
}
mCalcGrid [mNewLocation].F = mNewG + mH;
mOpen.Push(mNewLocation);
mCalcGrid [mNewLocation].Status = mOpenNodeValue;
}
mCloseNodeCounter++;
mCalcGrid [mLocation].Status = mCloseNodeValue;
}
if (mFound)
{
mClose.Clear();
int posX = end.X;
int posY = end.Y;
PathFinderNodeFast fNodeTmp = mCalcGrid [(end.Y * mGridX) + end.X];
totalCost = fNodeTmp.G;
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);
posX = fNode.PX;
posY = fNode.PY;
fNodeTmp = mCalcGrid [(posY * mGridX) + 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);
return(mClose);
}
return(null);
}
public List <PathFinderNode> FindPath(PathFindingPoint start, PathFindingPoint 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();
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;
}
if (mLocation == mEndLocation)
{
mCalcGrid[mLocation].Status = mCloseNodeValue;
mFound = true;
break;
}
if (mCloseNodeCounter > mSearchLimit)
{
mStopped = true;
//mCompletedTime = HighResolutionTime.GetTime();
return(null);
}
mLocationX = (ushort)(mLocation & mGridXMinus1);
mLocationY = (ushort)(mLocation >> mGridYLog2);
// if (mPunishChangeDirection)
// mHoriz = (mLocationX - mCalcGrid[mLocation].PX);
//Lets calculate each successors
int maxi;
if (mHexagonalGrid)
{
maxi = mDiagonals ? 6 : 4;
}
else
{
maxi = mDiagonals ? 8 : 4;
}
for (int i = 0; i < maxi; i++)
{
if (mHexagonalGrid)
{
if (mLocationX % 2 == 0)
{
mNewLocationX = (ushort)(mLocationX + mDirectionHex0[i, 0]);
mNewLocationY = (ushort)(mLocationY + mDirectionHex0[i, 1]);
}
else
{
mNewLocationX = (ushort)(mLocationX + mDirectionHex1[i, 0]);
mNewLocationY = (ushort)(mLocationY + mDirectionHex1[i, 1]);
}
}
else
{
mNewLocationX = (ushort)(mLocationX + mDirection[i, 0]);
mNewLocationY = (ushort)(mLocationY + mDirection[i, 1]);
}
if (mNewLocationY >= mGridY)
{
continue;
}
if (mNewLocationX >= mGridX)
{
continue;
}
// Unbreakeable?
mNewLocation = (mNewLocationY << mGridYLog2) + mNewLocationX;
// int gridValue = (mGrid[mNewLocationX, mNewLocationY] & mGridBit) > 0 ? 1: 0;
int gridValue = mGrid[mNewLocation] > 0 ? 1: 0;
if (gridValue == 0)
{
continue;
}
// Check custom validator
if (mOnCellCross != null)
{
gridValue += mOnCellCross(mNewLocation);
}
if (mHeavyDiagonals && i > 3)
{
mNewG = mCalcGrid[mLocation].G + (int)(gridValue * 2.41f);
}
else
{
mNewG = mCalcGrid[mLocation].G + gridValue;
}
// 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;
int dist = Math.Abs(mNewLocationX - end.X);
switch (mFormula)
{
default:
case HeuristicFormula.Manhattan:
mH = mHEstimate * (dist + Math.Abs(mNewLocationY - end.Y));
break;
case HeuristicFormula.MaxDXDY:
mH = mHEstimate * (Math.Max(dist, Math.Abs(mNewLocationY - end.Y)));
break;
case HeuristicFormula.DiagonalShortCut:
int h_diagonal = Math.Min(dist, Math.Abs(mNewLocationY - end.Y));
int h_straight = (dist + 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(dist, 2) + Math.Pow((mNewLocationY - end.Y), 2)));
break;
case HeuristicFormula.EuclideanNoSQR:
mH = (int)(mHEstimate * (Math.Pow(dist, 2) + Math.Pow((mNewLocationY - end.Y), 2)));
break;
case HeuristicFormula.Custom1:
PathFindingPoint dxy = new PathFindingPoint(dist, 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.001f);
// }
mCalcGrid[mNewLocation].F = mNewG + mH;
//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;
}
//mCompletedTime = HighResolutionTime.GetTime();
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);
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);
mStopped = true;
return(mClose);
}
mStopped = true;
return(null);
// }
}
