private int PopLowestFCostNodeIndexFromOpenSet(NativeList <int> openSet, NativeArray <NodePathFindInfo> nodesInfo)
{
int foundAtIndex = -1;
int lowestIndex = -1;
int lowestFCostHCost = int.MaxValue;
int lowestFCostVal = int.MaxValue;
for (int i = 0; i < openSet.Length; i++)
{
int currNodeIndex = openSet[i];
NodePathFindInfo info = nodesInfo[currNodeIndex];
if (info.FCost < lowestFCostVal || info.FCost == lowestFCostVal && info.hCost < lowestFCostHCost)
{
foundAtIndex = i;
lowestIndex = currNodeIndex;
lowestFCostHCost = info.hCost;
lowestFCostVal = info.FCost;
}
}
openSet.RemoveAtSwapBack(foundAtIndex);
return(lowestIndex);
}
public void Execute(int index)
{
NativeArray <NodePathFindInfo> nodesInfo = new NativeArray <NodePathFindInfo>(numNodes, Allocator.Temp);
NativeList <int> openSet = new NativeList <int>(numNodes / 4, Allocator.Temp);
NativeArray <byte> closedSet = new NativeArray <byte>(numNodes, Allocator.Temp);
int startNodeIndex = startNodesIndices[index];
int endNodeIndex = endNodeIndices[index];
// when an index is invalid, it's set to -1
if (startNodeIndex < 0 || endNodeIndex < 0)
{
nextNodesIndices[index] = -1;
return;
}
NativeArray <byte> openSetContains = new NativeArray <byte>(numNodes, Allocator.Temp);
// set the info for the first node
nodesInfo[startNodeIndex] = new NodePathFindInfo(0, GetHeuristic(startNodeIndex, endNodeIndex), -1);
openSet.AddNoResize(startNodeIndex);
while (openSet.Length > 0)
{
int currNodeIndex = PopLowestFCostNodeIndexFromOpenSet(openSet, nodesInfo);
// we've reached the goal
if (currNodeIndex == endNodeIndex)
{
ReconstructPath(index, startNodeIndex, nodesInfo);
nodesInfo.Dispose();
openSetContains.Dispose();
openSet.Dispose();
closedSet.Dispose();
return;
}
// add it to the closed set by setting a flag at its index
closedSet[currNodeIndex] = 1;
NodePathFindInfo currNodeInfo = nodesInfo[currNodeIndex];
// go over the neighbors
int start = currNodeIndex * numNeighbors;
int end = start + numNeighbors;
for (int i = start; i < end; i++)
{
int neighborIndex = nodesNeighbors[i].neighborIndex;
bool valid = nodesNeighbors[i].isValid;
// if it does not have neighbor or was already expanded
if (!valid || closedSet[neighborIndex] == 1)
{
continue;
}
NodeType nodeType = nodesTypes[neighborIndex];
// can't be walked by
if ((int)nodeType > 0)
{
continue;
}
NodePathFindInfo neighborNodeInfo = nodesInfo[neighborIndex];
int newGCost = currNodeInfo.gCost + GetHeuristic(currNodeIndex, neighborIndex);
// not in open set
if (openSetContains[neighborIndex] != 1)
{
// update parent, costs, and add to the open set
neighborNodeInfo.gCost = newGCost;
neighborNodeInfo.hCost = GetHeuristic(neighborIndex, endNodeIndex);
neighborNodeInfo.parentNodeIndex = currNodeIndex;
nodesInfo[neighborIndex] = neighborNodeInfo;
openSet.AddNoResize(neighborIndex);
openSetContains[neighborIndex] = 1;
}
else if (newGCost < neighborNodeInfo.gCost)
{
// update parent, and gCost (hCost is already calculated)
neighborNodeInfo.gCost = newGCost;
neighborNodeInfo.parentNodeIndex = currNodeIndex;
nodesInfo[neighborIndex] = neighborNodeInfo;
}
}
}
nextNodesIndices[index] = -1;
nodesInfo.Dispose();
openSetContains.Dispose();
openSet.Dispose();
closedSet.Dispose();
}
