A* works. see comments in update method on how to use.