A*算法实现（lisp）

A*算法（lisp-gbb）

(load "e:/gbbopen/initiate.lisp")
:gbbopen-user
:agenda-shell-user
;;;Enter the gbbopen environment
;;;---------------------------------------------
(defconstant map-size 10)
(define-unit-class found-traps () (location))
(defun a-star (ksa)
	(setf blocklist '((2 2)(3 3)(4 4)(5 5)))
	(setf pos-sn '(0 0))
	(setf enode '((8 2) (8 9)))
	(run pos-sn enode)
)
(define-ks ks-start 
   :trigger-events ((control-shell-started-event))
   :execution-function 'a-star
)
(defun minofnums(mlist) ;;;min data of list
		(defvar *mymin* (car mlist))
		(if mlist
			(progn (setf *mymin* (if (< *mymin* (car mlist)) *mymin* (car mlist)))
			(minofnums (cdr mlist)))
			*mymin* 
		)
)
(defun compute-fx(node enode father)
	(setf gx-father (cadr (aref gridmap (car(cadddr father)) (cadr(cadddr father)) )))
	(setf gx-f2n (sqrt (+ (expt ( - (car(cadddr father)) (car node)) 2) (expt ( - (cadr(cadddr father)) (cadr node)) 2))))
	(setf gvalue (+ gx-f2n gx-father))
	(setf hx-n2enode '())
	(dotimes (i (length enode))
		(setf en (nth i enode))
		(push (sqrt (+ (expt (- (car en) (car node)) 2) (expt (- (cadr en) (cadr node)) 2))) hx-n2enode)
	)
	(setf min-hx-n2enode (minofnums hx-n2enode))
	(setf fvalue (+ gvalue min-hx-n2enode))
)
(defun update-fx(node enode father);;;node position(x y)only  enode father all information
	(compute-fx node enode father)
	(setf pre-fvalue (car(aref gridmap (car node)(cadr node))))
	(if (< fvalue pre-fvalue)
		(list (setf (car(aref gridmap (car node)(cadr node))) fvalue)
			  (setf (cadddr(aref gridmap (car node)(cadr node))) (caddr father))
		)
	)	
)
(defun set-fx(node enode father);;;node position(x y)only  enode father all information
	(compute-fx node enode father)
	(setf (car(aref gridmap (car node)(cadr node))) fvalue)
	(setf (cadr(aref gridmap (car node)(cadr node))) gvalue)
	(setf (cadddr(aref gridmap (car node)(cadr node))) (caddr father))
)
(defun run(pos-sn pos-en)
	;;;Parameter is position(x y)
	(setf gridmap (make-array (list map-size map-size) :initial-element 0))
	(dotimes (i map-size)
		(dotimes (j map-size)
			(setf grid-pos '())
			(push j grid-pos)
			(push i grid-pos)
			(setf item (list 0 0 grid-pos (list 0 0)))
			(setf (aref gridmap i j) item)
		)
	);;;Create GridMap for A*
	(setf openlist '()) ;;;all information of node 
	(setf closelist '()) 
	(setf cl '())
	(setf goal '(0 0 (0 0)(0 0)))
	(setf founed-goal 0)
	(setf flag-break 0);;;while break
	(push (aref gridmap (car pos-sn)(cadr pos-sn)) openlist)
	(while (and (> (length openlist) 0) (= flag-break 0));;;Find the node with the smallest fx in openlist
		(setf min-data (car(car openlist)))
		(setf min-index 0)
		(dotimes (i (length openlist))
			(setf index i)
			(if (< (car(nth index openlist)) min-data)
				(list
				(setf min-data (car(nth index openlist)) )
				(setf min-index index)))
		)
		(setf cnode (nth min-index openlist))
		(push (caddr(nth min-index openlist)) cl)
		;;;(remove (nth min-index openlist) openlist)
		(write min-index)
		(setf openlist (remove (nth min-index openlist) openlist))
		;;;(write openlist)
		(push cnode closelist)
		;;;extend node
		(extend cnode)
		(if (or (= (length openlist) 0) 
		        (member (caddr cnode) pos-en :test #'equal))
			(setf flag-break 1)
		)
		;;;(write 1)
	)
	(if (member (caddr cnode) pos-en :test #'equal)
		(setf goal cnode)
	)
)
(defun get-route()
	(setf minrote '())
	(setf current-node goal)
	(setf flag-break2 0)
	(while (= flag-break2 0) 
		(push (caddr current-node) minrote)
		(setf current-node (aref gridmap (car(cadddr current-node)) (cadr(cadddr current-node))))
		(if (and (= (car(caddr current-node)) (car pos-sn))
				 (= (cadr(caddr current-node)) (cadr pos-sn))
			)
			(setf flag-break2 1)
		)
	)
	(push pos-sn minrote)
)
(defun get-neighbor(cnode)
	(setf offsets '((-1 1) (0 1) (1 1) (-1 0) (1 0) (-1 -1) (0 -1) (1 -1)))
	(setf nodes-neighbor '())
	(setf x (car(caddr cnode)))
	(setf y (cadr(caddr cnode)))
	(dotimes (i (length offsets))
		(setf pos-new '())
		(setf x-new (+ x (car(nth i offsets))))
		(setf y-new (+ y (cadr(nth i offsets))))
		(push y-new pos-new)
		(push x-new pos-new)
		(if (or (< x-new 0)
				(< y-new 0)
				(> x-new (- map-size 1))
				(> y-new (- map-size 1)))
				(setf enpty 1) ;;;不执行
				(push pos-new nodes-neighbor)
		)
	)
)
(defun extend(cnode)
	(get-neighbor cnode)
	(setf closelist-pos '())	;;;Position(x y)only
	(dotimes (i (length closelist))
		(push (caddr(nth i closelist)) closelist-pos)
	)
	(setf openlist-pos '())		;;;Position(x y)only
	(dotimes (j (length openlist))
		(push (caddr(nth j openlist)) openlist-pos)
	)
	(dotimes (i (length nodes-neighbor))
		(if(and (not(member (nth i nodes-neighbor) closelist-pos :test #'equal))
			    (not(member (nth i nodes-neighbor) blocklist :test #'equal)))
			(if(not(member  (nth i nodes-neighbor) openlist-pos :test #'equal))
				(list
					(set-fx (nth i nodes-neighbor) enode cnode)
					(push (aref gridmap (car(nth i nodes-neighbor))(cadr(nth i nodes-neighbor))) openlist)
				)
				(update-fx (nth i nodes-neighbor) enode cnode)
			)
		)
	)
)
(start-control-shell)

图片参照