Added pathfinding algorithm and needed sub functions

The algorithm can be tested with the function
testDijkstra(start, goal, paths);
Merge branch 'master' of https://github.com/hyzor/AiLab2