Working on switching to 2D coordinate AStar routing.  Currently broken.