Implement basic pathfinder

dataserver.lisp

@@ -113,6 +113,18 @@
    graph-tracks ("/graph/paths" :method :get) ()
    ("Return a simple graph of paths")
    (tracks:gvdump *trackdb*))
+  (ywsw:safe-text-route
+   find-path ("/find-path/:from-pos/:from-side/:to-pos/:to-side" :method :get)
+   (&path (from-pos 'string) (from-side 'integer)
+	  (to-pos 'string) (to-side 'integer))
+   ("Find path between two entries in the track database")
+   (multiple-value-bind (path dist)
+       (tracks:dijkstra *trackdb*
+			(tracks:trackside (aux:string-to-v3d from-pos) from-side)
+			(tracks:trackside (aux:string-to-v3d to-pos) to-side))
+     (if path (format nil "Distance: ~a~%~{~#[~;To~:;From~] ~a~@{~%~#[~;To~:;Via~] ~a~}~}"
+		      dist path)
+	 "No path")))
   (ywsw:defsafe
       docroute-master ("/doc" :method :get) ()
       (let ((entries (loop for i being the hash-keys of easy-routes::*routes*

helpers.lisp

@@ -27,6 +27,9 @@
   (y 0 :type lua-number)
   (z 0 :type lua-number))
 
+(defmacro v3d (x y z)
+  `(make-v3d :x ,x :y ,y :z ,z))
+
 (defmethod print-object ((obj v3d) stream)
   (with-accessors ((x v3d-x) (y v3d-y) (z v3d-z)) obj
     (print-unreadable-object (obj stream)

tracks/conns.lisp

@@ -4,6 +4,11 @@
   (:pos (error "no coordinates specified") :type aux:v3d)
   (:side (error "no side specified") :type (integer 0 15)))
 
+(defmacro trackside (pos side)
+  (alexandria:once-only
+   (pos side)
+   `(if (and ,pos ,side) (make-trackside :pos ,pos :side ,side))))
+
 (defmethod print-object ((obj trackside) s)
   (with-accessors ((p trackside-pos) (side trackside-side)) obj
     (print-unreadable-object (obj s)

tracks/database.lisp

@@ -26,6 +26,18 @@
 	  (let ((,track (get-track ,trackdb ,pos)))
 	    (when ,track ,@body)))))))
 
+(defmacro direct-next (trackdb trackside &optional no-eol-p)
+  (alexandria:with-gensyms (dir track i c d connects)
+    (alexandria:once-only
+     (trackdb trackside no-eol-p)
+     `(with-track-at-ts (,trackdb ,trackside nil ,dir ,track)
+	(loop with ,connects = (track-connects ,track)
+	   for ,i in (cddr (aref ,connects ,dir))
+	   for ,c = (car (aref ,connects ,i))
+	   and ,d = (cadr (aref ,connects ,i))
+	   when (or (not ,no-eol-p) ,c)
+	   collect (cons ,c ,d))))))
+
 (defun optimize-track-database (tdb)
   (loop for pos in (loop for i being the hash-keys of tdb collect i)
      for track = (gethash pos tdb)

tracks/package.lisp

@@ -1,7 +1,9 @@
 (defpackage :tracks
   (:use :cl)
   (:export :init-tracks :import-data
-	   :dump-track-definitions :gvdump))
+	   :dump-track-definitions :gvdump
+	   :trackside
+	   :dijkstra))
 (in-package :tracks)
 
 (defparameter *hdiff*

tracks/path.lisp

@@ -0,0 +1,30 @@
+(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 while (> (hash-table-count unknown) 0) do
+	   (multiple-value-bind (u d) (shortest-unseen)
+	     (remhash u unknown)
+	     (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))))