public static List <Point3d> RunAstar(Point3d person, Point3d interest, List <Mesh> obstacles)
{
//Run solver
//Declare Lists
List <A_star> open = new List <A_star>();
List <A_star> closed = new List <A_star>();
List <Point3d> Pointpaths = new List <Point3d>();
List <Point3d> finalPath = new List <Point3d>();
List <A_star> children = new List <A_star>();
//Declare var
Point3d position;
Boolean run = true;
//Movement list
List <Point3d> move = new List <Point3d>();
move.Add(new Point3d(0, -1, 0));
move.Add(new Point3d(0, 1, 0));
move.Add(new Point3d(-1, 0, 0));
move.Add(new Point3d(1, 0, 0));
move.Add(new Point3d(-1, -1, 0));
move.Add(new Point3d(-1, 1, 0));
move.Add(new Point3d(1, -1, 0));
move.Add(new Point3d(1, 1, 0));
//Declare start and end
A_star start = new A_star(null, person);
A_star end = new A_star(null, interest);
//Add startpoint to open list
open.Add(start);
// Join obstacles in one Mesh
Mesh obsMesh = new Mesh();
foreach (Mesh mesh in obstacles)
{
obsMesh.Append(mesh);
}
//Pathfinding
while (run)
{
//Get current node
A_star currentNode = open[0];
int currentIndex = 0;
for (int i = 0; i < open.Count; i++)
{
if (open[i].f < currentNode.f)
{
currentNode = open[i];
currentIndex = i;
}
}
//Remove current node from open list and add to closed
open.RemoveAt(currentIndex);
closed.Add(currentNode);
//If at endpoint, traceback the parents
position = currentNode.Position;
Pointpaths.Add(position);
if (currentNode.Position.DistanceTo(interest) < 2)
{
A_star tracer = currentNode;
while (tracer.g > 0)
{
finalPath.Add(tracer.Position);
tracer = tracer.Parent;
}
//Set Outputs
finalPath.Add(start.Position);
finalPath.Reverse();
finalPath.Add(end.Position);
return(finalPath);
}
//Generate children
children.Clear();
foreach (Point3d new_position in move)
{
Point3d child_position = new Point3d(currentNode.Position.X + new_position.X, currentNode.Position.Y + new_position.Y, currentNode.Position.Z + new_position.Z);
//Check if new point is in obstacle
if (obsMesh.IsPointInside(child_position, 0, false))
{
continue;
}
A_star new_node = new A_star(currentNode, child_position);
children.Add(new_node);
}
//Find best child
foreach (A_star child in children)
{
Boolean breakout = false;
foreach (A_star close in closed)
{
if (child.Position.DistanceTo(close.Position) < 1)
{
breakout = true;
break;
}
}
if (breakout == true)
{
continue;
}
child.g = currentNode.g + 1;
child.h = child.Position.DistanceTo(end.Position);
child.f = child.g + child.h;
for (int i = 0; i < open.Count; i++)
{
double dist = child.Position.DistanceTo(open[i].Position);
if (child.Position.DistanceTo(open[i].Position) < 1)
{
if (child.g > open[i].g)
{
breakout = true;
break;
}
else
{
open.RemoveAt(i);
}
}
}
if (breakout == true)
{
continue;
}
open.Add(child);
if (open.Count > 10000)
{
run = false;
}
}
}
return(finalPath);
}
public static Results RunAstar3d(Point3d person, Point3d interest, List <Mesh> obstacles, List <Mesh> Floors, double slope, double Person_Height)
{
//Run solver
//Declare Lists
List <A_star> open = new List <A_star>();
List <A_star> closed = new List <A_star>();
List <Point3d> Pointpaths = new List <Point3d>();
List <Point3d> finalPath = new List <Point3d>();
List <A_star> children = new List <A_star>();
List <Point3d> closedlist = new List <Point3d>();
//Declare var
Point3d position;
Boolean run = true;
//Movement list
List <Point3d> move = new List <Point3d>();
move.Add(new Point3d(0, -1, 0));
move.Add(new Point3d(0, 1, 0));
move.Add(new Point3d(-1, 0, 0));
move.Add(new Point3d(1, 0, 0));
move.Add(new Point3d(-1, -1, 0));
move.Add(new Point3d(-1, 1, 0));
move.Add(new Point3d(1, -1, 0));
move.Add(new Point3d(1, 1, 0));
////Interest on mesh
List <Point3d> intList = new List <Point3d>
{
interest
};
try
{
Point3d[] templist = Rhino.Geometry.Intersect.Intersection.ProjectPointsToMeshes(Floors, intList, new Vector3d(0, 0, -1), 0);
double mindist = double.MaxValue;
Point3d temp_pos = interest;
for (int i = 0; i < templist.Length; i++)
{
double dist = templist[i].DistanceTo(interest);
if (dist < mindist)
{
temp_pos = templist[i];
mindist = dist;
}
}
interest = temp_pos;
}
catch (Exception)
{
foreach (A_star node in closed)
{
closedlist.Add(node.Position);
}
return(new Results {
Path = finalPath, Closed = closedlist, Error = "Interest not placed above a floor"
});;
}
////Start on mesh
List <Point3d> startList = new List <Point3d>
{
person
};
try
{
Point3d[] templist = Rhino.Geometry.Intersect.Intersection.ProjectPointsToMeshes(Floors, startList, new Vector3d(0, 0, -1), 0);
double mindist = double.MaxValue;
Point3d temp_pos = person;
for (int i = 0; i < templist.Length; i++)
{
double dist = templist[i].DistanceTo(person);
if (dist < mindist)
{
temp_pos = templist[i];
mindist = dist;
}
}
person = temp_pos;
}
catch (Exception)
{
foreach (A_star node in closed)
{
closedlist.Add(node.Position);
}
return(new Results {
Path = finalPath, Closed = closedlist, Error = "Person not placed above a floor"
});;
}
//Declare start and end
A_star start = new A_star(null, person);
A_star end = new A_star(null, interest);
//Add startpoint to open list
open.Add(start);
// Join obstacles in one Mesh
Mesh obsMesh = new Mesh();
foreach (Mesh mesh in obstacles)
{
obsMesh.Append(mesh);
}
//Pathfinding
while (run)
{
//Get current node
if (open.Count == 0)
{
break;
}
A_star currentNode = open[0];
int currentIndex = 0;
for (int i = 0; i < open.Count; i++)
{
if (open[i].f < currentNode.f)
{
currentNode = open[i];
currentIndex = i;
}
}
//Remove current node from open list and add to closed
open.RemoveAt(currentIndex);
closed.Add(currentNode);
//If at endpoint, traceback the parents
position = currentNode.Position;
Pointpaths.Add(position);
if (currentNode.Position.DistanceTo(interest) < 2)
{
//Check for final obstacle
Vector3d vectorToFinal = interest - currentNode.Position;
Ray3d ray = new Ray3d(currentNode.Position, vectorToFinal);
double rayT = Rhino.Geometry.Intersect.Intersection.MeshRay(obsMesh, ray);
if (rayT < 0 | ray.PointAt(rayT).DistanceTo(currentNode.Position) > 2)
{
A_star tracer = currentNode;
while (tracer.g > 0)
{
finalPath.Add(tracer.Position);
tracer = tracer.Parent;
}
//Set Outputs
finalPath.Add(start.Position);
finalPath.Reverse();
finalPath.Add(end.Position);
foreach (A_star node in closed)
{
closedlist.Add(node.Position);
}
return(new Results {
Path = finalPath, Closed = closedlist
});
}
}
//Generate children
children.Clear();
foreach (Point3d new_position in move)
{
List <Point3d> stupidlist = new List <Point3d>();
Point3d child_position = new Point3d(currentNode.Position.X + new_position.X, currentNode.Position.Y + new_position.Y, currentNode.Position.Z + new_position.Z);
child_position.Z = child_position.Z + Person_Height;
stupidlist.Add(child_position);
//Project point to terrain
//Try z positive
try
{
Point3d[] child_positions = Rhino.Geometry.Intersect.Intersection.ProjectPointsToMeshes(Floors, stupidlist, new Vector3d(0, 0, -1), 0);
double mindist = double.MaxValue;
Point3d temp_child = child_position;
for (int i = 0; i < child_positions.Length; i++)
{
double dist = child_positions[i].DistanceTo(child_position);
if (dist < mindist)
{
temp_child = child_positions[i];
mindist = dist;
}
}
child_position = temp_child;
}
catch (Exception)
{
continue;
}
//Check if new point is in obstacle
if (obsMesh.IsPointInside(child_position, 0, false))
{
continue;
}
//Check if max sloping
if (slope != 0)
{
double terrain_slope = (child_position.Z - currentNode.Position.Z) / Math.Sqrt(Math.Pow(child_position.Y - currentNode.Position.Y, 2) + Math.Pow(child_position.X - currentNode.Position.X, 2));
if ((Math.Atan(terrain_slope) * 180 / Math.PI) > slope)
{
continue;
}
}
A_star new_node = new A_star(currentNode, child_position);
children.Add(new_node);
}
//Find best child
foreach (A_star child in children)
{
Boolean breakout = false;
foreach (A_star close in closed)
{
if (child.Position.DistanceTo(close.Position) < 1)
{
breakout = true;
break;
}
}
if (breakout == true)
{
continue;
}
double height_dif = child.Position.Z - interest.Z;
child.g = currentNode.g + 1;
child.h = child.Position.DistanceTo(end.Position) + (Math.Abs(height_dif) * 100);
child.f = child.g + child.h;
for (int i = 0; i < open.Count; i++)
{
double dist = child.Position.DistanceTo(open[i].Position);
if (child.Position.DistanceTo(open[i].Position) < 1)
{
if (child.g > open[i].g)
{
breakout = true;
break;
}
else
{
open.RemoveAt(i);
}
}
}
if (breakout == true)
{
continue;
}
open.Add(child);
if (open.Count > 100000)
{
run = false;
}
}
}
foreach (A_star node in closed)
{
closedlist.Add(node.Position);
}
return(new Results {
Path = finalPath, Closed = closedlist, Error = $"Exceeded count threshold of {open.Count}"
});
}
public A_star(A_star parent, Point3d position)
{
Parent = parent;
Position = position;
}
public static List <Point3d> RunAstarTerrain(Point3d person, Point3d interest, List <Mesh> obstacles, List <Mesh> terrain, double slope)
{
//Run solver
//Declare Lists
List <A_star> open = new List <A_star>();
List <A_star> closed = new List <A_star>();
List <Point3d> Pointpaths = new List <Point3d>();
List <Point3d> finalPath = new List <Point3d>();
List <A_star> children = new List <A_star>();
//Declare var
Point3d position;
Boolean run = true;
//Movement list
List <Point3d> move = new List <Point3d>();
move.Add(new Point3d(0, -1, 0));
move.Add(new Point3d(0, 1, 0));
move.Add(new Point3d(-1, 0, 0));
move.Add(new Point3d(1, 0, 0));
move.Add(new Point3d(-1, -1, 0));
move.Add(new Point3d(-1, 1, 0));
move.Add(new Point3d(1, -1, 0));
move.Add(new Point3d(1, 1, 0));
//Interest on mesh
List <Point3d> intList = new List <Point3d>();
intList.Add(interest);
try
{
interest = Rhino.Geometry.Intersect.Intersection.ProjectPointsToMeshes(terrain, intList, new Vector3d(0, 0, 1), 0)[0];
}
catch (Exception)
{
try
{
interest = Rhino.Geometry.Intersect.Intersection.ProjectPointsToMeshes(terrain, intList, new Vector3d(0, 0, -1), 0)[0];
}
catch (Exception)
{
return(null);
}
}
//Declare start and end
A_star start = new A_star(null, person);
A_star end = new A_star(null, interest);
//Add startpoint to open list
open.Add(start);
// Join obstacles in one Mesh
Mesh obsMesh = new Mesh();
foreach (Mesh mesh in obstacles)
{
obsMesh.Append(mesh);
}
//Pathfinding
while (run)
{
//Get current node
if (open.Count == 0)
{
break;
}
A_star currentNode = open[0];
int currentIndex = 0;
for (int i = 0; i < open.Count; i++)
{
if (open[i].f < currentNode.f)
{
currentNode = open[i];
currentIndex = i;
}
}
//Remove current node from open list and add to closed
open.RemoveAt(currentIndex);
closed.Add(currentNode);
//If at endpoint, traceback the parents
position = currentNode.Position;
Pointpaths.Add(position);
if (currentNode.Position.DistanceTo(interest) < 2)
{
A_star tracer = currentNode;
while (tracer.g > 0)
{
finalPath.Add(tracer.Position);
tracer = tracer.Parent;
}
//Set Outputs
finalPath.Add(start.Position);
finalPath.Reverse();
finalPath.Add(end.Position);
return(finalPath);
}
//Generate children
children.Clear();
foreach (Point3d new_position in move)
{
List <Point3d> stupidlist = new List <Point3d>();
Point3d child_position = new Point3d(currentNode.Position.X + new_position.X, currentNode.Position.Y + new_position.Y, currentNode.Position.Z + new_position.Z);
stupidlist.Add(child_position);
//Project point to terrain
//Try z positive
try
{
child_position = Rhino.Geometry.Intersect.Intersection.ProjectPointsToMeshes(terrain, stupidlist, new Vector3d(0, 0, 1), 0)[0];
}
catch (Exception)
{
try
{
//Try z Negative
child_position = Rhino.Geometry.Intersect.Intersection.ProjectPointsToMeshes(terrain, stupidlist, new Vector3d(0, 0, -1), 0)[0];
}
catch (Exception)
{
continue;
}
}
//Check if new point is in obstacle
if (obsMesh.IsPointInside(child_position, 0, false))
{
continue;
}
//Check if max sloping
if (slope != 0)
{
double terrain_slope = (child_position.Z - currentNode.Position.Z) / Math.Sqrt(Math.Pow(child_position.Y - currentNode.Position.Y, 2) + Math.Pow(child_position.X - currentNode.Position.X, 2));
if (terrain_slope * 100 > slope)
{
continue;
}
}
A_star new_node = new A_star(currentNode, child_position);
children.Add(new_node);
}
//Find best child
foreach (A_star child in children)
{
Boolean breakout = false;
foreach (A_star close in closed)
{
if (child.Position.DistanceTo(close.Position) < 1)
{
breakout = true;
break;
}
}
if (breakout == true)
{
continue;
}
child.g = currentNode.g + 1;
child.h = child.Position.DistanceTo(end.Position);
child.f = child.g + child.h;
for (int i = 0; i < open.Count; i++)
{
double dist = child.Position.DistanceTo(open[i].Position);
if (child.Position.DistanceTo(open[i].Position) < 1)
{
if (child.g > open[i].g)
{
breakout = true;
break;
}
else
{
open.RemoveAt(i);
}
}
}
if (breakout == true)
{
continue;
}
open.Add(child);
if (open.Count > 10000)
{
run = false;
}
}
}
return(finalPath);
}
