;;;; Path Planning in 2 Dimensions with Convex Polygonal Obstacles
(defstructure (path-planning-problem (:include problem)
(:constructor create-path-planning-problem))
"A problem involving moving among polygonal obstacles in 2D space.
A state is the current vertex."
scene)
(defun make-path-planning-problem (&key scene)
"Define a constructor to build a problem, using the scene properly."
(create-path-planning-problem
:scene scene
:initial-state (scene-start scene)
:goal (scene-goal scene)))
(defmethod successors ((problem path-planning-problem) v1)
"Return a list of (action . state) pairs, where the state is another
vertex that is visible from the current vertex v1, and the action is a
delta (dx dy) from v1 to the new one."
(let ((p1 (vertex-xy v1)))
(mapcar #'(lambda (v2) (let ((p2 (vertex-xy v2)))
(cons (@ (- (xy-x p2) (xy-x p1))
(- (xy-y p2) (xy-y p1)))
v2)))
(vertices-visible-from v1 (path-planning-problem-scene problem)))))
(defmethod edge-cost ((problem path-planning-problem) node action vertex)
"The cost of an action is its distance."
(declare-ignore node vertex)
(xy-distance '(0 0) action))
(defmethod h-cost ((problem path-planning-problem) vertex)
"The heuristic cost is the straight-line distance to the goal."
(xy-distance (vertex-xy vertex) (vertex-xy (problem-goal problem))))
;;;; Defining the Vertex, Line, Polygon and Scene Types
(defstructure 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
)
(defmethod print-structure ((v vertex) stream)
(format stream "#" (xy-x (vertex-xy v)) (xy-y (vertex-xy v))))
(defstructure line
xy1 xy2)
(defstructure polygon
vertices n)
(defstructure scene
polygons ; polygons comprising scene
start ; vertex for start
goal ; vertex for goal
)
;;; 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
(let ((result nil))
(for each poly in (scene-polygons scene) do
(cond ((member v (polygon-vertices poly))
(push (vertex-c-neighbor v) result)
(push (vertex-a-neighbor v) result))
(t (for each v2 in (polygon-vertices poly) do
(when (visible-p (vertex-xy v) (vertex-xy v2) scene)
(push v2 result))))))
result)))
(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