/// <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; }