pathfinding code snippets in matlab