33 lines
1.4 KiB
Common Lisp
33 lines
1.4 KiB
Common Lisp
(in-package :tracks)
|
|
|
|
(defun dijkstra (tdb from to)
|
|
(let ((unknown (make-hash-table :test #'equalp))
|
|
(dist (make-hash-table :test #'equalp))
|
|
(prev (make-hash-table :test #'equalp)))
|
|
(loop for pos being the hash-keys of tdb using (hash-value track) do
|
|
(loop for fdir from 0 to 15 for conns across (track-connects track) when conns do
|
|
(let ((ts (trackside pos fdir)))
|
|
(setf (gethash ts unknown) t)
|
|
(setf (gethash ts dist) most-positive-fixnum)
|
|
(setf (gethash ts prev) nil))))
|
|
(setf (gethash to dist) most-positive-fixnum)
|
|
(setf (gethash from dist) 0)
|
|
(labels ((shortest-unseen ()
|
|
(let ((trackside nil) (dst most-positive-fixnum))
|
|
(loop for ts being the hash-keys of unknown
|
|
for d = (gethash ts dist)
|
|
when (<= d dst) do (psetf dst d trackside ts))
|
|
(values trackside dst))))
|
|
(loop with contp = t while (and (> (hash-table-count unknown) 0) contp)
|
|
do (multiple-value-bind (u d) (shortest-unseen)
|
|
(remhash u unknown)
|
|
(if (or (>= d most-positive-fixnum) (equalp u to))
|
|
(setf contp nil)
|
|
(loop for (v . len) in (direct-next tdb u t) for alt = (+ d len)
|
|
when (< alt (gethash v dist)) do
|
|
(setf (gethash v dist) alt)
|
|
(setf (gethash v prev) u))))))
|
|
(let ((path nil) (distance (gethash to dist)))
|
|
(loop for i = to then (gethash i prev) while i do (push i path))
|
|
(values (if (equalp (car path) from) path nil) distance))))
|