Hierarchical Pathfinding

Pathfinding is a ubiquitous problem in AI and a lot of research has gone into finding effective  techniques for it. The beloved A* is usually our target weapon for search problems. Although A* is optimal it might suffer from the problem of exhausting large search spaces. This often warrants the use alternate techniques as iterative deepening to our search algorithm (e.g. IDA*) for effective solutions.  However in games and motion planning the most used algorithm is some sort of hierarchical search e.g. Hierarchical A*.

Over the past couple of weekends I've been hacking into writing my own hierarchical A* solution. I finally completed (well it can obviously still be improved) my solution this weekend. I also used Apple's new shiny programming language "Swift" to write it. I also used Adi Botea, Martin Muller and Jonathon Schaeffer's research paper on Near Optimal Hierarchical Pathfinding as my reference algorithm for implementation ^[1] (The scope of the algorithm is beyond this blog post. I would encourage you to read the research paper to understand the algorithm completely.)

Algorithm

The essence of a hierarchical algorithm is to construct abstract graphs at higher levels of hierarchy and start your initial search from these higher levels moving down towards the original graph. The main complexity of the algorithm is to construct and maintain these graphs. It is also these abstract graphs which drive the performance of our search since the search space in higher level graphs is far reduced and we can theoretically build our solution incrementally.

Clusters and Entrances

The algorithm in itself is not very complicated. You initially have a grid space (m x n) space which is divided into clusters, where each cluster is of size (c x c). The cluster boundaries are marked with entrance points. Clusters are entered and exited via these entrance points. The way Botea's research defines an entrance point is to find a contiguous boundary between two clusters that allow free movement i.e. nodes on both clusters are walkable. From such a contiguous region you usually choose the middle point as the entrance point.

 func createEdgesAcrossClusters(entrances: [Entrance]) {
    for entrance in entrances {
      let idx1 = world.graph.nodeIdForPositionWithRow(
        entrance.center1Row, col: entrance.center1Col)
      let idx2 = world.graph.nodeIdForPositionWithRow(
        entrance.center2Row, col: entrance.center2Col)
      if let absId1 = nodeIdToAbsNodeId[idx1!],
        absId2 = nodeIdToAbsNodeId[idx2!] {
          let n1 = absGraph.nodeById(absId1)
          let n2 = absGraph.nodeById(absId2)
          let e1 = addEdgeBetweenAbsNodes(n1, n2, withCost: 1)
          let e2 = addEdgeBetweenAbsNodes(n2, n1, withCost: 1)
          e1.isInter = true
          e2.isInter = true
      } else {
        assertionFailure("Cannot find graph nodes with index \(idx1) and \(idx2)")
      }
    }
  }

Abstract Graph

From the clusters and entrances created above we next create an abstract graph. As entrances exist as entrance points for each entrance point we create two abstract nodes, one for each cluster. These abstract nodes created are connected by inter-cluster edges each having a weight of 1. All abstract nodes within a cluster are also connected by edges marking the distance needed to travel within them. It is easy to visualize this abstract graph.

Search

Finally we are ready to run our search algorithm. However before doing that we insert the start and end nodes into our abstract graph. Once we have a path from a higher level of hierarchy we move to the next lower level and finally end up in the original graph wherein we reconstruct our final solution.

func pathFromRow(startRow: Int, col startCol: Int,
    toRow endRow: Int, col endCol: Int) -> [Point]? {
      guard let (absStartId, absEndId) = addToAbsGraphStart(
        startRow, startCol, end: endRow, endCol) else {
          assertionFailure("Could not add start and end abs nodes.")
          return nil
      }

      defer {
        removeFromAbsGraphStartRow(startRow, col: startCol,
          endRow: endRow, col: endCol)
      }

      if let absPath = absWorld?.searchPathFrom(absStartId!, to: absEndId!) {
        return constructPathFromAbsGraphPath(absPath)
      } else {
        return nil
      }
  }

Big Wins/Caveat

The algorithm wins big in it's incremental build up. If the start and end points are widely separated there would be a lot of clusters between them and we only really need to know/compute how to start our movement and then construct the rest of the path on the fly. 

One caveat the algorithm suffers from is dynamic environments. Since in dynamic environments the cluster entrances are no longer static we need to rebuild such entrances whenever the environment in a cluster changes. However clusters are usually small in size (10x10 or 20x20) and thus rebuilding their information might not be too much of overhead. 

Swift Learnings

The implementation in Swift was a nice working experience. A strong type system with a protocol oriented programming approach does make a lot of things nice. For instance it is much easier to rationale about the algorithm and why it's not working. The support for "Optionals" in Swift is pretty nice and expresses the programmer intent in a much cleaner way. "guard" and "defer" are well designed and thought about operations and do make the code look much more cleaner. The functional approach to writing algorithms might not always be the best performance wise but is certainly easier to understand and read and I guess optimize in a multi-threaded environment. 

Swift is still an evolving language so it still lacks a few things e.g. Self referential generic types are not truly supported. This was a big pain and the Swift team has come up with a hackish solution of wrapping such protocols in "Any" structs 

protocol OpenList {
  typealias ItemType
  func isEmpty() -> Bool
  func addNode(node: ItemType)
}

class AnyOpenList : OpenList {
  typealias ItemType = T

  let _isEmpty: Void -> Bool
  let _addNode: (T -> Void)

  init(_ u: U) {
    _isEmpty = u.isEmpty
    _addNode = u.addNode
  }

  func isEmpty() -> Bool { return _isEmpty() }

  func addNode(node: T) { _addNode(node) }
}

Output

Here is an example of how clusters are created in an open space environment. The white squares represent walkable grids. Non-walkable grid spaces are marked with black (missing here). The nodes marked as grey are part of the abstract graphs. The red lines are the edges in the abstract graph. All of the edges are bidirectional.

Note that the yellow line marks an optimal A* search from red to green. Here below is the my implementation of hierarchical A* in the above graph. You will observe how the final path follows the cluster entrances. Also you can note that the hierarchical solution is "not optimal" which is obvious since our solution is only cluster-optimal and we trade-off optimality in the hierarchical graph for speed.

Finally here is a working solution in a much more complex graph. You can see how the different clusters are created and joined together.

---

1 Near Optimal Hierarchical Path-Finding https://webdocs.cs.ualberta.ca/~mmueller/ps/hpastar.pdf^↩