public LinkedList <Vector2Int> GetPath(Vector2Int start, Vector2Int end, uint maxSteps)
{
if (start == end) //trivial case
{
Reset();
SetDestination(start, end);
state = AStarAlgorithmState.FINISHED;
}
else
{
bool ended = false;
state = AStarAlgorithmState.IN_PROCESS;
Reset(); //stage 0
SetDestination(start, end, maxSteps);
CalculateTargetDistance();
InitializeHeap(); //stage 1
ended = TestInvalidFirstNodePosition();
while (!ended)
{
ended = SearchMinimun(); //stage 2
}
}
return(GetFinalPath());
}
private bool TestInvalidFirstNodePosition()
{
LinkedListNode <Vector2Int> i = Heap.First;
if (GridMap.instance.grid[i.Value.x, i.Value.y].Node.AvaibleAdjacentNodes == 0)
{
Debug.LogWarning("initial node in a non valid position");
Vector3Int temporalPositionAndMin = new Vector3Int(-1, -1, int.MaxValue);
temporalPositionAndMin = GetMinimumAroundNode(i.Value.x, i.Value.y, ref GridMap.instance.grid[i.Value.x, i.Value.y].Node.FromInitialCost);
if (temporalPositionAndMin.z == int.MaxValue) // node is surrounded
{
state = AStarAlgorithmState.NO_AVAILABLE_SOLUTION;
Debug.LogError("Starting node completaly surrounded");
return(true);
}
else //add to heap
{
GridMap.instance.grid[temporalPositionAndMin.x, temporalPositionAndMin.y].Node.FromInitialCost =
temporalPositionAndMin.z - GridMap.instance.grid[temporalPositionAndMin.x, temporalPositionAndMin.y].Node.FromFinalCost; //maybe the cost has changed
GridMap.instance.grid[temporalPositionAndMin.x, temporalPositionAndMin.y].Node.SetFinalCost();
GridMap.instance.grid[temporalPositionAndMin.x, temporalPositionAndMin.y].Node.visited = true;
GridMap.instance.grid[temporalPositionAndMin.x, temporalPositionAndMin.y].Node.stepsUsed =
GridMap.instance.grid[i.Value.x, i.Value.y].Node.stepsUsed + 1;
//Staff to set nodes relationships
GridMap.instance.grid[i.Value.x, i.Value.y].Node.AddChill(temporalPositionAndMin.x, temporalPositionAndMin.y);
GridMap.instance.grid[temporalPositionAndMin.x, temporalPositionAndMin.y].Node.SetParent(i.Value.x, i.Value.y);
Heap.AddFirst(new Vector2Int(temporalPositionAndMin.x, temporalPositionAndMin.y));
}
}
return(false);
}
private bool SearchMinimun()
{
Vector3Int temporalPositionAndMin = new Vector3Int(0, 0, int.MaxValue);
Vector3Int holdedPositionAndMin = new Vector3Int(-1, -1, int.MaxValue);
Vector2Int fromInitialNodePosition = new Vector2Int(0, 0);
LinkedListNode <Vector2Int> i = Heap.First;
while (i != null)
{
if (GridMap.instance.grid[i.Value.x, i.Value.y].Node.AvaibleAdjacentNodes == 0) //delete node
{
if (i.Previous != null)
{
i = i.Previous;
Heap.Remove(i.Next);
i = i.Next;
continue;
}
else
{
i = i.Next; //if null, bucle ends
Heap.RemoveFirst();
continue;
}
}
temporalPositionAndMin = GetMinimumAroundNode(i.Value.x, i.Value.y, ref GridMap.instance.grid[i.Value.x, i.Value.y].Node.FromInitialCost);
if (temporalPositionAndMin.z == int.MaxValue)//never has to enter here because adjacence valor dont allow it, but doe to some error on planification
// can enter here with a node with all exits with max cost, so in this happens we elimitate the node in the heap and advance next
{
//Debug.LogWarning("min search arround this node has maximun int value");
if (i.Previous != null)
{
i = i.Previous;
Heap.Remove(i.Next);
i = i.Next;
continue;
}
else
{
i = i.Next; //if null, bucle ends
Heap.RemoveFirst();
continue;
}
}
if (temporalPositionAndMin.z < holdedPositionAndMin.z)//new min
{
holdedPositionAndMin = temporalPositionAndMin;
fromInitialNodePosition = new Vector2Int(i.Value.x, i.Value.y);
}
i = i.Next;
}
if (Heap.Count == 0)
{
state = AStarAlgorithmState.NO_AVAILABLE_SOLUTION;
Debug.LogError("No solve in path");
return(true);
}
//Staff to do around the node
GridMap.instance.grid[holdedPositionAndMin.x, holdedPositionAndMin.y].Node.FromInitialCost =
holdedPositionAndMin.z - GridMap.instance.grid[holdedPositionAndMin.x, holdedPositionAndMin.y].Node.FromFinalCost;; //maybe the cost has changed
GridMap.instance.grid[holdedPositionAndMin.x, holdedPositionAndMin.y].Node.SetFinalCost();
UpdateAdjacentAvaibles(holdedPositionAndMin.x, holdedPositionAndMin.y);
GridMap.instance.grid[holdedPositionAndMin.x, holdedPositionAndMin.y].Node.visited = true;
GridMap.instance.grid[holdedPositionAndMin.x, holdedPositionAndMin.y].Node.stepsUsed =
GridMap.instance.grid[fromInitialNodePosition.x, fromInitialNodePosition.y].Node.stepsUsed + 1;
//Staff to set nodes relationships
GridMap.instance.grid[fromInitialNodePosition.x, fromInitialNodePosition.y].Node.AddChill(holdedPositionAndMin.x, holdedPositionAndMin.y);
GridMap.instance.grid[holdedPositionAndMin.x, holdedPositionAndMin.y].Node.SetParent(fromInitialNodePosition.x, fromInitialNodePosition.y);
if (holdedPositionAndMin.x == endNodePos.x && holdedPositionAndMin.y == endNodePos.y) //we reach the exit
{
state = AStarAlgorithmState.FINISHED;
return(true);
}
else if (GridMap.instance.grid[holdedPositionAndMin.x, holdedPositionAndMin.y].Node.stepsUsed <= maxSteps) //continue
{
//Add to heap
if (GridMap.instance.grid[endNodePos.x, endNodePos.y].Node.AvaibleAdjacentNodes == 0 && (Mathf.Abs(endNodePos.x - holdedPositionAndMin.x) <= 1 && Mathf.Abs(endNodePos.y - holdedPositionAndMin.y) <= 1))
//final node is in an invalid cell, soo we stay in the closest cell, and we resolved as a limitated by steps case
{
state = AStarAlgorithmState.LIMITED_BY_STEPS;
lastStepPos = new Vector2Int(holdedPositionAndMin.x, holdedPositionAndMin.y);
Debug.LogWarning("final node is in a non valid position");
return(true);
}
Heap.AddFirst(new Vector2Int(holdedPositionAndMin.x, holdedPositionAndMin.y));
return(false);
}
else //limited by steps
{
lastStepPos = new Vector2Int(holdedPositionAndMin.x, holdedPositionAndMin.y);
state = AStarAlgorithmState.LIMITED_BY_STEPS;
return(true);
}
}
