Pathfinding – our own A* pathfinder

Using the built-in functions of the NavMesh package might be enough for some, but in many cases we need customized pathfinding for our projects. Knowing how to implement, or even better, understanding A* (a-star) pathfinding, can take us a long way in our AI endeavors. It's easy to implement and very versatile. Correctly set up, it will always find the shortest path (and pretty fast too!). One of the drawbacks is that it can be memory-intensive in large areas if not kept in check.

A* is an algorithm that finds the shortest path in a graph. It's good at finding this quickly using heuristics, or an estimation of the cost to get from a position in the graph to the goal position.

Finding a good value for the heuristic (H) is very important in order to make it effective. In technical terms, H needs to be admissible. This means that the estimated cost should never exceed the actual cost.

Each position, called a node, will keep track of how the cost from the starting node to itself, using the current path. It will then choose the next node to go to base on this, cost plus the cost to the next node plus the estimated cost to the goal node.

A* could be said to work something like this; imagine that we're trying to find our way to a castle, through a maze. We're at an intersection, and can choose either the left path or the right path. We can see the castle in the distance to our left. We don't know anything about either path beyond the point where we're standing, but at least, taking the left path brings us closer to the castle, so it's natural to test that path.

Now, it could very well be that the left path is wrong, and much longer. That's the reason it also keeps track of how far it's travelled along the path. This is called G. The longer it travels along a path, the higher G will become. If the path also starts to deviate from the way to the castle, H will rise again. At some point G plus H might be higher than it would be at the entrance to the right path at the intersection. Then it will hop back to that point and see where the other path leads, until the point where G plus H along that path is higher.

This way, the AI using A* knows it's always traveled the shortest path once it exits the maze.

In this recipe, we'll use an estimated cost to the goal, H, that is the distance as-the-bird-flies between two nodes. This will guarantee that H is admissible and always equal to or less than the actual distance to travel.

We'll use the distance between nodes to calculate the cost to travel between them. This will be a lot to take in, but once done, we have a pathfinder we can use for many different applications.

How to do it...

We'll start by defining the node object, in a bean pattern. This will be implemented by performing the following steps:

  1. We create a new class called WaypointNode that extends AbstractControl.
  2. It needs three integers, f, h, and g.
  3. We also have to add two Booleans, open and closed, to aid the pathfinder, a list of other nodes, called connections, it's current position stored in Vector3f and another node as parent.

Now we can create the pathfinder itself. This will be implemented by performing the following steps. We create a new class called AStarPathfinder.

  1. The pathfinder class needs a list of nodes, called openList, which are the nodes currently considered.
  2. It has to know of the startNode and goalNode.
  3. The pathfind method is the heart of the class. We can take a look at it in full, before explaining it, as shown in the following code:
    private void pathfind() {
  openList.add(startNode);
  WaypointNode current;
  while(!openList.isEmpty()) {
    current = openList.get(0);
    for (WaypointNode neighbor : current.getConnections()) {
      if (!neighbor.isClosed()) {
        if (!neighbor.isOpen()) {
          openList.add(neighbor);
          neighbor.setOpen(true);
          setParent(current, neighbor);
        } else if (current.getG() + neighbor.getPosition().distance(goalNode.getPosition()) < neighbor.getG()) { // new path is shorter
          setParent(current, neighbor);
        }
      }
    }
      openList.remove(current);
    current.setClosed(true);
    if (goalNode.isClosed()) {
      break;
    }
    // sort list
    Collections.sort(openList, waypointComparator);
  }
  backtrack();
}
  4. It should begin by adding the startNode to openList.
  5. Next, we define a while loop that always picks the first node in openList.
  6. Inside this loop, we create another for loop that iterates through all the currently selected connected nodes, called neighbors.
  7. If the neighboring node is not in openList, it should be added there. It should also set the current node to parentNode of the neighbor node, as shown in the following code:
    openList.add(neighbor);
neighbor.setOpen(true);
neighbor.setParent(current);
  8. While doing this, g of the neighbor should be set to current node's G plus the distance between the two nodes, as shown in the following code:
    neighbor.setG(current.getG() + (int) (current.getPosition().distance(neighbor.getPosition()) * multiple));
  9. Also, if H has not already been calculated for neighbor, it should, by measuring the distance between neighbor and goalNode. F should be updated by adding G and H together, as shown in the following code:
    if(neighbor.getH() == 0){
  neighbor.setH((int) (neighbor.getPosition().distance(goalNode.getPosition()) * multiple));
}
neighbor.updateF();
  10. It might also be that a shorter path has been discovered since neighbor was calculated. In this case, the neighbor should be updated again with the current node as parent. Do that and repeat the previous two steps.
  11. If neighbor is closed, it shouldn't do anything with it.
  12. Once the neighbors have been parsed, the current node should be removed from openList. openList should then be reordered according to the total cost, F, of the nodes.
  13. The looping of openList should exit, either when it's empty, or when the goalNode has been reached, which is indicated by when it's closed.
  14. When the pathfinding is done, the shortest path can be extracted by going through the parent nodes starting with the goalNode, as shown in the following code. Reversing the resulting list will yield the best path, from startNode to goalNode. This can be implemented as follows:
    private void backtrack() {
  List<WaypointNode> path = new ArrayList<WaypointNode>();
  path.add(goalNode);
  WaypointNode parent = goalNode;
  while (parent != null) {
    parent = parent.getParent();
    path.add(parent);
  }
}

How it works...

The node bean that we created stores information about the state of the node, which is set by the pathfinder as it passes, or considers passing a node. The g value is the total cost to this node, along the current path, from the starting node. h is the estimated value left to the goalNode. In this recipe, it's the shortest distance possible. To be the most effective, it should be as close to the actual distance as possible, without exceeding it. This is to guarantee that it finds the shortest path. F is simply g and h added together, becoming the total estimated cost of the path using this node, and is the value used by the algorithm to consider.

These values are stored as integers, rather than floats. This is better both for memory and processing purposes. We get around lower-than-one distances by multiplying them with 100.

It also keeps track of whether it's currently open or closed. It's quicker to query the node itself, than seeing if the list contains it. The node actually has three states, either open, closed, or the standard, neither which is when it has not yet been considered for the path. The parent of a node defines from which other node the path came to this node.

openList contains all the nodes the pathfinder is currently considering. It starts with only the startNode, adding all its neighbors, since none are either open or closed at this stage. It also sets the parent of the node, calculates the cost to get to this node, and estimates the cost left to the goal (if it has not been calculated before). It only needs to do this once per node, as long as the goal is not moving.

Now, openList has a few new nodes to work with, and the current node is removed from the list. At the end of the while loop, we sort openList according to f-cost of the nodes, so that it always starts looking at the node with the lowest total cost. This is to make sure it doesn't spend any unnecessary time looking at paths which are not optimal.

The algorithm can be considered to be successful once the goalNode has been put in openList and is set to closed. We can't end searching just because the goalNode enters openList. Since we also reconsider nodes if we find a shorter path to the node, we want to check all the goalNodes neighbors as well before ending the search.

If there is no path available to the goalNode, openList will become empty before the goalNode is closed, and the search will fail.

..................Content has been hidden....................

You can't read the all page of ebook, please click here login for view all page.
Reset
3.15.26.221