GitHub Community
Extending 2D A Pathfinding to 3D Routing with Elevation Data #179902
Unanswered
Ayhyg7
asked this question in
Programming Help
Replies: 2 comments 1 reply
-
Hi @Ayhyg7,1. Data Structure Enhancements
2. Heuristics Update
3. Obstacle Handling
4. Entity Abstraction
5. Backward Compatibility
6. Performance Optimization
7. Environmental Standards
Summary Table
|
Beta Was this translation helpful? Give feedback.
0 replies
-
|
Keep in mind that there are advanced software programs like these(Bentley
raceway and cable management,altium wire harness design software) for
cabling and piping, as well as error-prone manual methods. In the meantime,
there is a research gap where I can design methods such as plugins and
lightweight software for this purpose. Thank you for your help. If you can
help me write such a plugin and we can do this together, I would be
grateful.
On Wed, Nov 19, 2025 at 8:02 PM Milos Terzic ***@***.***> wrote:
Hi @Ayhyg7 <https://github.com/Ayhyg7>, 1. *Data Structure Enhancements*
- *Upgrade Node Representation:*
Change your nodes from 2D (x, y) to 3D (x, y, z), where z represents
the elevation.
- *Grid or Graph Model:*
If you use a grid, add a third dimension for elevation levels or
layers. For a graph-based approach, update each node and connection to
incorporate 3D coordinates.
2. *Heuristics Update*
- *Heuristic Function in A:**
Replace the 2D Euclidean distance with the 3D variant:
;; 3D Euclidean Distance
(sqrt (+ (expt (- x2 x1) 2) (expt (- y2 y1) 2) (expt (- z2 z1) 2)))
- This ensures the algorithm properly estimates cost in three
dimensions.
3. *Obstacle Handling*
- *3D Collision Detection:*
For 3DSOLID, SURFACE, and REGION objects, implement (or leverage
AutoCAD’s APIs for) 3D intersection tests instead of simple 2D bounding
checks.
- *Volumetric Occupancy Grid:*
Represent occupied/obstacle space using voxels (3D grid cells marked
as "occupied" or "free"). For precise routing, granularity may need to be
high.
- *Efficient Spatial Queries:*
Consider spatial indexing (e.g., octrees) to speed up collision and
routing decisions in complex environments.
4. *Entity Abstraction*
- *Entity Type Handling:*
Implement a function to extract or approximate the extents/bounds of
3DSOLID, SURFACE, and REGION and mark affected voxels as occupied.
- *Compatibility Layer:*
Encapsulate 2D and 3D logic in separate modules. Allow fallback to 2D
when elevation data is absent or 3D mode is not needed.
5. *Backward Compatibility*
- *Mode Switch:*
Allow the user to select 2D or 3D mode. Maintain compatibility by
keeping your 2D A* as a subset of your 3D logic (e.g., set z=0 for 2D
operations).
- *Workflow Integration:*
When only 2D data is available, project 3D results onto the 2D plane
for legacy workflows.
6. *Performance Optimization*
- *Reduce Search Space:*
Limit the 3D search to allowable z-levels relevant for cable routing
to avoid unnecessary computation.
- *Asynchronous or Incremental Processing:*
If processing becomes slow, consider breaking the pathfinding into
stages or allowing it to run asynchronously.
7. *Environmental Standards*
- *Parameterized Constraints:*
Allow the system to accept environmental limitations (max bend, min
clearance, routing forbidden zones) as parameters governing the cost
function or walkability.
------------------------------
*Summary Table*
Aspect 2D Approach 3D Extension (Recommended)
Node Structure (x, y) (x, y, z)
Heuristic 2D distance 3D distance
Obstacle Representation 2D polygons Voxels/3D bounding volumes
Entity Handling LWPOLYLINE, etc. 3DSOLID, SURFACE, REGION with bounding
extraction
Performance 2D grid/graph Sparse voxel grid, octree, spatial indexing
Compatibility 2D workflows only Mode switch or unified interface
------------------------------
—
Reply to this email directly, view it on GitHub
<#179902 (comment)>,
or unsubscribe
<https://github.com/notifications/unsubscribe-auth/BCK5G6AUZ3E3WOHCDFC55Z335SLRJAVCNFSM6AAAAACMPZQYUWVHI2DSMVQWIX3LMV43URDJONRXK43TNFXW4Q3PNVWWK3TUHMYTKMBRGU3DSMQ>
.
You are receiving this because you were mentioned.Message ID:
***@***.***>
(defun c:GridPointsGraphAStarPairsWithObstacles ( / p1 p2 x1 y1 x2 y2 numPoints
ensureLayer myAbs myMin myMax
rows cols dx dy startX startY
pickToIdx clamp colLastRow
heuristic neighbors
aStar reconstruct-path
idx->pt
prevLayer startPt endPt startIdx endIdx pathIdxs
drawPointThreshold tmp i numPairs pairN pathColor pathLayerName
blockedSet
ss n ent obj
verts
pointInPoly
gridPointInsideObstacles
obstaclePolygons
obstacleWindowP1 obstacleWindowP2
center radius ang major ratio
splineObj paramStart paramEnd paramStep param
pt newR newC newIdx dir
found
)
;; ---------- small helpers ----------
(defun ensureLayer (name color)
(if (not (tblsearch "LAYER" name)) (command "_.LAYER" "NEW" name ""))
(if color (command "_.LAYER" "COLOR" (itoa color) name ""))
)
(defun myAbs (x) (if (< x 0.0) (- x) x))
(defun myMin (a b) (if (< a b) a b))
(defun myMax (a b) (if (> a b) a b))
;; ---------- point-in-polygon test using raycasting (improved) ----------
(defun pointInPoly (pt poly / x y n i j xi yi xj yj intersect)
(setq x (car pt) y (cadr pt))
(setq n (length poly))
(setq j (1- n))
(setq i 0)
(setq intersect 0)
(repeat n
(setq xi (car (nth i poly))
yi (cadr (nth i poly))
xj (car (nth j poly))
yj (cadr (nth j poly)))
(if (and (<= (min yi yj) y)
(< y (max yi yj))
(< x (+ xi (/ (* (- xj xi) (- y yi)) (- yj yi)))))
(setq intersect (1+ intersect)))
(setq j i)
(setq i (1+ i))
)
(= (rem intersect 2) 1)
)
;; ---------- extract obstacle polygons within crossing window ----------
(princ "\nPick obstacle crossing window - first corner: ")
(setq obstacleWindowP1 (getpoint))
(princ "\nPick opposite corner: ")
(setq obstacleWindowP2 (getpoint))
(setq ss (ssget "_C" obstacleWindowP1 obstacleWindowP2 '((0 . "LWPOLYLINE,LINE,CIRCLE,ELLIPSE,SPLINE"))))
(setq obstaclePolygons '())
(if ss
(progn
(setq n 0)
(while (< n (sslength ss))
(setq ent (ssname ss n))
(setq obj (entget ent))
(setq verts '())
(cond
;; Handle LWPOLYLINE
((= (cdr (assoc 0 obj)) "LWPOLYLINE")
(foreach v (vl-remove-if-not '(lambda (x) (= (car x) 10)) obj)
(setq verts (cons (cdr v) verts))
)
(setq verts (reverse verts))
)
;; Handle LINE
((= (cdr (assoc 0 obj)) "LINE")
(setq verts (list (cdr (assoc 10 obj)) (cdr (assoc 11 obj))))
)
;; Handle CIRCLE - approximate with 36-sided polygon
((= (cdr (assoc 0 obj)) "CIRCLE")
(setq center (cdr (assoc 10 obj))
radius (cdr (assoc 40 obj))
ang 0.0)
(repeat 36
(setq verts (cons (list (+ (car center) (* radius (cos ang)))
(+ (cadr center) (* radius (sin ang))))
verts))
(setq ang (+ ang (/ (* 2.0 pi) 36.0)))
)
)
;; Handle ELLIPSE - approximate with 36 points
((= (cdr (assoc 0 obj)) "ELLIPSE")
(setq center (cdr (assoc 10 obj))
major (cdr (assoc 11 obj))
ratio (cdr (assoc 40 obj))
ang 0.0)
(repeat 36
(setq pt (list (* (cos ang) (car major))
(* (sin ang) (cadr major) ratio)))
(setq verts (cons (mapcar '+ center pt) verts))
(setq ang (+ ang (/ (* 2.0 pi) 36.0)))
)
)
;; Handle SPLINE - sample points along the spline
((= (cdr (assoc 0 obj)) "SPLINE")
(setq splineObj (vlax-ename->vla-object ent))
(setq paramStart (vlax-curve-getStartParam splineObj)
paramEnd (vlax-curve-getEndParam splineObj))
(if paramEnd
(progn
(setq paramStep (/ (- paramEnd paramStart) 35.0))
(setq param paramStart)
(while (<= param paramEnd)
(setq verts (cons (vlax-curve-getPointAtParam splineObj param) verts))
(setq param (+ param paramStep))
)
)
)
)
)
(if verts
(setq obstaclePolygons (cons verts obstaclePolygons))
)
(setq n (1+ n))
)
(princ (strcat "\nFound " (itoa (length obstaclePolygons)) " obstacles."))
)
(princ "\nNo obstacles found inside crossing window.")
)
;; ---------- main inputs ----------
(princ "\nSpecify bottom-left corner of rectangle for grid: ")
(setq p1 (getpoint))
(princ "\nSpecify top-right corner of rectangle: ")
(setq p2 (getpoint))
(princ "\nSpecify total number of grid points (up to 1,000,000): ")
(setq numPoints (getint))
(if (or (null numPoints) (< numPoints 2)) (setq numPoints 2))
(setq x1 (car p1) y1 (cadr p1))
(setq x2 (car p2) y2 (cadr p2))
;; normalize order
(if (< x2 x1) (setq tmp x1 x1 x2 x2 tmp))
(if (< y2 y1) (setq tmp y1 y1 y2 y2 tmp))
;; ---------- layers ----------
(setq prevLayer (getvar "CLAYER"))
(ensureLayer "GridPointsLayer" 7)
(ensureLayer "StartLayer" 3)
(ensureLayer "EndLayer" 5)
(ensureLayer "ObstacleLayer" 1)
;; ---------- draw obstacles for visualization ----------
(command "_.LAYER" "SET" "ObstacleLayer" "")
(foreach poly obstaclePolygons
(if (> (length poly) 1)
(progn
(apply 'command (append (list "_.PLINE") poly (list "")))
)
)
)
;; ---------- compute rows/cols ----------
(setq rows (fix (sqrt (float numPoints))))
(if (< rows 1) (setq rows 1))
(setq cols (fix (/ (+ numPoints rows -1) rows))) ; ceil
(while (< (* rows cols) numPoints) (setq cols (1+ cols)))
(setq dx (if (> cols 1) (/ (- x2 x1) (1- cols)) 0.0))
(setq dy (if (> rows 1) (/ (- y2 y1) (1- rows)) 0.0))
;; center grid
(setq startX (+ x1 (/ (- (- x2 x1) (* dx (1- cols))) 2.0)))
(setq startY (+ y1 (/ (- (- y2 y1) (* dy (1- rows))) 2.0)))
;; last row may be partial
(defun colLastRow ( / full last)
(setq full (* (1- rows) cols))
(setq last (- numPoints full))
(if (<= last 0) 0 last)
)
;; clamp
(defun clamp (v lo hi) (myMax lo (myMin hi v)))
;; pick -> nearest grid index
(defun pickToIdx (pt / cx cy c r idx lastCols)
(setq cx (/ (- (car pt) startX) (if (= dx 0.0) 1.0 dx)))
(setq cy (/ (- (cadr pt) startY) (if (= dy 0.0) 1.0 dy)))
(setq c (fix (+ 0.5 cx))) ; round
(setq r (fix (+ 0.5 cy)))
(setq c (clamp c 0 (1- cols)))
(setq r (clamp r 0 (1- rows)))
(setq idx (+ (* r cols) c))
(if (= r (1- rows))
(progn
(setq lastCols (colLastRow))
(if (= lastCols 0) (setq lastCols cols))
(if (>= c lastCols) (setq c (1- lastCols) idx (+ (* r cols) c)))
)
)
(if (>= idx numPoints) (setq idx (1- numPoints)))
idx
)
;; index -> coordinate
(defun idx->pt (idx / r c)
(setq r (fix (/ idx cols)))
(setq c (- idx (* r cols)))
(list (+ startX (* c dx)) (+ startY (* r dy)))
)
;; ---------- check if grid point is inside any obstacles ----------
(defun gridPointInsideObstacles (pt)
(setq found nil)
(foreach poly obstaclePolygons
(if (pointInPoly pt poly)
(setq found T)
)
)
found
)
;; ---------- BLOCKED storage ----------
(setq blockedSet '()) ; list of blocked node indices
;; Populate blockedSet with indices of points inside obstacles
(princ "\nChecking grid points against obstacles...")
(setq i 0)
(while (< i numPoints)
(if (gridPointInsideObstacles (idx->pt i))
(setq blockedSet (cons i blockedSet))
)
(setq i (1+ i))
)
(princ (strcat " " (itoa (length blockedSet)) " blocked points found."))
(defun isBlocked (idx) (if (member idx blockedSet) T nil))
;; ---------- A* helpers ----------
(defun heuristic (a b / ar ac br bc dr dc)
(setq ar (fix (/ a cols)) ac (- a (* ar cols)))
(setq br (fix (/ b cols)) bc (- b (* br cols)))
(setq dr (myAbs (- ar br)))
(setq dc (myAbs (- ac bc)))
(+ (* dr (myAbs dy)) (* dc (myAbs dx)))
)
;; neighbor generation (8-connected for better pathfinding)
(defun neighbors (idx / r c nbs lastCols)
(setq r (fix (/ idx cols)))
(setq c (- idx (* r cols)))
(setq nbs '())
(setq lastCols (colLastRow))
(if (= lastCols 0) (setq lastCols cols))
;; Check all 8 directions
(foreach dir '((-1 -1) (0 -1) (1 -1) (-1 0) (1 0) (-1 1) (0 1) (1 1))
(setq newR (+ r (car dir))
newC (+ c (cadr dir)))
;; Check if new position is valid
(if (and (>= newR 0) (< newR rows)
(>= newC 0)
(if (= newR (1- rows))
(< newC lastCols)
(< newC cols)))
(progn
(setq newIdx (+ (* newR cols) newC))
(if (not (isBlocked newIdx))
(setq nbs (cons newIdx nbs))
)
)
)
)
nbs
)
;; reconstruct path
(defun reconstruct-path (parent start goal / cur acc)
(setq cur goal acc (list goal))
(while (and cur (/= cur start))
(setq cur (cdr (assoc cur parent)))
(if cur (setq acc (cons cur acc)))
)
(if (equal cur start) acc nil)
)
;; A* search avoiding blocked nodes
(defun aStar (start goal / open closed g parent node curr nbr tentative old fscore result)
(if (or (isBlocked start) (isBlocked goal))
(progn
(princ "\nStart or goal is blocked!")
(setq result nil)
)
(progn
(setq g (list (cons start 0.0)))
(setq parent '())
(setq open (list (cons start (heuristic start goal))))
(setq closed '())
(setq result nil)
(while (and open (not result))
(setq open (vl-sort open '(lambda (a b) (< (cdr a) (cdr b)))))
(setq node (car open))
(setq open (cdr open))
(setq curr (car node))
(if (= curr goal)
(setq result (reconstruct-path parent start goal))
(progn
(setq closed (cons curr closed))
(foreach nbr (neighbors curr)
(if (not (member nbr closed))
(progn
;; Calculate cost - straight or diagonal
(setq tentative (+ (cdr (assoc curr g))
(if (or (= (abs (- nbr curr)) 1)
(= (abs (- nbr curr)) cols))
(if (or (= nbr (1- curr)) (= nbr (1+ curr)))
(myAbs dx)
(myAbs dy))
(sqrt (+ (expt (myAbs dx) 2) (expt (myAbs dy) 2))))))
(setq old (assoc nbr g))
(if (or (null old) (< tentative (cdr old)))
(progn
(if old
(setq g (subst (cons nbr tentative) old g))
(setq g (cons (cons nbr tentative) g)))
(if (assoc nbr parent)
(setq parent (subst (cons nbr curr) (assoc nbr parent) parent))
(setq parent (cons (cons nbr curr) parent)))
(setq fscore (+ tentative (heuristic nbr goal)))
;; Remove old entry if exists
(setq open (vl-remove-if '(lambda (x) (= (car x) nbr)) open))
(setq open (cons (cons nbr fscore) open))
)
)
)
)
)
)
)
)
)
)
result
)
;; ---------- optional sparse drawing of points ----------
(setq drawPointThreshold 10000)
(command "_.LAYER" "SET" "GridPointsLayer" "")
(if (<= numPoints drawPointThreshold)
(progn
(prompt (strcat "\nDrawing " (itoa numPoints) " grid points ..."))
(setq i 0)
(while (< i numPoints)
(if (not (isBlocked i))
(command "_.POINT" (idx->pt i))
)
(setq i (1+ i))
)
)
(prompt (strcat "\nGrid has " (itoa numPoints) " points (not drawing to keep things fast)."))
)
;; ---------- ask for PAIRS ----------
(princ "\nHow many (origin,destination) pairs? ")
(setq numPairs (getint))
(if (or (null numPairs) (< numPairs 1)) (setq numPairs 1))
(setq pairN 1)
(repeat numPairs
(prompt (strcat "\nPair " (itoa pairN) " — pick ORIGIN: "))
(setq startPt (getpoint ""))
(setq startIdx (pickToIdx startPt))
(command "_.LAYER" "SET" "StartLayer" "")
(command "_.POINT" (idx->pt startIdx))
(prompt (strcat "\nPair " (itoa pairN) " — pick DESTINATION: "))
(setq endPt (getpoint ""))
(setq endIdx (pickToIdx endPt))
(command "_.LAYER" "SET" "EndLayer" "")
(command "_.POINT" (idx->pt endIdx))
(princ "\nEnter color index for this path (1-255): ")
(setq pathColor (getint))
(if (or (null pathColor) (< pathColor 1) (> pathColor 255))
(setq pathColor 1)
)
(setq pathLayerName (strcat "PathLayer_" (itoa pathColor)))
(ensureLayer pathLayerName pathColor)
(prompt (strcat "\nRouting pair " (itoa pairN) " avoiding obstacles..."))
(setq pathIdxs (aStar startIdx endIdx))
(if pathIdxs
(progn
(command "_.LAYER" "SET" pathLayerName "")
(apply 'command (append (list "_.PLINE") (mapcar 'idx->pt pathIdxs) (list "")))
(princ (strcat " (nodes: " (itoa (length pathIdxs)) ")"))
)
(prompt "\nNo collision-free path found for this pair.")
)
(setq pairN (1+ pairN))
)
;; ---------- restore layer ----------
(command "_.LAYER" "SET" prevLayer "")
(princ)
)
|
Beta Was this translation helpful? Give feedback.
1 reply
-
|
In your opinion, if I perfect my software so that it can automatically route cables or pipes in complex AutoCAD drawings by simply selecting the origin and destination, and is very user-friendly, how much would it be worth? |
Beta Was this translation helpful? Give feedback.
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Uh oh!
There was an error while loading. Please reload this page.
Uh oh!
There was an error while loading. Please reload this page.
-
How can I effectively extend my existing 2D A* pathfinding system in AutoLISP to support true 3D routing with elevation data and volumetric obstacles while maintaining backward compatibility with my current 2D workflows, and what specific architectural changes should I implement to handle complex 3D entities like 3DSOLID, SURFACE, and REGION objects while ensuring optimal performance for cable routing across varied
Beta Was this translation helpful? Give feedback.
All reactions