private static void DestinationReached()
{
//Faster to do this to end loop than to check every AnalyzeNode () call
destinationIsReached = true;
GridHeap.FastClear();
outputPath.FastClear();
TracePath.FastClear();
currentNode = endNode;
//GridNode oldNode = null;
StartNodeIndex = startNode.gridIndex;
while (currentNode.gridIndex != StartNodeIndex)
{
//Sets CombinePathVersion while tracing path
TracePath.Add(currentNode);
//oldNode = currentNode;
currentNode = currentNode.parent;
}
currentNode = TracePath[TracePath.Count - 1];
for (i = TracePath.Count - 2; i >= 0; i--)
{
//oldNode = currentNode;
currentNode = TracePath.innerArray[i];
outputPath.Add(currentNode);
}
}
static void AnalyzeNode()
{
if (!GridHeap.Contains(neighbor))
{
AddBestNode();
GridHeap.Add(neighbor);
}
else if (newMovementCostToNeighbor < neighbor.gCost)
{
AddBestNode();
GridHeap.UpdateItem(neighbor);
}
}
static void AnalyzeNode()
{
if (!GridHeap.Contains(neighbor))
{
neighbor.gCost = newMovementCostToNeighbor;
//Optimized heuristic calculation
neighbor.CalculateHeuristic();
neighbor.parent = currentNode;
GridHeap.Add(neighbor);
}
else if (newMovementCostToNeighbor < neighbor.gCost)
{
neighbor.gCost = newMovementCostToNeighbor;
//Optimized heuristic calculation
neighbor.CalculateHeuristic();
neighbor.parent = currentNode;
GridHeap.UpdateItem(neighbor);
}
}
/// <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 FindRawPath(GridNode _startNode, GridNode _endNode, FastList <GridNode> _outputPath, int _unitSize = 1)
{
startNode = _startNode;
endNode = _endNode;
outputPath = _outputPath;
unitSize = _unitSize;
#region Broadphase and Preperation
if (endNode.Unwalkable)
{
return(false);
}
if (startNode.Unwalkable)
{
return(false);
}
outputPath.FastClear();
if (System.Object.ReferenceEquals(startNode, endNode))
{
outputPath.Add(endNode);
return(true);
}
GridHeap.FastClear();
GridClosedSet.FastClear();
#endregion
#region AStar Algorithm
GridHeap.Add(startNode);
GridNode.HeuristicTargetX = endNode.gridX;
GridNode.HeuristicTargetY = endNode.gridY;
GridNode.PrepareUnpassableCheck(unitSize); //Prepare Unpassable check optimizations
if (_endNode.Unwalkable)
{
return(false);
}
while (GridHeap.Count > 0)
{
currentNode = GridHeap.RemoveFirst();
#if false
Gizmos.DrawCube(currentNode.WorldPos.ToVector3(), Vector3.one);
#endif
if (currentNode.gridIndex == endNode.gridIndex)
{
//Retraces the path then outputs it into outputPath
//Also Simplifies the path
DestinationReached();
return(true);
}
/*
* for (i = 0; i < 8; i++) {
* neighbor = currentNode.NeighborNodes [i];
* if (CheckNeighborInvalid ()) {
* //continue;
* //microoptimization... continue is more expensive than letting the loop pass at the end
* } else {
* //0-3 = sides, 4-7 = diagonals
* if (i < 4) {
* newMovementCostToNeighbor = currentNode.gCost + 100;
* } else {
* if (i == 4) {
* if (!GridManager.UseDiagonalConnections)
* break;
* }
* newMovementCostToNeighbor = currentNode.gCost + 141;
* }
*
* AnalyzeNode();
* }
* }
*/
hasInvalidEdge = false;
for (int i = 0; i < 4; i++)
{
neighbor = currentNode.NeighborNodes[i];
if (CheckNeighborInvalid())
{
hasInvalidEdge = true;
}
else
{
newMovementCostToNeighbor = currentNode.gCost + 100;
AnalyzeNode();
}
}
if (hasInvalidEdge)
{
const int maxCornerObstructions = 2;
#region inlining diagonals
neighbor = currentNode.NeighborNodes[4];
if (!CheckNeighborInvalid())
{
if (GetObstructionCount(0, 1) <= maxCornerObstructions)
{
newMovementCostToNeighbor = currentNode.gCost + 141;
AnalyzeNode();
}
}
neighbor = currentNode.NeighborNodes[5];
if (!CheckNeighborInvalid())
{
if (GetObstructionCount(0, 2) <= maxCornerObstructions)
{
newMovementCostToNeighbor = currentNode.gCost + 141;
AnalyzeNode();
}
}
neighbor = currentNode.NeighborNodes[6];
if (!CheckNeighborInvalid())
{
if (GetObstructionCount(3, 1) <= maxCornerObstructions)
{
newMovementCostToNeighbor = currentNode.gCost + 141;
AnalyzeNode();
}
}
neighbor = currentNode.NeighborNodes[7];
if (!CheckNeighborInvalid())
{
if (GetObstructionCount(3, 2) <= maxCornerObstructions)
{
newMovementCostToNeighbor = currentNode.gCost + 141;
AnalyzeNode();
}
}
#endregion
}
else
{
//no need for specific stuff when edges are all valid
for (int i = 4; i < 8; i++)
{
neighbor = currentNode.NeighborNodes[i];
if (CheckNeighborInvalid())
{
}
else
{
newMovementCostToNeighbor = currentNode.gCost + 141;
AnalyzeNode();
}
}
}
GridClosedSet.Add(currentNode);
}
#endregion
return(false);
}
public static void Reset()
{
//Reset combine value so it doesn't overflow with multiple scenes
CombineIteration = 0;
GridHeap.Reset();
}
static bool CheckNeighborSearchable()
{
return(neighbor.IsNull() || GridHeap.Closed(neighbor));
}
/*
* static int GetObstructionCount(int index1, int index2)
* {
* if (CheckInvalid(currentNode.NeighborNodes[index1])) {
* if (CheckInvalid(currentNode.NeighborNodes[index2])) {
* return 2;
* }
* return 1;
* }
* if (CheckInvalid(currentNode.NeighborNodes[index2]))
* return 1;
* return 0;
* }*/
static bool CheckInvalid(GridNode gridNode)
{
return(gridNode.IsNull() || GridHeap.Closed(gridNode) || gridNode.Unpassable());
}
/// <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 FindRawPath(GridNode _startNode, GridNode _endNode, FastList <GridNode> _outputPath, int _unitSize)
{
//TODO: Not critical but there's a lot of room for better organization
//i.e. All these static variables and methods goes into individual singleton classes
startNode = _startNode;
endNode = _endNode;
outputPath = _outputPath;
unitSize = _unitSize;
#region Broadphase and Preperation
if (endNode.Unwalkable)
{
return(false);
}
if (startNode.Unwalkable)
{
return(false);
}
outputPath.FastClear();
if (System.Object.ReferenceEquals(startNode, endNode))
{
outputPath.Add(endNode);
return(true);
}
GridHeap.FastClear();
//POSBUG: Hash for end destination and frame count. *Most likely* won't overflow
//Or no need to factor in frame count
#endregion
#region AStar Algorithm
GridHeap.Add(startNode);
GridNode.HeuristicTargetX = endNode.gridX;
GridNode.HeuristicTargetY = endNode.gridY;
GridNode.PrepareUnpassableCheck(unitSize); //Prepare Unpassable check optimizations
if (_endNode.Unwalkable)
{
return(false);
}
destinationIsReached = false;
SearchCount = 0;
CombineVersionSet = CombineIteration * GridManager.MaxIndex + endNode.gridIndex;
if (lastGridIndex == endNode.gridIndex)
{
CombineVersionCheck = CombineVersionSet;
}
else
{
if (CombineVersionCheck != DefaultCombineVersion)
{
CombineIteration++;
CombineVersionCheck = DefaultCombineVersion;
}
}
lastGridIndex = endNode.gridIndex;
while (GridHeap.Count > 0)
{
SearchCount++;
rawNode = GridHeap.RemoveFirst();
if (rawNode.gridIndex == endNode.gridIndex)
{
//We found our way to the end node!
DestinationReached();
return(true);
}
if (CombineVersionCheck != DefaultCombineVersion)
{
if (rawNode.CombinePathVersion == CombineVersionCheck)
{
//We found our way onto an existing path!
DestinationReached(true);
return(true);
}
}
#if true
#region Allows diagonal access when edges are blocked
for (i = 0; i < 4; i++)
{
neighbor = rawNode.NeighborNodes [i];
neighbor = rawNode.NeighborNodes [i];
if (CheckNeighborSearchable() == false)
{
if (neighbor.Unpassable() == false)
{
newMovementCostToNeighbor = rawNode.gCost + 141;
ProcessNode();
}
else if (neighbor.gridIndex == EndNodeIndex)
{
AddBestNode();
DestinationReached();
return(true);
}
}
}
for (int i = 4; i < 8; i++)
{
neighbor = rawNode.NeighborNodes [i];
if (CheckNeighborSearchable() == false)
{
if (neighbor.Unpassable() == false)
{
newMovementCostToNeighbor = rawNode.gCost + 141;
ProcessNode();
}
else if (neighbor.gridIndex == EndNodeIndex)
{
AddBestNode();
DestinationReached();
return(true);
}
}
}
GridHeap.Close(rawNode);
#endregion
#else
hasInvalidEdge = false;
for (int i = 0; i < 4; i++)
{
neighbor = currentNode.NeighborNodes[i];
if (CheckNeighborInvalid())
{
hasInvalidEdge = true;
}
else
{
newMovementCostToNeighbor = currentNode.gCost + 100;
AnalyzeNode();
}
}
if (hasInvalidEdge)
{
const int maxCornerObstructions = 2;
#region inlining diagonals
neighbor = currentNode.NeighborNodes[4];
if (!CheckNeighborInvalid())
{
if (GetObstructionCount(0, 1) <= maxCornerObstructions)
{
newMovementCostToNeighbor = currentNode.gCost + 141;
AnalyzeNode();
}
}
neighbor = currentNode.NeighborNodes[5];
if (!CheckNeighborInvalid())
{
if (GetObstructionCount(0, 2) <= maxCornerObstructions)
{
newMovementCostToNeighbor = currentNode.gCost + 141;
AnalyzeNode();
}
}
neighbor = currentNode.NeighborNodes[6];
if (!CheckNeighborInvalid())
{
if (GetObstructionCount(3, 1) <= maxCornerObstructions)
{
newMovementCostToNeighbor = currentNode.gCost + 141;
AnalyzeNode();
}
}
neighbor = currentNode.NeighborNodes[7];
if (!CheckNeighborInvalid())
{
if (GetObstructionCount(3, 2) <= maxCornerObstructions)
{
newMovementCostToNeighbor = currentNode.gCost + 141;
AnalyzeNode();
}
}
#endregion
}
else
{
//no need for specific stuff when edges are all valid
for (int i = 4; i < 8; i++)
{
neighbor = currentNode.NeighborNodes[i];
if (CheckNeighborInvalid())
{
}
else
{
newMovementCostToNeighbor = currentNode.gCost + 141;
AnalyzeNode();
}
}
}
#endif
}
#endregion
return(destinationIsReached);
}
static bool CheckNeighborSearchable()
{
return(neighbor.IsNotNull() && GridHeap.Closed(neighbor) == false);
}