private void FindPath(int2 startingPostion, int2 endPostion)
{
int2 gridSize = new int2(4, 4);
NativeArray <PathNode> pathNodeArray = new NativeArray <PathNode>(gridSize.x * gridSize.y, Allocator.Temp);
for (int x = 0; x < gridSize.x; x++)
{
for (int y = 0; y < gridSize.y; y++)
{
PathNode pathNode = new PathNode();
pathNode.x = x;
pathNode.y = y;
pathNode.Index = CalculateIndex(x, y, gridSize.x);
pathNode.gCost = int.MaxValue;
pathNode.hCost = CalculateDistanceCost(new int2(x, y), endPostion);
pathNode.CalculateFcost();
pathNode.isWalkble = true;
pathNode.cameFromNodeIndex = -1;
pathNodeArray[pathNode.Index] = pathNode;
}
}
NativeArray <int2> neighborOffsetArray = new NativeArray <int2>(new int2[] {
new int2(-1, 0),
new int2(+1, 0),
new int2(0, +1),
new int2(0, -1),
}, Allocator.Temp);
int endNodeIndex = CalculateIndex(endPostion.x, endPostion.y, gridSize.x);
PathNode startNode = pathNodeArray[CalculateIndex(startingPostion.x, startingPostion.y, gridSize.x)];
startNode.gCost = 0;
startNode.CalculateFcost();
pathNodeArray[startNode.Index] = startNode;
NativeList <int> openList = new NativeList <int>(Allocator.Temp);
NativeList <int> closedList = new NativeList <int>(Allocator.Temp);
openList.Add(startNode.Index);
while (openList.Length > 0)
{
int currentNodeIndex = GetLowestCostFNodeIndex(openList, pathNodeArray);
PathNode currentNode = pathNodeArray[currentNodeIndex];
if (currentNodeIndex == endNodeIndex)
{
// we are at the end
break;
}
// remove current
for (int i = 0; i < openList.Length; i++)
{
if (openList[i] == currentNodeIndex)
{
openList.RemoveAtSwapBack(i);
break;
}
}
closedList.Add(currentNodeIndex);
for (int i = 0; i < neighborOffsetArray.Length; i++)
{
int2 neighborOffset = neighborOffsetArray[i];
int2 neighborPosition = new int2(currentNode.x + neighborOffset.x, currentNode.y + neighborOffset.y);
if (!IsPositionInsiderGrid(neighborPosition, gridSize))
{
// Not valid
continue;
}
int neighborNodeIndex = CalculateIndex(neighborPosition.x, neighborPosition.y, gridSize.x);
if (closedList.Contains(neighborNodeIndex))
{
//Searched already
continue;
}
PathNode neighborNode = pathNodeArray[neighborNodeIndex];
if (!neighborNode.isWalkble)
{
//not walkable
continue;
}
int2 currentNodePos = new int2(currentNode.x, currentNode.y);
int tentaiveGCost = currentNode.gCost + CalculateDistanceCost(currentNodePos, neighborPosition);
if (tentaiveGCost < neighborNode.gCost)
{
neighborNode.cameFromNodeIndex = currentNodeIndex;
neighborNode.gCost = tentaiveGCost;
neighborNode.CalculateFcost();
pathNodeArray[neighborNodeIndex] = neighborNode;
if (!openList.Contains(neighborNode.Index))
{
openList.Add(neighborNode.Index);
}
}
}
}
PathNode endNode = pathNodeArray[endNodeIndex];
if (endNode.cameFromNodeIndex == -1)
{
//no
Debug.Log("didnt Find it");
}
else
{
//yes
NativeList <int2> path = CalculatePath(pathNodeArray, endNode);
foreach (int2 pathPosition in path)
{
Debug.Log(pathPosition);
}
path.Dispose();
}
pathNodeArray.Dispose();
neighborOffsetArray.Dispose();
openList.Dispose();
closedList.Dispose();
}