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