Path planning algorithm (finding C-Space with single source shortest path algorithms) - View it on GitHub