Optimizations in Map::findPath