From 9e83a3962dd1a03b327e10826b5754a4fa9c9cb4 Mon Sep 17 00:00:00 2001 From: David Thompson Date: Tue, 17 Oct 2017 22:05:47 -0400 Subject: Add generalized A* path finding algorithm. * chickadee/path-finding.scm: New file. * Makefile.am (SOURCES): Add it. --- Makefile.am | 1 + chickadee/path-finding.scm | 77 ++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 78 insertions(+) create mode 100644 chickadee/path-finding.scm 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 +;;; +;;; 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 +;;; . + +;;; 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 + (%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)))))) -- cgit v1.2.3