Dijkstra implemented successfully

Now I need to implement my path flattening algorithm which removes vertices from the path when the vertices before and after it have a direct line of sigh.