A simple implemenation for calculation the shortest path between discrete points