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)


#!/bin/sh
#|-*- mode:lisp -*-|#
#|
exec ros -Q -- $0 "$@"
|#
(progn ;;init forms
  (ros:ensure-asdf)
  #+quicklisp(ql:quickload '() :silent t)
  )
 
(defpackage :ros.script.floyd-warshall.3861181636
  (:use :cl))
(in-package :ros.script.floyd-warshall.3861181636)
 
;;;
;;; Floyd-Warshall algorithm.
;;;
;;; See https://en.wikipedia.org/w/index.php?title=Floyd%E2%80%93Warshall_algorithm&oldid=1082310013
;;;
;;; Translated from the Scheme. Small improvements (or what might be
;;; considered improvements), and some type specialization, have been
;;; added.
;;;
 
;;;-------------------------------------------------------------------
;;;
;;; A square array will be represented by an ordinary Common Lisp
;;; array, but accessed through our own functions (which look similar
;;; to, although not identical to, the corresponding Scheme
;;; functions).
;;;
;;; Square arrays are indexed *starting at one*.
;;;
 
(defun make-arr (n &key (element-type t) initial-element)
  (make-array (list n n) :element-type element-type
                         :initial-element initial-element))
 
(defun arr-set (arr i j x)
  (setf (aref arr (- i 1) (- j 1)) x))
 
(defun arr-ref (arr i j)
  (aref arr (- i 1) (- 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 integers from 1 .. n.
;;;
;;; A difference from the Scheme implementation is that here we do not
;;; assume the floating point supports "infinities". In the Scheme we
;;; did, because in R7RS small there is support for such infinities
;;; (although the standard does not *require* them). Also because
;;; alternatives were not yet apparent to this author. :)
;;;
 
(defvar *floatpt* 'single-float)
(defconstant nil-vertex 0)
 
(defun floyd-warshall (edges)
  (let* ((n
           ;; Set n to the maximum vertex number. By design, n also
           ;; equals the number of vertices.
           (max (apply #'max (mapcar #'car edges))
                (apply #'max (mapcar #'caddr edges))))
 
         (distance
           ;; The distances are initialized to a purely arbitrary
           ;; value. An entry in the "distance" array is meaningful
           ;; *only* if the corresponding entry in "next-vertex" is
           ;; not the nil-vertex.
           (make-arr n :element-type *floatpt*
                       :initial-element (coerce 12345 *floatpt*)))
 
         (next-vertex
           ;; Unless later set otherwise, an entry in "next-vertex"
           ;; will be the nil-vertex.
           (make-arr n :element-type 'fixnum
                       :initial-element nil-vertex)))
 
    (defun dist (p q) (arr-ref distance p q))
    (defun next (p q) (arr-ref next-vertex p q))
 
    (defun set-dist (p q x) (arr-set distance p q x))
    (defun set-next (p q x) (arr-set next-vertex p q x))
 
    (defun nilnext (p q) (= (next p q) nil-vertex))
 
    ;; Initialize "distance" and "next-vertex".
    (loop for edge in edges
          do (let ((u (car edge))
                   (weight (cadr edge))
                   (v (caddr edge)))
               (set-dist u v weight)
               (set-next u v v)))
    (loop for v from 1 to n
          do (progn
               ;; The distance from a vertex to itself = 0.0.
               (set-dist v v (coerce 0 *floatpt*))
               (set-next v v v)))
 
    ;; Perform the algorithm.
    (loop
      for k from 1 to n
      do (loop
           for i from 1 to n
           do (loop
                for j from 1 to n
                do (and (not (nilnext i k))
                        (not (nilnext k j))
                        (let* ((dist-ikj (+ (dist i k) (dist k j))))
                          (when (or (nilnext i j)
                                    (< dist-ikj (dist i j)))
                            (set-dist i j dist-ikj)
                            (set-next i j (next 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.
;;;
 
(defun find-path (next-vertex u v)
  (if (= (arr-ref next-vertex u v) nil-vertex)
      (list)
      (cons u (let ((i u))
                (loop while (/= i v)
                      do (setf i (arr-ref next-vertex i v))
                      collect i)))))
 
;;;-------------------------------------------------------------------
 
(defun directed-vertex-list-to-string (lst)
  (if (not lst)
      ""
      (let ((s (write-to-string (car lst))))
        (loop for u in (cdr lst)
              do (setf s (concatenate 'string s " -> "
                                      (write-to-string u))))
        s)))
 
;;;-------------------------------------------------------------------
 
(defun main (&rest argv)
  (declare (ignorable argv))
  (let ((example-graph
          (mapcar (lambda (x) (list (coerce (car x) 'fixnum)
                                    (coerce (cadr x) *floatpt*)
                                    (coerce (caddr x) 'fixnum)))
                  '((1 -2 3)
                    (3 2 4)
                    (4 -1 2)
                    (2 4 1)
                    (2 3 3)))))
    (multiple-value-bind (n distance next-vertex)
        (floyd-warshall example-graph)
      (princ "  pair    distance   path")
      (terpri)
      (princ "-------------------------------------")
      (terpri)
      (loop
        for u from 1 to n
        do (loop
             for v from 1 to n
             do (unless (= u v)
                  (format
                   t " ~A ~7@A     ~A~%"
                   (directed-vertex-list-to-string (list u v))
                   (if (= (arr-ref next-vertex u v) nil-vertex)
                       "   no path"
                       (write-to-string (arr-ref distance u v)))
                   (directed-vertex-list-to-string
                    (find-path next-vertex u v)))))))))
 
;;;-------------------------------------------------------------------
;;; vim: set ft=lisp lisp: