SRC2-Dijkstramap
Use dijkstramap.cpp to get the lengths of least cost path from each point in the map to the goal.
Note: The output is in index format. Change it to grid format.
One thing that you can add is the best parent for each point corresponding to each goal.
Cite: https://www.srcmake.com/home/cpp-shortest-path-dijkstra