A* algorithm shortest path finder for 5 Axis laser ablation
This Grasshopper plugin uses variation of A*(A star) search algorithm to find shortest path on a given network of 2D/3D point cloud. Added heuristic method, this algorithm will prioritize nodes in a most relevant direction for faster performance in most cases.
Any network of lines can be used regardless of dimension. This algorithm is intended to be used for finding optimized tool path on 2D/3D Laser CNC but can be used for other purpose too.
▲ test run of shortest path finding on delaunay network
Unique nodes within the linked network will be listed. Each node have properties as follows.
- Unique identifier
- a list of neighboring nodes (node numbers, distance to each nodes)
- parent node that minimizes cost from starting node to this node
- first time visit checker
- cost score
Cost score is updated if sum of distance from new parent node and cost score of parent is smaller than the previous cost score. Algorithm terminates if the cost score of target node is determined and all its adjacent nodes have been checked out.
Resulting path will be traced backwards following parents of each nodes starting from target node.
private void RunScript(List<Line> lines, int spt, int ept, ref object startPoint, ref object searchPoint, ref object nodeSet, ref object costAtNode, ref object shortestPth)
{
//get document tolerance
double docTolerance = Rhino.RhinoDoc.ActiveDoc.ModelAbsoluteTolerance;
//get node points
List<Point3d> pts = new List<Point3d>();
List<Point3d> ptSet = new List<Point3d>();
//search duplicates and destroy
foreach(Line ln in lines){
pts.Add(ln.From);
pts.Add(ln.To);
}
ptSet.Add(pts[0]);
pts.RemoveAt(0);
foreach(Point3d pt in pts){
bool duplicated = false;
foreach(Point3d p in ptSet){
if(pt == p){
duplicated = true;
break;
}
else{
duplicated = false;
}
}
if(duplicated == false){
ptSet.Add(pt);
}
else{}
}
pts = null;
//nodes initialize
List<Node> nodes = new List<Node>();
for(int i = 0;i < ptSet.Count;i++){
Node nde = new Node(i, ptSet[i]);
nodes.Add(nde);
}
//get neighbour index
foreach(Node nde in nodes){
List<Line> neighbourLines = new List<Line>();
foreach(Line ln in lines){
double dist = nde.Location.DistanceTo(ln.ClosestPoint(nde.Location, true));
if(dist <= docTolerance){
neighbourLines.Add(ln);
}
else{}
}
foreach(Line nl in neighbourLines){
foreach(Node nd in nodes){
double dist = nd.Location.DistanceTo(nl.ClosestPoint(nd.Location, true));
if(dist <= docTolerance && nd.Index != nde.Index){
nde.Neighbours.Add(nd.Index);
}
else{}
}
}
}
//Node generations
List<Node> currentGen = new List<Node>();
List<Node> nextGen = new List<Node>();
//starting node initialization
nodes[spt].Intensity = 0;
currentGen.Add(nodes[spt]);
//score calculation on nodes
while(nodes[ept].Parent == -1){
foreach(Node cnd in currentGen){
nodes[cnd.Index].Visited = true;
foreach(int nbrNo in cnd.Neighbours){
double dist = cnd.Location.DistanceTo(nodes[nbrNo].Location);
if(nodes[nbrNo].Visited == false){
nodes[nbrNo].Visited = true;
nodes[nbrNo].Intensity = cnd.Intensity + dist;
nodes[nbrNo].Parent = cnd.Index;
if(nodes[nbrNo].Neighbours.FindIndex(item => item == cnd.Index) != -1){
nodes[nbrNo].Neighbours.RemoveAt(nodes[nbrNo].Neighbours.FindIndex(item => item == cnd.Index));
}
else{}
}
else if(nodes[nbrNo].Intensity > 0){
if(nodes[nbrNo].Intensity > cnd.Intensity + dist){
nodes[nbrNo].Intensity = cnd.Intensity + dist;
nodes[nbrNo].Parent = cnd.Index;
if(nodes[nbrNo].Neighbours.FindIndex(item => item == cnd.Index) != -1){
nodes[nbrNo].Neighbours.RemoveAt(nodes[nbrNo].Neighbours.FindIndex(item => item == cnd.Index));
}
}
else{}
}
else{}
if(nextGen.FindIndex(item => item.Index == nodes[nbrNo].Index) == -1){
nextGen.Add(nodes[nbrNo]);
}
else{}
}
}
currentGen = nextGen;
nextGen = new List<Node>();
}
//shortest path
Node cNode = nodes[ept];
List<Point3d> interpPt = new List<Point3d>();
while(cNode.Index != nodes[spt].Index){
interpPt.Add(cNode.Location);
cNode = nodes[cNode.Parent];
}
interpPt.Add(nodes[spt].Location);
Curve shortestPath = Curve.CreateInterpolatedCurve(interpPt, 1);
//visual purpose
startPoint = nodes[spt].Location;
searchPoint = nodes[ept].Location;
nodeSet = ptSet;
List <double> nCount = new List<double>();
foreach(Node nd in nodes){
nCount.Add(nd.Intensity);
}
costAtNode = nCount;
shortestPth = shortestPath;
}
//node class
public class Node
{
public Node(int idx, Point3d pt){
this.Index = idx;
this.Intensity = 0;
this.Neighbours = new List<int>();
this.Visited = false;
this.Location = pt;
this.Parent = -1;
}
public int Index{get;set;}
public bool Visited{get;set;}
public double Intensity{get;set;}
public List<int> Neighbours{get;set;}
public Point3d Location{get;set;}
public int Parent {get;set;}
}