blob: adeb9d5897a98bb9467c994183305c3101e0db73 (
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
|
;;; Copyright © 2017 David Thompson <dthompson2@worcester.edu>
;;;
;;; Licensed under the Apache License, Version 2.0 (the "License");
;;; you may not use this file except in compliance with the License.
;;; You may obtain a copy of the License at
;;;
;;; http://www.apache.org/licenses/LICENSE-2.0
;;;
;;; Unless required by applicable law or agreed to in writing, software
;;; distributed under the License is distributed on an "AS IS" BASIS,
;;; WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
;;; See the License for the specific language governing permissions and
;;; limitations under the License.
;;; 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))))))
|
