2 – Implementation

Now that we’re read up on the who, what, and why, it’s time we moved into the how.

But, before we start dipping into pathfinding, we need a path with a Start and and a Goal, right? So, we need a map!

Ladies and Gents, welcome to the mundane world of Programma Arte!

Eat your heart out, Tolken!

Beautiful, isn’t it? It’s a simple world, to say the least, with a few different tile types. These tiles will determine our Heuristic Cost when traversing the world.

  • The Grass Tiles are the easiest to traverse. They represent plain fields. Heuristic Cost: 0.0.
  • The Sand Tiles are only slightly more difficult to traverse than Grass. Walking in soft sand gives under your footstep, meaning you need more energy to push off in each step. Heuristic Cost: 0.5.
  • The Snow Tiles are cold and harsh. In some areas, it’s the only way to get around. Heuristic Cost: 10.0.
  • The Mountain Tiles are rocky, rough, and rugged. These areas should be avoided, and our agents will struggle traversing this terrain. Heuristic Cost: 100.0.
  • The Water Tiles are impassible by most. All that armor will have you sink like a rock. Heuristic Cost: 999.0.
  • The Town Tiles are actually quite nice to visit! Taverns, shops, NPCs… It’s like Castlevania 2! Heuristic Cost: -0.5.
  • The Bridge Tiles provide some easy access over the Water, just make sure you don’t fall off! Heuristic Cost: 0.5.

Now that we have our map, lets define our Waypoints!

Waypoint to Waypoint

Alright, so, what is a Waypoint, right? Might as well add this to our Glossary, as we’re going to be using it a lot. A Waypoint is a connecting point on a path. They will be our start points, our goal points, and all the points in between. Our Waypoints contain 3 members: a 3D Point, a Heuristic Value (that we can set manually), and list of surrounding connections.

As I mentioned before, this is using Unity. For those unfamiliar, Unity attaches a Transform to each GameObject in the Scene, so we’ll already have a 3D Point (or a Vector3 in Unity terms). So here is the Waypoint code:

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class Waypoint : MonoBehaviour {

    public Waypoint[] connections;
    public float heuristic = 0.0f;

    public Color DebugColor = Color.grey;

    private void OnDrawGizmos()
        Gizmos.color = DebugColor;
        Gizmos.DrawSphere(transform.position, 1f);

Now, You’ll notice I have a DebugColor also on this script. This is for Unity’s Editor so that I can have a visual when editing these Waypoints. The OnDrawGizmos function is only for the Unity Editor, so I can draw little colored spheres for my Waypoints. So, really, it is just the Connections, the Heuristic, and the members inherited from Monobehaviour.

So, now that we have our Waypoint class, lets place some Waypoints! For Programma Arte, I manually painstakingly placed each Waypoint and connected them by hand (You’re lucky I love you people (lets just be friends)).

16 x 16 Grid... You're welcome.

To help (slightly) with this, Unity does have an Editor API that can build tools and visual effects for you. With this, I was able to draw the tiny little arrows (it’s not much, but it helped, trust me). For any other Unity folks out there, maybe this will help you, too:

using System.Collections;
using System.Collections.Generic;
using UnityEngine;
using UnityEditor;

public class WaypointEditor : Editor
    float size = 3f;

    protected virtual void OnSceneGUI()
        Handles.color = ((Waypoint)target).DebugColor;
        Transform transform = ((Waypoint)target).transform;
        foreach (Waypoint wp in ((Waypoint)target).connections)
            if (wp == null)
                Quaternion.LookRotation(wp.transform.position - transform.position),

    private void DebugDraw()
        if (Event.current.type == EventType.Repaint)
            Transform transform = ((Waypoint)target).transform;
            Handles.color = Handles.xAxisColor;
                transform.position + new Vector3(3f, 0f, 0f),
                transform.rotation * Quaternion.LookRotation(Vector3.right),
            Handles.color = Handles.yAxisColor;
                transform.position + new Vector3(0f, 3f, 0f),
                transform.rotation * Quaternion.LookRotation(Vector3.up),
            Handles.color = Handles.zAxisColor;
                transform.position + new Vector3(0f, 0f, 3f),
                transform.rotation * Quaternion.LookRotation(Vector3.forward),

So, there we go. Our Waypoints are placed and set up properly (well, mine are. See you in 45 minutes, chump). Now, lets put them to work.

Release the KMStar

Yes. I called it KMStar. Insert narcissistic/megalomaniac joke here, and lets move on.

Here is our beast. The engine. The brain. This class has really just 2 jobs: contain a list of all Waypoints, and provide a function for other objects to call to generate a path to their goal. Once the path is created, it’ll be up to the AI Agents to figure out what to do with them.

The first things I want to cover are the Data Structures we’ll be using. We just have one nested enum, and one nested class in our KMStar class.

This is our enum, HeuristicAlgorithm:

public enum HeuristicAlgorithm

Remember how I told you that the Heuristic Calculation is where we got to play? Well, these enum values will act on a switch that will control how we play. This list is also shorter than the one I have in my downloadable example. I’ll let you guys play around with it. I’ll explain what each one of these are when we get to the CalculateHeuristic function.

Next up is our nested Node class. “Now hold on, there, Keith,” I hear you say, rudely interrupting me, “We already have a Waypoint class. Why do we need to have a Node class? Won’t that get confusing?” And I see what you’re getting at, but there’s a reason we have a Node and a Waypoint. The Node class is like an extended Waypoint to be used solely for the calculations done inside of our FindPath function. Lets take a moment and look at the class:

private class Node
	public Waypoint waypoint;
	public Node from;
	public float score;

	public Node(Waypoint w, Node n, float s)
		waypoint = w;
		from = n;
		score = s;

	public override string ToString()
		return string.Format("Node({0},{1})", waypoint.gameObject.name, score);

	public string DebugPath()
		return ToString() + " <- " + (from != null ? from.DebugPath() : "START");

So, yes, inside Node we have a waypoint member, but we also have a Node from member, too. Why do we need that? Well, when we calculate out the path, every time we add a new Waypoint to our Open list, we need to know where it came from. We can’t store this on the Waypoint itself, because, well, FindPath will be running multiple times on the same dataset at the same time, so the values will constantly be changing. We only use Nodes for our calculation inside FindPath per instance of the function getting called.

We also have a score member, too. The score is the return value from CalculateHeuristic, and we store these, mainly, so we don’t have to calculate them each time (because we really shouldn’t, since most of the HeuristicAlgorithms are based off of distance).

Alright, we have our 2 Data Structures. Now, get ready for the list of members:

public Waypoint[] wayPoints;

Wait, that’s it?!


So, KMStar doesn’t really have any members, because all the data it needs in order to run are provided to it via the parameters of FindPath. The only thing it needs are the WayPoints. Bonus: If you use Unity, the Waypoints can be stored as children under the GameObject that has KMStar as a component. That way, you can get all the Waypoints simply by doing this:

private void Awake()
	wayPoints = GetComponentsInChildren();

Anywho, moving on, lets get into the helper methods that we’ll be using in FindPath.

Helpful Helpers

Firstoff is a function called HScore. This function is so difficult, you guys:

private float HScore(Waypoint w, bool ignoreWaypointHeuristic = false)
    return (ignoreWaypointHeuristic ? 0 : w.heuristic);

Yeah, no, I lied. “Why do we even need this? It’s just a getter for the Waypoint‘s heuristic value, with an added boolean that makes it return 0 if true. Why!?” You’re really testing me with these sassy questions, man. This is a helper function. It’s purpose will be much clearer later in the code, but, if you must know, it helps our code look a lot cleaner. We’ll be using the Waypoint‘s heuristic value a lot, and we have the ability to ignore that value. Makes sense to have a small one-liner to wrap it in.

Alright, next up is BuildFinalPath. We haven’t even found the path yet, and we’re already talking about finalizing it. But I already told you how the final path is put together back in Concept And Facts, so you should know this. Once our algorithm reaches the goal, the path it build is backwards, so we need to reverse the path via the from Nodes. Our FindPath algorithm, also, is only working in Nodes, and we need to return Waypoints. So this function removes our Node wrapper and gives us just the chocolatey Waypoint inside.

private List BuildFinalPath(List nodes)
    List path = new List();
    Node n = nodes[nodes.Count - 1];
    while(n != null)
        //Debug.Log("Adding " + n + " to the path!");
        n = n.from;
    return path;

I also left in a few commented-out debug statements for you guys to test this all out with when you run it. Be careful, though, they’ll slow you down pretty bad!

Next up is the second most-important function in KMStar: CalculateHeuristic. This is where HeuristicAlgorithm comes into play. First, lets look at the parameters and return type:

private float CalculateHeuristic(HeuristicAlgorithm algorithm, 
                                 Node from, 
                                 Waypoint w1, 
                                 Waypoint w2, 
                                 bool ignoreWaypointHeuristic = false)

So, obviously, we’re returning the calculated score here. Just a simple float value. As for the parameters…

  • HeuristicAlgorithm algorithm – Hey, its our enum! This function actually does the math we need to calculate the score for traveling to that Waypoint!
  • Node from – In some of the calculations (but not all) we need to know the Node we’re coming from. We can’t just make Waypoints w1 and w2 Nodes, because then we’ll be creating unnecessary Nodes that’ll just fall out of scope. Best to keep this field in.
  • Waypoint w1 – This is the current Waypoint that we are checking. This might be a little confusing, but, if the last field was where we’re coming from, this the the field we’re going to (one of the adjacent to from).
  • Waypoint w2 – And this guy is the goal. This is our destination Waypoint. Basically it’s from -> w1 -> ... -> w2 (Goal).
  • bool ignoreWaypointHeuristic – This guy again… But this is where we actually use that HScore function for our calculations, so, this time, it’s moreso important.

Alright, we know what toys we have to play with, so lets see how to play.

First, we define some variables we’re going to use in some of our calculations.

float cost = 0, max = 0, min = 0, h1 = 0, h2 = 0;

Next, we begin our switch statement, with switch (algorithm) {


This is the basic functionality of the A* algorithm. This just performs the Vector Distance algorithm (which should be available in most 3D Math libraries, but, in case it isn’t, here you go) on the Waypoint being checked (w1) and the Goal (w2) and adds the HScore of w1. It’s very simple, and will get you the basic A* functionality.

 case HeuristicAlgorithm.Vector3Distance:
    return Vector3.Distance(w1.transform.position, w2.transform.position) 
           + HScore(w1, ignoreWaypointHeuristic);


Manhattan Distance algorithm is an interesting one. It works almost like the Vector3Distance algorithm, except that it’s designed for use on a grid (hence why it’s called Manhattan). So, think of this as just 4-way movement (very good if you’re making a tile-based game, or using limited hardware).

case HeuristicAlgorithm.Manhattan:
    return (Mathf.Abs(w1.transform.position.x - w2.transform.position.x) +
            Mathf.Abs(w1.transform.position.y - w2.transform.position.y) +
            Mathf.Abs(w1.transform.position.z - w2.transform.position.z)) 
            + HScore(w1, ignoreWaypointHeuristic);


Like I said, with this part, we get to have fun. This is an algorithm I made up, by getting the largest value of subtracting the 2 HScores of w1 and w2, and then adding that to the Distance. This mixes up the path a bit, so it won’t go right for the goal optimally.

case HeuristicAlgorithm.CostOfMovement:
    h1 = HScore(w1, ignoreWaypointHeuristic);
    h2 = HScore(w2, ignoreWaypointHeuristic);
    cost = Mathf.Max(Mathf.Abs(h2 - h1),
                     Mathf.Abs(h1 - h2));
    return Vector3.Distance(w1.transform.position, w2.transform.position) + cost;


Same deal as above, except that we multiply the HScores by -1, and then subtract to find the smallest value. This makes any Waypoint with a low heuristic appear very expensive, and vice-versa.

case HeuristicAlgorithm.InverseCOM:
    h1 = HScore(w1, ignoreWaypointHeuristic) * -1;
    h2 = HScore(w2, ignoreWaypointHeuristic) * -1;
    cost = Mathf.Min(h1 - h2, h2 - h1);
    return Vector3.Distance(w1.transform.position, w2.transform.position) + cost;


Screw it. Just return the HScore of w1 and move on.

case HeuristicAlgorithm.HScore:
    return HScore(w1, ignoreWaypointHeuristic);


And there we have it! Now, by all means, feel free to play around with these values! Make your own! There’s at least 4 more I made that I didn’t add in here (but are in the example). But now that our calculation function is defined, and we have our helpers, it’s time for the big one…


At this point, we know the algorithm, we have our helper functions, we know where and what the calculations are, so lets shut up already and get this sucker defined.

First thing first, lets look at the function name and all its glory.

public IEnumerator FindPath(Waypoint start, 
                            Waypoint end, 
                            System.Action pathCallback, 
                            HeuristicAlgorithm heuristicAlgorithm = HeuristicAlgorithm.Vector3Distance,
                            bool ignoreWaypointHeuristic = false)

OK, my non-Unity people will probably be scratching their heads at IEnumerator.

Real brief, Unity can use Coroutines as a means of breaking up a function’s execution over several frames, as to not entirely hose up an update loop for one function. An excerpt from the manual reads:

“A coroutine is like a function that has the ability to pause execution and return control to Unity but then to continue where it left off on the following frame.”

Their example is a Fade function, that decreases a color’s alpha value in a for loop over time. Since this is just a for loop, it’ll run til completion on that frame, meaning the color will look like it goes instantly from opaque to transparent. However, we can run this, instead, in a Coroutine, with the same for loop, but with a call to yield return null after each iteration of the loop. That’ll break up the functions execution across several frames, and it’ll appear to fade. Nifty! I recommend anyone unfamiliar with this concept to check it out. It’s not multithreading or hyperthreading. They also have plenty of faults, as well. Don’t just use them all willy-nilly, dagnabbit.

Anyway, lets get back to FindPath. In the parameters, we have 2 Waypoints, start and end. Pretty clear what these are supposed to be, at this point. But we also have this System.Action pathCallback thing. What is that for? Well, my little KMStarlettes (I’m not sorry), this is our callback function to return the finalized path (created by our helper, BuildFinalPath) to our interfacing object that needs the path. Since this is called in a Coroutine, we can’t simply return the path at the end of the function’s execution, because the program doesn’t know when it will finish executing! So we need this callback to bring it all back home. After that, we have 2 easy parameters: heuristicAlgorithm for setting our algorithm type, and… oh, it’s ignoreWaypointHeuristic again. Good to see you, again

Diving into FindPath, we start out C-style by defining all our variables at the top (Have you guys even seen C Code? Does anyone out there still use it?! What year is it?! You damn kids, get off my lawn!!!)

// Return Value (in Node form)
List path = new List();
// Open Waypoints
List open = new List();
// Closed Waypoints
Queue closed = new Queue();

Lets look at each one of these, beyond the simple comments:

  • List path – This variable really doesn’t get touched until we find the end Waypoint. Once we do, the end gets added to the path, and then our BuildFInalPath fills out the rest.
  • List open – Alright, so this is my fake Priority Queue. C# doesn’t really have a good one, unless you consider using Linq like I have. Linq makes things pretty stupid-easy when it comes to quickly sorting collections, at the cost of some overhead. For our example, it’ll work just fine. The open List will contain our adjacent Nodes that have had their score calculated, and will return to us the Node with the lowest score.
  • List closed – Once we process a Waypoint, we don’t want to travel back to it, so we close it off.

With all that out of the way, we start our algorithm! First thing we do, is we make our start point a Node and add it to our open List.

open.Add(new Node(start, null, CalculateHeuristic(heuristicAlgorithm, null, start, end, ignoreWaypointHeuristic)));

And now, all at once, lets check out the while loop that drives the heart of this beast.

while(open.Count > 0)
    // Step 1: Get the next waypoint to process, based on the heuristic
    Node nextWaypoint = null;
    // Our psuedo-Priority Queue Linq Query will order everything in the open list by score.
    // Meaning the top is the best score. Meaning no loop required to search.
    IEnumerable query = open.OrderBy(node => node.score);
    nextWaypoint = query.First();
    if(nextWaypoint.waypoint == end)
    if (nextWaypoint == null)
        yield return null;

    // Step 2: Add the connections that haven't been visited to the open queue
    foreach(Waypoint c in nextWaypoint.waypoint.connections)
        if(closed.Contains(c) == false)
            open.Add(new Node(c, nextWaypoint, CalculateHeuristic(heuristicAlgorithm, nextWaypoint, c, end, ignoreWaypointHeuristic)));
    yield return null;
yield return null;

There. Figure it out. Bye!

OK, sorry. Here’s the breakdown.

We start at our while loop. Simply put, we want to make sure that we still have Nodes to process. If we run out, if there is no possible way of reaching the goal, then the while loop will end, and we build a path from what we have. This shouldn’t ever be the case, but, hey, good to have a fallback option, right?

Once we’re in the while loop, we go into Step 1, and it flows just like the algorithm does. we use open.OrderBy (a Linq extension method) to get the Node with the lowest score. If that Node just so happens to be the end we’re looking for, then we break the loop and build our path! Yup, that’s the end, right there!

But lets say that we didn’t reach the end (which will happen 99% of the time), then, what we do is take the Node we just got, remove it from the open list, and — wait, quick check: if that Node somehow null‘d itself (did you make a dynamic Waypoint world? You sneaky devil), check it, and restart the loop if it is) — then we add our visited Node to the closed queue so we never have to see it again. Yay!

From there, we hit Step 2. We’re not completely done with our visited Node. We’re now going to rummage through all its connections and, if it’s not already in the closed queue, we add them to the open list. With all these new Nodes being made, we make sure to set our visited Node to the from member of the Node. After all that is done, we yield return null, and start the loop over again.

Ladies and Gents, that’s it. We now have our KMStar class set up, ready to go.

Now we just need to make some classes that will actually make it, you know, go.

>>> Coming up, Usage! >>>

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out /  Change )

Google photo

You are commenting using your Google account. Log Out /  Change )

Twitter picture

You are commenting using your Twitter account. Log Out /  Change )

Facebook photo

You are commenting using your Facebook account. Log Out /  Change )

Connecting to %s

This site uses Akismet to reduce spam. Learn how your comment data is processed.