Commit 9a6c99d1 authored by Pascal J. Bourguignon's avatar Pascal J. Bourguignon

Added a-star.lisp

parent 548249a6
;;;; -*- mode:lisp;coding:utf-8 -*-
;;;;FILE: a-star.lisp
;;;;LANGUAGE: Common-Lisp
;;;;SYSTEM: Common-Lisp
;;;; Implements the A* algorithm.
;;;; cf.
;;;; <PJB> Pascal J. Bourguignon <>
;;;; 2014-10-22 <PJB> Created.
;;;; AGPL3
;;;; Copyright Pascal J. Bourguignon 2014 - 2014
;;;; This program is free software: you can redistribute it and/or modify
;;;; it under the terms of the GNU Affero General Public License as published by
;;;; the Free Software Foundation, either version 3 of the License, or
;;;; (at your option) any later version.
;;;; This program is distributed in the hope that it will be useful,
;;;; but WITHOUT ANY WARRANTY; without even the implied warranty of
;;;; GNU Affero General Public License for more details.
;;;; You should have received a copy of the GNU Affero General Public License
;;;; along with this program. If not, see <>.
(:use "COMMON-LISP")
(:export "+INFINITY+" "FIND-PATH")
(:documentation "The A* algorithm."))
(defmacro with-functions ((&rest fnames) &body body)
`(flet ,(mapcar (lambda (fname)
(if (listp fname)
(destructuring-bind (name &rest parameters) fname
`(,name ,parameters (funcall ,name ,@parameters)))
`(,fname (&rest arguments) (apply ,fname arguments))))
(declare (inline ,@(mapcar (lambda (fname) (if (listp fname) (first fname) fname)) fnames)))
(defconstant +infinity+ most-positive-long-float)
(defun find-path (successors previous set-previous cost set-cost estimate-distance goalp
start-node goal-node)
DO: Implement the A* algorithm.
SUCCESSORS: a function giving for each node, the list of its
PREVIOUS: a getter giving the current previous node for the
given node.
SET-PREVIOUS: a setter to set the current previous node for the
given node. (lambda (new-previous node))
COST: a getter giving the current cost of the given
node. The default should be +INFINITY+.
SET-COST: a setter setting the current cost of the given
node. (lambda (new-cost node))
ESTIMATE-DISTANCE: a function of two nodes returning a cost estimate
of the distance between them.
GOALP: a predicate indicating whether the node is the
GOAL-NODE (or close enough).
START-NODE: the start node of the searched path.
GOAL-NODE: the end node of the searched path.
RETURN: a path, ie. a list of nodes from START-NODE to
(with-functions (successors previous set-previous cost set-cost estimate-distance goalp)
(flet ((build-path (node)
:with path = '()
:for current = node :then (previous current)
:while current
:do (push current path)
:finally (return path)))
(choose-node (reachable)
:with min-cost = +infinity+
:with best-node = nil
:for node :in reachable
:for cost-start-to-node = (cost node)
:for cost-node-to-goal = (estimate-distance node goal-node)
:for total-cost = (+ cost-start-to-node cost-node-to-goal)
:do (print (list node '/ cost-start-to-node '+ cost-node-to-goal '= total-cost '/ min-cost))
:when (< total-cost min-cost)
:do (setf min-cost total-cost
best-node node)
:finally (return best-node))))
(declare (inline build-path choose-node))
(set-cost 0 start-node)
:with reachable = (list start-node)
:with explored = '()
:while reachable
:do (let ((node (choose-node reachable)))
(when (goalp node)
(return-from find-path (build-path node)))
(setf reachable (delete node reachable))
(push node explored)
(let ((new-reachable (set-difference (successors node) explored)))
(dolist (adjacent new-reachable)
(unless (member adjacent reachable)
(push adjacent reachable))
(let ((new-cost (1+ (cost node))))
(when (< new-cost (cost adjacent))
(set-previous node adjacent)
(set-cost new-cost adjacent))))))))))
(defun g1-successors (node)
"Represents the graph"
(ecase node
(a '(b f))
(b '(a c))
(c '(b))
(d '())
(e '(j))
(f '(a k))
(g '())
(h '())
(j '(e o))
(k '(f l))
(l '(k m q))
(m '(l n))
(n '(m o))
(o '(n j t))
(p '())
(q '(l v))
(r '())
(s '())
(t '(o y))
(u '())
(v '(q w))
(w '(v x))
(x '(w y))
(y '(x t))))
(defun g1-coordinates (node)
(ecase node
(a #c(0 0))
(b #c(1 0))
(c #c(2 0))
(d #c(3 0))
(e #c(4 0))
(f #c(0 1))
(g #c(1 1))
(h #c(2 1))
(i #c(3 1))
(j #c(4 1))
(k #c(0 2))
(l #c(1 2))
(m #c(2 2))
(n #c(3 2))
(o #c(4 2))
(p #c(0 3))
(q #c(1 3))
(r #c(2 3))
(s #c(3 3))
(t #c(4 3))
(u #c(0 4))
(v #c(1 4))
(w #c(2 4))
(x #c(3 4))
(y #c(4 4))))
(defun g1-distance (a b)
(abs (- (g1-coordinates a) (g1-coordinates b))))
(defun test/g1 (&key (start 'a) (goal 't))
(multiple-value-call (function find-path)
(function g1-successors)
(let ((p (make-hash-table)))
(values (lambda (node) (gethash node p))
(lambda (new-previous node) (setf (gethash node p) new-previous))))
(let ((c (make-hash-table)))
(values (lambda (node) (gethash node c +infinity+))
(lambda (new-cost node) (setf (gethash node c) new-cost))))
(function g1-distance)
(lambda (node) (eql node goal))
start goal))
(defun test ()
(assert (equal (test/g1 :start 'a :goal 'x)
'(a f k l q v w x)))
(assert (equal (test/g1 :start 'a :goal 'y)
'(a f k l m n o t y)))
;;;; THE END ;;;;
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment