How to Adapt A* Pathfinding to a 2D Grid-Based Platformer: Implementation

Now that we have a good idea of how our A* platforming algorithm will work, it's time to actually code it. Rather than build it from scratch, we'll adapt an existing A* pathfinding system to add the new platfomer compatibility.

The implementation we'll adapt in this tutorial is Gustavo Franco's grid-based A* pathfinding system, which is written in C#; if you're not familiar with it, read his explanation of all the separate parts before continuing. If you haven't read the previous tutorial in this series, which gives a general overview of how this adaptation will work, read that first.

Note: the complete source code for this tutorial can be found in this GitHub repo, in the Implementation folder. 

Demo

You can play the Unity demo, or the WebGL version (64MB), to see the final result in action. Use WASD to move the character, left-click on a spot to find a path you can follow to get there, and right-click a cell to toggle the ground at that point.

Setting Up the Game Project

We need to set some rules for the example game project used in this tutorial. You can of course change these rules when implementing this algorithm in your own game!

Setting Up the Physics

The physics rules used in the example project are very simple. 

When it comes to horizontal speed, there is no momentum at all. The character can change directions immediately, and immediately moves in that direction at full speed. This makes it much easier for the algorithm to find a correct path, because we don't have to care about the character's current horizontal speed. It also makes it easier to create a path-following AI, because we don't have to make the character gain any momentum before performing a jump.

We use four constants to define and restrict character movement:

• Gravity
• Maximum falling speed
• Walking speed
• Jumping speed

Here's how they're defined for this example project:

public const float cGravity = -1030.0f;
public const float cMaxFallingSpeed = -900.0f;
public const float cWalkSpeed = 160.0f;
public const float cJumpSpeed = 410.0f;

The base unit used in the game is a pixel, so the character will move 160 pixels per second horizontally (when walking or jumping); when jumping, the character's vertical speed will be set to 410 pixels per second. In the test project the character's falling speed is limited to 900, so there is no possibility of it falling through the tiles. The gravitation is set to be -1030 pixels per second².

The character's jump height is not fixed: the longer the jump key is pressed, the higher the character will jump. This is achieved by setting the character's speed to be no more than 200 pixels per second once the jump key is no longer pressed:

if (!mInputs[(int)KeyInput.Jump] && mSpeed.y > 0.0f)
{
    mSpeed.y = Mathf.Min(mSpeed.y, 200.0f);
    mFramesFromJumpStart = 100;
}

Setting Up the Grid

The grid is a simple array of bytes which represent the cost of movement to a specific cell (where 0 is reserved for blocks which the character cannot move through). 

We will not go deep into the weights in this tutorial; we'll actually be using just two values: 0 for solid blocks, and 1 for empty cells. 

The algorithm used in this tutorial requires the grid's width and height to be a power of two, so keep that in mind.

Here's an example of a grid array and an in-game representation of it.

private byte[,] mGrid = {{ 0, 1, 1, 1 }
                         { 0, 1, 1, 1 }
                         { 0, 1, 0, 0 }
                         { 0, 0, 0, 0 }};

A Note on Threading

Normally we would set up the pathfinder process in a separate thread, so there are no freezes in the game while it is working, but in this tutorial we'll use a single threaded version, due to the limitations of the WebGL platform which this demo runs on. 

The algorithm itself can be run in a separate thread, so you should have no problems with merging it into your code in that way if you need to.

Adding the Jump Values to the Nodes

Remember from the theory overview that nodes are distinguished not just by x- and y-coordinates, but also by jump values. In the standard implementation of A*, x- and y-coordinates are sufficient to define a node, so we need to modify it to use jump values as well.

From this point on, we'll be modifying the core PathFinderFast.cs source file from Gustavo Franco's implementation.

Re-Structuring the List of Nodes

First, we'll add a new list of nodes for each grid cell; this list will replace mCalcGrid from the original algorithm:

private List<PathFinderNodeFast>[] nodes;

Note that PathFinderFast uses one-dimensional arrays, rather than a 2D array as we might expect to use when representing a grid. 

This is not a problem, because we know the grid's width and height, so instead of accessing the data by X and Y indices, we'll access it by a single int which is calculated using  location = (y << gridWidthLog2) + x. (This is a slightly faster version of a classic location = (y * gridWidth) + x). 

