Pathfinding a maze with the Floyd-Warshall algorithm

Question

I'm trying to use Floyd's algorithm to generate a quickest-route matrix through a maze with 98 waypoints (located at each of the maze vertices). As the algorithm runs, it populates two matrices - the Distance matrix (with the most optimal distance between two nodes), and the Path matrix (with the next node to go to for the most optimal path between any two nodes).

The distance matrix is initialized with an adjacency matrix I generate in previous code. I also generate a data structure containing pointers to each of the four possible neighbor waypoints for each waypoint, but I did not used this data structure in generating the path matrix.

Here's my c# code:

        // Initialize distance path matrix
        distanceMatrix = adjacencyMatrix;

        // Initialize path matrix
        int N = (WaypointList.Count);
        pathMatrix = new int[N, N];

        for (int i = 0; i < N; i++)
            for (int t = 0; t < N; t++)
                pathMatrix[i,t] = t;

        // Floyd-Warshall algorithm            
        for (int k = 0; k < N; ++k)
            for (int i = 0; i < N; ++i)
                for (int j = 0; j <= i; ++j)
                {
                    int currentCombo = distanceMatrix[i, k] + distanceMatrix[k, j];
                    if (currentCombo < distanceMatrix[i, j])
                    {                            
                        distanceMatrix[j, i] = distanceMatrix[i, j] = currentCombo;
                        pathMatrix[j, i] = pathMatrix[i, j] = k;
                    }
                }

I believe I am computing the Path matrix incorrectly, because the bot using the Path matrix generated by this code is failing in the majority of situations (as the Path matrix is telling to to go through a wall, etc).

I've been staring at this code for hours, and I'm stumped as to what I've done wrong. How can I generate the correct Path matrix for use in my maze navigation code?

Answer 1

When you are setting pathMatrix[i, j] = k, you are assuming that this means that the path from node i to j will start by going to node k. But in fact, what it means is that the path from i to j will go through k at some point, not necessarily on the first move.

What you need to do is the following, assuming that there's a path from i to j:

target = j
while there is no edge from i to target:
    target = pathMatrix[i, target]

This will set target to the next node to go to from i.