public PathFinderNodeFast UpdateStatus(byte newStatus)
{
PathFinderNodeFast newNode = this;
newNode.Status = newStatus;
return(newNode);
}
private PawnPath FinalizedPath(int finalIndex)
{
var newPath = this.map.pawnPathPool.GetEmptyPawnPath();
int parentIndex = finalIndex;
#if DEBUG
int prevKnownCost = calcGrid[finalIndex].knownCost;
int actualCost = 0;
#endif
while (true)
{
PathFinderNodeFast pathFinderNodeFast = calcGrid[parentIndex];
int newParentIndex = pathFinderNodeFast.parentIndex;
newPath.AddNode(map.cellIndices.IndexToCell(parentIndex));
#if DEBUG
actualCost += prevKnownCost - pathFinderNodeFast.knownCost;
prevKnownCost = pathFinderNodeFast.knownCost;
var hDiscrepancy = actualCost - pathFinderNodeFast.heuristicCost;
DebugFlash(map.cellIndices.IndexToCell(parentIndex), hDiscrepancy / 100f, "\n\n" /*+ actualCost + "\n"*/ + hDiscrepancy);
#endif
if (parentIndex == newParentIndex)
{
break;
}
parentIndex = newParentIndex;
}
newPath.SetupFound(calcGrid[curIndex].knownCost);
PfProfilerEndSample();
return(newPath);
}
private PawnPath FinalizedPath(int finalIndex, bool usedRegionHeuristics)
{
PawnPath emptyPawnPath = map.pawnPathPool.GetEmptyPawnPath();
int num = finalIndex;
while (true)
{
PathFinderNodeFast pathFinderNodeFast = calcGrid[num];
int parentIndex = pathFinderNodeFast.parentIndex;
emptyPawnPath.AddNode(map.cellIndices.IndexToCell(num));
if (num == parentIndex)
{
break;
}
num = parentIndex;
}
emptyPawnPath.SetupFound((float)calcGrid[finalIndex].knownCost, usedRegionHeuristics);
return(emptyPawnPath);
}
private PawnPath FinalizedPath(int finalIndex)
{
PawnPath emptyPawnPath = this.map.pawnPathPool.GetEmptyPawnPath();
int num = finalIndex;
while (true)
{
PathFinderNodeFast pathFinderNodeFast = this.calcGrid[num];
int parentIndex = pathFinderNodeFast.parentIndex;
emptyPawnPath.AddNode(this.map.cellIndices.IndexToCell(num));
if (num != parentIndex)
{
num = parentIndex;
continue;
}
break;
}
emptyPawnPath.SetupFound((float)this.calcGrid[finalIndex].knownCost);
return(emptyPawnPath);
}
private WorldPath FinalizedPath(int lastTile)
{
WorldPath emptyWorldPath = Find.WorldPathPool.GetEmptyWorldPath();
int num = lastTile;
while (true)
{
PathFinderNodeFast pathFinderNodeFast = calcGrid[num];
int parentTile = pathFinderNodeFast.parentTile;
int num2 = num;
emptyWorldPath.AddNodeAtStart(num2);
if (num2 == parentTile)
{
break;
}
num = parentTile;
}
emptyWorldPath.SetupFound((float)calcGrid[lastTile].knownCost);
return(emptyWorldPath);
}
public List <Vector2i> FindPath(Vector2i start, Vector2i end, int characterWidth, int characterHeight, short maxCharacterJumpHeight, int numberOfTries)
{
lock (this)
{
while (touchedLocations.Count > 0)
{
nodes[touchedLocations.Pop()].Clear();
}
var inSolidTile = true;
//This starts at the bottom left and checks rightward for intervening ground nodes. If it's empty we're good. If it's not we move to the left a bit to try again
// for (var i = 0; i < characterWidth; ++i)
// {
// inSolidTile = false;
// for (var w = 0; w < characterWidth; ++w)
// {
// if ((end.x + w < 0 || end.x + w >= mLevel.mWidth) || mGrid[end.x + w, end.y] != null || mGrid[end.x + w, end.y + characterHeight - 1] != null)
// {
// inSolidTile = true;
// break;
// }
//
// }
// if (inSolidTile == false)
// {
// for (var h = 1; h < characterHeight - 1; ++h)
// {
// if ((end.y + h < 0 || end.y + h >= mLevel.mHeight) || mGrid[end.x, end.y + h] != null || mGrid[end.x + characterWidth - 1, end.y + h] != null)
// {
// inSolidTile = true;
// break;
// }
// }
// }
//
// //TODO: end.x -= characterWidth - 1 is going waaay to far to the left in order to get a valid end spot I think
// if (inSolidTile)
// end.x -= /*characterWidth -*/ 1;
// else
// break;
// }
//Checking if the Endpoint is too small to fit the character
//Trying to Check from character center outward (shifting left, then right 1 tile at a time
//If that fails then keep moving the endpoint up until you have a valid endpoint, or you hit the top of the map (Should probably limit this to a certain number of tries)
int halfWidth = Mathf.FloorToInt(characterWidth / 2);
int origX = end.x;
while (inSolidTile || end.y >= mLevel.mHeight - characterHeight)
{
for (var i = 1; i < characterWidth + 1; ++i)
{
inSolidTile = false;
for (var w = 0; w <= halfWidth; ++w)
{
if ((end.x + w < 0 || end.x + w >= mLevel.mWidth) || mGrid[end.x + w, end.y] != null || mGrid[end.x + w, end.y + characterHeight - 1] != null)
{
inSolidTile = true;
break;
}
if ((end.x - w < 0 || end.x - w >= mLevel.mWidth) || mGrid[end.x - w, end.y] != null || mGrid[end.x - w, end.y + characterHeight - 1] != null)
{
inSolidTile = true;
break;
}
}
if (inSolidTile == false)
{
for (var h = 1; h < characterHeight - 1; ++h)
{
if ((end.y + h < 0 || end.y + h >= mLevel.mHeight) || mGrid[end.x - halfWidth, end.y + h] != null || mGrid[end.x + halfWidth, end.y + h] != null)
{
inSolidTile = true;
break;
}
}
}
if (inSolidTile)
{
if (i % 2 == 0)
{
end.x = origX + (i / 2);
}
else
{
float iHalf = i / 2f;
end.x = origX - Mathf.CeilToInt(iHalf);
}
}
else
{
break;
}
}
//If we're still in a space too small. Move the endpoint up one and check there (Possibly extremely inefficient
if (inSolidTile)
{
end.y++;
}
}
//No actual space (I don't think it will ever hit this since it should keep checking upwards until it hits the top)
if (inSolidTile == true)
{
return(null);
}
mFound = false;
mStop = false;
mStopped = false;
mCloseNodeCounter = 0;
mOpenNodeValue += 2;
mCloseNodeValue += 2;
mOpen.Clear();
mLocation.xy = (start.y << mGridXLog2) + start.x;
mLocation.z = 0;
mEndLocation = (end.y << mGridXLog2) + end.x;
PathFinderNodeFast firstNode = new PathFinderNodeFast();
firstNode.G = 0;
firstNode.F = mHEstimate;
firstNode.PX = (ushort)start.x;
firstNode.PY = (ushort)start.y;
firstNode.PZ = 0;
firstNode.Status = mOpenNodeValue;
bool startsOnGround = false;
//TODO: Check character width from bottom center instead of bottom left
for (int x = start.x; x < start.x + characterWidth; ++x)
{
if (mLevel.IsGround(x, start.y - 1))
{
startsOnGround = true;
break;
}
}
if (startsOnGround)
{
firstNode.JumpLength = 0;
}
else
{
firstNode.JumpLength = (short)(maxCharacterJumpHeight * 2);
}
nodes[mLocation.xy].Add(firstNode);
touchedLocations.Push(mLocation.xy);
mOpen.Push(mLocation);
while (mOpen.Count > 0 && !mStop)
{
mLocation = mOpen.Pop();
//Is it in closed list? means this node was already processed
if (nodes[mLocation.xy][mLocation.z].Status == mCloseNodeValue)
{
continue;
}
mLocationX = (ushort)(mLocation.xy & mGridXMinus1);
mLocationY = (ushort)(mLocation.xy >> mGridXLog2);
if (mLocation.xy == mEndLocation)
{
nodes[mLocation.xy][mLocation.z] = nodes[mLocation.xy][mLocation.z].UpdateStatus(mCloseNodeValue);
mFound = true;
break;
}
//No path found
if (mCloseNodeCounter > mSearchLimit)
{
end = FindAlternativeEndPoint(start, end);
if (end == null)
{
mStopped = true;
return(null);
}
end = FindFirstOpenTileBelow(end);
if (end == null)
{
mStopped = true;
return(null);
}
if (numberOfTries >= 50)
{
mStopped = true;
return(null);
}
else
{
return(FindPath(start, end, characterWidth, characterHeight, maxCharacterJumpHeight, numberOfTries + 1));
}
}
//Lets calculate each successors
for (var i = 0; i < (mDiagonals ? 8 : 4); i++)
{
mNewLocationX = (ushort)(mLocationX + mDirection[i, 0]);
mNewLocationY = (ushort)(mLocationY + mDirection[i, 1]);
mNewLocation = (mNewLocationY << mGridXLog2) + mNewLocationX;
//Cut children loop early if child is out of bounds of maptile array
if (mNewLocationX < 0 || mNewLocationX >= mLevel.mWidth || mNewLocationY < 0 || mNewLocationY >= mLevel.mHeight)
{
goto CHILDREN_LOOP_END;
}
var onGround = false;
var atCeiling = false;
//TODO: Check character with from bottom center instead of bottom left
// for (var w = 0; w < characterWidth; ++w)
// {
// if (mGrid[mNewLocationX + w, mNewLocationY] != null
// || mGrid[mNewLocationX + w, mNewLocationY + characterHeight - 1] != null)
// goto CHILDREN_LOOP_END;
//
// if (mLevel.IsGround(mNewLocationX + w, mNewLocationY - 1))
// onGround = true;
// else if (mGrid[mNewLocationX + w, mNewLocationY + characterHeight] != null)
// atCeiling = true;
// }
// for (var h = 1; h < characterHeight - 1; ++h)
// {
// if (mGrid[mNewLocationX, mNewLocationY + h] != null
// || mGrid[mNewLocationX + characterWidth - 1, mNewLocationY + h] != null)
// goto CHILDREN_LOOP_END;
// }
//Trying to check from center out
try {
//int halfWidth = Mathf.FloorToInt(characterWidth/2);
for (var w = 0; w <= halfWidth; ++w)
{
if (mGrid[mNewLocationX + w, mNewLocationY] != null ||
mGrid[mNewLocationX + w, mNewLocationY + characterHeight - 1] != null)
{
goto CHILDREN_LOOP_END;
}
if (mLevel.IsGround(mNewLocationX + w, mNewLocationY - 1))
{
onGround = true;
}
else if (mGrid[mNewLocationX + w, mNewLocationY + characterHeight] != null)
{
atCeiling = true;
}
//Checking Other side of character center
if (mGrid[mNewLocationX - w, mNewLocationY] != null ||
mGrid[mNewLocationX - w, mNewLocationY + characterHeight - 1] != null)
{
goto CHILDREN_LOOP_END;
}
if (mLevel.IsGround(mNewLocationX - w, mNewLocationY - 1))
{
onGround = true;
}
else if (mGrid[mNewLocationX - w, mNewLocationY + characterHeight] != null)
{
atCeiling = true;
}
}
for (var h = 1; h < characterHeight - 1; ++h)
{
if (mGrid[mNewLocationX - halfWidth, mNewLocationY + h] != null ||
mGrid[mNewLocationX + halfWidth /* characterWidth - 1*/, mNewLocationY + h] != null)
{
goto CHILDREN_LOOP_END;
}
}
} catch (IndexOutOfRangeException e) {
//Index out of range exception just means we're checking past the map bounds. Ignore the proposed node
goto CHILDREN_LOOP_END;
}
//calculate a proper jumplength value for the successor
var jumpLength = nodes[mLocation.xy][mLocation.z].JumpLength;
short newJumpLength = jumpLength;
if (atCeiling)
{
if (mNewLocationX != mLocationX)
{
newJumpLength = (short)Mathf.Max(maxCharacterJumpHeight * 2 + 1, jumpLength + 1);
}
else
{
newJumpLength = (short)Mathf.Max(maxCharacterJumpHeight * 2, jumpLength + 2);
}
}
else if (onGround)
{
newJumpLength = 0;
}
else if (mNewLocationY > mLocationY)
{
if (jumpLength < 2) //first jump is always two block up instead of one up and optionally one to either right or left
{
newJumpLength = 3;
}
else if (jumpLength % 2 == 0)
{
newJumpLength = (short)(jumpLength + 2);
}
else
{
newJumpLength = (short)(jumpLength + 1);
}
}
else if (mNewLocationY < mLocationY)
{
if (jumpLength % 2 == 0)
{
newJumpLength = (short)Mathf.Max(maxCharacterJumpHeight * 2, jumpLength + 2);
}
else
{
newJumpLength = (short)Mathf.Max(maxCharacterJumpHeight * 2, jumpLength + 1);
}
}
else if (!onGround && mNewLocationX != mLocationX)
{
newJumpLength = (short)(jumpLength + 1);
}
if (jumpLength >= 0 && jumpLength % 2 != 0 && mLocationX != mNewLocationX)
{
continue;
}
//if we're falling and successor's height is bigger than ours, skip that successor
if (jumpLength >= maxCharacterJumpHeight * 2 && mNewLocationY > mLocationY)
{
continue;
}
if (newJumpLength >= maxCharacterJumpHeight * 2 + 6 && mNewLocationX != mLocationX && (newJumpLength - (maxCharacterJumpHeight * 2 + 6)) % 8 != 3)
{
continue;
}
//The commented out part here would normally be "1" for empty space, but in this case it would be checking a null object so we'll go with this for now
//If Jumping is too prevalent (Or not enough) change the amount we divide the newJumpLength by here. 4 is the default from the algorithm but it sometimes makes the path pretty jumpy
//**NOTE** I changed the newJumpLength Division value to 2, it seemed to fix the issue where jumping all the time was the best path.
mNewG = nodes[mLocation.xy][mLocation.z].G + /*mGrid[mNewLocationX, mNewLocationY]*/ 1 + newJumpLength / 4;
if (nodes[mNewLocation].Count > 0)
{
int lowestJump = short.MaxValue;
bool couldMoveSideways = false;
for (int j = 0; j < nodes[mNewLocation].Count; ++j)
{
if (nodes[mNewLocation][j].JumpLength < lowestJump)
{
lowestJump = nodes[mNewLocation][j].JumpLength;
}
if (nodes[mNewLocation][j].JumpLength % 2 == 0 && nodes[mNewLocation][j].JumpLength < maxCharacterJumpHeight * 2 + 6)
{
couldMoveSideways = true;
}
}
if (lowestJump <= newJumpLength && (newJumpLength % 2 != 0 || newJumpLength >= maxCharacterJumpHeight * 2 + 6 || couldMoveSideways))
{
continue;
}
}
switch (mFormula)
{
default:
case HeuristicFormula.Manhattan:
mH = mHEstimate * (Mathf.Abs(mNewLocationX - end.x) + Mathf.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:
var h_diagonal = Math.Min(Math.Abs(mNewLocationX - end.x), Math.Abs(mNewLocationY - end.y));
var 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:
var dxy = new Vector2i(Math.Abs(end.x - mNewLocationX), Math.Abs(end.y - mNewLocationY));
var Orthogonal = Math.Abs(dxy.x - dxy.y);
var Diagonal = Math.Abs(((dxy.x + dxy.y) - Orthogonal) / 2);
mH = mHEstimate * (Diagonal + Orthogonal + dxy.x + dxy.y);
break;
}
PathFinderNodeFast newNode = new PathFinderNodeFast();
newNode.JumpLength = newJumpLength;
newNode.PX = mLocationX;
newNode.PY = mLocationY;
newNode.PZ = (byte)mLocation.z;
newNode.G = mNewG;
newNode.F = mNewG + mH;
newNode.Status = mOpenNodeValue;
if (nodes[mNewLocation].Count == 0)
{
touchedLocations.Push(mNewLocation);
}
nodes[mNewLocation].Add(newNode);
mOpen.Push(new Location(mNewLocation, nodes[mNewLocation].Count - 1));
CHILDREN_LOOP_END:
continue;
}
nodes[mLocation.xy][mLocation.z] = nodes[mLocation.xy][mLocation.z].UpdateStatus(mCloseNodeValue);
mCloseNodeCounter++;
}
if (mFound)
{
mClose.Clear();
var posX = end.x;
var posY = end.y;
var fPrevNodeTmp = new PathFinderNodeFast();
var fNodeTmp = nodes[mEndLocation][0];
var fNode = end;
var fPrevNode = end;
var loc = (fNodeTmp.PY << mGridXLog2) + fNodeTmp.PX;
while (fNode.x != fNodeTmp.PX || fNode.y != fNodeTmp.PY)
{
var fNextNodeTmp = nodes[loc][fNodeTmp.PZ];
if ((mClose.Count == 0) ||
(mLevel.IsOneWayPlatform(fNode.x, fNode.y - 1)) ||
(mGrid[fNode.x, fNode.y - 1] != null && mLevel.IsOneWayPlatform(fPrevNode.x, fPrevNode.y - 1)) ||
(fNodeTmp.JumpLength == 3) ||
(fNextNodeTmp.JumpLength != 0 && fNodeTmp.JumpLength == 0) || //mark jumps starts
(fNodeTmp.JumpLength == 0 && fPrevNodeTmp.JumpLength != 0) || //mark landings
(fNode.y > mClose[mClose.Count - 1].y && fNode.y > fNodeTmp.PY) ||
(fNode.y < mClose[mClose.Count - 1].y && fNode.y < fNodeTmp.PY) ||
((mLevel.IsGround(fNode.x - 1, fNode.y) || mLevel.IsGround(fNode.x + 1, fNode.y)) &&
fNode.y != mClose[mClose.Count - 1].y && fNode.x != mClose[mClose.Count - 1].x))
{
mClose.Add(fNode);
}
fPrevNode = fNode;
posX = fNodeTmp.PX;
posY = fNodeTmp.PY;
fPrevNodeTmp = fNodeTmp;
fNodeTmp = fNextNodeTmp;
loc = (fNodeTmp.PY << mGridXLog2) + fNodeTmp.PX;
fNode = new Vector2i(posX, posY);
}
mClose.Add(fNode);
mStopped = true;
//Debug.Log ("Number of Tries for successful path: " + numberOfTries);
return(mClose);
}
mStopped = true;
//TODO: Verify that finding an alternate end point path is the most efficient thing to do here. Seems like it could be slow
return(FindPath(start, FindAlternativeEndPoint(start, end), characterWidth, characterHeight, maxCharacterJumpHeight, numberOfTries + 1));
//return null
}
}
public List <PathFinderNode> FindPath(Point start, Point end, bool _ignoreObstacle)
{
lock (this)
{
mPerFormanceWatcher.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;
mCalcGridStatus[mLocation] = 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)
if (mCalcGridStatus[mLocation] == mCloseNodeValue)
{
continue;
}
mLocationX = (ushort)(mLocation & mGridXMinus1);
mLocationY = (ushort)(mLocation >> mGridYLog2);
if (mLocation == mEndLocation)
{
//mCalcGrid[mLocation].Status = mCloseNodeValue;
mCalcGridStatus[mLocation] = mCloseNodeValue;
mFound = true;
mPerFormanceWatcher.Record(mClose.Count);
break;
}
if (mCloseNodeCounter > mSearchLimit)
{
mStopped = true;
mCompletedTime = mPerFormanceWatcher.End();
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] == PathManager.GRID_VALUE_CANNOT)
{
continue;
}
if (_ignoreObstacle)
{
if (mHeavyDiagonals && i > 3)
{
mNewG = mCalcGrid[mLocation].G + PathManager.GRID_VALUE_GOOD + PathManager.GRID_ADD_VALUE_DIAGONAL;
}
else
{
mNewG = mCalcGrid[mLocation].G + PathManager.GRID_VALUE_GOOD;
}
}
else
{
if (mHeavyDiagonals && i > 3)
{
//mNewG = mCalcGrid[mLocation].G + (int) (mGrid[mNewLocationX, mNewLocationY] * 2.41);
mNewG = mCalcGrid[mLocation].G + mGrid[mNewLocationX, mNewLocationY] + PathManager.GRID_ADD_VALUE_DIAGONAL;
}
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)
if (mCalcGridStatus[mNewLocation] == mOpenNodeValue || mCalcGridStatus[mNewLocation] == 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));
mH = (Math.Abs(mNewLocationX - end.X) + Math.Abs(mNewLocationY - end.Y)) << mHEstimate;
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;
//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;
mCalcGridStatus[mNewLocation] = mOpenNodeValue;
}
mCloseNodeCounter++;
//mCalcGrid[mLocation].Status = mCloseNodeValue;
mCalcGridStatus[mLocation] = mCloseNodeValue;
}
mCompletedTime = mPerFormanceWatcher.End();
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;
//mCalcGrid = null;
return(mClose);
}
mStopped = true;
//mCalcGrid = null;
return(null);
}
}
public List <Vector2> FindPath(Vector2 startPixels, Vector2 endPixels, int characterWidth, int characterHeightPixels, short maxCharacterJumpHeight)
{
var end = endPixels / 20;
var start = startPixels / 20;
var characterHeight = characterHeightPixels / 20;
lock (this)
{
while (touchedLocations.Count > 0)
{
nodes[touchedLocations.Pop()].Clear();
}
//if (mGrid[(int)end.X, (int)end.Y] == 0)
// return null;
mFound = false;
mStop = false;
mStopped = false;
mCloseNodeCounter = 0;
mOpenNodeValue += 2;
mCloseNodeValue += 2;
mOpen.Clear();
mLocation.xy = ((int)start.Y << mGridX) + (int)start.X;
mLocation.z = 0;
mEndLocation = ((int)end.Y << mGridXLog2) + (int)end.X;
PathFinderNodeFast firstNode = new PathFinderNodeFast();
firstNode.G = 0;
firstNode.F = mHEstimate;
firstNode.PX = (ushort)start.X;
firstNode.PY = (ushort)start.Y;
firstNode.PZ = 0;
firstNode.Status = mOpenNodeValue;
if (mMap.IsGround((int)start.X, (int)start.Y - 1))
{
firstNode.JumpLength = 0;
}
else
{
firstNode.JumpLength = (short)(maxCharacterJumpHeight * 2);
}
nodes[mLocation.xy].Add(firstNode);
touchedLocations.Push(mLocation.xy);
mOpen.Push(mLocation);
while (mOpen.Count > 0 && !mStop)
{
mLocation = mOpen.Pop();
//Is it in closed list? means this node was already processed
if (nodes[mLocation.xy][mLocation.z].Status == mCloseNodeValue)
{
continue;
}
mLocationX = (ushort)(mLocation.xy & mGridXMinus1);
mLocationY = (ushort)(mLocation.xy >> mGridXLog2);
if (mLocation.xy == mEndLocation)
{
nodes[mLocation.xy][mLocation.z] = nodes[mLocation.xy][mLocation.z].UpdateStatus(mCloseNodeValue);
mFound = true;
break;
}
if (mCloseNodeCounter > mSearchLimit)
{
mStopped = true;
return(null);
}
//Lets calculate each successors
for (var i = 0; i < (mDiagonals ? 8 : 4); i++)
{
mNewLocationX = (ushort)(mLocationX + mDirection[i, 0]);
mNewLocationY = (ushort)(mLocationY + mDirection[i, 1]);
//if(mDirection[i, 1] == -1)
//{
// mNewLocationY = (ushort)(mLocationY + 1);
//}
mNewLocation = (mNewLocationY << mGridXLog2) + mNewLocationX;
var onGround = false;
var atCeiling = false;
//if (mNewLocationX == mGrid.GetLength(0)) mNewLocationX = (ushort)(mNewLocationX - 1);
//if (mNewLocationY == mGrid.GetLength(1)) mNewLocationY = (ushort)(mNewLocationY - 1);
if (mGrid[mNewLocationX, mNewLocationY] == 0)
{
goto CHILDREN_LOOP_END;
}
if (mMap.IsGround(mNewLocationX, mNewLocationY - 1))
{
onGround = true;
}
else if (mGrid[mNewLocationX, mNewLocationY + characterHeight] == 0)
{
atCeiling = true;
}
//calculate a proper jumplength value for the successor
var jumpLength = nodes[mLocation.xy][mLocation.z].JumpLength;
short newJumpLength = jumpLength;
if (atCeiling)
{
if (mNewLocationX != mLocationX)
{
newJumpLength = (short)Math.Max(maxCharacterJumpHeight * 2 + 1, jumpLength + 1);
}
else
{
newJumpLength = (short)Math.Max(maxCharacterJumpHeight * 2, jumpLength + 2);
}
}
else if (onGround)
{
newJumpLength = 0;
}
else if (mNewLocationY > mLocationY)
{
if (jumpLength < 2) //first jump is always two block up instead of one up and optionally one to either right or left
{
newJumpLength = 3;
}
else if (jumpLength % 2 == 0)
{
newJumpLength = (short)(jumpLength + 2);
}
else
{
newJumpLength = (short)(jumpLength + 1);
}
}
else if (mNewLocationY < mLocationY)
{
if (jumpLength % 2 == 0)
{
newJumpLength = (short)Math.Max(maxCharacterJumpHeight * 2, jumpLength + 2);
}
else
{
newJumpLength = (short)Math.Max(maxCharacterJumpHeight * 2, jumpLength + 1);
}
}
else if (!onGround && mNewLocationX != mLocationX)
{
newJumpLength = (short)(jumpLength + 1);
}
if (jumpLength >= 0 && jumpLength % 2 != 0 && mLocationX != mNewLocationX)
{
continue;
}
//if we're falling and succeor's height is bigger than ours, skip that successor
if (jumpLength >= maxCharacterJumpHeight * 2 && mNewLocationY > mLocationY)
{
continue;
}
if (newJumpLength >= maxCharacterJumpHeight * 2 + 6 && mNewLocationX != mLocationX && (newJumpLength - (maxCharacterJumpHeight * 2 + 6)) % 8 != 3)
{
continue;
}
mNewG = nodes[mLocation.xy][mLocation.z].G + mGrid[mNewLocationX, mNewLocationY] + newJumpLength / 4;
if (nodes[mNewLocation].Count > 0)
{
int lowestJump = short.MaxValue;
bool couldMoveSideways = false;
for (int j = 0; j < nodes[mNewLocation].Count; ++j)
{
if (nodes[mNewLocation][j].JumpLength < lowestJump)
{
lowestJump = nodes[mNewLocation][j].JumpLength;
}
if (nodes[mNewLocation][j].JumpLength % 2 == 0 && nodes[mNewLocation][j].JumpLength < maxCharacterJumpHeight * 2 + 6)
{
couldMoveSideways = true;
}
}
if (lowestJump <= newJumpLength && (newJumpLength % 2 != 0 || newJumpLength >= maxCharacterJumpHeight * 2 + 6 || couldMoveSideways))
{
continue;
}
}
switch (mFormula)
{
default:
case HeuristicFormula.Manhattan:
mH = mHEstimate * (Math.Abs(mNewLocationX - (int)end.X) + Math.Abs(mNewLocationY - (int)end.Y));
break;
case HeuristicFormula.MaxDXDY:
mH = mHEstimate * (Math.Max(Math.Abs(mNewLocationX - (int)end.X), Math.Abs(mNewLocationY - (int)end.Y)));
break;
case HeuristicFormula.DiagonalShortCut:
var h_diagonal = Math.Min(Math.Abs(mNewLocationX - (int)end.X), Math.Abs(mNewLocationY - (int)end.Y));
var h_straight = (Math.Abs(mNewLocationX - (int)end.X) + Math.Abs(mNewLocationY - (int)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 - (int)end.Y), 2)));
break;
case HeuristicFormula.EuclideanNoSQR:
mH = (int)(mHEstimate * (Math.Pow((mNewLocationX - end.X), 2) + Math.Pow((mNewLocationY - (int)end.Y), 2)));
break;
case HeuristicFormula.Custom1:
var dxy = new Vector2(Math.Abs(end.X - mNewLocationX), Math.Abs(end.Y - mNewLocationY));
var Orthogonal = Math.Abs(dxy.X - dxy.Y);
var Diagonal = Math.Abs(((dxy.X + dxy.Y) - Orthogonal) / 2);
mH = mHEstimate * (int)(Diagonal + Orthogonal + dxy.X + dxy.Y);
break;
}
PathFinderNodeFast newNode = new PathFinderNodeFast();
newNode.JumpLength = newJumpLength;
newNode.PX = mLocationX;
newNode.PY = mLocationY;
newNode.PZ = (byte)mLocation.z;
newNode.G = mNewG;
newNode.F = mNewG + mH;
newNode.Status = mOpenNodeValue;
if (nodes[mNewLocation].Count == 0)
{
touchedLocations.Push(mNewLocation);
}
nodes[mNewLocation].Add(newNode);
mOpen.Push(new Location(mNewLocation, nodes[mNewLocation].Count - 1));
CHILDREN_LOOP_END:
continue;
}
nodes[mLocation.xy][mLocation.z] = nodes[mLocation.xy][mLocation.z].UpdateStatus(mCloseNodeValue);
mCloseNodeCounter++;
}
if (mFound)
{
mClose.Clear();
var posX = end.X;
var posY = end.Y;
var fPrevNodeTmp = new PathFinderNodeFast();
var fNodeTmp = nodes[mEndLocation][0];
var fNode = end;
var fPrevNode = end;
var loc = (fNodeTmp.PY << mGridXLog2) + fNodeTmp.PX;
while (fNode.X != fNodeTmp.PX || fNode.Y != fNodeTmp.PY)
{
var fNextNodeTmp = nodes[loc][fNodeTmp.PZ];
if ((mClose.Count == 0) ||
(fNextNodeTmp.JumpLength != 0 && fNodeTmp.JumpLength == 0) ||
(fNodeTmp.JumpLength == 3 && fPrevNodeTmp.JumpLength != 0) ||
(fNodeTmp.JumpLength == 0 && fPrevNodeTmp.JumpLength != 0) ||
(fNode.Y > mClose[mClose.Count - 1].Y && fNode.Y > fNodeTmp.PY) ||
((mMap.IsGround((int)fNode.X - 1, (int)fNode.Y) || mMap.IsGround((int)fNode.X + 1, (int)fNode.Y)) &&
fNode.Y != mClose[mClose.Count - 1].Y && fNode.X != mClose[mClose.Count - 1].X))
{
mClose.Add(fNode);
}
fPrevNode = fNode;
posX = fNodeTmp.PX;
posY = fNodeTmp.PY;
fPrevNodeTmp = fNodeTmp;
fNodeTmp = fNextNodeTmp;
loc = (fNodeTmp.PY << mGridXLog2) + fNodeTmp.PX;
fNode = new Vector2(posX, posY);
}
mClose.Add(fNode);
mStopped = true;
return(mClose);
}
mStopped = true;
return(null);
}
}
public KeyValuePair <Point, float>[] FindPath(Point start, Point end, float rangePow)
{
lock (this)
{
var Stopwatch = System.Diagnostics.Stopwatch.StartNew();
if (start == end || rangePow >= Math.Sqrt(Math.Pow(start.X - end.X, 2) + Math.Pow((start.Y - end.Y), 2)))
{
return(new KeyValuePair <Point, float> [0]);
}
// 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;
Stopwatch.Stop();
mCompletedTime = Stopwatch.Elapsed;
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[(mNewLocationY << mGridYLog2) + mNewLocationX] != 0)
{
continue;
}
if (mHeavyDiagonals && i > 3)
{
mNewG = mCalcGrid[mLocation].G + 1.4142f; //(int) (mGrid[mNewLocationX, mNewLocationY] * 2.41);
}
else
{
mNewG = mCalcGrid[mLocation].G + 1; // 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:
double Pow = Math.Pow((mNewLocationX - end.X), 2) + Math.Pow((mNewLocationY - end.Y), 2);
if (mNewLocation == mEndLocation || rangePow >= Pow)
{
mFound = true;
}
else
{
mH = (int)(mHEstimate * Pow);
}
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 = (int)(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;
if (mFound)
{
break;
}
}
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
if (mFound)
{
break;
}
}
Stopwatch.Stop();
mCompletedTime = mCompletedTime = Stopwatch.Elapsed;
if (mFound)
{
mClose.Clear();
PathFinderNodeFast fNodeTmp = mCalcGrid[mNewLocation];
PathFinderNode fNode;
fNode.F = fNodeTmp.F;
fNode.G = fNodeTmp.G;
//fNode.H = 0;
fNode.PX = fNodeTmp.PX;
fNode.PY = fNodeTmp.PY;
fNode.X = mNewLocationX;
fNode.Y = mNewLocationY;
while (fNode.X != fNode.PX || fNode.Y != fNode.PY)
{
//if (range < Math.Sqrt(Math.Pow(fNode.PX - end.X, 2) + Math.Pow((fNode.PY - end.Y), 2)))
mClose.Add(new KeyValuePair <Point, float>(new Point(fNode.X, fNode.Y), fNode.G));
#if DEBUGON
if (mDebugFoundPath && PathFinderDebug != null)
{
PathFinderDebug(fNode.PX, fNode.PY, fNode.X, fNode.Y, PathFinderNodeType.Path, fNode.F, fNode.G);
}
#endif
fNodeTmp = mCalcGrid[(fNode.PY << mGridYLog2) + fNode.PX];
fNode.F = fNodeTmp.F;
fNode.G = fNodeTmp.G;
//fNode.H = 0;
fNode.X = fNode.PX;
fNode.Y = fNode.PY;
fNode.PX = fNodeTmp.PX;
fNode.PY = fNodeTmp.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
mStopped = true;
return(mClose.ToArray());
}
mStopped = true;
return(null);
}
}
public List <PathFinderNode> FindPath(PathFindingPoint start, PathFindingPoint end, out int 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();
int[] sideCosts = null;
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;
if (mHexagonalGrid)
{
maxi = 6;
}
else
{
maxi = mDiagonals ? 8 : 4;
}
bool hasSideCosts = false;
if (mHexagonalGrid)
{
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]);
}
if (mNewLocationY >= mGridY)
{
continue;
}
if (mNewLocationX >= mGridX)
{
continue;
}
// Unbreakeable?
mNewLocation = (mNewLocationY << mGridYLog2) + mNewLocationX;
if (!mGrid[mNewLocation].canCross)
{
continue;
}
int 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 && 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);
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;
}
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(int CellStartIndex, int CellEndIndex)
{
mFound = false;
mCloseNodeCounter = 0;
if (mOpenNodeValue > 250)
{
mOpenNodeValue = 1;
mCloseNodeValue = 2;
}
else
{
mOpenNodeValue += 2;
mCloseNodeValue += 2;
}
mOpen.Clear();
mClose.Clear();
mLocation = CellStartIndex;
mEndLocation = CellEndIndex;
mCalcGrid [mLocation].G = 0;
mCalcGrid [mLocation].F = mHEstimate;
mCalcGrid [mLocation].PIndex = CellStartIndex;
mCalcGrid [mLocation].Status = mOpenNodeValue;
Vector3 p0 = mCells[1].sphereCenter;
Vector3 p1 = mCells[1].neighbours[0].sphereCenter;
float distCellToCell = Vector3.Angle(p0, p1) / 180f;
float distPerDegree = 0;
// Compute spherical distance
if (mFormula == HeuristicFormula.SphericalDistance)
{
distPerDegree = 1f / (distCellToCell * 180f);
}
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 > mSearchLimit)
{
return(null);
}
//Lets calculate each successors
int maxi = mCells [mLocation].neighbours.Length;
for (int i = 0; i < maxi; i++)
{
mNewLocation = mCells [mLocation].neighboursIndices [i];
if (!mCells[mNewLocation].visible)
{
continue;
}
int gridValue = mGrid [mNewLocation] > 0 ? 1 : 0;
if (gridValue == 0)
{
continue;
}
gridValue += mCells[mLocation].GetNeighbourCost(i);
// Check custom validator
if (mOnCellCross != null)
{
gridValue += mOnCellCross(mNewLocation);
}
mNewG = mCalcGrid [mLocation].G + gridValue;
//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].PIndex = mLocation;
mCalcGrid [mNewLocation].G = mNewG;
int dist = 1;
switch (mFormula)
{
case HeuristicFormula.SphericalDistance:
dist += (int)(Vector3.Angle(mCells [mEndLocation].sphereCenter, mCells [mNewLocation].sphereCenter) * distPerDegree); // 1000f);
break;
case HeuristicFormula.Euclidean:
dist += (int)(Vector3.Distance(mCells [mEndLocation].sphereCenter, mCells [mNewLocation].sphereCenter) / distCellToCell);
break;
case HeuristicFormula.EuclideanNoSQR:
dist += (int)(Vector3.SqrMagnitude(mCells [mEndLocation].sphereCenter - mCells [mNewLocation].sphereCenter) / distCellToCell);
break;
}
mH = dist; //mHEstimate * dist;
mCalcGrid [mNewLocation].F = mNewG + mH;
mOpen.Push(mNewLocation);
mCalcGrid [mNewLocation].Status = mOpenNodeValue;
}
mCloseNodeCounter++;
mCalcGrid [mLocation].Status = mCloseNodeValue;
}
//mCompletedTime = HighResolutionTime.GetTime();
if (mFound)
{
mClose.Clear();
int pos = CellEndIndex;
PathFinderNodeFast fNodeTmp = mCalcGrid [CellEndIndex];
PathFinderNode fNode;
fNode.F = fNodeTmp.F;
fNode.G = fNodeTmp.G;
fNode.H = 0;
fNode.PIndex = fNodeTmp.PIndex;
fNode.index = CellEndIndex;
while (fNode.index != fNode.PIndex)
{
mClose.Add(fNode);
pos = fNode.PIndex;
fNodeTmp = mCalcGrid [pos];
fNode.F = fNodeTmp.F;
fNode.G = fNodeTmp.G;
fNode.H = 0;
fNode.PIndex = fNodeTmp.PIndex;
fNode.index = pos;
}
mClose.Add(fNode);
return(mClose);
}
return(null);
}
public List <Position> FindPath(Unit unit, Position start, Position 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; // (start.Y << mGridYLog2) + start.X;
mEndLocation = end; //(end.Y << mGridYLog2) + end.X;
if (!mCalcGrid.ContainsKey(mLocation))
{
mCalcGrid.Add(mLocation, new PathFinderNodeFast());
}
mCalcGrid[mLocation].G = 0;
mCalcGrid[mLocation].F = mHEstimate;
mCalcGrid[mLocation].PX = start.X;
mCalcGrid[mLocation].PY = 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.X;// (mLocation & mGridXMinus1);
//mLocationY = (ushort)mLocation.Y; // (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 = (mLocation.X - mCalcGrid[mLocation].PX);
}
//Lets calculate each successors
//Position p = new Position(mLocationX, mLocationY);
Tile t = map.GetTile(mLocation);
//for (int i=0; i<(mDiagonals ? 8 : 4); i++)
foreach (Tile n in t.Neighbors)
{
if (!IgnoreVisibility)
{
if (unit != null && !unit.Owner.VisiblePositions.Contains(n.Pos))
{
continue;
}
}
//mNewLocationX = (ushort)n.Pos.X; //(ushort) (mLocationX + mDirection[i,0]);
//mNewLocationY = (ushort)n.Pos.Y; // (ushort) (mLocationY + mDirection[i,1]);
mNewLocation = n.Pos; //(mNewLocationY << mGridYLog2) + mNewLocationX;
//if (mNewLocationX >= mGridX || mNewLocationY >= mGridY)
// continue;
if (unit != null)
{
Unit otherUnit = map.Units.GetUnitAt(mNewLocation); //Xnew Position(mNewLocationX, mNewLocationY));
if (otherUnit != null)
{
// Do not hit ourselfes
//if (otherUnit.Owner == unit.Owner)
if (mNewLocation != end)
{
// Do not hit anyone but at end
continue;
}
}
}
if (n.Pos != end && !n.CanMoveTo())
{
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 ((mNewLocation.X - mLocation.X) != 0)
{
if (mHoriz == 0)
{
mNewG += Math.Abs(mNewLocation.X - end.X) + Math.Abs(mNewLocation.Y - end.Y);
}
}
if ((mNewLocation.Y - mLocation.Y) != 0)
{
if (mHoriz != 0)
{
mNewG += Math.Abs(mNewLocation.X - end.X) + Math.Abs(mNewLocation.Y - end.Y);
}
}
}
//Is it open or closed?
if (!mCalcGrid.ContainsKey(mNewLocation))
{
mCalcGrid.Add(mNewLocation, new PathFinderNodeFast());
}
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 = mLocation.X;
mCalcGrid[mNewLocation].PY = mLocation.Y;
mCalcGrid[mNewLocation].G = mNewG;
switch (mFormula)
{
default:
case HeuristicFormula.Manhattan:
mH = mHEstimate * (Math.Abs(mNewLocation.X - end.X) + Math.Abs(mNewLocation.Y - end.Y));
break;
case HeuristicFormula.MaxDXDY:
mH = mHEstimate * (Math.Max(Math.Abs(mNewLocation.X - end.X), Math.Abs(mNewLocation.Y - end.Y)));
break;
case HeuristicFormula.DiagonalShortCut:
int h_diagonal = Math.Min(Math.Abs(mNewLocation.X - end.X), Math.Abs(mNewLocation.Y - end.Y));
int h_straight = (Math.Abs(mNewLocation.X - end.X) + Math.Abs(mNewLocation.Y - end.Y));
mH = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal);
break;
case HeuristicFormula.Euclidean:
mH = (int)(mHEstimate * Math.Sqrt(Math.Pow((mNewLocation.Y - end.X), 2) + Math.Pow((mNewLocation.Y - end.Y), 2)));
break;
case HeuristicFormula.EuclideanNoSQR:
mH = (int)(mHEstimate * (Math.Pow((mNewLocation.X - end.X), 2) + Math.Pow((mNewLocation.Y - end.Y), 2)));
break;
case HeuristicFormula.Custom1:
Position dxy = new Position(Math.Abs(end.X - mNewLocation.X), Math.Abs(end.Y - mNewLocation.Y));
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 = mLocation.X - end.X;
int dy1 = mLocation.Y - 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;
//PathFinderNodeFast fNodeTmp = mCalcGrid[(end.Y << mGridYLog2) + end.X];
PathFinderNodeFast fNodeTmp = mCalcGrid[end];
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];
Position pos = new Position(posX, posY);
if (!mCalcGrid.ContainsKey(pos))
{
mCalcGrid.Add(pos, new PathFinderNodeFast());
}
fNodeTmp = mCalcGrid[pos];
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;
List <Position> route = new List <Position>();
if (mClose != null && mClose.Count > 0)
{
Move move = new Move();
move.MoveType = MoveType.Move;
move.Positions = new List <Position>();
for (int ndx = mClose.Count - 1; ndx >= 0; ndx--)
{
route.Add(new Position(mClose[ndx].X, mClose[ndx].Y));
}
}
return(route);
}
mStopped = true;
return(null);
}
}
public List <PathFinderNode> FindPath(Vector2i start, Vector2i end)
{
int h;
int newLocation;
ushort locationX;
ushort locationY;
ushort newLocationX;
ushort newLocationY;
int newG;
int closeNodeCounter = 0;
bool found = false;
List <PathFinderNode> output;
int location = (start.Y << mGridYLog2) + start.X;
int endLocation = (end.Y << mGridYLog2) + end.X;
mStop = false;
mStopped = false;
mOpenNodeValue += 2;
mCloseNodeValue += 2;
mOpen.Clear();
mCalcGrid[location].G = 0;
mCalcGrid[location].F = mHEstimate;
mCalcGrid[location].PX = (ushort)start.X;
mCalcGrid[location].PY = (ushort)start.Y;
mCalcGrid[location].Status = mOpenNodeValue;
mOpen.Enqueue(location);
while (mOpen.Count > 0 && !mStop)
{
location = mOpen.Dequeue();
// Is it in closed list? means this node was already processed
if (mCalcGrid[location].Status == mCloseNodeValue)
{
continue;
}
locationX = (ushort)(location & mGridXMinus1);
locationY = (ushort)(location >> mGridYLog2);
if (location == endLocation)
{
mCalcGrid[location].Status = mCloseNodeValue;
found = true;
break;
}
if (closeNodeCounter > mSearchLimit)
{
mStopped = true;
return(null);
}
if (mPunishChangeDirection)
{
mHoriz = (locationX - mCalcGrid[location].PX);
}
// Calculate each successor
for (int i = 0; i < (mDiagonals ? 8 : 4); i++)
{
newLocationX = (ushort)(locationX + mDirection[i, 0]);
newLocationY = (ushort)(locationY + mDirection[i, 1]);
newLocation = (newLocationY << mGridYLog2) + newLocationX;
if (newLocationX >= mGridX || newLocationY >= mGridY)
{
continue;
}
// Unbreakable?
if (mGrid[newLocationX, newLocationY] == 0)
{
continue;
}
if (mHeavyDiagonals && i > 3)
{
newG = mCalcGrid[location].G + (int)(mGrid[newLocationX, newLocationY] * 2.41);
}
else
{
newG = mCalcGrid[location].G + mGrid[newLocationX, newLocationY];
}
if (mPunishChangeDirection)
{
if ((newLocationX - locationX) != 0)
{
if (mHoriz == 0)
{
newG += Math.Abs(newLocationX - end.X) + Math.Abs(newLocationY - end.Y);
}
}
if ((newLocationY - locationY) != 0)
{
if (mHoriz != 0)
{
newG += Math.Abs(newLocationX - end.X) + Math.Abs(newLocationY - end.Y);
}
}
}
// Is it open or closed?
if (mCalcGrid[newLocation].Status == mOpenNodeValue || mCalcGrid[newLocation].Status == mCloseNodeValue)
{
// The current node has less code than the previous? then skip this node
if (mCalcGrid[newLocation].G <= newG)
{
continue;
}
}
mCalcGrid[newLocation].PX = locationX;
mCalcGrid[newLocation].PY = locationY;
mCalcGrid[newLocation].G = newG;
switch (mFormula)
{
default:
case HeuristicFormula.Manhattan:
h = mHEstimate * (Math.Abs(newLocationX - end.X) + Math.Abs(newLocationY - end.Y));
break;
case HeuristicFormula.MaxDXDY:
h = mHEstimate * (Math.Max(Math.Abs(newLocationX - end.X), Math.Abs(newLocationY - end.Y)));
break;
case HeuristicFormula.DiagonalShortCut:
int h_diagonal = Math.Min(Math.Abs(newLocationX - end.X), Math.Abs(newLocationY - end.Y));
int h_straight = (Math.Abs(newLocationX - end.X) + Math.Abs(newLocationY - end.Y));
h = (mHEstimate * 2) * h_diagonal + mHEstimate * (h_straight - 2 * h_diagonal);
break;
case HeuristicFormula.Euclidean:
h = (int)(mHEstimate * Math.Sqrt(Math.Pow((newLocationY - end.X), 2) + Math.Pow((newLocationY - end.Y), 2)));
break;
case HeuristicFormula.EuclideanNoSQR:
h = (int)(mHEstimate * (Math.Pow((newLocationX - end.X), 2) + Math.Pow((newLocationY - end.Y), 2)));
break;
case HeuristicFormula.Custom1:
Vector2i dxy = new Vector2i(Math.Abs(end.X - newLocationX), Math.Abs(end.Y - newLocationY));
int Orthogonal = Math.Abs(dxy.X - dxy.Y);
int Diagonal = Math.Abs(((dxy.X + dxy.Y) - Orthogonal) / 2);
h = mHEstimate * (Diagonal + Orthogonal + dxy.X + dxy.Y);
break;
}
if (mTieBreaker)
{
int dx1 = locationX - end.X;
int dy1 = locationY - end.Y;
int dx2 = start.X - end.X;
int dy2 = start.Y - end.Y;
int cross = Math.Abs(dx1 * dy2 - dx2 * dy1);
h = (int)(h + cross * 0.001);
}
mCalcGrid[newLocation].F = newG + h;
mOpen.Enqueue(newLocation);
mCalcGrid[newLocation].Status = mOpenNodeValue;
}
closeNodeCounter++;
mCalcGrid[location].Status = mCloseNodeValue;
}
if (found)
{
output = new List <PathFinderNode>();
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)
{
output.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;
}
output.Add(fNode);
mStopped = true;
return(output);
}
mStopped = true;
return(null);
}
public List <PathFinderNode> FindPath(Vector2i start, Vector2i end)
{
bool found = false;
bool stop = false;
int heuristicEstimate = 2;
int closeNodeCounter = 0;
int directionCount = _diagonals ? 8 : 4;
// Instead of clearing the grid each time, I change node state values and simply ignore the other values.
// It's faster than clearing the grid (not much, but it is).
_openNodeValue += 2;
_closeNodeValue += 2;
_open.Clear();
_close.Clear();
int location = EncodeLocation(start.X, start.Y);
int endLocation = EncodeLocation(end.X, end.Y);
ushort locationX = 0, locationY = 0;
_calcGrid[location].goneCost = 0;
_calcGrid[location].cost = 0 + heuristicEstimate;
_calcGrid[location].parentX = (ushort)start.X;
_calcGrid[location].parentY = (ushort)start.Y;
_calcGrid[location].state = _openNodeValue;
_open.Push(location);
while (_open.Count > 0 && !stop)
{
location = _open.Pop();
// Is it in closed list? means this node was already processed
if (_calcGrid[location].state == _closeNodeValue)
{
continue;
}
DecodeLocation(location, ref locationX, ref locationY);
if (location == endLocation)
{
_calcGrid[location].state = _closeNodeValue;
found = true;
break;
}
if (closeNodeCounter > _searchLimit)
{
// Evaluated nodes exceeded limit : path not found
Console.WriteLine("Pathfinder searchLimit exceed");
return(null);
}
// Let's calculate each successors
for (int i = 0; i < directionCount; ++i)
{
ushort newLocationX = (ushort)(locationX + _directions[i][0]);
ushort newLocationY = (ushort)(locationY + _directions[i][1]);
int newLocation = EncodeLocation(newLocationX, newLocationY);
int newGoneCost;
// Outside the grid?
if (newLocationX >= _gridSizeX || newLocationY >= _gridSizeY)
{
continue;
}
// Not crossable?
if (_grid[newLocation] == 0)
{
continue;
}
if (_avoidDiagonalCross)
{
//
// +----+----+----+
// | | | 3 |
// | | | |
// +----+----+----+
// | | 2 |XXXX| Diagonals are allowed,
// | | |XXXX| but going through 1, 2 then 3 should be avoided,
// +----+----+----+ because there are contiguous uncrossable cells.
// | | 1 |XXXX| (A square object cannot go from 2 to 3 for example, it will have to bypass the corner).
// | | |XXXX|
// +----+----+----+
//
if (i > 3)
{
if (_grid[EncodeLocation(locationX + _directions[i][0], locationY)] == 0 ||
_grid[EncodeLocation(locationX, locationY + _directions[i][1])] == 0)
{
continue;
}
}
}
if (_heavyDiagonals && i > 3)
{
newGoneCost = _calcGrid[location].goneCost + (int)(_grid[newLocation] * 2.41f);
}
else
{
newGoneCost = _calcGrid[location].goneCost + _grid[newLocation];
}
//Is it open or closed?
if (_calcGrid[newLocation].state == _openNodeValue || _calcGrid[newLocation].state == _closeNodeValue)
{
// The current node has less code than the previous? then skip this node
if (_calcGrid[newLocation].goneCost <= newGoneCost)
{
continue;
}
}
_calcGrid[newLocation].parentX = locationX;
_calcGrid[newLocation].parentY = locationY;
_calcGrid[newLocation].goneCost = newGoneCost;
// Heuristic : manhattan distance
int heuristic = heuristicEstimate * (Math.Abs(newLocationX - end.X) + Math.Abs(newLocationY - end.Y));
_calcGrid[newLocation].cost = newGoneCost + heuristic;
//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
_open.Push(newLocation);
_calcGrid[newLocation].state = _openNodeValue;
}
closeNodeCounter++;
_calcGrid[location].state = _closeNodeValue;
}
if (found)
{
_close.Clear();
int posX = end.X;
int posY = end.Y;
PathFinderNodeFast tmpNode = _calcGrid[EncodeLocation(end.X, end.Y)];
PathFinderNode node = new PathFinderNode();
node.cost = tmpNode.cost;
node.goneCost = tmpNode.goneCost;
node.heuristic = 0;
node.parentX = tmpNode.parentX;
node.parentY = tmpNode.parentY;
node.x = end.X;
node.y = end.Y;
while (node.x != node.parentX || node.y != node.parentY)
{
_close.Add(node);
posX = node.parentX;
posY = node.parentY;
tmpNode = _calcGrid[EncodeLocation(posX, posY)];
node.cost = tmpNode.cost;
node.goneCost = tmpNode.goneCost;
node.heuristic = 0;
node.parentX = tmpNode.parentX;
node.parentY = tmpNode.parentY;
node.x = posX;
node.y = posY;
}
_close.Add(node);
// Path found
return(_close);
}
// Path not found
Console.WriteLine("Pathfinder path not found");
return(null);
}
public List <Microsoft.Xna.Framework.Point> FindPath(Microsoft.Xna.Framework.Point start, Microsoft.Xna.Framework.Point end)
{
lock (this) {
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();
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;
return(null);
}
if (mPunishChangeDirection)
{
mHoriz = (mLocationX - mCalcGrid[mLocation].PX);
}
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;
}
if (Grid[mNewLocationX, mNewLocationY] == 0)
{
continue;
}
if (mHeavyDiagonals && i > 3)
{
mNewG = mCalcGrid[mLocation].G + (int)(Grid[mNewLocationX, mNewLocationY] * 2.41);
}
else
{
mNewG = mCalcGrid[mLocation].G + Grid[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);
}
}
}
if (mCalcGrid[mNewLocation].Status == mOpenNodeValue || mCalcGrid[mNewLocation].Status == mCloseNodeValue)
{
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
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
}
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;
List <Microsoft.Xna.Framework.Point> copy = new List <Microsoft.Xna.Framework.Point>();
foreach (PathFinderNode node in mClose)
{
copy.Add(new Microsoft.Xna.Framework.Point(node.X, node.Y));
}
return(copy);
}
mStopped = true;
return(null);
}
}
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);
}
}
public List <PathFinderNode> FindPath(Point Start, Point End)
{
lock (this)
{
_Found = false;
_Stop = false;
_Stopped = false;
_CloseNodeCounter = 0;
_OpenNodeValue += 2;
_CloseNodeValue += 2;
_Open.Clear();
_Close.Clear();
_Location = (Start.Y << _GridYLog2) + Start.X;
_EndLocation = (End.Y << _GridYLog2) + End.X;
_CalcGrid[_Location].G = 0;
_CalcGrid[_Location].F = _HEstimate;
_CalcGrid[_Location].PX = (ushort)Start.X;
_CalcGrid[_Location].PY = (ushort)Start.Y;
_CalcGrid[_Location].Status = _OpenNodeValue;
_Open.Push(_Location);
while (_Open.Count > 0 && !_Stop)
{
_Location = _Open.Pop();
if (_CalcGrid[_Location].Status == _CloseNodeValue)
{
continue;
}
_LocationX = (ushort)(_Location & _GridXMinus1);
_LocationY = (ushort)(_Location >> _GridYLog2);
if (_Location == _EndLocation)
{
_CalcGrid[_Location].Status = _CloseNodeValue;
_Found = true;
break;
}
if (_CloseNodeCounter > _SearchLimit)
{
_Stopped = true;
return(null);
}
if (_PunishChangeDirection)
{
_Horiz = (_LocationX - _CalcGrid[_Location].PX);
}
for (int i = 0; i < 8; i++)
{
_NewLocationX = (ushort)(_LocationX + _Direction[i, 0]);
_NewLocationY = (ushort)(_LocationY + _Direction[i, 1]);
_NewLocation = (_NewLocationY << _GridYLog2) + _NewLocationX;
if (_NewLocationX >= _GridX || _NewLocationY >= _GridY)
{
continue;
}
if (_Grid[_NewLocationX, _NewLocationY] == 0)
{
continue;
}
if (_HeavyDiagonals && i > 3)
{
_NewG = _CalcGrid[_Location].G + (int)(_Grid[_NewLocationX, _NewLocationY] * 2.41);
}
else
{
_NewG = _CalcGrid[_Location].G + _Grid[_NewLocationX, _NewLocationY];
}
if (_PunishChangeDirection)
{
if ((_NewLocationX - _LocationX) != 0)
{
if (_Horiz == 0)
{
_NewG += Math.Abs(_NewLocationX - End.X) + Math.Abs(_NewLocationY - End.Y);
}
}
if ((_NewLocationY - _LocationY) != 0)
{
if (_Horiz != 0)
{
_NewG += Math.Abs(_NewLocationX - End.X) + Math.Abs(_NewLocationY - End.Y);
}
}
}
if (_CalcGrid[_NewLocation].Status == _OpenNodeValue || _CalcGrid[_NewLocation].Status == _CloseNodeValue)
{
if (_CalcGrid[_NewLocation].G <= _NewG)
{
continue;
}
}
_CalcGrid[_NewLocation].PX = _LocationX;
_CalcGrid[_NewLocation].PY = _LocationY;
_CalcGrid[_NewLocation].G = _NewG;
_H = _HEstimate * (Math.Abs(_NewLocationX - End.X) + Math.Abs(_NewLocationY - End.Y));
if (_TieBreaker)
{
int dx1 = _LocationX - End.X;
int dy1 = _LocationY - End.Y;
int dx2 = Start.X - End.X;
int dy2 = Start.Y - End.Y;
int cross = Math.Abs(dx1 * dy2 - dx2 * dy1);
_H = (int)(_H + cross * 0.001);
}
_CalcGrid[_NewLocation].F = _NewG + _H;
_Open.Push(_NewLocation);
_CalcGrid[_NewLocation].Status = _OpenNodeValue;
}
_CloseNodeCounter++;
_CalcGrid[_Location].Status = _CloseNodeValue;
}
if (_Found)
{
_Close.Clear();
int posX = End.X;
int posY = End.Y;
PathFinderNodeFast fNodeTmp = _CalcGrid[(End.Y << _GridYLog2) + 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;
try
{
while (fNode.X != fNode.PX || fNode.Y != fNode.PY)
{
_Close.Add(fNode);
posX = fNode.PX;
posY = fNode.PY;
fNodeTmp = _CalcGrid[(posY << _GridYLog2) + 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;
}
}
catch
{
_Stopped = true;
return(null);
}
_Close.Add(fNode);
_Stopped = true;
return(_Close);
}
_Stopped = true;
return(null);
}
}
public List <PathFinderNode> FindPath(PathFinderPoint start, PathFinderPoint end)
{
lock (this)
{
//HighResolutionTime.Start();
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;
}
mLocationX = (ushort)(mLocation & mGridXMinus1);
mLocationY = (ushort)(mLocation >> mGridYLog2);
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;
}
// TEST - kill ALL diags
if (mLocationX != mNewLocationX && mLocationY != mNewLocationY)
{
//continue;
}
// NavigateCorners conditions:
// if both new x AND y are greater: check (x+1, y), (x, y+1)
// if both new x AND y are smaller: check (x-1, y), (x, y-1)
// if new x is smaller and y are greater: check (x-1, y), (x, y+1)
// if new x is greater and y are smaller: check (x, y-1), (x+1, y)
// Corner navigation - ie. don't take diagonals across corners of occupied cells
// If new x is greater: check (x+1, y)
if (mNewLocationX > mLocationX && mGrid[mLocationX + 1, mLocationY] == 0)
{
continue;
}
// If new x is smaller: check (x-1, y)
if (mNewLocationX < mLocationX && mGrid[mLocationX - 1, mLocationY] == 0)
{
continue;
}
// If new y is greater: check (x, y+1)
if (mNewLocationY > mLocationY && mGrid[mLocationX, mLocationY + 1] == 0)
{
continue;
}
// If new y is smaller: check (x, y-1)
if (mNewLocationY < mLocationY && mGrid[mLocationX, mLocationY - 1] == 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:
PathFinderPoint dxy = new PathFinderPoint(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;
//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);
}
}
public List <PathFinderNode> FindPath(Point2D start, Point2D end)
{
lock (this)
{
//清空上次寻路余留的数据
Array.Clear(mCalcGrid, 0, mCalcGrid.Length);
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;
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;
}
if (mCalcGrid[mNewLocation].Status == mCloseNodeValue && !mReopenCloseNodes)
{
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);
}
}
}
mNewG = mNewG + GetPunishNum(mNewLocationX, mNewLocationY);
//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:
Point2D dxy = new Point2D(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
}
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
//进行简单的平滑处理
//mClose = Floyd(mClose);
mStopped = true;
return(mClose);
}
mStopped = true;
return(null);
}
}
public List <Vector2i> FindPath(Vector2i start, Vector2i end, int characterWidth, int characterHeight, short maxCharacterJumpHeight, bool useLedges)
{
lock (this)
{
while (touchedLocations.Count > 0)
{
nodes[touchedLocations.Pop()].Clear();
}
var inSolidTile = false;
for (var i = 0; i < 2; ++i)
{
inSolidTile = false;
for (var w = 0; w < characterWidth; ++w)
{
if (mGrid[end.x + w, end.y] == 0 ||
mGrid[end.x + w, end.y + characterHeight - 1] == 0)
{
inSolidTile = true;
break;
}
}
if (inSolidTile == false)
{
for (var h = 1; h < characterHeight - 1; ++h)
{
if (mGrid[end.x, end.y + h] == 0 ||
mGrid[end.x + characterWidth - 1, end.y + h] == 0)
{
inSolidTile = true;
break;
}
}
}
if (inSolidTile)
{
end.x -= characterWidth - 1;
}
else
{
break;
}
}
if (inSolidTile == true)
{
return(null);
}
mFound = false;
mStop = false;
mStopped = false;
mCloseNodeCounter = 0;
mOpenNodeValue += 2;
mCloseNodeValue += 2;
mOpen.Clear();
mLocation.xy = (start.y << mGridXLog2) + start.x;
mLocation.z = 0;
mEndLocation = (end.y << mGridXLog2) + end.x;
PathFinderNodeFast firstNode = new PathFinderNodeFast();
firstNode.G = 0;
firstNode.F = mHEstimate;
firstNode.PX = (ushort)start.x;
firstNode.PY = (ushort)start.y;
firstNode.PZ = 0;
firstNode.Status = mOpenNodeValue;
bool startsOnGround = false;
for (int x = start.x; x < start.x + characterWidth; ++x)
{
if (mMap.IsGround(x, start.y - 1))
{
startsOnGround = true;
break;
}
}
if (startsOnGround)
{
firstNode.JumpLength = 0;
}
else
{
firstNode.JumpLength = (short)(maxCharacterJumpHeight * 2);
}
nodes[mLocation.xy].Add(firstNode);
touchedLocations.Push(mLocation.xy);
mOpen.Push(mLocation);
while (mOpen.Count > 0 && !mStop)
{
mLocation = mOpen.Pop();
//Is it in closed list? means this node was already processed
if (nodes[mLocation.xy][mLocation.z].Status == mCloseNodeValue)
{
continue;
}
mLocationX = (ushort)(mLocation.xy & mGridXMinus1);
mLocationY = (ushort)(mLocation.xy >> mGridXLog2);
if (mLocation.xy == mEndLocation)
{
nodes[mLocation.xy][mLocation.z] = nodes[mLocation.xy][mLocation.z].UpdateStatus(mCloseNodeValue);
mFound = true;
break;
}
if (mCloseNodeCounter > mSearchLimit)
{
mStopped = true;
return(null);
}
//Lets calculate each successors
for (var i = 0; i < (mDiagonals ? 8 : 4); i++)
{
mNewLocationX = (ushort)(mLocationX + mDirection[i, 0]);
mNewLocationY = (ushort)(mLocationY + mDirection[i, 1]);
mNewLocation = (mNewLocationY << mGridXLog2) + mNewLocationX;
var jumpLength = nodes[mLocation.xy][mLocation.z].JumpLength;
short newJumpLength = jumpLength;
var onGround = false;
var atCeiling = false;
for (var w = 0; w < characterWidth; ++w)
{
if (mGrid[mNewLocationX + w, mNewLocationY] == 0 ||
mGrid[mNewLocationX + w, mNewLocationY + characterHeight - 1] == 0)
{
goto CHILDREN_LOOP_END;
}
if (mMap.IsGround(mNewLocationX + w, mNewLocationY - 1))
{
onGround = true;
}
else if (mGrid[mNewLocationX + w, mNewLocationY + characterHeight] == 0)
{
atCeiling = true;
}
/*else if (canGrabLedge && !onGround && !atCeiling)
* {
* for (int h = 1; h < characterHeight; ++h)
* {
* if (mGrid[mNewLocationX + w, mNewLocationY + characterHeight + h] == 0)
* {
* canGrabLedge = false;
* break;
* }
* }
* }*/
}
for (var h = 1; h < characterHeight - 1; ++h)
{
if (mGrid[mNewLocationX, mNewLocationY + h] == 0 ||
mGrid[mNewLocationX + characterWidth - 1, mNewLocationY + h] == 0)
{
goto CHILDREN_LOOP_END;
}
}
//calculate a proper jumplength value for the successor
if (onGround)
{
newJumpLength = 0;
}
else if (atCeiling)
{
if (mNewLocationX != mLocationX)
{
newJumpLength = (short)Mathf.Max(maxCharacterJumpHeight * 2 + 1, jumpLength + 1);
}
else
{
newJumpLength = (short)Mathf.Max(maxCharacterJumpHeight * 2, jumpLength + 2);
}
}
else if (useLedges && jumpLength > 0 &&
jumpLength <= maxCharacterJumpHeight * 2 + 6 &&
((mGrid[mNewLocationX + characterWidth, mNewLocationY + characterHeight - 1] == 0 && mGrid[mNewLocationX + characterWidth, mNewLocationY + characterHeight] != 0) ||
(mGrid[mNewLocationX - 1, mNewLocationY + characterHeight - 1] == 0 && mGrid[mNewLocationX - 1, mNewLocationY + characterHeight] != 0)))
{
newJumpLength = cLedgeGrabJumpValue;
}
else if (mNewLocationY > mLocationY)
{
if (jumpLength == cLedgeGrabJumpValue)
{
newJumpLength = 3;
}
else if (jumpLength < 2 && maxCharacterJumpHeight > 2) //first jump is always two block up instead of one up and optionally one to either right or left
{
newJumpLength = 3;
}
else if (jumpLength % 2 == 0)
{
newJumpLength = (short)(jumpLength + 2);
}
else
{
newJumpLength = (short)(jumpLength + 1);
}
}
else if (mNewLocationY < mLocationY)
{
if (jumpLength == cLedgeGrabJumpValue)
{
newJumpLength = (short)(maxCharacterJumpHeight * 2 + 4);
}
else if (jumpLength % 2 == 0)
{
newJumpLength = (short)Mathf.Max(maxCharacterJumpHeight * 2, jumpLength + 2);
}
else
{
newJumpLength = (short)Mathf.Max(maxCharacterJumpHeight * 2, jumpLength + 1);
}
}
else if (!onGround && mNewLocationX != mLocationX)
{
if (jumpLength == cLedgeGrabJumpValue)
{
newJumpLength = (short)(maxCharacterJumpHeight * 2 + 3);
}
else
{
newJumpLength = (short)Mathf.Max(jumpLength + 1, 1);
}
}
if (jumpLength == cLedgeGrabJumpValue && mLocationX != mNewLocationX && newJumpLength < maxCharacterJumpHeight * 2)
{
continue;
}
if (jumpLength >= 0 && jumpLength % 2 != 0 && mLocationX != mNewLocationX)
{
continue;
}
if ((newJumpLength == 0 && mNewLocationX != mLocationX && jumpLength + 1 >= maxCharacterJumpHeight * 2 + 6 && (jumpLength + 1 - (maxCharacterJumpHeight * 2 + 6)) % 8 <= 1) ||
(newJumpLength >= maxCharacterJumpHeight * 2 + 6 && mNewLocationX != mLocationX && (newJumpLength - (maxCharacterJumpHeight * 2 + 6)) % 8 != 7))
{
continue;
}
//if we're falling and succeor's height is bigger than ours, skip that successor
if (jumpLength >= maxCharacterJumpHeight * 2 && mNewLocationY > mLocationY)
{
continue;
}
mNewG = nodes[mLocation.xy][mLocation.z].G + mGrid[mNewLocationX, mNewLocationY] + newJumpLength / 4;
if (nodes[mNewLocation].Count > 0)
{
int lowestJump = short.MaxValue;
int lowestG = short.MaxValue;
bool couldMoveSideways = false;
for (int j = 0; j < nodes[mNewLocation].Count; ++j)
{
if (nodes[mNewLocation][j].JumpLength < lowestJump)
{
lowestJump = nodes[mNewLocation][j].JumpLength;
}
if (nodes[mNewLocation][j].G < lowestG)
{
lowestG = nodes[mNewLocation][j].G;
}
if (nodes[mNewLocation][j].JumpLength % 2 == 0 && nodes[mNewLocation][j].JumpLength < maxCharacterJumpHeight * 2 + 6)
{
couldMoveSideways = true;
}
}
// The current node has smaller cost than the previous? then skip this node
if (lowestG <= mNewG && lowestJump <= newJumpLength && (newJumpLength % 2 != 0 || newJumpLength >= maxCharacterJumpHeight * 2 + 6 || couldMoveSideways))
{
continue;
}
}
switch (mFormula)
{
default:
case HeuristicFormula.Manhattan:
mH = mHEstimate * (Mathf.Abs(mNewLocationX - end.x) + Mathf.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:
var h_diagonal = Math.Min(Math.Abs(mNewLocationX - end.x), Math.Abs(mNewLocationY - end.y));
var 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:
var dxy = new Vector2i(Math.Abs(end.x - mNewLocationX), Math.Abs(end.y - mNewLocationY));
var Orthogonal = Math.Abs(dxy.x - dxy.y);
var Diagonal = Math.Abs(((dxy.x + dxy.y) - Orthogonal) / 2);
mH = mHEstimate * (Diagonal + Orthogonal + dxy.x + dxy.y);
break;
}
PathFinderNodeFast newNode = new PathFinderNodeFast();
newNode.JumpLength = newJumpLength;
newNode.PX = mLocationX;
newNode.PY = mLocationY;
newNode.PZ = (byte)mLocation.z;
newNode.G = mNewG;
newNode.F = mNewG + mH;
newNode.Status = mOpenNodeValue;
if (nodes[mNewLocation].Count == 0)
{
touchedLocations.Push(mNewLocation);
}
nodes[mNewLocation].Add(newNode);
mOpen.Push(new Location(mNewLocation, nodes[mNewLocation].Count - 1));
CHILDREN_LOOP_END:
continue;
}
nodes[mLocation.xy][mLocation.z] = nodes[mLocation.xy][mLocation.z].UpdateStatus(mCloseNodeValue);
mCloseNodeCounter++;
}
if (mFound)
{
mClose.Clear();
var posX = end.x;
var posY = end.y;
var fPrevNodeTmp = new PathFinderNodeFast();
var fNodeTmp = nodes[mEndLocation][0];
var fNode = end;
var fPrevNode = end;
var loc = (fNodeTmp.PY << mGridXLog2) + fNodeTmp.PX;
while (fNode.x != fNodeTmp.PX || fNode.y != fNodeTmp.PY)
{
var fNextNodeTmp = nodes[loc][fNodeTmp.PZ];
//Debug.Log("Pos: " + fNode.x + ", " + fNode.y + " Jump: " + fNodeTmp.JumpLength);
if ((mClose.Count == 0) ||
(mMap.IsOneWayPlatform(fNode.x, fNode.y - 1)) ||
(mGrid[fNode.x, fNode.y - 1] == 0 && mMap.IsOneWayPlatform(fPrevNode.x, fPrevNode.y - 1)) ||
(fNodeTmp.JumpLength == 3) ||
(fNextNodeTmp.JumpLength != 0 && fNodeTmp.JumpLength == 0) || //mark jumps starts
(fNodeTmp.JumpLength == 0 && fPrevNodeTmp.JumpLength != 0) || //mark landings
(fNode.y > mClose[mClose.Count - 1].y && fNode.y > fNodeTmp.PY) ||
(/*(fNodeTmp.PY != fNode.y || mGrid[fNodeTmp.PX, fNodeTmp.PY - 1] != 0) && */ fNodeTmp.JumpLength == cLedgeGrabJumpValue /* && !mMap.IsGround(fNode.x, fNode.y - 1)*/) ||
(fNode.y < mClose[mClose.Count - 1].y && fNode.y < fNodeTmp.PY) ||
((mMap.IsGround(fNode.x - 1, fNode.y) || mMap.IsGround(fNode.x + 1, fNode.y)) &&
fNode.y != mClose[mClose.Count - 1].y && fNode.x != mClose[mClose.Count - 1].x))
{
mClose.Add(fNode);
}
fPrevNode = fNode;
posX = fNodeTmp.PX;
posY = fNodeTmp.PY;
fPrevNodeTmp = fNodeTmp;
fNodeTmp = fNextNodeTmp;
loc = (fNodeTmp.PY << mGridXLog2) + fNodeTmp.PX;
fNode = new Vector2i(posX, posY);
}
mClose.Add(fNode);
mStopped = true;
return(mClose);
}
mStopped = true;
return(null);
}
}
