Implementing Overflow's Pathfinding from http://www.powerbot.org/community/topic/1089140-pathfinder/