Added visibility graphs and shortest path algorithm