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.
Instructions
Double click on the grid to set the start point (green) and once again to set the end point (red).
Click on Step to perform a single operation of the planner, and Run to visualize the algorithm.
Select a map and click on Load to change the weights of the grid.
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.
- If it is:
- Check to see if it is in the open list
- If it is:
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!
