I feel like giving the solution to this problem would be too confusing so I will just tell you how to approach this problem
1) Construct distance matrix
- there are 6 x 6 = 36 nodes on the map and between each of the nodes either exists an edge of length 1, 1.4 or inf. make a 36 x 36 matrix that contains these distances if they exist.
2) use dijkstras alrogithm on each node. Use a priority queue and add the first node to the queue. Then for each of the 36 nodes that are "connected" to the current node put their minimum distance to the first node. Then recusively find the distance to second node.