public unsafe void Execute()
{
int PATH_NODE_ARRAY_SIZE = 1000;
// Node sets
NativeMinHeap openSet = new NativeMinHeap();
openSet.Initialize(PATH_NODE_ARRAY_SIZE, -1, PATH_NODE_ARRAY_SIZE + 1);
NativeArray <int> localIndexArray = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
NativeArray <int> graphIndexArray = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
NativeArray <int> parentArray = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
NativeArray <int> hCostArray = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);
NativeArray <int> gCostArray = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
NativeArray <int> fCostArray = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);
NativeArray <Blittable_Bool> openArray = new NativeArray <Blittable_Bool>(PATH_NODE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
NativeArray <Blittable_Bool> closedArray = new NativeArray <Blittable_Bool>(PATH_NODE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
int intValue = -1;
bool bValue = false;
void *pointer = ( void * )&intValue;
UnsafeUtility.MemCpyReplicate(parentArray.GetUnsafePtr(), pointer, sizeof(int), PATH_NODE_ARRAY_SIZE);
intValue = int.MaxValue;
UnsafeUtility.MemCpyReplicate(gCostArray.GetUnsafePtr(), pointer, sizeof(int), PATH_NODE_ARRAY_SIZE);
pointer = ( void * )&bValue;
UnsafeUtility.MemCpyReplicate(openArray.GetUnsafePtr(), pointer, sizeof(bool), PATH_NODE_ARRAY_SIZE);
UnsafeUtility.MemCpyReplicate(closedArray.GetUnsafePtr(), pointer, sizeof(bool), PATH_NODE_ARRAY_SIZE);
int4 graphClusterSizeVector = new int4(5);
int4 graphCellLengthVector = new int4(4);
int4 clusterPositionXVector = new int4(2);
int4 clusterPositionYVector = new int4(2);
int4 loopIndex = new int4();
int4 localCol = new int4();
int4 localRow = new int4();
int4 clusterX = graphClusterSizeVector * clusterPositionXVector;
int4 clusterY = graphClusterSizeVector * clusterPositionYVector;
int4 localIndex = localCol + localRow * graphClusterSizeVector;
int4 graphCol = localCol + clusterX;
int4 graphRow = localRow + clusterY;
int4 graphArrayIndex = graphCol + graphRow * graphCellLengthVector;
int vectorLoopLength = 100;
int i = 0;
for ( ; i < vectorLoopLength; i += 4)
{
loopIndex = new int4(i, i + 1, i + 2, i + 3);
localCol = loopIndex / graphClusterSizeVector;
localRow = loopIndex % graphClusterSizeVector;
localIndex = localCol + localRow * graphClusterSizeVector;
graphCol = localCol + clusterX;
graphRow = localRow + clusterY;
graphArrayIndex = graphCol + graphRow * graphCellLengthVector;
localIndexArray.ReinterpretStore(localIndex.x, localIndex);
graphIndexArray.ReinterpretStore(localIndex.x, graphArrayIndex);
ints[i] = graphArrayIndex;
}
}
/// <summary> Traces path using some kind of A* algorithm </summary>
/// <param name="start"> Start index 2d </param>
/// <param name="destination"> Destination index 2d </param>
/// <param name="moveCost"> Move cost data 2d array </param>
/// <param name="moveCost_width"> 2d array's width </param>
/// <param name="heuristic_cost"> Cost heuristic multiplier. Figure out yourself what value works best for your specific moveCost data </param>
/// <param name="heuristic_search"> Search heuristic multiplier. Figure out yourself what value works best for your specific moveCost data </param>
/// <param name="output_path"> Resulting path goes here </param>
public unsafe AStarJob
(
INT2 start,
INT2 destination,
NativeArray <float> moveCost,
int moveCost_width,
float heuristic_cost,
float heuristic_search,
NativeList <int2> output_path
)
{
this.start = start;
this.destination = destination;
this.moveCost = moveCost;
this.moveCost_width = moveCost_width;
this.Results = output_path;
this.heuristic_cost = heuristic_cost;
this.heuristic_search = heuristic_search;
int length = moveCost.Length;
int start1d = Index2dTo1d(start, moveCost_width);
_F_ = new NativeArray <float>(length, Allocator.TempJob, NativeArrayOptions.UninitializedMemory);
solution = new NativeArray <int2>(length, Allocator.TempJob);
frontier = new NativeMinHeap <int2, AStarJobComparer>(
new AStarJobComparer(_F_, moveCost_width, destination, heuristic_search),
Allocator.TempJob, length
);
visited = new NativeHashMap <int2, byte>(length, Allocator.TempJob); //TODO: use actual hashSet once available
neighbours = new NativeList <int2>(8, Allocator.TempJob);
}
private static void Allocate(int capacity, Allocator allocator, out NativeMinHeap nativeMinHeap)
{
var size = (long)UnsafeUtility.SizeOf <PathNode>() * capacity;
if (allocator <= Allocator.None)
{
throw new ArgumentException("Allocator must be Temp, TempJob or Persistent", nameof(allocator));
}
if (capacity < 0)
{
throw new ArgumentOutOfRangeException(nameof(capacity), "Length must be >= 0");
}
if (size > int.MaxValue)
{
throw new ArgumentOutOfRangeException(nameof(capacity),
$"Length * sizeof(T) cannot exceed {(object)int.MaxValue} bytes");
}
nativeMinHeap.m_Buffer = UnsafeUtility.Malloc(size, UnsafeUtility.AlignOf <PathNode>(), allocator);
nativeMinHeap.m_capacity = capacity;
nativeMinHeap.m_AllocatorLabel = allocator;
nativeMinHeap.m_MinIndex = 0;
nativeMinHeap.m_MaxIndex = capacity - 1;
nativeMinHeap.m_head = -1;
nativeMinHeap.m_length = 0;
#if ENABLE_UNITY_COLLECTIONS_CHECKS
DisposeSentinel.Create(out nativeMinHeap.m_Safety, out nativeMinHeap.m_DisposeSentinel, 1, allocator);
#endif
}
protected override void OnCreateManager()
{
MaxLenght = 2500;
MaxAgents = SpawnAgentSystem.maxLimit + 300;
// allocation of shared data structures
OpenSet = new NativeMinHeap(MaxLenght * MaxAgents, Allocator.Persistent);
ClosedSet = new NativeArray <MinHeapNode>(MaxLenght * MaxAgents, Allocator.Persistent);
Gcosts = new NativeArray <int>(MaxLenght * MaxAgents, Allocator.Persistent);
Enabled = false;
}
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);
}
private static void Allocate(int capacity, Allocator allocator, out NativeMinHeap <TValue, TPriority> nativeMinHeap)
{
var size = (long)UnsafeUtility.SizeOf <NativeMinHeapNode <TValue, TPriority> >() * capacity;
// Check for valid allocation
#if ENABLE_UNITY_COLLECTIONS_CHECKS
if (allocator <= Allocator.None)
{
throw new ArgumentException("Allocator must be Temp, TempJob or Persistent", nameof(allocator));
}
if (capacity < 0)
{
throw new ArgumentOutOfRangeException(nameof(capacity), "Length must be >= 0");
}
if (size > int.MaxValue)
{
throw new ArgumentOutOfRangeException(nameof(capacity), $"Length * sizeof(T) cannot exceed {(object)int.MaxValue} bytes");
}
if (!UnsafeUtility.IsBlittable <TValue>())
{
throw new ArgumentException(string.Format("{0} used in NativeMinHeap<{0}> must be blittable", typeof(TValue)));
}
#endif
// Allocate buffer
nativeMinHeap.m_Buffer = UnsafeUtility.Malloc(size, UnsafeUtility.AlignOf <NativeMinHeapNode <TValue, TPriority> >(), allocator);
nativeMinHeap.m_Length = capacity;
nativeMinHeap.m_AllocatorLabel = allocator;
nativeMinHeap.size = 0;
#if ENABLE_UNITY_COLLECTIONS_CHECKS
nativeMinHeap.m_MinIndex = 0;
nativeMinHeap.m_MaxIndex = capacity - 1;
DisposeSentinel.Create(out nativeMinHeap.m_Safety, out nativeMinHeap.m_DisposeSentinel, 1, allocator);
#endif
}
public void Execute()
{
int pathNodeArraySize = clusterSize * clusterSize;
int2 startPosition = nodePositions[startNode];
int2 endPosition = nodePositions[endNode];
// Node sets
NativeMinHeap openSet = new NativeMinHeap();
openSet.Initialize(pathNodeArraySize, 0, pathNodeArraySize + 1);
NativeArray <int> localIndexArray = new NativeArray <int>(pathNodeArraySize, Allocator.Temp);
NativeArray <int> graphIndexArray = new NativeArray <int>(pathNodeArraySize, Allocator.Temp);
NativeArray <int> parentArray = new NativeArray <int>(pathNodeArraySize, Allocator.Temp);
NativeArray <int> hCostArray = new NativeArray <int>(pathNodeArraySize, Allocator.Temp);
NativeArray <int> gCostArray = new NativeArray <int>(pathNodeArraySize, Allocator.Temp);
NativeArray <int> fCostArray = new NativeArray <int>(pathNodeArraySize, Allocator.Temp);
NativeArray <bool> openArray = new NativeArray <bool>(pathNodeArraySize, Allocator.Temp);
NativeArray <bool> closedArray = new NativeArray <bool>(pathNodeArraySize, Allocator.Temp);
// Initialize nodes from ReadOnly grid array
for (int localRow = 0; localRow < clusterSize; localRow++)
{
for (int localCol = 0; localCol < clusterSize; localCol++)
{
int localIndex = localCol + localRow * clusterSize;
int graphCol = localCol + clusterSize * clusterPosition.x;
int graphRow = localRow + clusterSize * clusterPosition.y;
int graphArrayIndex = graphCol + graphRow * numCells;
localIndexArray[localIndex] = localIndex;
graphIndexArray[localIndex] = graphArrayIndex;
parentArray[localIndex] = -1;
gCostArray[localIndex] = int.MaxValue;
openArray[localIndex] = false;
closedArray[localIndex] = false;
}
}
// Get and cache the start and end pathNodeIndices
int endNodeX = endPosition.x - clusterPosition.x * clusterSize;
int endNodeY = endPosition.y - clusterPosition.y * clusterSize;
int startNodeX = startPosition.x - clusterPosition.x * clusterSize;
int startNodeY = startPosition.y - clusterPosition.y * clusterSize;
int endNodeIndex = endNodeX + endNodeY * clusterSize;
int startNodeIndex = startNodeX + startNodeY * clusterSize;
// Initialize the starting pathNode
int graphIndex = graphIndexArray[startNodeIndex];
int2 nodePosition = nodePositions[graphIndex];
int hCost = ManhattenDistance(nodePosition, endPosition);
gCostArray[startNodeIndex] = 0;
hCostArray[startNodeIndex] = hCost;
fCostArray[startNodeIndex] = hCost;
openArray[startNodeIndex] = true;
// Add the start pathNode to the openSet
openSet.Enqueue(startNodeIndex, hCost);
while (openSet.Length > 0)
{
// Cache the pathNodeIndex we are working with during this iteration
int currentNodeIndex = openSet.DequeueMin(closedArray);
// Break if we reached our goal
if (currentNodeIndex == endNodeIndex)
{
break;
}
openArray[currentNodeIndex] = false;
closedArray[currentNodeIndex] = true;
int2 currentNodePosition = nodePositions[graphIndexArray[currentNodeIndex]];
int2 neighbourKey = neighbourKeys[graphIndexArray[currentNodeIndex]];
for (int i = neighbourKey.x; i < neighbourKey.y; i++)
{
int2 neighbourPosition = nodePositions[neighbours[i]];
int2 localNeighbourPosition = neighbourPosition - clusterPosition * clusterSize;
int localNeighbourIndex = localNeighbourPosition.x + localNeighbourPosition.y * clusterSize;
// Skip if its closed (already searched)
if (closedArray[localNeighbourIndex])
{
continue;
}
// Calculate the cost to move from current node to neighbour node
int distanceCost = ManhattenDistance(currentNodePosition, neighbourPosition);
int tentativeCost = gCostArray[currentNodeIndex] + distanceCost;
if (tentativeCost < gCostArray[localNeighbourIndex])
{
int newHCost = ManhattenDistance(neighbourPosition, endPosition);
parentArray[localNeighbourIndex] = currentNodeIndex;
hCostArray[localNeighbourIndex] = newHCost;
gCostArray[localNeighbourIndex] = tentativeCost;
fCostArray[localNeighbourIndex] = tentativeCost + hCost;
if (!openArray[localNeighbourIndex])
{
openArray[localNeighbourIndex] = true;
openSet.Enqueue(localNeighbourIndex, fCostArray[localNeighbourIndex]);
}
}
}
}
NativeList <int2> pathFound = new NativeList <int2>(Allocator.Temp);
NativeList <int2> smoothedPath = new NativeList <int2>(Allocator.Temp);
int nodeIndex = endNodeIndex;
bool smoothPath = true;
if (parentArray[nodeIndex] == -1)
{
openSet.Dispose();
localIndexArray.Dispose();
graphIndexArray.Dispose();
parentArray.Dispose();
hCostArray.Dispose();
gCostArray.Dispose();
fCostArray.Dispose();
openArray.Dispose();
closedArray.Dispose();
pathFound.Dispose();
smoothedPath.Dispose();
path = new NativeList <int2>(Allocator.Temp);
}
else
{
pathFound.Add(endPosition);
smoothedPath.Add(endPosition);
while (parentArray[nodeIndex] != -1)
{
pathFound.Add(nodePositions[graphIndexArray[parentArray[nodeIndex]]]);
//smoothedPath.Add( nodePositions[ graphIndexArray[ parentArray[ nodeIndex ] ] ] );
nodeIndex = parentArray[nodeIndex];
}
}
if (smoothPath && pathFound.Length > 2) // If its less than or equal 2 theres no need to smooth the path
{
int fromIndex = 0;
bool foundPath = false;
while (!foundPath)
{
int currentIndex = fromIndex + 2; // Because the next index is always going to be in line of sight
if (currentIndex > pathFound.Length - 1)
{
break;
}
while (true)
{
int stopIndex = currentIndex - 1;
int graphNodeArrayIndex = pathFound[stopIndex].x + pathFound[stopIndex].y * numCells;
int2 start = pathFound[fromIndex];
int2 end = pathFound[currentIndex];
if (!LOS(start.x, start.y, end.x, end.y))
{
smoothedPath.Add(nodePositions[graphNodeArrayIndex]);
fromIndex = stopIndex;
break;
}
else
{
if (currentIndex >= pathFound.Length - 1)
{
foundPath = true;
break;
}
currentIndex++;
}
}
}
}
openSet.Dispose();
localIndexArray.Dispose();
graphIndexArray.Dispose();
parentArray.Dispose();
hCostArray.Dispose();
gCostArray.Dispose();
fCostArray.Dispose();
openArray.Dispose();
closedArray.Dispose();
pathFound.Dispose();
for (int i = 0; i < smoothedPath.Length; i++)
{
path.Add(smoothedPath[i]);
}
smoothedPath.Dispose();
}
private NativeList <int3> FindAbstractPath(NativeList <int> startNodes, int2 startPosition, int2 endPosition)
{
#region DATA SETUP
int EDGE_ARRAY_SIZE = graphIntraEdges.Length;
// Node sets
NativeMinHeap openSet = new NativeMinHeap();
openSet.Initialize(EDGE_ARRAY_SIZE, -1, EDGE_ARRAY_SIZE + 10);
NativeArray <int> graphNodeIntraIndexArray = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp);
NativeArray <int> graphNodeInterIndexArray = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp);
NativeArray <int> parentEdgeArray = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp);
NativeArray <int> parentPathIndexArray = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp);
NativeArray <int> hCostArray = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp);
NativeArray <int> gCostArray = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp);
NativeArray <int> fCostArray = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp);
NativeArray <Blittable_Bool> openArray = new NativeArray <Blittable_Bool>(EDGE_ARRAY_SIZE, Allocator.Temp);
NativeArray <Blittable_Bool> closedArray = new NativeArray <Blittable_Bool>(EDGE_ARRAY_SIZE, Allocator.Temp);
void DisposeNativeContainers()
{
openSet.Dispose();
graphNodeIntraIndexArray.Dispose();
graphNodeInterIndexArray.Dispose();
parentEdgeArray.Dispose();
parentPathIndexArray.Dispose();
hCostArray.Dispose();
gCostArray.Dispose();
fCostArray.Dispose();
openArray.Dispose();
closedArray.Dispose();
}
// Initialize default array values
for (int i = 0; i < EDGE_ARRAY_SIZE; i++)
{
graphNodeIntraIndexArray[i] = graphIntraEdges[i];
graphNodeInterIndexArray[i] = graphInterEdges[i];
parentEdgeArray[i] = -1;
parentPathIndexArray[i] = -1;
gCostArray[i] = int.MaxValue;
openArray[i] = false;
closedArray[i] = false;
}
// Initialize the starting nodes(edges) from the parameter
for (int i = 0; i < startNodes.Length; i++)
{
int nodeIndex = startNodes[i];
int edgeIndex = graphNodeToEdge[nodeIndex];
hCostArray[edgeIndex] = ManhattenDistance(graphNodePositions[nodeIndex], endPosition);
gCostArray[edgeIndex] = ManhattenDistance(startPosition, graphNodePositions[nodeIndex]);
fCostArray[edgeIndex] = gCostArray[edgeIndex] + hCostArray[edgeIndex];
openSet.Enqueue(edgeIndex, fCostArray[edgeIndex]);
}
// So we can test when we have reached the end
int2 endClusterPosition = new int2(
endPosition.x / graphClusterLength,
endPosition.y / graphClusterLength);
int endEdgeIndex = -1;
#endregion
#region FIND PATH
while (openSet.Length > 0)
{
int currentEdge = openSet.DequeueMin(closedArray);
int currentInterNode = graphNodeInterIndexArray[currentEdge];
int2 clusterPosition = new int2(
graphNodePositions[currentInterNode].x / graphClusterLength,
graphNodePositions[currentInterNode].y / graphClusterLength);
if (clusterPosition.Equals(endClusterPosition))
{
endEdgeIndex = currentEdge;
break;
}
openArray[currentEdge] = false;
closedArray[currentEdge] = true;
int2 interNodeNeighbours =
graphEdgeNeighbourList[currentInterNode];
for (int i = interNodeNeighbours.x; i < interNodeNeighbours.y; i++)
{
int neighbourNode = graphEdgeNeighbours[i];
int neighbourEdge = graphNodeToEdge[neighbourNode];
if (closedArray[neighbourEdge])
{
continue;
}
int distanceCost = ManhattenDistance(
graphNodePositions[currentInterNode], graphNodePositions[neighbourNode]);
int tentativeCost =
distanceCost + gCostArray[currentEdge];
if (tentativeCost < gCostArray[neighbourEdge])
{
int newHCost = ManhattenDistance(graphNodePositions[neighbourNode], endPosition);
parentEdgeArray[neighbourEdge] = currentEdge;
parentPathIndexArray[neighbourEdge] = i;
hCostArray[neighbourEdge] = newHCost;
gCostArray[neighbourEdge] = tentativeCost;
fCostArray[neighbourEdge] = newHCost + tentativeCost;
if (!openArray[neighbourEdge])
{
openArray[neighbourEdge] = true;
openSet.Enqueue(neighbourEdge, fCostArray[neighbourEdge]);
}
}
}
}
#endregion
#region TRACE PATH
NativeList <int3> pathPositions = new NativeList <int3>(Allocator.Temp);
if (endEdgeIndex != -1)
{
// Add the end position and the inter-node it connects to
int interNodeIndex =
graphNodeInterIndexArray[endEdgeIndex];
pathPositions.Add(new int3(
endPosition.x,
endPosition.y,
0));
pathPositions.Add(new int3(
graphNodePositions[interNodeIndex].x,
graphNodePositions[interNodeIndex].y,
0));
// Trace the path
int edgeIndex = endEdgeIndex;
while (parentEdgeArray[edgeIndex] != -1)
{
int3 pathNodeData = new int3(
graphNodePositions[graphNodeIntraIndexArray[edgeIndex]].x,
graphNodePositions[graphNodeIntraIndexArray[edgeIndex]].y,
parentPathIndexArray[edgeIndex]);
pathPositions.Add(pathNodeData);
edgeIndex = parentEdgeArray[edgeIndex];
/*int2 pathPositionKey =
* graphEdgePaths[ parentPathIndexArray[ edgeIndex ] ];
* for ( int i = pathPositionKey.x; i < pathPositionKey.y; i++ )
* pathPositions.Add( graphEdgePathPositions[ i ] );*/
}
// Add the internode of the first edge to the path and the starting position
int interEdge =
graphNodeToEdge[graphNodeInterIndexArray[edgeIndex]];
int interNode =
graphNodeInterIndexArray[interEdge];
pathPositions.Add(new int3(
graphNodePositions[interNode].x,
graphNodePositions[interNode].y,
0));
pathPositions.Add(new int3(
startPosition.x,
startPosition.y,
0));
DisposeNativeContainers();
return(pathPositions);
}
else
{
DisposeNativeContainers();
return(pathPositions);
}
#endregion
}
private NativeList <int2> FindLowLevelPathInCluster(int2 startPosition, int2 endPosition, int2 clusterPosition)
{
#region DATA SETUP
int PATH_NODE_ARRAY_SIZE = graphClusterLength * graphClusterLength;
// Node sets
NativeMinHeap openSet = new NativeMinHeap();
openSet.Initialize(PATH_NODE_ARRAY_SIZE, -1, PATH_NODE_ARRAY_SIZE + 1);
NativeArray <int> localIndexArray = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);
NativeArray <int> graphIndexArray = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);
NativeArray <int> parentArray = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);
NativeArray <int> hCostArray = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);
NativeArray <int> gCostArray = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);
NativeArray <int> fCostArray = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);
NativeArray <Blittable_Bool> openArray = new NativeArray <Blittable_Bool>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);
NativeArray <Blittable_Bool> closedArray = new NativeArray <Blittable_Bool>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);
// Initialize nodes from ReadOnly grid array
for (int localRow = 0; localRow < graphClusterLength; localRow++)
{
for (int localCol = 0; localCol < graphClusterLength; localCol++)
{
int localIndex = localCol + localRow * graphClusterLength;
int graphCol = localCol + graphClusterLength * clusterPosition.x;
int graphRow = localRow + graphClusterLength * clusterPosition.y;
int graphArrayIndex = graphCol + graphRow * graphCellLength;
localIndexArray[localIndex] = localIndex;
graphIndexArray[localIndex] = graphArrayIndex;
parentArray[localIndex] = -1;
gCostArray[localIndex] = int.MaxValue;
openArray[localIndex] = false;
closedArray[localIndex] = false;
}
}
// Get and cache the start and end pathNodeIndices
int endNodeX = endPosition.x - clusterPosition.x * graphClusterLength;
int endNodeY = endPosition.y - clusterPosition.y * graphClusterLength;
int startNodeX = startPosition.x - clusterPosition.x * graphClusterLength;
int startNodeY = startPosition.y - clusterPosition.y * graphClusterLength;
int endNodeIndex = endNodeX + endNodeY * graphClusterLength;
int startNodeIndex = startNodeX + startNodeY * graphClusterLength;
// Initialize the starting pathNode
int hCost = ManhattenDistance(graphNodePositions[graphIndexArray[startNodeIndex]], endPosition);
gCostArray[startNodeIndex] = 0;
hCostArray[startNodeIndex] = hCost;
fCostArray[startNodeIndex] = hCost;
openArray[startNodeIndex] = true;
// Add the starting node to the open set
openSet.Enqueue(startNodeIndex, hCost);
#endregion
#region SEARCH GRAPH
while (openSet.Length > 0)
{
// Cache the pathNodeIndex we are working with during this iteration
int currentNodeIndex =
openSet.DequeueMin(localIndexArray, closedArray);
openArray[currentNodeIndex] = false;
closedArray[currentNodeIndex] = true;
// Break if we reached our goal
if (currentNodeIndex == endNodeIndex)
{
break;
}
int2 currentNodePosition =
graphNodePositions[graphIndexArray[currentNodeIndex]];
for (int nIndex = 0; nIndex < neighbourOffsetArray.Length; nIndex++)
{
int2 neighbourPosition =
currentNodePosition + neighbourOffsetArray[nIndex];
if (!ValidateGridPosition(neighbourPosition, clusterPosition, graphClusterLength))
{
continue;
}
int graphNeighbour =
neighbourPosition.x + neighbourPosition.y * graphCellLength;
if (graphNodeWalkables[graphNeighbour] > 0)
{
continue;
}
// Get the local neighbour index
int2 localNeighbourPosition = new int2(
neighbourPosition.x - clusterPosition.x * graphClusterLength,
neighbourPosition.y - clusterPosition.y * graphClusterLength);
int localNeighbourIndex =
localNeighbourPosition.x + localNeighbourPosition.y * graphClusterLength;
// Skip if its closed (already searched)
if (closedArray[localNeighbourIndex])
{
continue;
}
// Calculate the cost to move from current node to neighbour node
int distanceCost =
ManhattenDistance(currentNodePosition, neighbourPosition);
int tentativeCost =
gCostArray[currentNodeIndex] + distanceCost;
if (tentativeCost < gCostArray[localNeighbourIndex])
{
int newHCost = ManhattenDistance(neighbourPosition, endPosition);
parentArray[localNeighbourIndex] = currentNodeIndex;
hCostArray[localNeighbourIndex] = newHCost;
gCostArray[localNeighbourIndex] = tentativeCost;
fCostArray[localNeighbourIndex] = tentativeCost + hCost;
if (!openArray[localNeighbourIndex])
{
openArray[localNeighbourIndex] = true;
openSet.Enqueue(localNeighbourIndex, fCostArray[localNeighbourIndex]);
}
}
}
}
#endregion
#region TRACE PATH
NativeList <int2> path = new NativeList <int2>(Allocator.Temp);
NativeList <int2> smoothedPath = new NativeList <int2>(Allocator.Temp);
int nodeIndex = endNodeIndex;
if (parentArray[endNodeIndex] == -1)
{
openSet.Dispose();
localIndexArray.Dispose();
graphIndexArray.Dispose();
parentArray.Dispose();
hCostArray.Dispose();
gCostArray.Dispose();
fCostArray.Dispose();
openArray.Dispose();
closedArray.Dispose();
return(path);
}
else
{
smoothedPath.Add(endPosition);
path.Add(endPosition);
while (parentArray[nodeIndex] != -1)
{
path.Add(graphNodePositions[graphIndexArray[parentArray[nodeIndex]]]);
nodeIndex = parentArray[nodeIndex];
}
}
#endregion
#region SMOOTH PATH
if (path.Length > 2) // If its less than or equal 2 theres no need to smooth the path
{
int fromIndex = 0;
bool foundPath = false;
while (!foundPath)
{
int currentIndex = fromIndex + 2; // Because the next index is always going to be in line of sight
if (currentIndex > path.Length - 1)
{
break;
}
while (true)
{
int stopIndex = currentIndex - 1;
int graphNodeArrayIndex = path[stopIndex].x + path[stopIndex].y * graphCellLength;
int2 start = path[fromIndex];
int2 end = path[currentIndex];
if (!LOSBetween2Nodes(start, end))
{
smoothedPath.Add(graphNodePositions[graphNodeArrayIndex]);
fromIndex = stopIndex;
break;
}
else
{
if (currentIndex >= path.Length - 1)
{
foundPath = true;
break;
}
currentIndex++;
}
}
}
}
#endregion
#region RETURN
openSet.Dispose();
localIndexArray.Dispose();
graphIndexArray.Dispose();
parentArray.Dispose();
hCostArray.Dispose();
gCostArray.Dispose();
fCostArray.Dispose();
openArray.Dispose();
closedArray.Dispose();
path.Dispose();
return(smoothedPath);
#endregion
}
protected override void OnUpdate()
{
var mapSettings = GetSingleton <MapSettings>();
var neighbors = _neighbors;
// The data is in not valid state
if (mapSettings.Tiles.Length < 1)
{
return;
}
// Other data
var movementComponents = GetComponentDataFromEntity <MovementCost>(true);
var tilesSize = mapSettings.Tiles.Length;
var commandBuffer = _eseCommandBufferSystem.CreateCommandBuffer();
Entities
.WithoutBurst()
.WithNone <Waypoint>()
.ForEach((Entity requestEntity, ref PathRequest pathRequest) =>
{
_markerAStar.Begin();
#region Setup
_markerSetup.Begin();
NativeArray <float2> costs = new NativeArray <float2>(tilesSize, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
NativeArray <byte> closeSet = new NativeArray <byte>(tilesSize, Allocator.Temp);
NativeArray <int> camesFrom = new NativeArray <int>(tilesSize, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
NativeMinHeap minSet = new NativeMinHeap(tilesSize * 2, Allocator.Temp);
_markerSetupData.Begin();
// Setup initial data
for (int i = 0; i < tilesSize; i++)
{
costs[i] = new float2 {
x = 0, y = float.MaxValue
};
camesFrom[i] = -1;
}
_markerSetupData.End();
// Shortcuts
int startTile = Index1D(pathRequest.Start, tilesSize);
int endTile = Index1D(pathRequest.End, tilesSize);
costs[startTile] = 0;
_markerSetup.End();
#endregion Setup
#region Search
_markerSearch.Begin();
int lastTile = startTile;
// While not in destination and not at dead end
while (lastTile != endTile && lastTile != -1)
{
// Mark current tile as visited
closeSet[lastTile] = 1;
for (int i = 0; i < neighbors.Length; ++i)
{
var neighbor = neighbors[i];
// Find linear neighbor index
var neighborIndex = neighbor.Of(lastTile, tilesSize);
// Check if neighbor exists
if (neighborIndex != -1)
{
// Previous + current cost
_markerMovementData.Begin();
var currentCost = movementComponents[mapSettings.Tiles[neighborIndex]];
_markerMovementData.End();
if (currentCost.Cost == MovementCost.IMPOSSIBLE)
{
closeSet[neighborIndex] = 0;
continue;
}
var costG = costs[lastTile].x + neighbor.Distance * currentCost.Cost;
var costF = costG + Implementation.Heuristic(pathRequest, neighborIndex, tilesSize);
if (costs[neighborIndex].y > costF)
{
// Update cost and path
costs[neighborIndex] = new float2(costG, costF);
camesFrom[neighborIndex] = lastTile;
// Update min set
if (closeSet[neighborIndex] == 0)
{
minSet.Push(new MinHeapNode(neighborIndex, costF));
}
}
}
}
lastTile = Implementation.FindCurrent(minSet, closeSet);
}
_markerSearch.End();
#endregion Search
#region ReconstructPath
_markerReconstruct.Begin();
var waypoints = commandBuffer.AddBuffer <Waypoint>(requestEntity);
// Travel back through path
while (lastTile != -1)
{
waypoints.Add(new Waypoint()
{
Position = Index2D(lastTile, tilesSize)
});
lastTile = camesFrom[lastTile];
}
_markerReconstruct.End();
#endregion ReconstructPath
commandBuffer.RemoveComponent <PathRequest>(requestEntity);
#region Cleanup
_markerCleanup.Begin();
// Dispose all temporary data
costs.Dispose();
closeSet.Dispose();
camesFrom.Dispose();
minSet.Dispose();
_markerCleanup.End();
#endregion Cleanup
_markerAStar.End();
}).Run();
}
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 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();
}
private NativeList <AbstractNode> FindAbstractPathInGraph(int2 startEdgesKey, int2 startPosition, int2 endPosition)
{
#region DATA SETUP
int EDGE_ARRAY_SIZE = graphIntraEdges.Length;
// Node sets
NativeMinHeap openSet = new NativeMinHeap();
openSet.Initialize(EDGE_ARRAY_SIZE, -1, EDGE_ARRAY_SIZE + 10);
NativeArray <int> parentEdgeArray = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
NativeArray <int> parentPathIndexArray = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
NativeArray <int> hCostArray = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp);
NativeArray <int> gCostArray = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
NativeArray <int> fCostArray = new NativeArray <int>(EDGE_ARRAY_SIZE, Allocator.Temp);
NativeArray <bool> openArray = new NativeArray <bool>(EDGE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.ClearMemory);
NativeArray <bool> closedArray = new NativeArray <bool>(EDGE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.ClearMemory);
int intValue = -1;
void *pointer = ( void * )&intValue;
UnsafeUtility.MemCpyReplicate(parentEdgeArray.GetUnsafePtr(), pointer, sizeof(int), EDGE_ARRAY_SIZE);
UnsafeUtility.MemCpyReplicate(parentPathIndexArray.GetUnsafePtr(), pointer, sizeof(int), EDGE_ARRAY_SIZE);
intValue = int.MaxValue;
UnsafeUtility.MemCpyReplicate(gCostArray.GetUnsafePtr(), pointer, sizeof(int), EDGE_ARRAY_SIZE);
// Initialize the starting nodes(edges) from the parameter
for (int i = startEdgesKey.x; i < startEdgesKey.y; i++)
{
int nodeIndex = graphIntraEdges[i];
int edgeIndex = graphNodeToEdge[nodeIndex];
hCostArray[edgeIndex] = ManhattenDistance(graphNodePositions[nodeIndex], endPosition);
gCostArray[edgeIndex] = ManhattenDistance(startPosition, graphNodePositions[nodeIndex]);
fCostArray[edgeIndex] = gCostArray[edgeIndex] + hCostArray[edgeIndex];
openSet.Enqueue(edgeIndex, fCostArray[edgeIndex]);
}
// So we can test when we have reached the end
int2 endClusterPosition = new int2(
endPosition.x / graphClusterSize,
endPosition.y / graphClusterSize);
int endEdgeIndex = -1;
#endregion
#region FIND PATH
while (openSet.Length > 0)
{
int currentEdge = openSet.DequeueMin(closedArray);
int currentInterNode = graphInterEdges[currentEdge]; // graphNodeInterIndexArray[ currentEdge ];
int2 clusterPosition = new int2(
graphNodePositions[currentInterNode].x / graphClusterSize,
graphNodePositions[currentInterNode].y / graphClusterSize);
if (clusterPosition.Equals(endClusterPosition))
{
endEdgeIndex = currentEdge;
break;
}
openArray[currentEdge] = false;
closedArray[currentEdge] = true;
int2 interNodeNeighbours = graphEdgeNeighbourList[currentInterNode];
for (int i = interNodeNeighbours.x; i < interNodeNeighbours.y; i++)
{
int neighbourNode = graphEdgeNeighbours[i];
int neighbourEdge = graphNodeToEdge[neighbourNode];
if (closedArray[neighbourEdge])
{
continue;
}
int distanceCost = ManhattenDistance(
graphNodePositions[currentInterNode], graphNodePositions[neighbourNode]);
int tentativeCost =
distanceCost + gCostArray[currentEdge];
if (tentativeCost < gCostArray[neighbourEdge])
{
int newHCost = ManhattenDistance(graphNodePositions[neighbourNode], endPosition);
parentEdgeArray[neighbourEdge] = currentEdge;
parentPathIndexArray[neighbourEdge] = i;
hCostArray[neighbourEdge] = newHCost;
gCostArray[neighbourEdge] = tentativeCost;
fCostArray[neighbourEdge] = newHCost + tentativeCost;
if (!openArray[neighbourEdge])
{
openArray[neighbourEdge] = true;
openSet.Enqueue(neighbourEdge, fCostArray[neighbourEdge]);
}
}
}
}
#endregion
#region TRACE PATH
NativeList <AbstractNode> pathNodes = new NativeList <AbstractNode>(Allocator.Temp);
if (endEdgeIndex != -1)
{
// Add the end position and the inter-node it connects to
int interNodeIndex = graphInterEdges[endEdgeIndex];
pathNodes.Add(new AbstractNode
{
position = endPosition,
path = 0
});
pathNodes.Add(new AbstractNode
{
position = graphNodePositions[interNodeIndex],
path = 0
});
// Trace the path
int edgeIndex = endEdgeIndex;
while (parentEdgeArray[edgeIndex] != -1)
{
AbstractNode node = new AbstractNode();
node.position = graphNodePositions[graphIntraEdges[edgeIndex]];
node.path = parentPathIndexArray[edgeIndex];
pathNodes.Add(node);
edgeIndex = parentEdgeArray[edgeIndex];
}
// Add the internode of the first edge to the path and the starting position
int interEdge = graphNodeToEdge[graphInterEdges[edgeIndex]];
int interNode = graphInterEdges[interEdge];
pathNodes.Add(new AbstractNode
{
position = graphNodePositions[interNode],
path = 0
});
pathNodes.Add(new AbstractNode
{
position = startPosition,
path = 0
});
openSet.Dispose();
parentEdgeArray.Dispose();
parentPathIndexArray.Dispose();
hCostArray.Dispose();
gCostArray.Dispose();
fCostArray.Dispose();
openArray.Dispose();
closedArray.Dispose();
return(pathNodes);
}
else
{
openSet.Dispose();
parentEdgeArray.Dispose();
parentPathIndexArray.Dispose();
hCostArray.Dispose();
gCostArray.Dispose();
fCostArray.Dispose();
openArray.Dispose();
closedArray.Dispose();
return(pathNodes);
}
#endregion
}
private FixedList4096 <int2> FindLowLevelPathInCluster(int2 startPosition, int2 endPosition, int2 clusterPosition)
{
#region DATA SETUP
int PATH_NODE_ARRAY_SIZE = graphClusterSize * graphClusterSize;
// Node sets
NativeMinHeap openSet = new NativeMinHeap();
openSet.Initialize(PATH_NODE_ARRAY_SIZE, -1, PATH_NODE_ARRAY_SIZE + 1);
NativeArray <int> graphIndexArray = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
NativeArray <int> parentArray = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
NativeArray <int> hCostArray = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);
NativeArray <int> gCostArray = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.UninitializedMemory);
NativeArray <int> fCostArray = new NativeArray <int>(PATH_NODE_ARRAY_SIZE, Allocator.Temp);
NativeArray <bool> openArray = new NativeArray <bool>(PATH_NODE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.ClearMemory);
NativeArray <bool> closedArray = new NativeArray <bool>(PATH_NODE_ARRAY_SIZE, Allocator.Temp, NativeArrayOptions.ClearMemory);
int intValue = -1;
void *pointer = ( void * )&intValue;
UnsafeUtility.MemCpyReplicate(parentArray.GetUnsafePtr(), pointer, sizeof(int), PATH_NODE_ARRAY_SIZE);
intValue = int.MaxValue;
UnsafeUtility.MemCpyReplicate(gCostArray.GetUnsafePtr(), pointer, sizeof(int), PATH_NODE_ARRAY_SIZE);
/*for ( int localRow = 0; localRow < graphClusterSize; localRow++ )
* {
* for ( int localCol = 0; localCol < graphClusterSize; localCol++ )
* {
* int2 localPos = new int2( localCol , localRow );
* int2 graphPos = localPos + graphClusterPos;
*
* int localIndex = localCol + localRow * graphClusterSize;
* int graphArrayIndex = graphPos.x + graphPos.y * graphCellLength;
*
* graphIndexArray[ localIndex ] = graphArrayIndex;
* }
* }*/
int2 graphClusterPos = graphClusterSize * clusterPosition;
int4 graphClusterX = new int4(clusterPosition.x * graphClusterSize);
int4 graphClusterY = new int4(clusterPosition.y * graphClusterSize);
int4 indexOffest = new int4(0, 1, 2, 3);
int vectorLoopLength = graphClusterSize - 4;
for (int localRow = 0; localRow < graphClusterSize; localRow++)
{
int localCol = 0;
for ( ; localCol < vectorLoopLength; localCol += 4)
{
int localIndex = localCol + localRow * graphClusterSize;
int4 localCol4 = new int4(localCol);
int4 localRow4 = new int4(localRow);
int4 graphX = localCol4 + indexOffest + graphClusterX;
int4 graphY = localRow4 + graphClusterY;
int4 graphArrayIndex = graphX + graphY * graphCellLength;
graphIndexArray.ReinterpretStore <int4>(localIndex, graphArrayIndex);
}
for ( ; localCol < graphClusterSize; localCol++)
{
int2 localPos = new int2(localCol, localRow);
int2 graphPos = localPos + graphClusterPos;
int localIndex = localCol + localRow * graphClusterSize;
int graphArrayIndex = graphPos.x + graphPos.y * graphCellLength;
graphIndexArray[localIndex] = graphArrayIndex;
}
}
// Get and cache the start and end pathNodeIndices
int2 clusterNodePos = clusterPosition * graphClusterSize;
int2 endNodePos = endPosition - clusterNodePos;
int2 startNodePos = startPosition - clusterNodePos;
int endNodeIndex = endNodePos.x + endNodePos.y * graphClusterSize;
int startNodeIndex = startNodePos.x + startNodePos.y * graphClusterSize;
// Initialize the starting pathNode
int hCost = ManhattenDistance(graphNodePositions[graphIndexArray[startNodeIndex]], endPosition);
gCostArray[startNodeIndex] = 0;
hCostArray[startNodeIndex] = hCost;
fCostArray[startNodeIndex] = hCost;
openArray[startNodeIndex] = true;
// Add the starting node to the open set
openSet.Enqueue(startNodeIndex, hCost);
#endregion
#region SEARCH GRAPH
while (openSet.Length > 0)
{
// Cache the pathNodeIndex we are working with during this iteration
int currentNodeIndex = openSet.DequeueMin(closedArray);
openArray[currentNodeIndex] = false;
closedArray[currentNodeIndex] = true;
// Break if we reached our goal
if (currentNodeIndex == endNodeIndex)
{
break;
}
int2 currentNodePosition = graphNodePositions[graphIndexArray[currentNodeIndex]];
int2 neighbourKey = graphNodeNeighbourKeys[graphIndexArray[currentNodeIndex]];
int i = neighbourKey.x;
int vectorLength = neighbourKey.y - 4;
/*for ( ; i < neighbourKey.y; i++ )
* {
* int2 neighbourPosition = graphNodePositions[ graphNodeNeighbours[ i ] ];
* int2 localNeighbourPosition = neighbourPosition - clusterPosition * graphClusterSize;
* int localNeighbourIndex = localNeighbourPosition.x + localNeighbourPosition.y * graphClusterSize;
*
* // Skip if its closed (already searched)
* if ( closedArray[ localNeighbourIndex ] )
* continue;
*
* // Calculate the cost to move from current node to neighbour node
* int distanceCost = ManhattenDistance( currentNodePosition , neighbourPosition );
* int tentativeCost = gCostArray[ currentNodeIndex ] + distanceCost;
*
* if ( tentativeCost < gCostArray[ localNeighbourIndex ] )
* {
* int newHCost = ManhattenDistance( neighbourPosition , endPosition );
*
* parentArray[ localNeighbourIndex ] = currentNodeIndex;
* hCostArray[ localNeighbourIndex ] = newHCost;
* gCostArray[ localNeighbourIndex ] = tentativeCost;
* fCostArray[ localNeighbourIndex ] = tentativeCost + hCost;
*
* if ( !openArray[ localNeighbourIndex ] )
* {
* openArray[ localNeighbourIndex ] = true;
* openSet.Enqueue( localNeighbourIndex , fCostArray[ localNeighbourIndex ] );
* }
* }
* }*/
/*int4 clusterXScaled = new int4( clusterPosition.x * graphClusterSize );
* int4 clusterYScaled = new int4( clusterPosition.y * graphClusterSize );
*
* for ( ; i < vectorLength; i += 4 )
* {
* int2x4 neighbourPosition = graphNodePositions.ReinterpretLoad<int2x4>( graphNodeNeighbours[ i ] );
* int4 neighbourX = new int4( neighbourPosition.c0.x , neighbourPosition.c1.x , neighbourPosition.c2.x , neighbourPosition.c3.x );
* int4 neighbourY = new int4( neighbourPosition.c0.y , neighbourPosition.c1.y , neighbourPosition.c2.y , neighbourPosition.c3.y );
*
* int4 localNeighbourX = neighbourX - clusterXScaled;
* int4 localNeighbourY = neighbourY - clusterYScaled;
* int4 localNeighbourIndex = localNeighbourX + localNeighbourY * graphClusterSize;
*
* int4 closedArrayBranch = math.select( new int4( 1 ) , new int4( 0 ) , closedArray.ReinterpretLoad<bool4>( localNeighbourIndex.x ) );
*
* int4 differenceX = neighbourX - new int4( currentNodePosition.x );
* int4 differenceY = neighbourY - new int4( currentNodePosition.y );
*
* int4 distanceCost = math.abs( differenceX ) + math.abs( differenceY );
* int4 tentativeCost = new int4( gCostArray[ currentNodeIndex ] + distanceCost );
*
* int4 gcost = gCostArray.ReinterpretLoad<int4>( localNeighbourIndex.x ) * closedArrayBranch;
* bool4 lesser = tentativeCost < gcost;
*
* if ( lesser.x )
* {
* int newHCost = ManhattenDistance( neighbourPosition.c0 , endPosition );
*
* parentArray[ localNeighbourIndex.x ] = currentNodeIndex;
* hCostArray[ localNeighbourIndex.x ] = newHCost;
* gCostArray[ localNeighbourIndex.x ] = tentativeCost.x;
* fCostArray[ localNeighbourIndex.x ] = tentativeCost.x + hCost;
*
* if ( !openArray[ localNeighbourIndex.x ] )
* {
* openArray[ localNeighbourIndex.x ] = true;
* openSet.Enqueue( localNeighbourIndex.x , fCostArray[ localNeighbourIndex.x ] );
* }
* }
* if ( lesser.y )
* {
* int newHCost = ManhattenDistance( neighbourPosition.c1 , endPosition );
*
* parentArray[ localNeighbourIndex.y ] = currentNodeIndex;
* hCostArray[ localNeighbourIndex.y ] = newHCost;
* gCostArray[ localNeighbourIndex.y ] = tentativeCost.y;
* fCostArray[ localNeighbourIndex.y ] = tentativeCost.y + hCost;
*
* if ( !openArray[ localNeighbourIndex.y ] )
* {
* openArray[ localNeighbourIndex.y ] = true;
* openSet.Enqueue( localNeighbourIndex.y , fCostArray[ localNeighbourIndex.y ] );
* }
* }
* if ( lesser.z )
* {
* int newHCost = ManhattenDistance( neighbourPosition.c2 , endPosition );
*
* parentArray[ localNeighbourIndex.z ] = currentNodeIndex;
* hCostArray[ localNeighbourIndex.z ] = newHCost;
* gCostArray[ localNeighbourIndex.z ] = tentativeCost.z;
* fCostArray[ localNeighbourIndex.z ] = tentativeCost.z + hCost;
*
* if ( !openArray[ localNeighbourIndex.z ] )
* {
* openArray[ localNeighbourIndex.z ] = true;
* openSet.Enqueue( localNeighbourIndex.z , fCostArray[ localNeighbourIndex.z ] );
* }
* }
* if ( lesser.w )
* {
* int newHCost = ManhattenDistance( neighbourPosition.c3 , endPosition );
*
* parentArray[ localNeighbourIndex.w ] = currentNodeIndex;
* hCostArray[ localNeighbourIndex.w ] = newHCost;
* gCostArray[ localNeighbourIndex.w ] = tentativeCost.w;
* fCostArray[ localNeighbourIndex.w ] = tentativeCost.w + hCost;
*
* if ( !openArray[ localNeighbourIndex.w ] )
* {
* openArray[ localNeighbourIndex.w ] = true;
* openSet.Enqueue( localNeighbourIndex.w , fCostArray[ localNeighbourIndex.w ] );
* }
* }
* }*/
for ( ; i < neighbourKey.y; i++)
{
int2 neighbourPosition = graphNodePositions[graphNodeNeighbours[i]];
int2 localNeighbourPosition = neighbourPosition - clusterPosition * graphClusterSize;
int localNeighbourIndex = localNeighbourPosition.x + localNeighbourPosition.y * graphClusterSize;
// Skip if its closed (already searched)
if (closedArray[localNeighbourIndex])
{
continue;
}
// Calculate the cost to move from current node to neighbour node
int distanceCost = ManhattenDistance(currentNodePosition, neighbourPosition);
int tentativeCost = gCostArray[currentNodeIndex] + distanceCost;
if (tentativeCost < gCostArray[localNeighbourIndex])
{
int newHCost = ManhattenDistance(neighbourPosition, endPosition);
parentArray[localNeighbourIndex] = currentNodeIndex;
hCostArray[localNeighbourIndex] = newHCost;
gCostArray[localNeighbourIndex] = tentativeCost;
fCostArray[localNeighbourIndex] = tentativeCost + newHCost;
if (!openArray[localNeighbourIndex])
{
openArray[localNeighbourIndex] = true;
openSet.Enqueue(localNeighbourIndex, fCostArray[localNeighbourIndex]);
}
}
}
}
#endregion
#region TRACE PATH
NativeList <int2> path = new NativeList <int2>(Allocator.Temp);
FixedList4096 <int2> smoothedPath = new FixedList4096 <int2>(); //Allocator.Temp );
int nodeIndex = endNodeIndex;
if (parentArray[endNodeIndex] == -1)
{
openSet.Dispose();
graphIndexArray.Dispose();
parentArray.Dispose();
hCostArray.Dispose();
gCostArray.Dispose();
fCostArray.Dispose();
openArray.Dispose();
closedArray.Dispose();
return(smoothedPath);
}
else
{
smoothedPath.Add(endPosition);
path.Add(endPosition);
while (parentArray[nodeIndex] != -1)
{
path.Add(graphNodePositions[graphIndexArray[parentArray[nodeIndex]]]);
nodeIndex = parentArray[nodeIndex];
}
}
#endregion
#region SMOOTH PATH
if (path.Length > 2) // If its less than or equal 2 theres no need to smooth the path
{
int fromIndex = 0;
int currentIndex = fromIndex + 2;
int stopIndex = path.Length - 1;
while (currentIndex <= stopIndex)
{
int2 start = path[fromIndex];
int2 end = path[currentIndex];
if (!LOS_Node(start, end))
{
int nextFromIndex = currentIndex - 1;
int graphNodeArrayIndex = path[nextFromIndex].x + path[nextFromIndex].y * graphCellLength;
smoothedPath.Add(graphNodePositions[graphNodeArrayIndex]);
fromIndex = nextFromIndex;
}
currentIndex++;
}
}
#endregion
#region RETURN
openSet.Dispose();
graphIndexArray.Dispose();
parentArray.Dispose();
hCostArray.Dispose();
gCostArray.Dispose();
fCostArray.Dispose();
openArray.Dispose();
closedArray.Dispose();
path.Dispose();
return(smoothedPath);
#endregion
}
