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