blob: cf7cc6bd63193af721f1352df87c1bcb8d4bd1db (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
|
;;; Copyright © 2017 David Thompson <dthompson2@worcester.edu>
;;;
;;; 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 data path-finding)
#:use-module (chickadee data 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 distance)
"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 returns the cost of moving from the first to
the second as a number. DISTANCE 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 (distance 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))))))
|