Because we need a grid that is three-dimensional (to incorporate the jump values as a third "dimension"), we'll need to add another integer, which will be a node's index in a list at a particular X and Y position. 

Note that we cannot merge all three coordinates into one integer, because the third dimension of the grid is not a constant size. We could consider using simply a three-dimensional grid, which would restrict the number of nodes possible at a particular (x, y) position—but if the array size on the "z-axis" were too small, then the algorithm could return an incorrect result, so we'll play it safe.

Here, then, is the struct that we will use to index a particular node:

public struct Location
{
    public Location(int xy, int z)
    {
        this.xy = xy;
        this.z = z;
    }

    public int xy;
    public int z;
}

The next thing we need to modify is the PathFinderNodeFast structure. There are two things we need to add here:

• The first is the index of a node's parent, which is basically the previous node from which we arrived to the current node. We need to have that index since we cannot identify the parent solely by its x- and y-coordinates. The x- and y-coordinates will point us to a list of nodes that are at that specific position, so we also need to know the index of our parent in that list. We'll name that index PZ.
• The other thing we need to add to the structure is the jump value.

Here's the old struct:

internal struct PathFinderNodeFast
{
    public int     F;
    public int     G;
    public ushort  PX;      //parent x
    public ushort  PY;      //parent y
    public byte    Status;
}

And here's what we'll modify it to:

internal struct PathFinderNodeFast
{
    public int     F;
    public int     G;
    public ushort  PX;          //parent y
    public ushort  PY;          //parent x
    public byte    Status;
    public byte    PZ;          //parent z
    public short   JumpLength;  //jump value
}

There's still one problem, though. When we use the algorithm once, it will populate the cells' lists of nodes. We need to clear those lists after each time the pathfinder is run, because if we don't, then those lists will grow all the time with each use of the algorithm, and the memory use will rise uncontrollably. 

The thing is, we don't really want to clear every list every time the pathfinder is run, because the grid can be huge and the path will likely never touch most of the grid's nodes. It would cause a big overhead, so it's better to only clear the lists that the algorithm went through. 

For that, we need an additional container which we'll use to remember which cells were touched:

private Stack<int> touchedLocations;

A stack will work fine; we'll just need to clear all the lists contained in it one by one.

Updating the Priority Queue

Now let's get our priority queue mOpen to work with the new index. 

The first thing we need to do is change the declaration to use Locations rather than integers—so, from:

private PriorityQueueB<int> mOpen = null;

to:

private PriorityQueueB<Location> mOpen = null;

Next, we need to change the queue's comparer to make it use the new structure. Right now it uses just an array of nodes; we need to change it so it uses an array of lists of nodes instead. We also need to make sure it compares the nodes using a Location instead of just an integer.

Here's the old code:

internal class ComparePFNodeMatrix : IComparer<int>
{
    PathFinderNodeFast[] mMatrix;

    public ComparePFNodeMatrix(PathFinderNodeFast[] matrix)
    {
        mMatrix = matrix;
    }

    public int Compare(int a, int b)
    {
        if (mMatrix[a].F > mMatrix[b].F)
            return 1;
        else if (mMatrix[a].F < mMatrix[b].F)
            return -1;
        return 0;
    }
}

And here's the new:

internal class ComparePFNodeMatrix : IComparer<Location>
{
    List<PathFinderNodeFast>[] mMatrix;
    public ComparePFNodeMatrix(List<PathFinderNodeFast>[] matrix)
    {
        mMatrix = matrix;
    }

    public int Compare(Location a, Location b)
    {
        if (mMatrix[a.xy][a.z].F > mMatrix[b.xy][b.z].F)
            return 1;
        else if (mMatrix[a.xy][a.z].F < mMatrix[b.xy][b.z].F)
            return -1;
        return 0;
    }
}

Now, let's initialize the lists of nodes and the touched locations stack when the pathfinder is created. Again, here's the old code:

if (mCalcGrid == null || mCalcGrid.Length != (mGridX * mGridY))
{
    mCalcGrid = new PathFinderNodeFast[mGridX * mGridY];
    mClose = new List<Vector2i>(mGridX * mGridY);
}

And here's the new:

