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