internal static int FindCurrent(NativeMinHeap minSet, NativeArray <byte> closeSet) { _markerFindCurrent.Begin(); while (minSet.HasNext()) { _markerPop.Begin(); var next = minSet.Pop(); _markerPop.End(); // Check if this is not visited tile if (closeSet[next.Position] == 0) { _markerFindCurrent.End(); return(next.Position); } } _markerFindCurrent.End(); return(-1); }
public void Execute() { int length = moveCost.Length; int start1d = Index2dTo1d(start, moveCost_width); { for (int i = _F_.Length - 1; i != -1; i--) { _F_[i] = float.MaxValue; } _F_[start1d] = 0; } { solution[start1d] = start; } { frontier.Push(start); } { visited.TryAdd(start, 0); } //solve; float euclideanMaxLength = EuclideanHeuristicMaxLength(length, moveCost_width); int2 node = int2.zero - 1; while (frontier.Count != 0 && math.any(node != destination)) { node = frontier.Pop(); //we grab candidate with lowest F so far int node1d = Index2dTo1d(node, moveCost_width); float node_f = _F_[node1d]; //lets check all its neighbours: EnumerateNeighbours(neighbours, moveCost_width, moveCost_width, node); int neighboursLength = neighbours.Length; for (int i = 0; i < neighboursLength; i++) { int2 neighbour = neighbours[i]; int neighbour1d = Index2dTo1d(neighbour, moveCost_width); bool isOrthogonal = math.any(node == neighbour); //is relative position orthogonal or diagonal float g = node_f + moveCost[neighbour1d] * (isOrthogonal ? 1f : 1.41421356237f); float h = EuclideanHeuristicNormalized(neighbour, destination, euclideanMaxLength) * heuristic_cost; float f = g + h; //update F when this connection is less costly: if ( f < _F_[neighbour1d] ) { _F_[neighbour1d] = f; solution[neighbour1d] = node; } //update frontier: if (visited.TryAdd(neighbour, 0)) { frontier.Push(neighbour); } } } //create path: BacktrackToPath(solution, moveCost_width, destination, Results); }
private void FindPath(int2 startPosition, int2 endPosition) { Debug.Log("Pathfinding to: " + endPosition); if (startPosition.Equals(endPosition) || Grid[GetIndex(endPosition)] == -1) { Debug.LogError("asking to go into an obstacle, or is already there!"); foundPath = false; return; } PathNode head = new PathNode(startPosition, CalculateDistanceCost(startPosition, endPosition)); OpenSet.Push(head); while (itterationLimit > 0 && OpenSet.HasNext()) { int currentIndex = OpenSet.Pop(); PathNode current = OpenSet[currentIndex]; int ind = GetIndex(current.Position); PathNode cameFromNode = CameFrom[ind]; if (current.Position.Equals(endPosition)) { //Found our destination, we will let the cleanup job handle the path reconstruction for now //ReconstructPath(startPosition, endPosition); return; } float initialCost = CostSoFar[GetIndex(current.Position)]; PathNode[] neighbourNodes = new PathNode[Neighbours.Length]; for (int i = 0; i < Neighbours.Length; i++) { int2 neighbour = Neighbours[i]; int2 position = current.Position + neighbour; if (position.x < 0 || position.x >= DimX || position.y < 0 || position.y >= DimY) { continue; } int index = GetIndex(position); float cellCost = GetCellCost(currentIndex, index, true); if (float.IsInfinity(cellCost)) { current.NextToObstacle = true; continue; } neighbourNodes[i] = new PathNode(position, cellCost); } if (!cameFromNode.Equals(null) && cameFromNode.NextToObstacle && current.NextToObstacle && IsDiagonal(current.Position, cameFromNode.Position)) { //In this case, the path came from point that was next to a obstacle, and is moving diagonally towards a point next to an obstacle. so we are assuming they are moving diagonally through the obstacle //TODO: this is not always the case, will need to resolve later continue; } for (int i = 0; i < neighbourNodes.Length; i++) { int2 neighbour = Neighbours[i]; PathNode neighbourNode = neighbourNodes[i]; int index = GetIndex(neighbourNode.Position); if (neighbourNode.Equals(null)) { Debug.Log("neighbour null"); continue; } float neighbourCost = 10; if ((math.abs(neighbour.x) + math.abs(neighbour.y)) == 2) { neighbourCost = 14; } float newCost = initialCost + neighbourCost + neighbourNode.ExpectedCost; float oldCost = CostSoFar[index]; if (!(oldCost <= 0) && !(newCost < oldCost)) { continue; } CostSoFar[index] = newCost; CameFrom[index] = current; neighbourNode.ExpectedCost = newCost + CalculateDistanceCost(neighbourNode.Position, endPosition); OpenSet.Push(neighbourNode); } itterationLimit--; } if (OpenSet.HasNext()) { //We ran out of itterations //We will just give out where we stapped at for now //TODO: fix this var currentIndex = OpenSet.Pop(); endPosition = OpenSet[currentIndex].Position; } }
public void Execute(Entity entity, int index, ref PathFindingRequest request) { _iterations = 0; _pathNodeCount = 0; //Generate Working Containers var openSet = new NativeMinHeap(MapSize, Allocator.Temp); var cameFrom = new NativeArray <int>(MapSize, Allocator.Temp); var costCount = new NativeArray <int>(MapSize, Allocator.Temp); for (var i = 0; i < MapSize; i++) { costCount[i] = int.MaxValue; } // Path finding var startId = request.StartId; var goalId = request.GoalId; openSet.Push(new MinHeapNode(startId, 0)); costCount[startId] = 0; var currentId = -1; while (_iterations < IterationLimit && openSet.HasNext()) { var currentNode = openSet[openSet.Pop()]; currentId = currentNode.Id; if (currentId == goalId) { break; } var neighboursId = new NativeList <int>(4, Allocator.Temp); Nodes[currentId].GetNeighbours(ref neighboursId); foreach (var neighbourId in neighboursId) { //if cost == -1 means obstacle, skip if (Nodes[neighbourId].GetCost() == -1) { continue; } var currentCost = costCount[currentId] == int.MaxValue ? 0 : costCount[currentId]; var newCost = currentCost + Nodes[neighbourId].GetCost(); //not better, skip if (costCount[neighbourId] <= newCost) { continue; } var priority = newCost + Nodes[neighbourId].Heuristic(goalId); openSet.Push(new MinHeapNode(neighbourId, priority)); cameFrom[neighbourId] = currentId; costCount[neighbourId] = newCost; } _iterations++; neighboursId.Dispose(); } //Construct path var buffer = ResultECB.AddBuffer <PathRoute>(index, entity); var nodeId = goalId; while (_pathNodeCount < PathNodeLimit && !nodeId.Equals(startId)) { buffer.Add(new PathRoute { Id = nodeId }); nodeId = cameFrom[nodeId]; _pathNodeCount++; } //Construct Result var success = true; var log = new NativeString64("Path finding success"); if (!openSet.HasNext() && currentId != goalId) { success = false; log = new NativeString64("Out of openset"); } if (_iterations >= IterationLimit && currentId != goalId) { success = false; log = new NativeString64("Iteration limit reached"); } else if (_pathNodeCount >= PathNodeLimit && !nodeId.Equals(startId)) { success = false; log = new NativeString64("Step limit reached"); } ResultECB.AddComponent(index, entity, new PathResult { Success = success, Log = log }); //Clean result at end of simulation CleanECB.DestroyEntity(index, entity); //Clear openSet.Dispose(); cameFrom.Dispose(); costCount.Dispose(); }
private void FindPathUsingAStar(int startNavUnit, int endNavUnit, NativeList <int> path) { if (startNavUnit < 0 || endNavUnit < 0 || !navUnits[startNavUnit].IsNavigable() || !navUnits[endNavUnit].IsNavigable() || startNavUnit == endNavUnit) { return; } Bounds endNavUnitBounds = navUnits[endNavUnit].GetRelativeBounds(); // SimplePriorityQueue<int> openList = new SimplePriorityQueue<int>(); // HashSet<int> closedList = new HashSet<int>(); NativeMinHeap <int> openList = new NativeMinHeap <int>(Allocator.Temp); NativeArray <bool> closedList = new NativeArray <bool>(navUnits.Length, Allocator.Temp); int l = 0; for (int j = 0; j < navGridSizeY; j++) { for (int k = 0; k < navGridSizeZ; k++) { for (int i = 0; i < navGridSizeX; i++) { navUnits[l] = navUnits[l].ResetPathFindingData(); l++; } } } openList.Push(startNavUnit, navUnits[startNavUnit].AStarData.F); while (openList.Size() > 0) { int curNavUnit = openList.Pop(); int3 curNavPos = GetPosFromIndex(curNavUnit); Bounds curNavUnitBounds = navUnits[curNavUnit].GetRelativeBounds(); for (int i = 0; i < neighborOffsets.Length; i++) { int3 neighborOffset = neighborOffsets[i]; int neighbor = GetIndexFromPos(curNavPos.x + neighborOffset.x, curNavPos.y + neighborOffset.y, curNavPos.z + neighborOffset.z); if (neighbor < 0 || !navUnits[neighbor].IsNavigable()) { continue; } if (neighbor == endNavUnit) { navUnits[neighbor] = navUnits[neighbor].SetPathFindingParentIndex(curNavUnit); BackTracePath(endNavUnit, path); return; } if (!closedList[neighbor]) { Bounds neighborBounds = navUnits[neighbor].GetRelativeBounds(); float newG = navUnits[curNavUnit].AStarData.G + Vector3.Distance(curNavUnitBounds.center, neighborBounds.center); float newH = Vector3.Distance(neighborBounds.center, endNavUnitBounds.center); float newF = newG + newH; if (newF < navUnits[neighbor].AStarData.F) { navUnits[neighbor] = navUnits[neighbor].UpdatePathFindingValues(newF, newG, newH, curNavUnit); openList.Push(neighbor, navUnits[neighbor].AStarData.F); } } } closedList[curNavUnit] = true; } openList.Dispose(); closedList.Dispose(); }