if (others == null || others.Length != (mGridX * mGridY))
{
	nodes = new List<PathFinderNodeFast>[mGridX * mGridY];
    touchedLocations = new Stack<int>(mGridX * mGridY);
    mClose = new List<Vector2i>(mGridX * mGridY);
}

for (var i = 0; i < others.Length; ++i)
{
	nodes[i] = new List<PathFinderNodeFast>(1);
}

Finally, let's create our priority queue using the new constructor:

mOpen = new PriorityQueueB<Location>(new ComparePFNodeMatrix(nodes));

Initializing the Algorithm

When we start the algorithm, we want to tell it how big our character is (in cells) and also how high the character can jump. 

(Note that, for this tutorial, we will not actually use characterWidth nor characterHeight; we will assume that the size of the character is a 1x1 block.)

Change this line:

public List<PathFinderNode> FindPath(Point start, Point end)

To this:

public List<Vector2i> FindPath(Vector2i start, Vector2i end, int characterWidth, int characterHeight, short maxCharacterJumpHeight)

First thing we need to do is clear the lists at the previously touched locations:

while (touchedLocations.Count > 0)
    others[touchedLocations.Pop()].Clear();

Next, we must make sure that the character can fit in the end location. (If it can't, there's no point in running the algorithm, because it will be impossible to find a valid path.)

if (mGrid[end.x, end.y] == 0)
	return null;

Now we can create a start node. Instead of setting the values in mCalcGrid, we need to add a node to the nodes list at a particular position. 

First, let's calculate the location of the node. Of course, to be able to do this, we also need to change the type of mLocation to Location.

Change this line:

mLocation = (start.y << mGridXLog2) + start.x;

To this:

mLocation.xy = (start.y << mGridXLog2) + start.x;
mLocation.z = 0;

The mEndLocation can be left as-is; we'll use this to check if we have already reached our goal, so we only need to check the X and Y positions in that case:

mEndLocation = (end.y << mGridXLog2) + end.x;

For the start node initialization, we need to reset the parent PZ to 0 and set the appropriate jump value. 

When the starting point is on the ground, the jump value should be equal to 0—but what if we're starting in the air? The simplest solution will be to set it to the falling value and not to worry about it too much; finding a path when starting in mid-air may be quite troublesome, so we'll take the easy way out.

Here's the old code:

mLocation                      = (start.y << mGridXLog2) + start.x;
mEndLocation                   = (end.y << mGridXLog2) + 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;

And the new:

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;

if (mMap.IsGround(start.x, start.y - 1))
    firstNode.JumpLength = 0;
else
	firstNode.JumpLength = (short)(maxCharacterJumpHeight * 2);

We must also add the node to the list at the start position:

nodes[mLocation.xy].Add(firstNode);

And we also need to remember that the start node list is to be cleared on the next run:

touchedLocations.Push(mLocation.xy);

Finally, the location is queued and we can start with the core algorithm. To sum up, this is what the initialization of the pathfinder run should look like:

while (touchedLocations.Count > 0)
    nodes[touchedLocations.Pop()].Clear();

if (mGrid[end.x, end.y] == 0)
	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;

if (mMap.IsGround(start.x, start.y - 1))
	firstNode.JumpLength = 0;
else
	firstNode.JumpLength = (short)(maxCharacterJumpHeight * 2);

nodes[mLocation.xy].Add(firstNode);
touchedLocations.Push(mLocation.xy);

mOpen.Push(mLocation);

Calculating a Successor

Checking the Position

We don't need to modify much in the node processing part; we just need to change mLocation to mLocation.xy so that mLocationX and mLocationY can be calculated.

Change this:

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 >> mGridXLog2);

    if (mLocation == mEndLocation)
    {
        mCalcGrid[mLocation].Status = mCloseNodeValue;
        mFound = true;
        break;
    }

    if (mCloseNodeCounter > mSearchLimit)
    {
        mStopped = true;
        return null;
    }

To this:

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;
    }

Note that, when we change the status of a node inside the nodes list, we use the UpdateStatus(byte newStatus) function. We cannot really change any of the members of the struct inside the list, since the list returns a copy of the node; we need to replace the whole node. The function simply returns a copied node with the Status changed to newStatus:

public PathFinderNodeFast UpdateStatus(byte newStatus)
{
    PathFinderNodeFast newNode = this;
    newNode.Status = newStatus;
    return newNode;
}

We also need to alter the way the successors are calculated:

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;

Here we just calculate the location of one of the successors; we need this so that we can find the relative position of the successor node.

Determining the Type of a Position

The next thing we need to know is what type of position a successor represents. 

We are interested in four variants here:

1. The character does not fit in the position. We assume that the cell position responds to the bottom-left "cell" of a character. If this is the case, then we can discard the successor, since there is no way to move the character through it.
2. The character fits in the position, and is on the ground. This means that the successor's jump value needs to be changed to 0.
3. The character fits in the position, and is just below the ceiling. This means that even if the character has enough speed to jump higher, it cannot, so we need to change the jump value appropriately.
4. The character simply fits in the spot and is neither on the ground nor at the ceiling.

First, let's assume that the character is neither on the ground nor at the ceiling:

var onGround = false;
var atCeiling = false;

Let's check if the character fits the new spot. If not, we can safely skip the successor and check the next one.

if (mGrid[mNewLocationX, mNewLocationY] == 0)
    continue;

To check if the character would be on the ground when in the new location, we just need to see if there's a solid block below the successor:

if (mGrid[mNewLocationX, mNewLocationY - 1] == 0)
    onGround = true;

Similarly, we check whether the character would be at the ceiling:

if (mGrid[mNewLocationX, mNewLocationY + 1] == 0)
    atCeiling = true;    

Calculating the Jump Value

The next thing we need to do is see whether this successor is a valid one or not, and if it is then calculate an appropriate JumpLength for it.

First, let's get the JumpLength of the parent node:

var jumpLength = nodes[mLocation.xy][mLocation.z].JumpLength;

We'll also declare the newJumpLength for the currently processed successor:

short newJumpLength = jumpLength;

Now we can calculate the newJumpLength. (How we do this is explained at the very beginning of the theory overview.)

If the successor node is on the ground, then the newJumpValue is equal to 0:

if (onGround)
    newJumpLength = 0;

Nothing to worry about here. It's important to check whether the character is on the ground first, because if the character is both on the ground and at the ceiling then we want to set the jump value to 0.

If  the position is at the ceiling then we need  to consider two cases: 

1. the character needs to drop straight down, or 
2. the character can still move one cell to either side.

In the first case, we need to set the newJumpLength to be at least maxCharacterJumpHeight * 2 + 1, because this value means that we are falling and our next move needs to be done vertically. 

In the second case, the value needs to be at least maxCharacterJumpHeight * 2. Since the value is even, the successor node will still be able to move either left or right.

else if (atCeiling)
{
    if (mNewLocationX != mLocationX)
        newJumpLength = (short)Mathf.Max(maxCharacterJumpHeight * 2 + 1, jumpLength + 1);
    else
        newJumpLength = (short)Mathf.Max(maxCharacterJumpHeight * 2, jumpLength + 2);
}

The "on ground" and "at ceiling" cases are solved; now we can get to calculating the jump value while in air.

Calculating the Jump Value in Mid-Air

First, let's handle the case in which the successor node is above the parent node.

If the jump length is even, then we increment it by two; otherwise, we increment it by one. This will result in an even value for newJumpLength:

else if (mNewLocationY > mLocationY)
{
	if (jumpLength % 2 == 0)
		newJumpLength = (short)(jumpLength + 2);
	else
		newJumpLength = (short)(jumpLength + 1);
}

Since, in an average jump, the character speed has its highest value at the jump start and end, we should represent this fact in the algorithm. 

We'll fix the jump start by forcing the algorithm to move two cells up if we just got off the ground. This can easily be achieved by swapping the jump value of 2 to 3 at the moment of the jump, because at the jump value of 3, the algorithm knows the character cannot go to the side (since 3 is an odd number). 

The jump curve will be changed to the following.

Let's also accommodate for this change in the code:

else if (mNewLocationY > mLocationY)
{
    if (jumpHeight < 2) //first jump is always two block up instead of one up and optionally one to either right or left
        newJumpHeight = 3;
	else if (jumpHeight % 2 == 0)
		newJumpHeight = (short)Mathf.Max(jumpHeight + 2, 2);
	else
		newJumpHeight = (short)Mathf.Max(jumpHeight + 1, 2);
}

