find_shortest_costmatrix_path function

Finds the shortest path between two states in a costmatrix