running a lisp program in Allegro CL

;;; path-planning.lisp
;;; Definitions for the problem of finding optimal paths in two dimensions
;;; with convex polygonal obstcles. The scene is specified with the
;;; visible vertices precalculated for each vertex.
 
(defstruct (vertex (:print-function print-vertex))
  xy           ;; the xy point for the vertex
  c-neighbor   ;; neighbour in clockwise direction
  a-neighbor   ;; neighbour in anti-clockwise direction
  visible      ;; list of vertices visible from here
  )
 
(defun print-vertex (v &optional (stream t) depth)
  (declare (ignore depth))
  (format stream "VERTEX~A" (vertex-xy v)))
 
(defstruct line
  xy1 xy2)
 
(defstruct polygon
  entry-vertex vertices n)
 
(defstruct scene
  polygons				; polygons comprising scene
  start-polygon				; polygon for start 
  goal-polygon				; polygon for goal
  )
 
;;;; Creating a Path Planning Problem
 
 
(defun path-planning-problem (scene)
  "Create a path-planning problem from a description of a scene."
  ;; States are represented by a single vertex.
  (let ((start (polygon-entry-vertex (scene-start-polygon scene)))
	(goal  (polygon-entry-vertex (scene-goal-polygon scene))))
    (make-problem
     :initial-state      start
     :successor-fn       #'(lambda (v1) (path-planning-successors v1 scene))
     :goal-test          #'(lambda (v1) (eq v1 goal))
     :g-cost-fn          nil
     :h-cost-fn          #'(lambda (v1) 
			     (xy-distance (vertex-xy v1) (vertex-xy goal)))
     :edge-cost-fn       #'(lambda (v1 v2) 
			     (xy-distance (vertex-xy v1) (vertex-xy v2)))
     :hash-key           #'vertex-xy
     :domain             "path-planning"
     )))
 
 
(defun path-planning-successors (v1 scene)
  "Return a list of (action . state) pairs, where the state is another
  vertex that is visible from here, and the action is a delta (dx dy)
  from the current vertex to the new one."
  (let ((p1 (vertex-xy v1)))
    (mapcar #'(lambda (v2) (let ((p2 (vertex-xy v2)))
			     (cons (make-xy :x (- (xy-x p2) (xy-x p1)) 
					    :y (- (xy-y p2) (xy-y p1)))
				   v2)))
	    (vertices-visible-from v1 scene))))
 
 
;;; Functions for testing whether one vertex is visible from another
 
(defun vertices-visible-from (v1 scene)
  "Find all the vertices that can be seen from this vertex."
  ;; When you find them, cache them under the vertex-visible slot.
  (or (vertex-visible v1)
      (setf (vertex-visible v1) (vertices-in-view v1 scene))))
	    
(defun vertices-in-view (v scene)
  "Find all the other vertices that can be seen from v."
  (delete v
   (with-collection ()
    (for each poly in (scene-polygons scene) do
	 (cond ((member v (polygon-vertices poly))
		(collect (vertex-c-neighbor v))
		(collect (vertex-a-neighbor v)))
	       (t (for each v2 in (polygon-vertices poly) do
		       (when (visible-p (vertex-xy v) (vertex-xy v2) scene)
			 (collect v2)))))))))
 
(defun visible-p (xy1 xy2 scene)
  "Predicate; return t iff xy1 is visible from xy2."
  (let ( (line (make-line :xy1 xy1 :xy2 xy2)) )
    (dolist (poly (scene-polygons scene) t)
      (if (line-intersects-poly? line poly) (return nil)))))
 
(defun line-intersects-poly? (line poly)
  "Predicate; return t iff line intersects poly."
  (dolist (v1 (polygon-vertices poly) nil)
    (let ((v2 (vertex-c-neighbor v1)))
      (if (intersects line 
                      (make-line :xy1 (vertex-xy v1) :xy2 (vertex-xy v2)))
	  (return t)))))
 
(defun intersects (l1 l2)   
;;; l1 is line ab; l2 is line cd
;;; assume the lines cross at alpha a + (1-alpha) b, 
;;;     also known as beta c + (1-beta) d
;;; line segments intersect if 0<alpha,beta<1 unless they're parallel
  (let* ((a (line-xy1 l1))
	 (b (line-xy2 l1))
	 (c (line-xy1 l2))
	 (d (line-xy2 l2))
	 (xa (xy-x a)) (ya (xy-y a))
	 (xb (xy-x b)) (yb (xy-y b))
	 (xc (xy-x c)) (yc (xy-y c))
	 (xd (xy-x d)) (yd (xy-y d))
	 (q (- (* (- xa xb) (- yc yd))
	       (* (- ya yb) (- xc xd)))))
    (unless (zerop q)
      (let ((alpha (/ (- (* (- xd xb) (- yc yd))
			 (* (- yd yb) (- xc xd)))
		      q))
	     (beta (/ (- (* (- xd xb) (- ya yb))
			   (* (- yd yb) (- xa xb)))
		         q)))
	(and (< 0 alpha 1) (< 0 beta 1))))))
 
 
;;;; Code for constructing the scene data structure
 
(defun create-scene (start goal polygon-data)
  (let ((scene
	 (make-scene
	  :start-polygon (create-polygon (list start))
	  :goal-polygon  (create-polygon (list goal)))))
    (setf (scene-polygons scene)
	  (cons (scene-start-polygon scene)
		(cons (scene-goal-polygon scene)
		      (mapcar #'create-polygon polygon-data))))
    scene))
 
(defun create-polygon (points)
  ;; Assumes that points are given in anticlockwise order (or in order, anyway)
  (let* ((vertices (mapcar #'(lambda (xy) (make-vertex :xy xy)) points))
	 (poly (make-polygon :vertices vertices)))
    (setf (polygon-entry-vertex poly) (first vertices))
    (setf (polygon-n poly) (length vertices))
    (dolist (v vertices)
      (let ((v2 (or (cadr (member v vertices :test #'eq))
		    (first vertices))))
	(setf (vertex-a-neighbor v) v2)
	(setf (vertex-c-neighbor v2) v)))
    poly))      
    
;;;; Specific scene, shown as Figure 4.17 [p 120]
 
(defparameter *scene-4.17*
  (create-scene 
   '(112 660) ;;start
   '(353 573) ;;goal
   ;; each polygon is represented as a list of (x y) coordinates of vertices
   '(((220 616) (220 666) (251 670) (272 647))
     ((341 655) (359 667) (374 651) (366 577))
     ((311 530) (311 559) (339 578) (361 560) (361 528) (336 516))
     ((105 628) (151 670) (180 629) (156 577) (113 587))
     ((118 517) (245 517) (245 557) (118 557))
     ((280 583) (333 583) (333 665) (280 665))
     ((252 594) (290 562) (264 538))
     ((198 635) (217 574) (182 574))
     ))
  "The scene in Figure 4.17 [p 120] with 8 obstacles.")

                                  

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:
76:
77:
78:
79:
80:
81:
82:
83:
84:
85:
86:
87:
88:
89:
90:
91:
92:
93:
94:
95:
96:
97:
98:
99:
100:
101:
102:
103:
104:
105:
106:
107:
108:
109:
110:
111:
112:
113:
114:
115:
116:
117:
118:
119:
120:
121:
122:
123:
124:
125:
126:
127:
128:
129:
130:
131:
132:
133:
134:
135:
136:
137:
138:
139:
140:
141:
142:
143:
144:
145:
146:
147:
148:
149:
150:
151:
152:
153:
154:
155:
156:
157:
158:
159:
160:
161:
162:
163:
164: