protected ListCopyable <Node> AstarSearch(Graph graph, ListCopyable <Node> visited, Node startNode, Node endNode, Constraint constraint, int threadIndex)
{
var openList = PoolQueue <Node> .Spawn(10);
startNode.startToCurNodeLen[threadIndex] = 0f;
openList.Enqueue(startNode);
startNode.isOpened[threadIndex] = true;
while (openList.Count > 0)
{
var node = openList.Dequeue();
node.isClosed[threadIndex] = true;
visited.Add(node);
if (node.index == endNode.index)
{
PoolQueue <Node> .Recycle(ref openList);
return(this.RetracePath(threadIndex, endNode));
}
var neighbors = node.GetConnections();
foreach (var conn in neighbors)
{
if (conn.index < 0)
{
continue;
}
var neighbor = graph.nodes[conn.index];
if (neighbor.isClosed[threadIndex] == true)
{
continue;
}
if (neighbor.IsSuitable(constraint) == false)
{
continue;
}
float ng = node.startToCurNodeLen[threadIndex] + conn.cost;
if (neighbor.isOpened[threadIndex] == false || ng < neighbor.startToCurNodeLen[threadIndex])
{
neighbor.startToCurNodeLen[threadIndex] = ng;
neighbor.parent[threadIndex] = node;
if (neighbor.isOpened[threadIndex] == false)
{
openList.Enqueue(neighbor);
visited.Add(neighbor);
neighbor.isOpened[threadIndex] = true;
}
}
}
}
PoolQueue <Node> .Recycle(ref openList);
return(null);
}
public static void Copy <T, TCopy>(ListCopyable <T> fromArr, ref ListCopyable <T> arr, TCopy copy) where TCopy : IArrayElementCopy <T>
{
if (fromArr == null)
{
if (arr != null)
{
for (int i = 0; i < arr.Count; ++i)
{
copy.Recycle(arr[i]);
}
PoolListCopyable <T> .Recycle(ref arr);
}
arr = null;
return;
}
if (arr == null || fromArr.Count != arr.Count)
{
if (arr != null)
{
ArrayUtils.Recycle(ref arr, copy);
}
arr = PoolListCopyable <T> .Spawn(fromArr.Count);
}
var cnt = arr.Count;
for (int i = 0; i < fromArr.Count; ++i)
{
var isDefault = i >= cnt;
T item = (isDefault ? default : arr[i]);
copy.Copy(fromArr[i], ref item);
if (isDefault == true)
{
arr.Add(item);
}
else
{
arr[i] = item;
}
}
}