summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--Makefile.am1
-rw-r--r--chickadee/path-finding.scm77
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))))))