/// <summary>
/// Finds a path and outputs it to <c>OutputPath</c>. Note: OutputPath is unpredictably changed.
/// </summary>
/// <returns>
/// Returns <c>true</c> if path was found and necessary, <c>false</c> if path to End is impossible or not found.
/// </returns>
/// <param name="startNode">Start node.</param>
/// <param name="endNode">End node.</param>
/// <param name="OutputPath">Return path.</param>
public static bool FindPath(GridNode startNode, GridNode endNode, FastList<GridNode> OutputPath)
{
#region Broadphase and Preperation
if (endNode.Unwalkable) {
return false;
}
if (startNode.Unwalkable) {
return false;
}
if (true) {
#region Obstruction Test
//Tests if there is a direct path. If there is, no need to run AStar.
x0 = startNode.gridX;
y0 = startNode.gridY;
x1 = endNode.gridX;
y1 = endNode.gridY;
if (y1 > y0)
compare1 = y1 - y0;
else
compare1 = y0 - y1;
if (x1 > x0)
compare2 = x1 - x0;
else
compare2 = x0 - x1;
steep = compare1 > compare2;
if (steep) {
t = x0; // swap x0 and y0
x0 = y0;
y0 = t;
t = x1; // swap x1 and y1
x1 = y1;
y1 = t;
}
if (x0 > x1) {
t = x0; // swap x0 and x1
x0 = x1;
x1 = t;
t = y0; // swap y0 and y1
y0 = y1;
y1 = t;
}
dx = x1 - x0;
dy = (y1 - y0);
if (dy < 0)
dy = -dy;
error = dx / 2;
ystep = (y0 < y1) ? 1 : -1;
y = y0;
for (x = x0; x <= x1; x++) {
retX = (steep ? y : x);
retY = (steep ? x : y);
if (GridManager.Grid [retX * GridManager.NodeCount + retY].Unwalkable) {
break;
} else if (x == x1) {
OutputPath.FastClear ();
OutputPath.Add (startNode);
OutputPath.Add (endNode);
return true;
}
error = error - dy;
if (error < 0) {
y += ystep;
error += dx;
}
}
#endregion
}
GridHeap.FastClear ();
GridClosedSet.FastClear ();
#endregion
#region AStar Algorithm
GridHeap.Add (startNode);
GridNode.HeuristicTargetX = endNode.gridX;
GridNode.HeuristicTargetY = endNode.gridY;
while (GridHeap.Count > 0) {
currentNode = GridHeap.RemoveFirst ();
GridClosedSet.Add (currentNode);
if (currentNode.gridIndex == endNode.gridIndex) {
OutputPath.FastClear ();
//Retraces the path then outputs it into OutputPath
//Also Simplifies the path
oldNode = endNode;
currentNode = endNode.parent;
oldX = int.MaxValue;
oldY = int.MaxValue;
StartNodeIndex = startNode.gridIndex;
//if (!endNode.Obstructed) OutputPath.Add (endNode);
while (oldNode.gridIndex != StartNodeIndex) {
newX = currentNode.gridX - oldNode.gridX;
newY = currentNode.gridY - oldNode.gridY;
if ((newX != oldX || newY != oldY))
{
OutputPath.Add (oldNode);
oldX = newX;
oldY = newY;
}
oldNode = currentNode;
currentNode = currentNode.parent;
}
OutputPath.Add (startNode);
OutputPath.Reverse ();
return true;
}
for (i = 0; i < 8; i++) {
neighbor = currentNode.NeighborNodes [i];
if (neighbor == null|| neighbor.Unwalkable || GridClosedSet.Contains (neighbor)) {
continue;
}
newMovementCostToNeighbor = currentNode.gCost + (currentNode.NeighborDiagnal [i] ? 141 : 100);
if (!GridHeap.Contains (neighbor)) {
neighbor.gCost = newMovementCostToNeighbor;
//Optimized heuristic calculation
neighbor.CalculateHeurustic ();
neighbor.parent = currentNode;
GridHeap.Add (neighbor);
} else if (newMovementCostToNeighbor < neighbor.gCost) {
neighbor.gCost = newMovementCostToNeighbor;
//Optimized heuristic calculation
neighbor.CalculateHeurustic ();
neighbor.parent = currentNode;
GridHeap.UpdateItem (neighbor);
}
}
}
#endregion
return false;
}
/// <summary>
/// Finds a path and outputs it to <c>OutputPath</c>. Note: OutputPath is unpredictably changed.
/// </summary>
/// <returns>
/// Returns <c>true</c> if path was found and necessary, <c>false</c> if path to End is impossible or not found.
/// </returns>
/// <param name="startNode">Start node.</param>
/// <param name="endNode">End node.</param>
/// <param name="OutputPath">Return path.</param>
public static bool FindPath(GridNode startNode, GridNode endNode, FastList <GridNode> OutputPath)
{
#region Broadphase and Preperation
if (endNode.Unwalkable)
{
return(false);
}
if (startNode.Unwalkable)
{
return(false);
}
if (true)
{
#region Obstruction Test
//Tests if there is a direct path. If there is, no need to run AStar.
x0 = startNode.gridX;
y0 = startNode.gridY;
x1 = endNode.gridX;
y1 = endNode.gridY;
if (y1 > y0)
{
compare1 = y1 - y0;
}
else
{
compare1 = y0 - y1;
}
if (x1 > x0)
{
compare2 = x1 - x0;
}
else
{
compare2 = x0 - x1;
}
steep = compare1 > compare2;
if (steep)
{
t = x0; // swap x0 and y0
x0 = y0;
y0 = t;
t = x1; // swap x1 and y1
x1 = y1;
y1 = t;
}
if (x0 > x1)
{
t = x0; // swap x0 and x1
x0 = x1;
x1 = t;
t = y0; // swap y0 and y1
y0 = y1;
y1 = t;
}
dx = x1 - x0;
dy = (y1 - y0);
if (dy < 0)
{
dy = -dy;
}
error = dx / 2;
ystep = (y0 < y1) ? 1 : -1;
y = y0;
for (x = x0; x <= x1; x++)
{
retX = (steep ? y : x);
retY = (steep ? x : y);
if (GridManager.Grid [retX * GridManager.NodeCount + retY].Unwalkable)
{
break;
}
else if (x == x1)
{
OutputPath.FastClear();
OutputPath.Add(startNode);
OutputPath.Add(endNode);
return(true);
}
error = error - dy;
if (error < 0)
{
y += ystep;
error += dx;
}
}
#endregion
}
GridHeap.FastClear();
GridClosedSet.FastClear();
#endregion
#region AStar Algorithm
GridHeap.Add(startNode);
GridNode.HeuristicTargetX = endNode.gridX;
GridNode.HeuristicTargetY = endNode.gridY;
while (GridHeap.Count > 0)
{
currentNode = GridHeap.RemoveFirst();
GridClosedSet.Add(currentNode);
if (currentNode.gridIndex == endNode.gridIndex)
{
OutputPath.FastClear();
//Retraces the path then outputs it into OutputPath
//Also Simplifies the path
oldNode = endNode;
currentNode = endNode.parent;
oldX = int.MaxValue;
oldY = int.MaxValue;
StartNodeIndex = startNode.gridIndex;
//if (!endNode.Obstructed) OutputPath.Add (endNode);
while (oldNode.gridIndex != StartNodeIndex)
{
newX = currentNode.gridX - oldNode.gridX;
newY = currentNode.gridY - oldNode.gridY;
if ((newX != oldX || newY != oldY))
{
OutputPath.Add(oldNode);
oldX = newX;
oldY = newY;
}
oldNode = currentNode;
currentNode = currentNode.parent;
}
OutputPath.Add(startNode);
OutputPath.Reverse();
return(true);
}
for (i = 0; i < 8; i++)
{
neighbor = currentNode.NeighborNodes [i];
if (neighbor == null || neighbor.Unwalkable || GridClosedSet.Contains(neighbor))
{
continue;
}
newMovementCostToNeighbor = currentNode.gCost + (currentNode.NeighborDiagnal [i] ? 141 : 100);
if (!GridHeap.Contains(neighbor))
{
neighbor.gCost = newMovementCostToNeighbor;
//Optimized heuristic calculation
neighbor.CalculateHeurustic();
neighbor.parent = currentNode;
GridHeap.Add(neighbor);
}
else if (newMovementCostToNeighbor < neighbor.gCost)
{
neighbor.gCost = newMovementCostToNeighbor;
//Optimized heuristic calculation
neighbor.CalculateHeurustic();
neighbor.parent = currentNode;
GridHeap.UpdateItem(neighbor);
}
}
}
#endregion
return(false);
}
