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