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/>.

17 ;;; Commentary:

18 ;;

19 ;; Generalized A* pathfinding algorithm.

20 ;;

21 ;;; Code

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*))

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))

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)))

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))))))