Optimized path finder algo