Article
astar-pathfinding

A* Path-Finding Interactive Demo in Jython

jgilbert on January 9, 2008

This demo was contributed by Justin Gilbert, game programming guru at Multimedia Games and Vanguard Games.

This applet was written in Python and compiled with Jython 2.2.1 for using Java 1.5.0.13. Make sure you have a recent version of the JVM installed. The file is 935 KB big, mostly code but including a few maps. See the A* code below.

A-Star Pathfinding

This applet requires Java 5 to be installed and enabled.


Instructions

  1. Double click on the grid to set the start point (green) and once again to set the end point (red).

  2. Click on Step to perform a single operation of the planner, and Run to visualize the algorithm.

  3. Select a map and click on Load to change the weights of the grid.

If you have any problems running multiple searches in succession, clear the grid and reload the map.

PathPlannerAStar

This class contains all the memory and logic needed to plan a solution given a start and end point. It assumes that we are using a two deimensional grid. Each cell of the grid can be represented by node containing the location and costs. It maintains an open and closed list nodes used in the A* algorithm. The open list is simply a python list where the closed list has been implemented as a hash table.

class PathPlannerAStar:

  m_nStartX = 0      
  m_nStartY = 0      
  m_nGoalX = 0      
  m_nGoalY = 0      
  m_pCurrentNode = None

  m_bFoundGoal = False
  m_bIsDone = False
  m_bDiagonalPenalty = True
  m_bHeuristicWeight = 1.1
  m_nClosedHashTableSize = 199
  m_dictClosedHashTable = dict()
  m_lsOpenList = list()
  m_lsSolution = list()

  m_lsSuccessorPoints = [(-1,-1),(-1,0),(-1,1),(0,-1),\
                                   (0,1),(1,-1),(1,0),(1,1)]

initWeights

Simple helper function to initialize the weights for each node in the grid. It defaults all values to a cost of 1.

  def initWeights(self):
    self.weightData = [MAP_WEIGHT1 for x in \
                              range(self.gridWidth * self.gridHeight)]

PlanPath

Start the planner off with a start and end location. Returns a boolean as to whether or not the goal was a valid cell.

  def PlanPath(self, nStart, nGoal):

First thing we want to do is make sure the planner can find a solution. If the goal is a wall, exit the function with a failing return value.

    if self.weightData[nGoal] == MAP_DATA_WALL:
      print "Goal is invalid : blocked cell"
      return False

Now convert the 1D index numbers into 2D coordinates for both the start and goal.

    self.m_nStartX = nStart % self.gridWidth
    self.m_nStartY = nStart / self.gridWidth
    self.m_nGoalX = nGoal % self.gridWidth
    self.m_nGoalY = nGoal / self.gridWidth

Reset the current node as well as the flags for finding a solution.

    self.m_pCurrentNode = None
    self.m_bFoundGoal = False
    self.m_bIsDone = False

Create the first node to insert into the open list. For every node we insert into the open list we have to compute its heuristic cost. This is simply a best guess at how costly it would be to travel from the node to the goal. Here we are using the distance between the two points.

    root = PlannerNode(self.m_nStartX,self.m_nStartY,None)
    root.ComputeHeuristicCost(self.m_nGoalX,self.m_nGoalY)
    self.SmartInsert(self.m_lsOpenList,root)

    return True

Run

Executes one step of the planner. If the open list is empty the planner is marked as done and no further work will be done.

  def Run(self):
   tempNode = None

Here we make sure there is still work to be done. If there is we pop the top node off the open list, assign our current node to it, and insert it into the closed list.

    if len(self.m_lsOpenList)>0:
      self.m_pCurrentNode = self.m_lsOpenList.pop()
      self.HashTableInsert(self.m_pCurrentNode)

Now compare the current node to the goal. If they match then we have found a path to the goal. Since each node has a link to its parent, we can easily create a solution set by traversing back to the start.

      if (self.m_pCurrentNode.x == self.m_nGoalX \
         and self.m_pCurrentNode.y == self.m_nGoalY):

        self.m_bFoundGoal = True
        self.m_bIsDone = True
        tempNode = self.m_pCurrentNode
        while(tempNode is not None):
          self.m_lsSolution.append(tempNode)
          tempNode = tempNode.parent

If the current node is not the goal then insert all of the node’s qualifying neighbors.

      self.InsertSuccessors(self.m_pCurrentNode)

Nothing left in the open list, which means no more work can be done. It is possible to finish without finding a solution.

    else:
      self.m_bIsDone = True

CleanUp

Clear out the lists and resets the current node.

 def CleanUp(self):
    self.m_dictClosedHashTable.clear()
    self.m_lsOpenList = list() 
    self.m_lsSolution = list()
    self.m_pCurrentNode = None

SmartInsert

Helper function. Performs insertion sort to keep nodes into the correct location.

  def SmartInsert(self, lsList, node):

    insertPosition = 0

    i = 0
    k = len(lsList) - 1

    while i <= k:
      l = (i + k) / 2
      i1 = node.Compare(lsList[l])
      if i1 < 0:
        i = l + 1
        insertPosition = -(i + 1)
      elif i1 > 0:
        k = l - 1
        insertPosition = -(i + 1)
      else:
        insertPosition = l
        break

    if insertPosition < 0:
      insertPosition= -insertPosition - 1

    lsList.insert(insertPosition,node)

HashTableInsert

