This implements the Floyd-Warshall All-Pairs-Shortest-Path algorithm to find shortest paths between every two nodes of a weighted directed graph.
% Dss0 is an NxN table of direct distances between two nodes, infinity being
% represented by a large value
% Dss is an NxN table of shortest distances between two nodes
% Pss is an NxN table such that on a shortest path from node i to node j,
% element (i,j) is the last node before j
apsp(Dss0, Dss, Pss):-
length(Dss0, N),
apsp_1(0, N, Pss0),
apsp_2(0, N, Dss0, Dss, Pss0, Pss).
apsp_1(N, N, []):-!.
apsp_1(I, N, [Ps|Pss]):-
replicate(N, I, Ps),
I1 is I + 1,
apsp_1(I1, N, Pss).
apsp_2(N, N, Dss, Dss, Pss, Pss):-!.
apsp_2(K, N, Dss0, Dss, Pss0, Pss):-
nth_member(Dss0, K, Dks), % Row K of Dss0
nth_member(Pss0, K, Pks), !, % Row K of Pss0
apsp_3(Dss0, Pss0, 0, K, Dks, Pks, Dss1, Pss1),
% Dss1 contains the lengths of shortest paths with
% intermediate nodes from the list [0,1,2,...,K]
% Pss1 contains the preceeding nodes on shortest paths
% with intermediate nodes from the list [0,1,2,...,K]
K1 is K + 1,
apsp_2(K1, N, Dss1, Dss, Pss1, Pss).
apsp_3([], _, _, _, _, _, [], []).
apsp_3([Dis0|Dss0], [Pis0|Pss0], I, K, Dks, Pks, [Dis|Dss], [Pis|Pss]):-
I \= K, % Avoid unnecessary calculations when I=K
nth_member(Dis0, K, Dik), !, % Element K of row I
apsp_4(Dis0, Pis0, Dks, Pks, 0, I, K, Dik, Dis, Pis),
I1 is I + 1,
apsp_3(Dss0, Pss0, I1, K, Dks, Pks, Dss, Pss).
apsp_3([Dis|Dss0], [Pis|Pss0], I, K, Dks, Pks, [Dis|Dss], [Pis|Pss]):-
I1 is I + 1,
apsp_3(Dss0, Pss0, I1, K, Dks, Pks, Dss, Pss).
apsp_4([], [], [], [], _, _, _, _, [], []).
apsp_4([Dij0|Dis0], [_|Pis0], [Dkj|Dks], [Pkj|Pks], J, I, K, Dik,
[Dij|Dis], [Pkj|Pis]):-
J \= I, J \= K, % Avoid unnecessary calculations when J=I or J=K
Dij is Dik + Dkj,
Dij < Dij0, !,
% Node K is a potential intermediate node on a shortest path
% between nodes I and J, and the distance from I to K plus the
% distance from K to J is less than the distance from I to J
J1 is J + 1,
apsp_4(Dis0, Pis0, Dks, Pks, J1, I, K, Dik, Dis, Pis).
apsp_4([Dij|Dis0], [Pij|Pis0], [_|Dks], [_|Pks], J, I, K, Dik,
[Dij|Dis], [Pij|Pis]):-
J1 is J + 1,
apsp_4(Dis0, Pis0, Dks, Pks, J1, I, K, Dik, Dis, Pis).
% Path is a shortest path from node I to node J
path(I, J, Pss, [I|Path]):-path_1(I, J, Pss, [], Path).
path_1(I, I, _, Path, Path):-!.
path_1(I, J, Pss, Path0, Path):-
nth_member(Pss, I, Pi),
nth_member(Pi, J, Pij), !,
path_1(I, Pij, Pss, [J|Path0], Path).
% Distance is the length of a shortest path from node I to node J
distance(I, J, Dss, Distance):-
nth_member(Dss, I, Di),
nth_member(Di, J, Distance), !.
/* length(Xs, L) is true if L is the number of elements in the list Xs. */
%length(Xs, L):-length_1(Xs, 0, L).
/* length_1(Xs, L0, L) is true if L is equal to L0 plus the number of */
/* elements in the list Xs. */
%length_1([], L, L).
%length_1([_|Xs], L0, L):-L1 is L0 + 1, length_1(Xs, L1, L).
/* nth_member(+Xs, ?N, ?X) is true if X is the N-th (base 0) element of the */
/* list Xs. */
nth_member(Xs, N, X):-nth_member_1(Xs, X, 0, N).
nth_member_1([X|_], X, I, I).
nth_member_1([_|Xs], X, I0, I):-
I1 is I0 + 1,
nth_member_1(Xs, X, I1, I).
/* replicate(N, X, Xs) is true if the list Xs contains N occurrences of the */
/* item X. */
replicate(0, _, []):-!.
replicate(I, X, [X|Xs]):-
I > 0,
I1 is I - 1,
replicate(I1, X, Xs).
test:-
/*
4 7
0-----1-----2
|\ |\ |
| \ | \ |
8| 1\ 2| \3 |2
| \ | \ |
| \| \|
3-----4-----5
1 6
*/
Dss0=[[ 0, 4,99, 8, 1,99],
[ 4, 0, 7,99, 2, 3],
[99, 7, 0,99,99, 2],
[ 8,99,99, 0, 1,99],
[ 1, 2,99, 1, 0, 6],
[99, 3, 2,99, 6, 0]],
apsp(Dss0, Dss, Pss),
write(Dss), nl,
% gives:
% Dss=[[0,3,8,2,1,6],
% [3,0,5,3,2,3],
% [8,5,0,8,7,2],
% [2,3,8,0,1,6],
% [1,2,7,1,0,5],
% [6,3,2,6,5,0]]
write(Pss), nl,
% gives:
% Pss=[[0,4,5,4,0,1],
% [4,1,5,4,1,1],
% [4,5,2,4,1,2],
% [4,4,5,3,3,1],
% [4,4,5,4,4,1],
% [4,5,5,4,1,5]]
path(0, 2, Pss, Path),
write(Path), nl,
% gives:
% Path=[0,4,1,5,2]
distance(0, 2, Dss, Distance),
write(Distance), nl.
% gives:
% Distance=8