/// <summary>
/// Core Process of Theta* pathfinding algorithm.
/// </summary>
/// <param name="run"> A Boolean value that declare starting of the algorithm.</param>
/// <param name="context"> A Brep representing the context of the navigation model.</param>
/// <param name="startPoint"> A Point3d representing the start location.</param>
/// <param name="goalPoint"> A Point3d representing the destination location.</param>
/// <param name="wallData"> A DataTree representing the locations of the obstacle cells in the navigation model.</param>
/// <param name="cellSize"> A double that indicates size of the navigation model's cells.</param>
/// <returns> A Polyline that represents the result of the Theta* pathfinding algorithm.</returns>
Polyline pathFinding(bool run, Brep context, Point3d startPoint, Point3d goalPoint, GH_Structure <GH_Integer> wallData, double cellSize)
{
int[] startID = new int[2];
int[] goalID = new int[2];
SimplePriorityQueue <Spot> openSet = new SimplePriorityQueue <Spot>();
List <Point3d> path = new List <Point3d>();
Polyline finalPaths = new Polyline();
int gridSize = wallData.Branches.Count;
double[] modelLocation = { context.Vertices[0].Location.X, context.Vertices[0].Location.Y };
Spot neighbor;
Spot current;
Spot[,] grid = new Spot[gridSize + 1, gridSize + 1];
Cell[,] cells = new Cell[gridSize, gridSize];
for (int i = 0; i < gridSize + 1; i++)
{
for (int j = 0; j < gridSize + 1; j++)
{
if (i < gridSize && j < gridSize)
{
cells[i, j] = new Cell(cellSize, wallData.Branches[i][j].Value);
}
grid[i, j] = new Spot(modelLocation, new int[] { i, j }, cellSize);
grid[i, j].g = float.MaxValue;
grid[i, j].f = float.MaxValue;
grid[i, j].getNeighbors(gridSize);
if (i == 0 && j == 0)
{
grid[i, j].adjCells.Add(cells[i, j]);
}
if (i == 0 && j == gridSize)
{
grid[i, j].adjCells.Add(cells[0, j - 1]);
}
if (i == gridSize && j == 0)
{
grid[i, j].adjCells.Add(cells[i - 1, j]);
}
if (i == gridSize && j == gridSize)
{
grid[i, j].adjCells.Add(cells[i - 1, j - 1]);
}
if (i == 0 && j > 0 && j < gridSize)
{
grid[i, j].adjCells.Add(cells[i, j]);
grid[i, j].adjCells.Add(cells[i, j - 1]);
}
if (j == 0 && i > 0 && i < gridSize)
{
grid[i, j].adjCells.Add(cells[i, j]);
grid[i, j].adjCells.Add(cells[i - 1, j]);
}
if (i == gridSize && j > 0 && j < gridSize)
{
grid[i, j].adjCells.Add(cells[i - 1, j - 1]);
grid[i, j].adjCells.Add(cells[i - 1, j]);
}
if (j == gridSize && i > 0 && i < gridSize)
{
grid[i, j].adjCells.Add(cells[i - 1, j - 1]);
grid[i, j].adjCells.Add(cells[i, j - 1]);
}
if (i > 0 && j > 0 && i < gridSize && j < gridSize)
{
grid[i, j].adjCells.Add(cells[i, j]);
grid[i, j].adjCells.Add(cells[i - 1, j]);
grid[i, j].adjCells.Add(cells[i - 1, j - 1]);
grid[i, j].adjCells.Add(cells[i, j - 1]);
}
}
}
for (int i = 0; i < gridSize; i++)
{
for (int j = 0; j < gridSize; j++)
{
cells[i, j].cornerSpots[0] = grid[i, j];
cells[i, j].cornerSpots[1] = grid[i + 1, j];
cells[i, j].cornerSpots[2] = grid[i + 1, j + 1];
cells[i, j].cornerSpots[3] = grid[i, j + 1];
}
}
double tempValue = double.MaxValue;
double tempDistance = new double();
for (int i = 0; i < grid.GetLength(0); i++)
{
for (int j = 0; j < grid.GetLength(1); j++)
{
tempDistance = startPoint.DistanceTo(grid[i, j].pos);
if (tempDistance < tempValue)
{
tempValue = tempDistance;
startID[0] = i;
startID[1] = j;
}
}
}
tempValue = double.MaxValue;
for (int i = 0; i < grid.GetLength(0); i++)
{
for (int j = 0; j < grid.GetLength(1); j++)
{
tempDistance = goalPoint.DistanceTo(grid[i, j].pos);
if (tempDistance < tempValue)
{
tempValue = tempDistance;
goalID[0] = i;
goalID[1] = j;
}
}
}
Spot start = grid[startID[0], startID[1]];
Spot goal = grid[goalID[0], goalID[1]];
start.g = 0;
start.parent = start;
start.he = Methods.calcHeuristic(start, goal);
openSet.Enqueue(start, start.f);
while (openSet.Count > 0)
{
current = openSet.Dequeue();
if (current == goal)
{
Methods.buildPath(path, current, start);
finalPaths = Methods.drawPath(path);
break;
}
current.closedSet = true;
Methods.updateBounds(grid, current, start);
for (int k = 0; k < current.neighbors.Count; k++)
{
neighbor = grid[current.neighbors[k][0], current.neighbors[k][1]];
if (neighbor.closedSet == false)
{
if (!openSet.Contains(neighbor))
{
neighbor.g = float.MaxValue;
}
Methods.updateVertex(openSet, current, neighbor, start, goal);
}
}
}
return(finalPaths);
}
public static float calcHeuristic(Spot s1, Spot s2)
{
return((float)s1.pos.DistanceTo(s2.pos));
}
public static void updateBounds(Spot[,] grid, Spot s, Spot start)
{
s.lb = -float.MaxValue;
s.ub = float.MaxValue;
float angle = new float();
Cell adjCell;
Spot neighbor;
if (s != start)
{
// Condition - 1
for (int i = 0; i < s.adjCells.Count; i++)
{
adjCell = s.adjCells[i];
if (adjCell.wall == 1)
{
for (int j = 0; j < adjCell.cornerSpots.Length; j++)
{
adjCell.cornerSpots[j].los = false;
angle = theta(s.pos, s.parent.pos, adjCell.cornerSpots[j].pos);
if (s.parent == adjCell.cornerSpots[j] || angle < 0 || (angle == 0 && s.parent.pos.DistanceTo(adjCell.cornerSpots[j].pos) <= s.parent.pos.DistanceTo(s.pos)))
{
s.lb = 0;
}
if (s.parent == adjCell.cornerSpots[j] || angle > 0 || (angle == 0 && s.parent.pos.DistanceTo(adjCell.cornerSpots[j].pos) <= s.parent.pos.DistanceTo(s.pos)))
{
s.ub = 0;
}
}
}
}
for (int i = 0; i < s.neighbors.Count; i++)
{
neighbor = grid[s.neighbors[i][0], s.neighbors[i][1]];
angle = theta(s.pos, s.parent.pos, neighbor.pos);
// Condition - 2
if (neighbor.closedSet == true && s.parent == neighbor.parent && neighbor != start)
{
if (neighbor.lb + angle <= 0)
{
s.lb = Math.Max(s.lb, neighbor.lb + angle);
}
if (neighbor.ub + angle >= 0)
{
s.ub = Math.Min(s.ub, neighbor.ub + angle);
}
}
// Condition - 3
if ((neighbor.closedSet == false || s.parent != neighbor.parent) && (s.parent.pos.DistanceTo(neighbor.pos) < s.parent.pos.DistanceTo(s.pos)) && s.parent != neighbor)
{
if (angle < 0)
{
s.lb = Math.Max(s.lb, angle);
}
if (angle > 0)
{
s.ub = Math.Min(s.ub, angle);
}
}
}
}
}
public static void updateVertex(SimplePriorityQueue <Spot> openSet, Spot s, Spot succ, Spot start, Spot goal)
{
double angle = theta(s.pos, s.parent.pos, succ.pos);
if (s != start && s.lb <= angle && s.ub >= angle)
{
// Path - 2
if (s.parent.g + s.parent.pos.DistanceTo(succ.pos) < succ.g && s.los)
{
succ.g = s.parent.g + (float)s.parent.pos.DistanceTo(succ.pos);
succ.parent = s.parent;
if (openSet.Contains(succ))
{
openSet.Remove(succ);
}
succ.f = succ.g + calcHeuristic(succ, goal);
openSet.Enqueue(succ, succ.f);
}
}
else
{
// Path - 1
if (s.g + s.pos.DistanceTo(succ.pos) < succ.g && s.los)
{
succ.g = s.g + (float)s.pos.DistanceTo(succ.pos);
succ.parent = s;
if (openSet.Contains(succ))
{
openSet.Remove(succ);
}
succ.f = succ.g + calcHeuristic(succ, goal);
openSet.Enqueue(succ, succ.f);
}
}
}