Inserts a node into the closed list. A hashing function is used to separate the nodes into buckets for faster lookup later on.

  def HashTableInsert(self, node):

    nIndex = COMPUTE_HASH_CODE(node.x,node.y) \
                                          % self.m_nClosedHashTableSize

    if self.m_dictClosedHashTable.has_key(nIndex):
      node.hashTableNext = self.m_dictClosedHashTable[nIndex]
    else:
      node.hashTableNext = None
    self.m_dictClosedHashTable[nIndex] = node

HashTableFind

Returns a node given it’s x and y coordinate.

  def HashTableFind(self, x, y):

    if self.m_dictClosedHashTable is None:
      return None

    nIndex = COMPUTE_HASH_CODE(x,y) % self.m_nClosedHashTableSize
    node = self.m_dictClosedHashTable[nIndex]

    while node is not None:
      if node.x == x and node.y == y:
        print "Found node!!!"
        return node
      node = node.hashTableNext

    return None

HashTableRemove

Removes a node from the closed list given it’s x and y coordinate.

  def HashTableRemove(self, x, y):

    nIndex = COMPUTE_HASH_CODE(x,y) % self.m_nClosedHashTableSize
    node = self.m_dictClosedHashTable[nIndex]
    prevNode = None

    while node is not None:

      if node.x == x and node.y == y:
        if prevNode is not None:
          prevNode.hashTableNext = node.hashTableNext
        else:
          self.m_dictClosedHashTable[nIndex] = node.hashTableNext

        node.hashTableNext = None
        return node

      prevNode = node
      node = node.hashTableNext

    return None

InsertSuccessors

This is where most of the work is done. It will insert any new valid node into the open list. The algorithm breaks down like this:

  • For each neighbor:
    • Make sure it is a valid cell (within map bounds, not a wall). If not, skip it.
    • Calculate the final cost of the node.
    • Check to see if it is in the closed list
      • If it is:
        • If the new cost is less than the found node’s cost, remove the found node from the closed list and insert it into the open list with the new cost. If not, skip it.
      • Otherwise:
        • Check to see if it is in the open list
          • If it is:
            • If the new cost is less than the found node’s cost, remove the found node from the open list and reinsert it with the new cost. If not, skip it.
          • Otherwise: Insert a new node with the new cost into the open list.
  def InsertSuccessors(self, pn):

    newNode = None
    nNewX = 0
    nNewY = 0
    fNewCost = 0.0
    bSkip = False

    for x,y in self.m_lsSuccessorPoints:
      nNewX = x + pn.x
      nNewY = y + pn.y
      bSkip = False
      newNode = None

While calculating each successor point, do a check for blocked cells.

      if nNewX >=0 and nNewY>=0 and nNewX < self.gridWidth and nNewY < self.gridHeight:   
        if self.weightData[nNewY * self.gridWidth + nNewX] == MAP_WEIGHT5:
          continue

Calculate the new cost. This is simply the cost of the parent node plus the weight for the given cell. There can also be a penalty added when traveling diagonally.

        fNewCost = pn.givenCost
        if x!=0 and y!=0 and self.m_bDiagonalPenalty == 1:
          fNewCost += MAP_WEIGHTS[self.weightData[nNewY * self.gridWidth + nNewX]] * 1.4142
        else:
          fNewCost += MAP_WEIGHTS[self.weightData[nNewY * self.gridWidth + nNewX]]

Now try to find the node in the closed list. If we find it, compare it’s given cost to the new cost. If the new node’s cost is cheaper, we want to remove it from the closed list and insert it with the new cost into the open list. In either case, if we found it in the closed list we need to skip ahead to the next neighbor.

        newNode = self.HashTableFind(nNewX,nNewY)

        if newNode is not None:

          if fNewCost < newNode.givenCost:

            self.HashTableRemove(nNewX,nNewY)
            newNode.parent = pn
            newNode.givenCost = fNewCost
            newNode.finalCost = newNode.givenCost + newNode.heuristicCost * self.m_bHeuristicWeight
            self.SmartInsert(self.m_lsOpenList,newNode)
          continue

Next, if it wasn’t in the closed list, try to find it in the open list. If we find it, compare it’s given cost to the new cost. If the new node’s cost is cheaper, reinsert the node with the new cost. This kills off paths that we can already determine to be more costly. Again, if we find it in the open list we want to skip ahead to the next neighbor.

        nSize = len(self.m_lsOpenList)
        for newNode in self.m_lsOpenList:
          if newNode.x == nNewX and newNode.y == nNewY:
            bSkip = True
            if fNewCost < newNode.givenCost:
              self.m_lsOpenList.remove(newNode)
              newNode.parent = pn
              newNode.givenCost = fNewCost
              newNode.finalCost = newNode.givenCost + newNode.heuristicCost * self.m_bHeuristicWeight
              self.SmartInsert(self.m_lsOpenList,newNode)
              break

        if bSkip:
          continue

It wasn’t in the open or closed list so insert a new node into the open list.

        newNode = PlannerNode(nNewX,nNewY,pn)
        newNode.ComputeHeuristicCost(self.m_nGoalX,self.m_nGoalY)
        newNode.givenCost = fNewCost
        newNode.finalCost = newNode.givenCost + newNode.heuristicCost * self.m_bHeuristicWeight
        self.SmartInsert(self.m_lsOpenList,newNode)

Comments and feedback about the application are very welcome!

astar-pathfinding

Alex J. Champandard on January 9, 2008

Discussion 0 Comments

If you'd like to add a comment or question on this page, simply log-in to the site. You can create an account from the sign-up page if necessary... It takes less than a minute!