Improved PathFinder, replaced wayPoint system by path