The Floyd–Warshall algorithm is an algorithm for finding shortest paths in a weighted graph with positive or negative edge weights.




Task
Find the lengths of the shortest paths between all pairs of vertices of the given directed graph. Your code may assume that the input has already been checked for loops, parallel edges and negative cycles.


Print the pair, the distance and (optionally) the path.




Example
pair     dist    path
1 -> 2    -1     1 -> 3 -> 4 -> 2
1 -> 3    -2     1 -> 3
1 -> 4     0     1 -> 3 -> 4
2 -> 1     4     2 -> 1
2 -> 3     2     2 -> 1 -> 3
2 -> 4     4     2 -> 1 -> 3 -> 4
3 -> 1     5     3 -> 4 -> 2 -> 1
3 -> 2     1     3 -> 4 -> 2
3 -> 4     2     3 -> 4
4 -> 1     3     4 -> 2 -> 1
4 -> 2    -1     4 -> 2
4 -> 3     1     4 -> 2 -> 1 -> 3


See also
 Floyd-Warshall Algorithm - step by step guide (youtube)


;;; Floyd-Warshall algorithm.
;;;
;;; See https://en.wikipedia.org/w/index.php?title=Floyd%E2%80%93Warshall_algorithm&oldid=1082310013
;;;
 
(import (scheme base))
(import (scheme cxr))
(import (scheme write))
 
;;;
;;; A square array will be represented by a cons-pair:
;;;
;;;    (vector-of-length n-squared . n)
;;;
;;; Arrays are indexed *starting at one*.
;;;
 
(define (make-arr n fill)
  (cons (make-vector (* n n) fill) n))
 
(define (arr-set! arr i j x)
  (let ((vec (car arr))
        (n (cdr arr)))
    (vector-set! vec (+ (- i 1) (* n (- j 1))) x)))
 
(define (arr-ref arr i j)
  (let ((vec (car arr))
        (n (cdr arr)))
    (vector-ref vec (+ (- i 1) (* n (- j 1))))))
 
;;;
;;; Floyd-Warshall.
;;;
;;; Input is a list of length-3 lists representing edges; each entry
;;; is:
;;;
;;;    (start-vertex edge-weight end-vertex)
;;;
;;; where vertex identifiers are (to help keep this example brief)
;;; integers from 1 .. n.
;;;
 
(define (floyd-warshall edges)
 
  (define n
    ;; Set n to the maximum vertex number. By design, n also equals
    ;; the number of vertices.
    (max (apply max (map car edges))
         (apply max (map caddr edges))))
 
  (define distance (make-arr n +inf.0))
  (define next-vertex (make-arr n #f))
 
  ;; Initialize "distance" and "next-vertex".
  (for-each (lambda (edge)
              (let ((u (car edge))
                    (weight (cadr edge))
                    (v (caddr edge)))
                (arr-set! distance u v weight)
                (arr-set! next-vertex u v v)))
            edges)
  (do ((v 1 (+ v 1)))
      ((< n v))
    (arr-set! distance v v 0)
    (arr-set! next-vertex v v v))
 
  ;; Perform the algorithm.
  (do ((k 1 (+ k 1)))
      ((< n k))
    (do ((i 1 (+ i 1)))
        ((< n i))
      (do ((j 1 (+ j 1)))
          ((< n j))
        (let ((dist-ij (arr-ref distance i j))
              (dist-ik (arr-ref distance i k))
              (dist-kj (arr-ref distance k j)))
          (let ((dist-ik+dist-kj (+ dist-ik dist-kj)))
            (when (< dist-ik+dist-kj dist-ij)
              (arr-set! distance i j dist-ik+dist-kj)
              (arr-set! next-vertex i j
                        (arr-ref next-vertex i k))))))))
 
  ;; Return the results.
  (values n distance next-vertex))
 
;;;
;;; Path reconstruction from the "next-vertex" array.
;;;
;;; The return value is a list of vertices.
;;;
 
(define (find-path next-vertex u v)
  (if (not (arr-ref next-vertex u v))
      (list)
      (let loop ((u u)
                 (path (list u)))
        (if (= u v)
            (reverse path)
            (let ((u^ (arr-ref next-vertex u v)))
              (loop u^ (cons u^ path)))))))
 
(define (display-path path)
  (let loop ((p path))
    (cond ((null? p))
          ((null? (cdr p)) (display (car p)))
          (else (display (car p))
                (display " -> ")
                (loop (cdr p))))))
 
(define example-graph
  '((1 -2 3)
    (3 2 4)
    (4 -1 2)
    (2 4 1)
    (2 3 3)))
 
(let-values (((n distance next-vertex)
              (floyd-warshall example-graph)))
  (display " pair   distance    path")
  (newline)
  (display "------------------------------------")
  (newline)
  (do ((u 1 (+ u 1)))
      ((< n u))
    (do ((v 1 (+ v 1)))
        ((< n v))
      (unless (= u v)
        (display u)
        (display " -> ")
        (display v)
        (let* ((s (number->string (arr-ref distance u v)))
               (slen (string-length s))
               (padding (- 7 slen)))
          (display (make-string padding #\space))
          (display s))
        (display "      ")
        (display-path (find-path next-vertex u v))
        (newline)))))