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