public List <PathFinderNode> FindPath(Point start, Point end, out int totalCost)
{
totalCost = 0;
mFound = false;
mCloseNodeCounter = 0;
if (mOpenNodeValue > 250)
{
Array.Clear(mCalcGrid, 0, mCalcGrid.Length);
mOpenNodeValue = 1;
mCloseNodeValue = 2;
}
else
{
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)
{
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 & mGridXMinus1);
mLocationY = (ushort)(mLocation >> mGridYLog2);
//Lets calculate each successors
int maxi = mDiagonals ? 8 : 4;
for (int i = 0; i < maxi; i++)
{
mNewLocationX = (mLocationX == 0 && mDirection [i, 0] < 0) ? (ushort)(mGridX - 1) : (ushort)(mLocationX + mDirection [i, 0]);
mNewLocationY = (ushort)(mLocationY + mDirection [i, 1]);
if (mNewLocationY >= mGridY)
{
continue;
}
if (mNewLocationX >= mGridX)
{
mNewLocationX = 0;
}
// Unbreakeable?
mNewLocation = (mNewLocationY << mGridYLog2) + mNewLocationX;
int gridValue = (mGrid [mNewLocation] & mGridBit) > 0 ? 1 : 0;
if (gridValue == 0)
{
continue;
}
// Check custom validator
if (mCustomCosts != null)
{
int customValue = mCustomCosts [mNewLocation];
if (mOnCellCross != null && customValue < 0)
{
customValue = mOnCellCross(mNewLocation);
}
if (customValue == 0)
{
continue;
}
else if (customValue < 0)
{
customValue = 0;
}
gridValue += customValue;
}
if (mHeavyDiagonals && i > 3)
{
mNewG = mCalcGrid [mLocation].G + (int)(gridValue * 2.41f);
}
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);
dist = Math.Min(dist, mGridX - dist);
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:
Point dxy = new Point(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;
}
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 << mGridYLog2) + 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 << 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);
return(mClose);
}
return(null);
}
public List <PathFinderNode> FindPath(Point start, Point end, out float totalCost)
{
totalCost = 0;
mFound = false;
mCloseNodeCounter = 0;
if (mOpenNodeValue > 250)
{
Array.Clear(mCalcGrid, 0, mCalcGrid.Length);
mOpenNodeValue = 1;
mCloseNodeValue = 2;
}
else
{
mOpenNodeValue += 2;
mCloseNodeValue += 2;
}
mOpen.Clear();
mClose.Clear();
callNumber++;
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
for (int i = 0; i < 6; i++)
{
int cellSide;
if (mLocationX % 2 == 0)
{
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];
}
if (mNewLocationY >= mGridY)
{
continue;
}
if (mNewLocationX >= mGridX)
{
continue;
}
// Unbreakeable?
mNewLocation = (mNewLocationY * mGridX) + mNewLocationX;
if (mGrid[mNewLocation].isBlocked)
{
continue;
}
if (mGrid[mNewLocation].altitude <mMinAltitude || mGrid[mNewLocation].altitude> mMaxAltitude)
{
continue;
}
if (mTerrainCapability != TERRAIN_CAPABILITY.Any)
{
bool isWater = mGrid[mNewLocation].isWater;
if (mTerrainCapability == TERRAIN_CAPABILITY.OnlyGround)
{
if (isWater)
{
continue;
}
}
else
{
if (!isWater)
{
continue;
}
}
}
float gridValue = 1;
float[] sideCosts = mGrid[mLocation].crossCost;
if (sideCosts != null)
{
gridValue = sideCosts[cellSide];
}
// Check custom validator
if (mOnCellCross != null)
{
if (mGrid[mNewLocation].cachedCallNumber != callNumber)
{
mGrid[mNewLocation].cachedCallNumber = callNumber;
mGrid[mNewLocation].cachedEventCostValue = mOnCellCross(mNewLocation);
}
gridValue += mGrid[mNewLocation].cachedEventCostValue;
}
if (gridValue <= 0)
{
gridValue = 1;
}
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:
float h_diagonal = Math.Min(dist, Math.Abs(mNewLocationY - end.Y));
float 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 * Mathf.Sqrt(Mathf.Pow(dist, 2) + Mathf.Pow((mNewLocationY - end.Y), 2));
break;
case HeuristicFormula.EuclideanNoSQR:
mH = mHEstimate * (Mathf.Pow(dist, 2) + Mathf.Pow((mNewLocationY - end.Y), 2));
break;
case HeuristicFormula.Custom1:
Point dxy = new Point(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[mEndLocation];
totalCost = fNodeTmp.G;
mGrid[mEndLocation].lastPathFindingCost = totalCost;
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;
int loc = (posY * mGridX) + posX;
fNodeTmp = mCalcGrid[loc];
fNode.F = fNodeTmp.F;
fNode.G = fNodeTmp.G;
mGrid[loc].lastPathFindingCost = 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);
}