(We'll fix the curve when the speed of the character is too high to go sideways when we validate the node later.)

Now let's handle the case in which the new node is below the previous one. 

If the new y-coordinate is lower than the parent's, that means we are falling. We calculate the jump value the same way we do when jumping up, but the minimum must be set to maxCharacterJumpHeight * 2. (That's because the character does not need to do a full jump to start falling—for example, it can simply walk off a ledge.) In that case the jump value should be changed from 1 to 6 immediately (in the case where the character's maximum jump height is 3):

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);
}

Validating the Successor

Now we have all the data we need to validate a successor, so let's get to it.

First, let's dismiss a node if its jump value is odd and the parent is either to the left or to the right. That's because if the jump value is odd, then that means the character went to the side once already and now it needs to move either one block up or down:

if (jumpLength % 2 != 0 && mLocationX != mNewLocationX)
    continue;

If the character is falling, and the child node is above the parent, then we should skip it. This is how we prevent jumping ad infinitum; once the jump value hits the threshold we can only go down.

if (jumpLength >= maxCharacterJumpHeight * 2 && mNewLocationY > mLocationY)
    continue;

If the node's jump value is larger than (six plus the fall threshold value), then we should stop allowing the direction change on every even jump value. This will prevent the algorithm giving incorrect values when the character is falling really fast, because in that case instead of 1 block to the side and 1 block down it would need to move 1 block to the side and 2 or more blocks down. (Right now, the character can move 3 blocks to the side after it starts falling, and then we allow it to move sideways every 4 blocks traversed vertically.)

if (newJumpLength >= maxCharacterJumpHeight * 2 + 6 && mNewLocationX != mLocationX && (newJumpLength - (maxCharacterJumpHeight * 2 + 6)) % 8 != 3)
    continue;

If there's a need for a more accurate jump check, then instead of dismissing the node in the way shown above, we could create a lookup table with data determining at which jump lengths the character would be able to move to the side.

Calculating the Cost

When calculating the node cost, we need to take into account the jump value.

Making the Character Move Sensibly

It's good to make the character stick to the ground as much as possible, because it will make its movement less jumpy when moving through flat terrain, and will also encourage it to use "safer" paths, which do not require long falls. 

We can easily make the character do this by increasing the cost of the node according to its jump value. Here's the old code:

mNewG = mCalcGrid[mLocation].G + mGrid[mNewLocationX, mNewLocationY];

And here's the new:

mNewG = nodes[mLocation.xy][mLocation.z].G + mGrid[mNewLocationX, mNewLocationY] + newJumpLength / 4;

The newJumpLength / 4 works well for most cases; we don't want the character to stick to the ground too much, after all.

Revisiting Nodes With Different Jump Values

Normally, when we've processed the node once, we set its status to closed and never bother with it again; however, as we've already discussed, we may need to visit a particular position in the grid more than once.

First, before we decide to skip the currently checked node, we need to see if there is any node at the current (x, y) position. If there are no nodes in there yet, then we surely cannot skip the current one:

if (nodes[mNewLocation].Count > 0)
{
}

The only condition which allows us to skip the node is this: the node does not allow for any new movement compared to the other nodes on the same position. 

The new movement can happen if:

• The currently processed node's jump value is lower than any of the other nodes at the same (x, y) position—in this case, the current node promises to let the character jump higher using this path than any other.
• The currently processed node's jump value is even, and all other nodes' jump values at the position are not. This basically means that this particular node allows for sideways movement at this position, while others force us to move either up or down.

The first case is simple: we want to look through the nodes with lower jump values since these let us jump higher. The second case comes out in more peculiar situations, such as this one:

Here, we cannot move sideways when jumping up because we wanted to force the algorithm to go up twice after starting a jump. The problem is that, even when falling down, the algorithm would simply ignore the node with the jump value of 8, because we have already visited that position and the previous node had a lower jump value of 3. That's why in this case it's important to not skip the node with an even (and reasonably low) jump value.

First, let's declare our variables that will let us know what the lowest jump value at the current (x, y) position is, and whether any of the nodes there allow sideways movement:

