public List <Point> CalculatePath(Point start, Point end, int characterWidth, int characterHeight, short maxJumpHeight)
{
lock (this) //locked to prevent multiple algorithms from running at the same time
{
//clear the lists at the previously touched locations
while (TouchedLocations.Count > 0)
{
nodes[TouchedLocations.Pop()].Clear();
}
//check if the bottom right of the character will be able to fit in the goal
bool inSolidTile = false;
for (var i = 0; i < characterWidth; ++i)
{
inSolidTile = false;
//check characterWidth number of nodes to the right of the goal
for (var w = 0; w < characterWidth; ++w)
{
if (Grid[end.X + w, end.Y] == 0 || Grid[end.X + w, end.Y - characterHeight + 1] == 0)
{
inSolidTile = true;
break;
}
}
if (inSolidTile == false)
{
//check characterHeight number of blocks
for (var h = 0; h < characterHeight; ++h)
{
if (Grid[end.X, end.Y - h] == 0 || Grid[end.X + characterWidth - 1, end.Y - h] == 0)
{
inSolidTile = true;
break;
}
}
}
if (inSolidTile)
{
end.X -= characterWidth - 1;
}
else
{
break;
}
}
if (inSolidTile == true)
{
Main.NewText("Character cannot fit in end location, exiting...");
return(null);
}
Found = false;
Stop = false;
mStopped = false;
CloseNodeCounter = 0;
OpenNodeValue += 2;
CloseNodeValue += 2;
Open.Clear();
Location.xy = (start.Y << GridXLog2) + start.X; //find starting node location in grid
Location.z = 0;
EndLocation = (end.Y << GridXLog2) + end.X; //do the same for the end location
Node firstNode = new Node(); //this is the start node
firstNode.G = 0;
firstNode.F = mHEstimate;
firstNode.PX = (ushort)start.X;
firstNode.PY = (ushort)start.Y;
firstNode.PZ = 0;
firstNode.Status = OpenNodeValue;
//check all nodes beneath the character to see if they are on the ground
bool startsOnGround = false;
for (int x = start.X; x < start.X + characterWidth; ++x)
{
if (Map.IsGround(x, start.Y + 1))
{
startsOnGround = true;
break;
}
}
if (startsOnGround)
{
firstNode.JumpLength = 0;
}
else
{
firstNode.JumpLength = (short)(maxJumpHeight * 2);
}
nodes[Location.xy].Add(firstNode);
TouchedLocations.Push(Location.xy);
Open.Push(Location); //push the starting node into the open set
//the actual algorithm starts here
while (Open.Count > 0 && !Stop)
{
Location = Open.Pop();
//is it in closed list? means this node was already processed and skip this iteration
if (nodes[Location.xy][Location.z].Status == CloseNodeValue)
{
continue;
}
//calculate node we are evaulating
LocationX = (ushort)(Location.xy & GridXMinus1);
LocationY = (ushort)(Location.xy >> GridXLog2);
//current node is the end location
if (Location.xy == EndLocation)
{
nodes[Location.xy][Location.z] = nodes[Location.xy][Location.z].UpdateStatus(CloseNodeValue);
Found = true;
break;
}
//closed nodes has reach threshold, either no path possible or just too many darn nodes to sift through
if (CloseNodeCounter > mSearchLimit)
{
Main.NewText("Hit maximum search limit threshold, exiting...");
mStopped = true;
return(null);
}
//calculate the nodes around the current one
for (var i = 0; i < (mDiagonals ? 8 : 4); i++)
{
NewLocationX = (ushort)(LocationX + mDirection[i, 0]);
NewLocationY = (ushort)(LocationY + mDirection[i, 1]);
NewLocation = (NewLocationY << GridXLog2) + NewLocationX;
var onGround = false;
var atCeiling = false;
for (var w = 0; w < characterWidth; ++w)
{
//check if top most and bottom most blocks of the character to see if they are a solid block
if (Grid[NewLocationX + w, NewLocationY] == 0 || Grid[NewLocationX + w, NewLocationY - characterHeight + 1] == 0)
{
goto CHILDREN_LOOP_END;
}
//any of the bottom nodes right above the ground (air block between them) then it is onGround
if (Map.IsGround(NewLocationX + w, NewLocationY + 1))
{
onGround = true;
}
else if (Grid[NewLocationX + w, NewLocationY - characterHeight] == 0) //any tiles above the character are solid then character would be at ceiling
{
atCeiling = true;
}
}
//check the left and right cells of the character, skip if they are blocks b/c character won't fit in that position
for (var h = 1; h < characterHeight - 1; ++h)
{
if (Grid[NewLocationX, NewLocationY - h] == 0 || Grid[NewLocationX + characterHeight - 1, NewLocationY - h] == 0)
{
goto CHILDREN_LOOP_END;
}
}
//calculate jumplength value for neighbor node
var jumpLength = nodes[Location.xy][Location.z].JumpLength;
short newJumpLength = jumpLength;
if (atCeiling)
{
if (NewLocationX != LocationX)
{
//character needs to drop straight down
//we are falling and our next move needs to be done vertically
newJumpLength = (short)Math.Max(maxJumpHeight * 2 + 1, jumpLength + 1);
}
else
{
//character can still move one cell to either side
//since value is even, the neighbor node will still be able to move either left or right
newJumpLength = (short)Math.Max(maxJumpHeight * 2, jumpLength + 2);
}
}
else if (onGround)
{
newJumpLength = 0;
}
//calcualting jump value mid-air
else if (NewLocationY < LocationY) // neighbor node is above parent node
{
if (jumpLength < 2) //first jump is always two blocks up instead of one up and optionally one to either right or left
{
newJumpLength = 3;
}
else if (jumpLength % 2 == 0)
{
//jump length is even, increment by 2 otherwise increment by 1
//this is to handle jumps that are straight up
newJumpLength = (short)(jumpLength + 2);
}
else
{
newJumpLength = (short)(jumpLength + 1);
}
}
else if (NewLocationY > LocationY) //neighbor node is below the parent
{
//same calculation for vertical jumps, just downwards
if (jumpLength % 2 == 0)
{
newJumpLength = (short)Math.Max(maxJumpHeight * 2, jumpLength + 2);
}
else
{
newJumpLength = (short)Math.Max(maxJumpHeight * 2, jumpLength + 1);
}
}
else if (!onGround && NewLocationX != LocationX) //node is to the left or right of parent node, same y-axis
{
newJumpLength = (short)(jumpLength + 1);
}
//dismiss this node if it's jump value is odd and the parent is either to the left/right
//character went to side already and needs to move up/down
if (jumpLength >= 0 && jumpLength % 2 != 0 && LocationX != NewLocationX)
{
continue;
}
//if we're falling and neighbor node is higher, skip impossible jump
if (jumpLength >= maxJumpHeight * 2 && NewLocationY < LocationY)
{
continue;
}
//prevent giving incorrect values when the character is falling really fast
//without this, character would be moving 1 block to side and 2 or more blocks down instead of 1 block to side and 1 block down
if (newJumpLength >= maxJumpHeight * 2 + 6 && NewLocationX != LocationX && (newJumpLength - (maxJumpHeight * 2 + 6)) % 8 != 3)
{
continue;
}
//cost higher if jump value higher
//divide by 4 to make character stick to ground more often and not get 2 jumpy
NewG = nodes[Location.xy][Location.z].G + Grid[NewLocationX, NewLocationY] + newJumpLength / 4;
//revisit nodes with different jump values
if (nodes[NewLocation].Count > 0)
{
int lowestJump = short.MaxValue;
bool couldMoveSideways = false;
for (int j = 0; j < nodes[NewLocation].Count; ++j)
{
if (nodes[NewLocation][j].JumpLength < lowestJump)
{
lowestJump = nodes[NewLocation][j].JumpLength;
}
if (nodes[NewLocation][j].JumpLength % 2 == 0 && nodes[NewLocation][j].JumpLength < maxJumpHeight * 2 + 6)
{
couldMoveSideways = true;
}
}
//skip node if jump value isn't lower than any of the other nodes at the same x, y (doesn't promise higher jump)
//and the currently processed node's jump value is even and all the others aren't (node allows for sideways movement while others only allow up or down)
if (lowestJump <= newJumpLength && (newJumpLength % 2 != 0 || newJumpLength >= maxJumpHeight * 2 + 6 || couldMoveSideways))
{
continue;
}
}
//calculate H (heuristic) cost
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:
var h_diagonal = Math.Min(Math.Abs(NewLocationX - end.X), Math.Abs(NewLocationY - end.Y));
var 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(NewLocationY - end.X, 2) + Math.Pow(NewLocationY - end.Y, 2)));
break;
case HeuristicFormula.Custom1:
var dxy = new Point(Math.Abs(end.X - NewLocationX), Math.Abs(end.Y - NewLocationY));
var orthogonal = Math.Abs(dxy.X - dxy.Y);
var diagonal = Math.Abs((dxy.X + dxy.Y - orthogonal) / 2);
H = mHEstimate * (diagonal + orthogonal + dxy.X + dxy.Y);
break;
}
//add node to the node list after checks and calculations, phew...
Node newNode = new Node();
newNode.JumpLength = newJumpLength;
newNode.PX = LocationX;
newNode.PY = LocationY;
newNode.PZ = (byte)Location.z;
newNode.G = NewG;
newNode.F = NewG + H;
newNode.Status = OpenNodeValue;
if (nodes[NewLocation].Count == 0)
{
TouchedLocations.Push(NewLocation);
}
nodes[NewLocation].Add(newNode);
Open.Push(new Location(NewLocation, nodes[NewLocation].Count - 1));
//removes the need to break loop and continue to next node
CHILDREN_LOOP_END:
continue;
}
//set status of parent node to "closed"
nodes[Location.xy][Location.z] = nodes[Location.xy][Location.z].UpdateStatus(CloseNodeValue);
CloseNodeCounter++;
}
//filter only necessary nodes
if (Found)
{
//start at the end
Close.Clear();
int posX = end.X;
int posY = end.Y;
Node fPrevNodeTmp = new Node();
Node fNodeTmp = nodes[EndLocation][0];
Point fNode = end;
Point fPrevNode = end;
//points to the next node to be evaluated
var loc = (fNodeTmp.PY << GridXLog2) + fNodeTmp.PX;
//keep going until parent's position == node's position (we hit the start node)
while (fNode.X != fNodeTmp.PX || fNode.Y != fNodeTmp.PY)
{
Node fNextNodeTmp = nodes[loc][fNodeTmp.PZ];
if (Close.Count == 0 || //add end node
Map.IsOneWayPlatform(fNode.X, fNode.Y + 1) ||
(Grid[fNode.X, fNode.Y + 1] == 0 && Map.IsOneWayPlatform(fPrevNode.X, fPrevNode.Y + 1)) ||
fNodeTmp.JumpLength == 3 || //add first jump up or first in air direction change
(fNextNodeTmp.JumpLength != 0 && fNodeTmp.JumpLength == 0) || //add landing node (node that has non-zero jump value becomes 0)
(fNodeTmp.JumpLength == 0 && fPrevNodeTmp.JumpLength != 0) //next node is on ground while next one isn't (landing node)
//node y-coordinate is higher than previous and next node in closed list (highest point of jump)
|| (fNode.Y > Close[Close.Count - 1].Y && fNode.Y > fNodeTmp.PY) ||
(fNode.Y < Close[Close.Count - 1].Y && fNode.Y < fNodeTmp.PY)
//next to an obstacle and previous node isn't aligned with current one either horizontally or vertically (went around an obstacle)
|| ((Map.IsGround(fNode.X - 1, fNode.Y) || Map.IsGround(fNode.X + 1, fNode.Y)) &&
fNode.Y != Close[Close.Count - 1].Y && fNode.X != Close[Close.Count - 1].X))
{
Close.Add(fNode);
}
fPrevNode = fNode;
posX = fNodeTmp.PX;
posY = fNodeTmp.PY;
fPrevNodeTmp = fNodeTmp;
fNodeTmp = fNextNodeTmp;
loc = (fNodeTmp.PY << GridXLog2) + fNodeTmp.PX;
fNode = new Point(posX, posY);
}
Close.Add(fNode); //at start of list which means fNode = start node
mStopped = true;
return(Close);
}
Main.NewText("No path found, exiting...");
mStopped = true;
return(null);
}
}
