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

[ymg3]

52 minutes ago, GLAVCVS said:

 

"I think I understand the problem now.

But it only happens with lines. With polylines, whether 3D or not, this doesn't happen.

The issue is that "getPointAtParam" doesn't work the same way with lines as it does with polylines.

If param=1, it doesn't return the expected result.

I've modified the code to fix this (I think) 

 

 

 

 @GLAVCVS Now you know why I had two different selection set for lines and polylines.

 

ymg

[marko_ribar]

I agree with ymg and thanks for improvement ab Dictionary... Nevertheless, IMHO the general case is to perform A* with 3D entities, so 3D polyline is my preferable result... You can explode polylines, select LINES in 2D, or 3D and perform A*; with my version you'll get 3D POLYLINE, after which you can just COPYBASE it with 0,0,0 base and put it into clipboard... Then, do several UNDO until before you exploded anything... Finally, you PASTECLIP with 0,0,0 from clipboard and if network was 3D, then 3D POLYLINE should be just fine, and if network was 2D, you explode 3D POLYLINE into LINE segments, and just do JOIN command on Previous selection - you'll get LWPOLYLINE to which you should just manually adjust lineweight... That's all, simple and unexaustive procedure... BTW., your filter '(0 . "*LINE") in acquirying selection sspl also involves SPLINE entities, for which A* isn't adequate... Perhaps, you should do it with '(0 . "LINE,LWPOLYLINE,POLYLINE") as proper filter... Little robust, but appropriate...

[ymg3]

@marko_ribar The 3d is a nice addition, but the general case is 2d in the context of graph.  Your suggestion for the filter is indeed more robust.

 

ymg

Edited 1 hour ago by ymg3

[GLAVCVS]

I forgot to include the creation of the 3D polyline.
This last code creates the path in 2D or 3D as needed.

 

;; 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.     ;
;;                                                                            ;
;|
Added or revised code by GLAVCVS (january 2026)
	-All set are grouped into one
	-An associative sparse matrix cell->handles is created for faster cell querying (using new 'addToDict' and 'getCell' functions)
	-The "edges" list is replaced with the local search retourned by 'getCell'
	
 T E S T S =====
 	fas:  4-5 x faster than previous fas
 	lsp:  7-8 x faster than previous lsp
|;
(defun c:A* (/ sspl	 i	   edges     startp    endp
	       openlst	 closelst  found     acdoc     Edgelay
	       Pathlay	 Pathcol   Pathlwt   lstClvs   path
	      )
  (vl-load-com)
  					; Changes values of following 4 global variables to suit your need.         ;

  (setq	Edgelay	"0"
	Pathlay	"0"
	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))
;;;      (addToDict en)
;;;      (setq edges (append edges (mk_edge (listpol2d en))))
;;;    )
;;;    nil
;;;  )
;;;
;;;  (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
;;;		  )
;;;      )
;;;    )
;;;  )
  
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;ADDED by GLAVCVS
  (if (setq sspl (ssget "X" (list '(0 . "*LINE") (cons 8 EdgeLay))))
    (foreach en	(mapcar (function cadr) (ssnamex sspl))
      (addToDict en)
    )
  )
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

  (setq	startp	(getpoint "\nPick Start Point: ")
					; Startpoint                    ;
	endp	(getpoint "\nPick End Point: ")
					; Endpoint                      ;
	openlst	(list (list startp '(0 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)); coge el primero (el que más progresa)
    (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
    (if (vl-some '(lambda(x) (not (equal (last x) 0.0 1e-3))) (setq path (get_path closelst)))
      (f3Dpol path Pathcol)
      (mk_lwp path)
    )
    (alert "No path was found")
  )
  (princ (strcat "\nExecution time:"
		 (itoa (- (getvar 'MILLISECS) ti))
		 " milliseconds."
	 )
  )
  (*error* nil)
  (princ)
)

;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;ADDED by GLAVCVS
;;;Create dictionary
;;;[Modified to consider all segments within any polyline]
(defun addToDict (en / p1 p2 id clv i l c a)
  (setq i -1 id (cdr (assoc 5 (setq l (entget en)))) c (= (cdr (assoc 0 l)) "LINE") a 10)
  (while (setq p (if (and (setq i (1+ i)) c) (cdr (assoc (+ a i) l)) (vlax-curve-getPointAtParam en i)))
    (if (setq val (assoc (setq clv (strcat (itoa (fix (car p))) "-" (itoa (fix (cadr p))))) lstClvs))
      (setq lstClvs (subst (append val (list (cons id i))) val lstClvs))
      (setq lstClvs (cons (list clv (cons id i)) lstClvs))
    )
  )
)
;;;return list cell
(defun getCell (pt / clv v lr id p p0 pr par l c oc)
  (defun oc (c e i) (if c (cdr (assoc (+ i 10) l)) (vlax-curve-getPointAtParam e i)))
  (if (setq val (assoc (setq clv (strcat (itoa (fix (car pt))) "-" (itoa (fix (cadr pt))))) lstClvs))
    (foreach par (cdr val)
      (setq e (handent (car par)) c (= (cdr (assoc 0 (setq l (entget e)))) "LINE"))
      (if (zerop (setq pr (cdr par)))
	(setq lr (cons (list (oc c e pr) (oc c e (1+ pr))) lr))
	(setq lr (cons (list (oc c e (1- pr)) (setq p0 (oc c e pr))) lr)
	      lr (if (setq p (oc c e (1+ pr))) (cons (list p0 p) lr) lr)
	)
      )
    )
  )
)
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;

;;                                                                            ;
;; 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 / lEdges edge pt fcost p1 p2 d temp)
  (setq	pt    (car node)
	fcost (caddr node)
  )
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
;;;added By GLAVCVS
  (setq lEdges (getCell pt))
  (foreach edge lEdges
    (setq p1 (car edge)
          p2 (cadr edge)
          d	(distance p1 p2)
	  temp	nil
    )
;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;
  
;;;  (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
      ((and (equal pt p1 1e-4) (not (memberfuzz p2 closelst)))
       (setq openlst (in_openlst (list p2 p1 (+ fcost d) (+ fcost d (distance p2 endp))) openlst))
;;;       (setq temp (list p2 p1 (+ fcost d) (+ fcost d (distance p2 endp))))
      )
      ((and (equal pt p2 1e-4) (not (memberfuzz p1 closelst)))
       (setq openlst (in_openlst (list p1 p2 (+ fcost d) (+ fcost d (distance p1 endp))) openlst))
;;;       (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                     ;
  (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) lst))

(defun f3Dpol (pts c / doc ep ll la e x)
  (setq doc (vla-get-ActiveDocument (vlax-get-acad-object))
        ep (if (= 1 (getvar "CVPORT")) (vla-get-PaperSpace doc) (vla-get-ModelSpace doc))
        ll (apply 'append pts)
        la (vlax-safearray-fill (vlax-make-safearray vlax-vbDouble (cons 0 (1- (length ll)))) (mapcar 'float ll))
	e (vla-Add3DPoly ep la)
	x (vla-put-Color e c)
  )
)

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