139 lines
3.5 KiB
Scheme
139 lines
3.5 KiB
Scheme
;;; 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)))))
|