diff options
| -rw-r--r-- | Makefile.am | 1 | ||||
| -rw-r--r-- | chickadee/path-finding.scm | 77 |
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)))))) |