int lowestJump = short.MaxValue;
bool couldMoveSideways = false;

Next, we need to iterate over all the nodes and set the declared variables to the appropriate values:

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;
}

As you can see, we not only check whether the node's jump value is even, but also whether the jump value is not too high to move sideways.

Finally, let's get to the condition which will decide whether we can skip the node or not:

if (lowestJump <= newJumpLength && (newJumpLength % 2 != 0 || newJumpLength >= maxCharacterJumpHeight * 2 + 6 || couldMoveSideways))
    continue;

As you can see, the node is skipped if the lowestJump is less or equal to the processed node's jump value and any of the other nodes in the list allowed for sideways movement.

We can leave the heuristic formula as-is; we don't need to change anything here:

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;
}

Tidying Up

Now, finally, since the node has passed all the checks, we can create an appropriate PathFinderNodeFast instance for it.

PathFinderNodeFast newNode = new PathFinderNodeFast();
newNode.JumpLength = newJumpLength;
newNode.PX = mLocationX;
newNode.PY = mLocationY;
newNode.PZ = (byte)mIdentifier.y;
newNode.G = mNewG;
newNode.F = mNewG + mH;
newNode.Status = mOpenNodeValue;

And we can also finally add the node to the node list at the mNewLocation. 

Before we do that, though, let's add the location to the touched locations stack if the list is empty. We'll know that we need to clear this location's list when we run the pathfinder again:

if (nodes[mNewLocation].Count == 0)
    touchedLocations.Push(mNewLocation);

nodes[mNewLocation].Add(newNode);
mOpen.Push(new Location(mNewLocation, nodes[mNewLocation].Count - 1));

After all the children have been processed, we can change the status of the parent to closed and increment the mCloseNodeCounter:

mCloseNodeCounter++;
nodes[mLocation.xy][mLocation.z] = nodes[mLocation.xy][mLocation.z].UpdateStatus(mCloseNodeValue);

In the end, the children's loop should look like this.

//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 onGround = false;
	var atCeiling = false;

    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)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 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 * (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;
}

Filtering the Nodes

We don't really need all the nodes that we'll get from the algorithm. Indeed, it will be much easier for us to write a path-following AI if we filter the nodes to a smaller set which we can work with more easily.

The node filtering process isn't actually part of the algorithm, but is rather an operation to prepare the output for further processing. It doesn't need to be executed in the PathFinderFast class itself, but that will be the most convenient place to do it for the purposes of this tutorial.

The node filtering can be done alongside the path following code; it is fairly unlikely that we'll filter the node set perfectly to suit our needs with our initial assumptions, so, often, a lot of tweaks will be needed. In this tutorial we'll go ahead and reduce the set to its final form right now, so later we can focus on the AI without having to modify the pathfinder class again.

We want our filter to let through any node that fulfills any of the following requirements:

1. It is the start node.
2. It is the end node.
3. It is a jump node.
4. It is a first in-air node in a side jump (a node with jump value equal to 3).
5. It is the landing node (a node that had a non-zeo jump value becomes 0).
6. It is the high point of the jump (the node between moving upwards and and falling downwards).
7. It is a node that goes around an obstacle.

Here are a couple of illustrations that show which nodes we want to keep. The red numbers show which of the above rules caused the filter to leave the node in the path:

Setting Up the Values

We filter the nodes as they get pushed to mClose list, so that means we'll go from the end node to the start node.

