Add generalized A* path finding algorithm.

* chickadee/path-finding.scm: New file.
* Makefile.am (SOURCES): Add it.

2 files changed, 78 insertions, 0 deletions

diff --git a/Makefile.am b/Makefile.am
index de01b08..54db2ac 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -74,6 +74,7 @@ SOURCES = 					\
   chickadee/scripting/script.scm		\
   chickadee/scripting/channel.scm		\
   chickadee/scripting.scm			\
+  chickadee/path-finding.scm			\
   chickadee.scm					\
   chickadee/buffer.scm
 
diff --git a/chickadee/path-finding.scm b/chickadee/path-finding.scm
new file mode 100644
index 0000000..9af01f8
--- /dev/null
+++ b/chickadee/path-finding.scm
@@ -0,0 +1,77 @@
+;;; Copyright © 2017 David Thompson <davet@gnu.org>
+;;;
+;;; Chickadee is free software: you can redistribute it and/or modify
+;;; it under the terms of the GNU General Public License as published
+;;; by the Free Software Foundation, either version 3 of the License,
+;;; or (at your option) any later version.
+;;;
+;;; Chickadee is distributed in the hope that it will be useful, but
+;;; WITHOUT ANY WARRANTY; without even the implied warranty of
+;;; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+;;; General Public License for more details.
+;;;
+;;; You should have received a copy of the GNU General Public License
+;;; along with this program.  If not, see
+;;; <http://www.gnu.org/licenses/>.
+
+;;; Commentary:
+;;
+;; Generalized A* pathfinding algorithm.
+;;
+;;; Code
+
+(define-module (chickadee path-finding)
+  #:use-module (chickadee heap)
+  #:use-module (srfi srfi-9)
+  #:export (make-path-finder
+            path-finder?
+            a*))
+
+(define-record-type <path-finder>
+  (%make-path-finder frontier came-from cost-so-far)
+  path-finder?
+  (frontier path-finder-frontier)
+  (came-from path-finder-came-from)
+  (cost-so-far path-finder-cost-so-far))
+
+(define (make-path-finder)
+  "Create a new path finder object."
+  (%make-path-finder (make-heap (lambda (a b) (< (cdr a) (cdr b))))
+                     (make-hash-table)
+                     (make-hash-table)))
+
+(define (a* path-finder start goal neighbors cost heuristic)
+  "Return a list of nodes forming a path from START to GOAL using
+PATH-FINDER.  NEIGHBORS is a procedure that accepts a node and returns
+a list of nodes that neighbor it.  COST is a procedure that accepts
+two neighboring nodes and the cost of moving from the first to the
+second as a number.  HEURISTIC is a procedure that accepts two nodes
+and returns an approximate distance between them."
+  (let ((frontier (path-finder-frontier path-finder))
+        (came-from (path-finder-came-from path-finder))
+        (cost-so-far (path-finder-cost-so-far path-finder)))
+    (heap-insert! frontier (cons start 0))
+    (hashq-set! came-from start #f)
+    (hashq-set! cost-so-far start 0)
+    (let loop ()
+      (unless (heap-empty? frontier)
+        (let ((current (car (heap-min frontier))))
+          (heap-remove! frontier)
+          (unless (eq? current goal)
+            (for-each (lambda (next)
+                        (let ((new-cost (+ (hashq-ref cost-so-far current)
+                                           (cost current next))))
+                          (when (or (not (hashq-ref cost-so-far next))
+                                    (< new-cost (hashq-ref cost-so-far next)))
+                            (hashq-set! cost-so-far next new-cost)
+                            (let ((priority (+ new-cost (heuristic goal next))))
+                              (heap-insert! frontier (cons next priority)))
+                            (hashq-set! came-from next current))))
+                      (neighbors current))
+            (loop)))))
+    ;; Walk backwards to build the path from start to goal as a list.
+    (let loop ((node goal)
+               (path '()))
+      (if (eq? node start)
+          (cons start path)
+          (loop (hashq-ref came-from node) (cons node path))))))