private unsafe void AStarSolver(int2 start, int2 goal, int index, Entity agent)
{
// data containers sliced from shared data structures
var openSet = OpenSet.Slice(index * _maxLength, _maxLength);
var closedSet = ClosedSet.Slice(index * _maxLength, _maxLength);
var G_Costs = Gcosts.Slice(index * _maxLength, _maxLength);
var neighbours = new NativeList <int2>(8, Allocator.TempJob);
// reset of data containers
openSet.Clear();
void *buffer = closedSet.GetUnsafePtr();
UnsafeUtility.MemClear(buffer, (long)closedSet.Length * UnsafeUtility.SizeOf <MinHeapNode>());
buffer = G_Costs.GetUnsafePtr();
UnsafeUtility.MemClear(buffer, (long)G_Costs.Length * UnsafeUtility.SizeOf <int>());
var startNode = new MinHeapNode(GridGeneratorSystem.grid[start.x, start.y], start);
openSet.Push(startNode);
while (openSet.HasNext())
{
var currentNode = openSet.Pop();
currentNode.IsClosed = 1;
closedSet[GetIndex(currentNode.Position)] = currentNode;
// if goal node is reached then the path is created and assigned to the agent entity
if (currentNode.Position.x == goal.x && currentNode.Position.y == goal.y)
{
var path = new NativeList <int2>(Allocator.TempJob);
var current = currentNode;
while (current.ParentPosition.x != -1)
{
path.Add(current.Position);
current = closedSet[GetIndex(current.ParentPosition)];
}
path.Add(current.Position);
CreatePath(agent, ref path);
path.Dispose();
break;
}
GetNeighbours(currentNode.Position, ref neighbours);
for (int i = 0; i < neighbours.Length; i++)
{
var neighbourEntity = GridGeneratorSystem.grid[neighbours[i].x, neighbours[i].y];
// if current neighbour is in closed list or is unwalkable then skip to next
if (closedSet[GetIndex(neighbours[i])].IsClosed == 1 || !Walkables[neighbourEntity].Value)
{
continue;
}
int costSoFar = G_Costs[GetIndex(currentNode.Position)] + Heuristics.OctileDistance(currentNode.Position, neighbours[i]);
if (G_Costs[GetIndex(neighbours[i])] == 0 || costSoFar < G_Costs[GetIndex(neighbours[i])])
{
// update costs
int h = Heuristics.OctileDistance(neighbours[i], goal);
int f = costSoFar + h;
G_Costs[GetIndex(neighbours[i])] = costSoFar;
var node = new MinHeapNode(neighbourEntity, neighbours[i], currentNode.Position, f, h);
// if openSet contains node => update node
openSet.IfContainsRemove(node.NodeEntity);
openSet.Push(node);
}
}
}
neighbours.Dispose();
}
