Leaderboard

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