if (mFound)
{
    mClose.Clear();
    var posX = end.x;
    var posY = end.y;

Before we start the filtering process, we need to set up a few variables to keep track of the context of the filtered node:

var fPrevNodeTmp = new PathFinderNodeFast();
var fNodeTmp = nodes[mEndLocation][0];

var fNode = end;
var fPrevNode = end;

fNode and fPrevNode are simple Vector2s, while fNodeTmp and fPrevNodeTmp are the PathFinderNodeFast nodes. We need both; we'll be using Vector2s to get the position of the nodes and PathFinderNodeFast objects to get the parent location, jump value, and everything else we'll need.

 var loc = (fNodeTmp.PY << mGridXLog2) + fNodeTmp.PX;

loc points to the XY position in the grid of the node that will be processed next iteration.

Defining the Loop

Now we can start our loop. We'll keep looping as long we don't get to the start node (at which point, the parent's position is equal to the node's position):

while(fNode.x != fNodeTmp.PX || fNode.y != fNodeTmp.PY)
{

We will need access to the next node as well as the previous one, so let's get it:

var fNextNodeTmp = nodes[loc][fNodeTmp.PZ];

Adding the End Node

Now let's start the filtering process. The start node will get added to the list at the very end, after all other items have been dealt with. Since we're going from the end node, let's be sure to include that one in our final path:

if ((mClose.Count == 0))
    mClose.Add(fNode);

If mClose is empty, that means we haven't pushed any nodes into it yet, which means the currently processed node is the end node, and since we want to include it in the final list, we add it to mClose.

Adding Jump Nodes

For the jump nodes, we'll want to use two conditions. 

The first condition is that the currently processed node's jump value is 0, and the previous node's jump value is not 0:

if ((mClose.Count == 0)
    || (fNodeTmp.JumpLength == 0 && fPrevNodeTmp.JumpLength != 0))
    mClose.Add(fNode);

The second condition is that the jump value is equal to 3. This basically is the first jump-up or first in-air direction change point in a particular jump:

if ((mClose.Count == 0)
    || (fNodeTmp.JumpLength == 0 && fPrevNodeTmp.JumpLength != 0)
    || (fNodeTmp.JumpLength == 3))
    mClose.Add(fNode);

Adding Landing Nodes

Now for the landing nodes:

if ((mClose.Count == 0)
    || (fNodeTmp.JumpLength == 0 && fPrevNodeTmp.JumpLength != 0)
    || (fNodeTmp.JumpLength == 3)
    || (fNextNodeTmp.JumpLength != 0 && fNodeTmp.JumpLength == 0)   )
    mClose.Add(fNode);

We detect the landing node by seeing that the next node is on the ground and the current node isn't. Remember that we are processing the nodes in reversed order, so in fact the landing is detected when the previous node is on the ground and the current isn't.

Adding Highest Point Nodes

Now let's add the jump high points. We detect these by seeing if both the previous and the next nodes are lower than the current node:

if ((mClose.Count == 0)
    || (fNodeTmp.JumpLength == 0 && fPrevNodeTmp.JumpLength != 0)
    || (fNodeTmp.JumpLength == 3)
    || (fNextNodeTmp.JumpLength != 0 && fNodeTmp.JumpLength == 0)
    || (fNode.y > mClose[mClose.Count - 1].y && fNode.y > fNodeTmp.PY))
    mClose.Add(fNode);

Note that, in the last case, we don't compare the current node's y-coordinate to fPrevNode.y, but rather to the previous pushed node's y-coordinate. That's because it may be the case that the previous node is on the same height with the current one, if the character moved to the side to reach it.

Adding Nodes that Go Around Obstacles

Finally, let's take care of the nodes that let us maneuver around the obstacles. If we're next to an obstacle and the previous pushed node isn't aligned with the current one either horizontally or vertically, then we assume that this node will align us with the obstacle and let us move cleanly over it if need be:

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(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);

Preparing for the Next Loop

After adding a node to the mClose list or disregarding it, we need to prepare the variables for the next iteration:

    fPrevNode = fNode;
    posX = fNodeTmp.PX;
    posY = fNodeTmp.PY;
    fPrevNodeTmp = fNodeTmp;
    fNodeTmp = fNextNodeTmp;
    loc = (fNodeTmp.PY << mGridXLog2) + fNodeTmp.PX;
    fNode = new Vector2i(posX, posY);
} 

As you can see, we calculate everything in the same way we prepare the loop for the first iteration.

Adding the Start Node

After all the nodes have been processed (and the loop is finished), we can add the start point to the list and finish the job:

    mClose.Add(fNode);

    mStopped = true;

    return mClose;
}

All Together

The whole path filtering procedure should look like this.

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)
            || (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)
            || (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;
}

Conclusion

The final product of the algorithm is a path found for a character which is one block wide and one block high with a defined maximum jump height. 

We can improve on this: we could allow the character's size to be varied, we could add support for one-way platforms, and we could code an AI bot to follow the path. We will address all of these things in the next part of the tutorial!