Move pathfinding module to math directory.
[chickadee.git] / chickadee / math / path-finding.scm
1 ;;; Copyright © 2017 David Thompson <davet@gnu.org>
2 ;;;
3 ;;; Chickadee is free software: you can redistribute it and/or modify
4 ;;; it under the terms of the GNU General Public License as published
5 ;;; by the Free Software Foundation, either version 3 of the License,
6 ;;; or (at your option) any later version.
7 ;;;
8 ;;; Chickadee is distributed in the hope that it will be useful, but
9 ;;; WITHOUT ANY WARRANTY; without even the implied warranty of
10 ;;; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11 ;;; General Public License for more details.
12 ;;;
13 ;;; You should have received a copy of the GNU General Public License
14 ;;; along with this program. If not, see
15 ;;; <http://www.gnu.org/licenses/>.
16
17 ;;; Commentary:
18 ;;
19 ;; Generalized A* pathfinding algorithm.
20 ;;
21 ;;; Code
22
23 (define-module (chickadee math path-finding)
24 #:use-module (chickadee heap)
25 #:use-module (srfi srfi-9)
26 #:export (make-path-finder
27 path-finder?
28 a*))
29
30 (define-record-type <path-finder>
31 (%make-path-finder frontier came-from cost-so-far)
32 path-finder?
33 (frontier path-finder-frontier)
34 (came-from path-finder-came-from)
35 (cost-so-far path-finder-cost-so-far))
36
37 (define (make-path-finder)
38 "Create a new path finder object."
39 (%make-path-finder (make-heap (lambda (a b) (< (cdr a) (cdr b))))
40 (make-hash-table)
41 (make-hash-table)))
42
43 (define (a* path-finder start goal neighbors cost distance)
44 "Return a list of nodes forming a path from START to GOAL using
45 PATH-FINDER. NEIGHBORS is a procedure that accepts a node and returns
46 a list of nodes that neighbor it. COST is a procedure that accepts
47 two neighboring nodes and returns the cost of moving from the first to
48 the second as a number. DISTANCE is a procedure that accepts two
49 nodes and returns an approximate distance between them."
50 (let ((frontier (path-finder-frontier path-finder))
51 (came-from (path-finder-came-from path-finder))
52 (cost-so-far (path-finder-cost-so-far path-finder)))
53 (heap-insert! frontier (cons start 0))
54 (hashq-set! came-from start #f)
55 (hashq-set! cost-so-far start 0)
56 (let loop ()
57 (unless (heap-empty? frontier)
58 (let ((current (car (heap-min frontier))))
59 (heap-remove! frontier)
60 (unless (eq? current goal)
61 (for-each (lambda (next)
62 (let ((new-cost (+ (hashq-ref cost-so-far current)
63 (cost current next))))
64 (when (or (not (hashq-ref cost-so-far next))
65 (< new-cost (hashq-ref cost-so-far next)))
66 (hashq-set! cost-so-far next new-cost)
67 (let ((priority (+ new-cost (distance goal next))))
68 (heap-insert! frontier (cons next priority)))
69 (hashq-set! came-from next current))))
70 (neighbors current))
71 (loop)))))
72 ;; Walk backwards to build the path from start to goal as a list.
73 (let loop ((node goal)
74 (path '()))
75 (if (eq? node start)
76 (cons start path)
77 (loop (hashq-ref came-from node) (cons node path))))))