-added Dijkstra algorithm
-added Waypoint remove function