/// <summary>
/// Modifies this container to contain values from both containers.
/// </summary>
/// <typeparam name="T">Source type of elements</typeparam>
/// <param name="container">Container to modify.</param>
/// <param name="other">The container to compare to this container.</param>
public static void UnionWith <T>(this UnsafeHashSet <T> container, FixedList4096 <T> other)
where T : unmanaged, IEquatable <T>
{
foreach (var item in other)
{
container.Add(item);
}
}
/// <summary>
/// Modifies this container to remove all values that are present in the other container.
/// </summary>
/// <typeparam name="T">Source type of elements</typeparam>
/// <param name="container">Container to modify.</param>
/// <param name="other">The container to compare to this container.</param>
public static void ExceptWith <T>(this NativeHashSet <T> container, FixedList4096 <T> other)
where T : unmanaged, IEquatable <T>
{
foreach (var item in other)
{
container.Remove(item);
}
}
/// <summary>
/// Modifies this container to keep only values that are present in both containers.
/// </summary>
/// <typeparam name="T">Source type of elements</typeparam>
/// <param name="container">Container to modify.</param>
/// <param name="other">The container to compare to this container.</param>
public static void IntersectWith <T>(this UnsafeHashSet <T> container, FixedList4096 <T> other)
where T : unmanaged, IEquatable <T>
{
var result = new UnsafeList <T>(container.Count(), Allocator.Temp);
foreach (var item in other)
{
if (container.Contains(item))
{
result.Add(item);
}
}
container.Clear();
container.UnionWith(result);
result.Dispose();
}
private NativeList <int2> FindConcretePathFromAbstract(NativeList <AbstractNode> abstractPath)
{
NativeList <int2> concretePath = new NativeList <int2>(Allocator.Temp);
#region START
int fromIndex = 0; // endPosition
int toIndex = 2; // endIntra, there is an inter before this
int2 endNode = abstractPath[0].position;
int2 endInter = abstractPath[1].position;
int2 endCluster = endNode / graphClusterSize;
concretePath.Add(endNode);
#endregion
#region END CLUSTER PATH
// Try to find low-level path in end cluster
int endClusterIndex = endCluster.x + endCluster.y * graphClusterSize;
if (graphClustersDense[endClusterIndex] != 0)
{
FixedList4096 <int2> lowLevelPath = FindLowLevelPathInCluster(endInter, endNode, endCluster);
if (lowLevelPath.Length == 0)
{
//lowLevelPath.Dispose();
concretePath.Dispose();
return(new NativeList <int2>());
}
for (int i = 0; i < lowLevelPath.Length; i++)
{
concretePath.Add(lowLevelPath[i]);
}
// if we find a path in the last cluster, we then start from the first index in the prevous cluster
// this is endPos -> endInter -> endIntra = fromIndex # 2
// we test to prev cluster which is endIntra -> prevIntra = toIndex = # 3
fromIndex = 2;
toIndex = 3;
}
else
{
//toIndex++;
}
#endregion
#region FILL ABSTRACT PATH
// We want to stop testing clusters once we reach the first one. We need to be able to
// find a low level path in the first cluster from the first intra-index to the start position
// We also want to be able test test if clusters are clear from the fromIndex to the startPosition
// if (start cluster is not dense, raycast from from to start)
// else (findLowLevel path from toIndex - 1 to startPosition)
// therefore, stopPoint is when toIndex == startPosition
int lastIndex = abstractPath.Length - 1;
while (toIndex < lastIndex)
{
#region CHECK FROM CLUSTER
// Check if our from cluster is dense
// If it is, add the cached path and then move the from and start positions up (back) one
int2 fromPosition = abstractPath[fromIndex].position;
int2 clusterPosition = fromPosition / graphClusterSize;
int clusterIndex = clusterPosition.x + clusterPosition.y * graphClusterSize;
if (graphClustersDense[clusterIndex] != 0)
{
int2 cachedPathKey = graphEdgePaths[abstractPath[fromIndex].path];
for (int i = cachedPathKey.x; i < cachedPathKey.y; i++)
{
concretePath.Add(graphEdgePathPositions[i]);
}
//fromIndex++;
//toIndex++;
}
#endregion
#region RAYCAST CLUSTERS LOOP
// Raycast until we are blocked or reach the end
// If we get blocked, make the from position = toPosition - 1
bool notBlockedOrAtEnd = true;
while (notBlockedOrAtEnd)
{
int2 start = abstractPath[fromIndex].position;
int2 end = abstractPath[toIndex].position;
if (!LOS_Cluster(start, end))
{
concretePath.Add(abstractPath[toIndex].position);
fromIndex = toIndex;
toIndex++;
notBlockedOrAtEnd = false;
}
else
{
toIndex++;
if (toIndex >= lastIndex)
{
notBlockedOrAtEnd = false;
}
}
}
#endregion
}
#endregion
#region START CLUSTER PATH
int2 toPosition = abstractPath[toIndex - 1].position;
int2 startPosition = abstractPath[lastIndex].position;
int2 firstClusterPosition = startPosition / graphClusterSize;
int firstClusterIndex = firstClusterPosition.x + firstClusterPosition.y * graphClusterSize;
if (graphClustersDense[firstClusterIndex] != 0)
{
FixedList4096 <int2> lowLevelPath = FindLowLevelPathInCluster(startPosition, toPosition, firstClusterPosition);
if (lowLevelPath.Length > 0)
{
for (int i = 0; i < lowLevelPath.Length; i++)
{
concretePath.Add(lowLevelPath[i]);
}
}
else
{
//lowLevelPath.Dispose();
concretePath.Dispose();
return(new NativeList <int2>(Allocator.Temp));
}
}
else
{
int2 endPosition = abstractPath[lastIndex].position;
concretePath.Add(endPosition);
}
#endregion
return(concretePath);
}
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
}
public void Execute(int jobIndex)
{
Entity entity = entities[jobIndex];
float2 startGridPositionAsFloat = startWorldPositions[jobIndex] / graphCellSize;
float2 endGridPositionAsFloat = endWorldPositions[jobIndex] / graphCellSize;
int2 startGridPosition = ( int2 )math.round(startGridPositionAsFloat);
int2 endGridPosition = ( int2 )math.round(endGridPositionAsFloat);
int2 startClusterPosition = startGridPosition / graphClusterSize;
int2 endClusterPosition = endGridPosition / graphClusterSize;
pathfindingStateComponentData[entity] = new FormationPathFindingState {
Value = PathfindingState.free
};
// If the start and end positions are in different clusters
if (!startClusterPosition.Equals(endClusterPosition))
{
int clusterIndex = startClusterPosition.x + startClusterPosition.y * graphNumClusters;
int2 clusterEdgesKey = graphClusterEdgesLists[clusterIndex];
// If there are no paths from start position to any of the edges in current cluster, break out
if (clusterEdgesKey.y - clusterEdgesKey.x > 0)
{
NativeList <AbstractNode> abstractPath = FindAbstractPathInGraph(clusterEdgesKey, startGridPosition, endGridPosition);
if (abstractPath.Length > 0)
{
pathPositionBufferEntity[entity].Clear();
pathPositionBufferEntity[entity].Add(new PathPosition {
Value = endWorldPositions[jobIndex]
});
NativeList <int2> concretePath = FindConcretePathFromAbstract(abstractPath);
for (int i = 1; i < concretePath.Length; i++)
{
float2 pathPosition = concretePath[i] * graphCellSize;
pathPositionBufferEntity[entity].Add(new PathPosition {
Value = pathPosition
});
}
pathIndexComponentData[entity] = new PathIndex {
Value = pathPositionBufferEntity[entity].Length - 1
};
concretePath.Dispose();
abstractPath.Dispose();
return;
}
abstractPath.Dispose();
}
}
else // OTHERWISE, the start and end positions are in the same cluster, so just do a quick low-level a* search
{
FixedList4096 <int2> path = FindLowLevelPathInCluster(startGridPosition, endGridPosition, startClusterPosition);
if (path.Length > 0)
{
pathPositionBufferEntity[entity].Clear();
pathPositionBufferEntity[entity].Add(new PathPosition {
Value = endWorldPositions[jobIndex]
});
for (int i = 1; i < path.Length; i++)
{
float2 pathPosition = path[i] * graphCellSize;
pathPositionBufferEntity[entity].Add(new PathPosition {
Value = pathPosition
});
}
pathIndexComponentData[entity] = new PathIndex {
Value = pathPositionBufferEntity[entity].Length - 1
};
return;
}
}
// If no path was found we will reach here
return;
}
