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); } }