Pathfinding in AutoCAD with the A-Star Algorithm (A*)

[heschr]

Hello,

 

for a project I needed to find the shortest way between two points in a network, so I searched for a pathfinding algorithm in AutoLISP. Since I did not find anything - here comes my implementation of the A* pathfinding algorithm. (If you don't know what A* is - I got my information here: http://en.wikipedia.org/wiki/A*_search_algorithm)

 

The program finds the shortest way in a network of polylines between two points, the polylines representing the paths you are allowed to move along. Two polylines are considered connected if one of their vertices match. The resulting path is returned as a new polyline.

 

The program and a test.dwg are attached to this post.

 

 

I would like to extent the network/edges in a way, that polylines are also considered connected if the vertice of one of them is on the line of another. I could use some help with that.

 

I hope this is useful for someone.

astar_01.lsp

test_01.dwg

Edited September 12, 2014 by heschr

[Shady_Potato]

This is Amazing work!, but what if there were multiple Objectives? Could there be a way to create a multi-objective pathfinder? For example In HVAC; Say If the Staring point is Given, But I want to Route my ducts to multiple Registers, Is there a way A* could do that?

 

[ronjonp]

There is also THIS.

[ymg3]

Here I've revised Helmut's code and made it faster.

 

;;                                                                            ;
;; Pathfinding with the A* algorithm      by ymg 22/07/2024                   ;
;;                                                                            ;
;; Revised a prog by  HELMUT SCHRÖDER - heschr@gmx.de - 2014-09-14            ;
;; found at Cadtutor.net                                                      ;
;;                                                                            ;
;; Kept the same format for edges list but added lines as valid choice        ;
;; Format: (((x1 y1) (x2 y2)) (((x2 y2) (x3 y3))....(xn yn)))                 ;
;;                                                                            ;
;; The user is asked to pick a start and an endpoint.                         ;
;; The program will find the shortest path in a network of connected          ;
;; polylines and/or lines and draw a new polyline representing the result.    ;
;;                                                                            ;
;; Two lists of nodes openlst and closelst are created from the above         ;
;; mentionned edges list. The format of a node list is:                       ;
;;  (((Point) (Prev Point) Cumulated_Distance Estimated_Total_Distance)...)   ;
;;                                                                            ;
;; Main change from origina are:                                              ;
;;   - cons the list instead of append                                        ;
;;   - vl-sort the openlist instead of the quicksort                          ;
;;   - Replaced and renamed some vars and subroutine.                         ;
;;   - Added fuzz 1e-4 to all points comparison                               ;
;;   - Change the get_path function                                           ;
;;   - Added line as possible edges                                           ;
;;   - Added an error handler                                                 ;
;;   - Added a timer to the search portion of the program                     ;
;;                                                                            ;
;; The above changes amounted to an acceleration of about 4x from the         ;
;; original program.                                                          ;
;;                                                                            :
;; If you compile this program to a .fas you'll get more than 10x faster.     ;
;;                                                                            ;

(defun c:A* ( / ssl ssp i edges startp endp openlst closelst found acdoc Edgelay Pathlay Pathcol Pathlwt)
  (vl-load-com)

  ; Changes values of following 4 global variables to suit your need.         ;
   
  (setq Edgelay "Edges" 
        Pathlay "Path"
	Pathcol 1       ; 1=Red 2=Yellow   etc.           ;
	Pathlwt 70      ; lineweight for path  0.7mm      ;
  )
   
  (or acdoc (setq acdoc (vla-get-activedocument (vlax-get-acad-object))))
   
  (set_errhandler '("CLAYER" "OSMODE" "CMDECHO"))

  (setvar 'CMDECHO 0)
  (setvar 'OSMODE  1)   
   
  (if (setq ssp (ssget '"X" (list (cons 0  "LWPOLYLINE") (cons 8  Edgelay))))
      (foreach en (mapcar (function cadr) (ssnamex ssp)) 
        (setq edges (append edges (mk_edge (listpol2d en))))     
     )
  )   
   
  (if (setq ssl (ssget '"X" (list (cons 0  "LINE") (cons 8  Edgelay))))
     (foreach en (mapcar (function cadr) (ssnamex ssl))
        (setq edges (cons  (list (butlast (vlax-curve-getstartpoint en)) (butlast (vlax-curve-getendpoint en))) edges))
     )
  )
   
  (setq startp  (butlast (getpoint "\nPick Start Point: "))                 ; Startpoint - reduced to 2D                      ;
        endp    (butlast (getpoint "\nPick End Point: "))                   ; Endpoint   - reduced to 2D                      ;
        openlst (list (list startp '(0 0) 0.0 (distance startp endp)))      ; Add starting node to openlst                    ;
  )
  
  (vla-startundomark acdoc)
   
  (setq ti (getvar 'MILLISECS)) 
   
  (while (and openlst (not found))    
     (setq node (car openlst))
     
     (if (equal (car node) endp 1e-4)
        (setq found T  closelst (cons node closelst))	
        (setq closelst (cons node closelst)
	      openlst  (upd_openlst edges node endp (cdr openlst) closelst)
        )
     )  
  )
  
  (if found
    (mk_lwp (get_path closelst)) 
    (alert "No path was found")    
  )
   
  (princ (strcat "\nExecution time:" (itoa (- (getvar 'MILLISECS) ti)) " milliseconds."))
  (*error* nil)
)

;;                                                                            ;
;; upd_openlst                                                                ;
;;                                                                            ;
;; Each node of the openlst is passed to this sub and we scan the edges list  ;
;; to find the corresponding edges. Then both points of the edges are tested  ;
;; for equality to the nodes.  The fixed cost (distance) is updated and so is ;
;; the estimated total distance. Updates are first put in a temporary node.   ;
;;                                                                            ;
;; We then proceed to test if the temp variable is already in the closelst    ;
;; and proceed to the next edge.                                              ;
;;                                                                            ;
;; If temp is true and temp is not in closelst we go to the recursive sub     ;
;; in_openlst which adjust the values and return the updated openlst          :
;;                                                                            ;
;; Upon return we sort the openlst on smallest estimated distance             ;
;; and return the openlst to the main routine                                 ;
;;                                                                            ;

(defun upd_openlst (edges node endp openlst closelst / pt fcost p1 p2 d temp)
  (setq pt (car node)  fcost (caddr node))
  (while edges     
    (setq p1 (caar edges) p2 (cadar edges) edges (cdr edges) d (distance p1 p2) temp nil)
    
    ;Testing both points of an edge and building a temporary node             ;
    (cond
      ((equal pt p1 1e-4) (setq temp (list p2 p1 (+ fcost d) (+ fcost d (distance p2 endp)))))
      ((equal pt p2 1e-4) (setq temp (list p1 p2 (+ fcost d) (+ fcost d (distance p1 endp)))))
    )
    (if (and temp (not (memberfuzz (car temp) closelst)))
      (setq openlst (in_openlst temp openlst))     
    )     
  )   
  ; Keep openlist sorted on smallest Estimated Total Cost                     ;
  (print (vl-sort openlst (function (lambda(a b)(< (cadddr a) (cadddr b))))) ) 
)

   
;in_lst  Replaced by memberfuzz                                               ;

;(defun in_lst (pt lst)
;  (cond
;     ((not lst) nil)
;     ((equal pt (caar lst) 1e-4) lst)
;     (T (in_lst pt (cdr lst)))
;  )
;)

; returns a new openlst with a double exchanged if cost is lower              ;
;;                                                                            ;
(defun in_openlst (node lst)   
  (cond
    ((not lst) (list node))
    ((equal (car node) (caar lst) 1e-4)
      (if (< (cadddr node) (cadddr (car lst)))
        (cons node (cdr lst))
        lst
      )
    )
    (T (cons (car lst) (in_openlst node (cdr lst)))) 
  )
)

(defun in_openlst2 (node lst / s c)
   (setq s (splitat (caar node) lst) c (cadddr node))
   (cond
      ((not lst) (list node))
      ((not (car s)) (cons node (cadr s)))
      ((not (cadr s)) (cons node (car s)))
      (T (if (< (cadddr node) (cadddr (cadr s)))
	    (append (car s) (cons node (cdr s)))
	    lst
	 ))   
      ;(T (c ns node lst))
   )
 )

    
;;                                                                            ;
;; listpol2D   by ymg    (Simplified a Routine by Gile Chanteau               ;
;;                                                                            ;
;; Parameter:  en,  Entity Name or Object Name of Any Type of Polyline        ;
;;                                                                            ;
;; Returns:    List of Points in 2D WCS                                       ;
;;                                                                            ;
;; Notes:      Requires butlast function for 2d points.                       ;
;;                                                                            ;

(defun listpol2d (en / i lst)
  (repeat (setq i (fix (1+ (vlax-curve-getEndParam en))))
     (setq lst (cons (butlast (vlax-curve-getPointAtParam en (setq i (1- i)))) lst))
  )
)


;;                                                                            ;
;; mk_edge                                                                    ;
;;                                                                            ;
;; From a list of consecutives points as supplied by listpol2D,               ;
;; Returns a list of edges  (((x1 y1)(x2 y2)) ((x2 y2)(x3 y3))...)            ;
;;                                                                            ;

(defun mk_edge (lst) 
  (mapcar (function (lambda (a b) (list a b ))) lst (cdr lst))
)

;;                                                                            ;
;; butlast                                                                    ;
;;                                                                            ;
;; Returns a list without the last item                                       ;
;; Used here mainly to change points to 2D                                    ;
;;                                                                            ;

(defun butlast (lst) (reverse (cdr (reverse lst))))


;;                                                                            ;
;; get_path                                                                   ;
;;                                                                            ;
;; Returns The list of points of shortest path found from closelst.           ;
;;                                                                            ;


(defun get_path (lst / path)
  (setq path (list (caar lst)) prev (cadar lst) lst (cdr lst))
  (while (setq lst  (memberfuzz prev lst))
     (setq prev (cadar lst)
           path (cons (caar lst) path)	   
     )
  )
  path 
)

;;                                                                            ;
;; memberfuzz  by Gile Chanteau                                               ;
;;                                                                            ;
;; Modified to work with nodes list                                           ;
;;                                                                            ;

(defun memberfuzz (p lst)
  (while (and lst (not (equal p (caar lst)  1e-4)))
    (setq lst (cdr lst))
  )
  lst
)

(defun splitat (p lst / tr)   
  (while (and lst (not (equal p (caar lst)  1e-4)))
    (setq tr (cons (car lst) tr) lst (cdr lst))
  )
  (list (reverse tr) lst)
)

(defun truncfuzz (p lst)
  (if (and lst (not (equal p (caar lst) 1e-4)))
    (cons (car lst) (truncfuzz p (cdr lst)))
  )
)

(defun posfuzz (p lst)
  (- (length lst) (length (memberfuzz p lst)))
)

(defun rotleft  (lst) (append (cdr lst) (list (car lst))))
(defun rotright (lst) (cons (last lst) (butlast lst)))

;;                                                                            ;
;; mk_lwp                                                                     ;
;;                                                                            ;
;; Draw an lwpolyline given a point list                                      ;
;;                                                                            ;
;; Will be drawn on layer with color and lineweight defined by Variables      ;
;; at beginnung of program.                                                   ;
;;                                                                            ;

(defun mk_lwp (pl)
  (entmakex 
    (append 
      (list 
        (cons 0   "LWPOLYLINE")
        (cons 100 "AcDbEntity")
        (cons 100 "AcDbPolyline")
	(cons 8    Pathlay)        
	(cons 62   Pathcol)        
        (cons 90   (length pl))
        (cons 70   0)
	(cons 370  Pathlwt)         
      )
      (mapcar (function (lambda (a) (cons 10 a))) pl)
    )
  )
)




;; Error Handler        by Elpanov Evgenyi                                    ;

(defun set_errhandler (l)
   (setq varl (mapcar (function (lambda (a) (list 'setvar a (getvar a)))) l))
)

(defun *error* (msg)
   (mapcar 'eval varl)
   (if (and msg (not (wcmatch (strcase msg) "*BREAK*,*CANCEL*,*EXIT*")))
      (princ (strcat "\nError: " msg))
   )
   (vla-endundomark acdoc)
   (princ)
)

(princ "A* to start")

 

Astar rev3.lsp astar test.dwg