public static IEnumerator A_Star(int startNode, int endNode, GroundGrid groundGrid, List <int> outPath)
{
int numNodes = groundGrid.AdjacencyMatrix.GetLength(0);
int numNeighbors = groundGrid.AdjacencyMatrix.GetLength(1);
//Debug.Log("num nodes: " + numNodes);
//Debug.Log("num neighbors: " + numNeighbors);
List <int> toVisit = new List <int>();
List <int> visited = new List <int>();
int[] parent = new int[numNodes];
float[] gCost = new float[numNodes];
float[] fCost = new float[numNodes];
for (int i = 0; i < numNodes; ++i)
{
parent[i] = -1;
fCost[i] = gCost[i] = float.PositiveInfinity;
}
toVisit.Add(startNode);
gCost[startNode] = 0.0f;
fCost[startNode] = groundGrid.Heuristic(startNode, endNode);
int count = 0;
while (toVisit.Any())
{
int cur = A_Star_getLowestCost(toVisit, fCost);
if (cur == endNode)
{
foreach (int item in CreatePathFromParent(endNode, parent))
{
outPath.Add(item);
}
groundGrid.DisplayPath(outPath);
yield break;
}
toVisit.Remove(cur);
visited.Add(cur);
groundGrid.SetNodeExplored(cur, true);
for (int i = 0; i < numNeighbors; ++i)
{
int neighbor = groundGrid.ConvertNeighborIndexToNodeIndex(cur, i);
if (groundGrid.AdjacencyMatrix[cur, i] == Mathf.Infinity || visited.Contains(neighbor) || neighbor == cur || neighbor == -1)
{
continue;
}
float gScore = gCost[cur] + groundGrid.AdjacencyMatrix[cur, i];
if (gScore < gCost[neighbor])
{
parent[neighbor] = cur;
gCost[neighbor] = gScore;
fCost[neighbor] = gCost[neighbor] + groundGrid.Heuristic(neighbor, endNode);
if (!toVisit.Contains(neighbor))
{
toVisit.Add(neighbor);
}
}
}
count++;
const int nodesExploredPerFrame = 50;
if (count >= nodesExploredPerFrame)
{
count -= nodesExploredPerFrame;
yield return(null);
}
}
}
public static IEnumerator RRT(int startNode, int endNode, GroundGrid groundGrid, Actor actor, List <int> outPath)
{
outPath.Clear();
foreach (Transform child in groundGrid.transform)
{
if (child.name == "Line")
{
GameObject.Destroy(child.gameObject);
}
}
int numNodes = groundGrid.Rows * groundGrid.Columns;
const int numAttempts = 2000;
int[] parent = new int[numNodes];
List <int> possibleNodesToPick = new List <int>();
for (int i = 0; i < numNodes; ++i)
{
if (!groundGrid.NodeIsOccupied(i))
{
possibleNodesToPick.Add(i);
}
parent[i] = -1;
}
List <int> nodes = new List <int>();
nodes.Add(startNode);
possibleNodesToPick.Remove(startNode);
for (int i = 0; i < numAttempts; ++i)
{
if (possibleNodesToPick.Count == 0)
{
yield break;
}
int randNode = RRT_GetRandomState(endNode, possibleNodesToPick);
int nearestNeighbor = RRT_GetNearestNeighbor(randNode, nodes, groundGrid);
//Select input to use
//For now any input is valid
//Determine new state
int newNode = -1;
if (RRT_StepTowards(nearestNeighbor, randNode, ref newNode, groundGrid, actor))
{
possibleNodesToPick.Remove(newNode);
nodes.Add(newNode);
DrawLine(groundGrid.GetNodePosition(nearestNeighbor), groundGrid.GetNodePosition(newNode), Color.red, groundGrid.transform, -1);
parent[newNode] = nearestNeighbor;
if (newNode == endNode)
{
foreach (int item in CreatePathFromParent(endNode, parent))
{
outPath.Add(item);
}
groundGrid.DisplayPath(outPath);
yield break;
}
groundGrid.SetNodeExplored(newNode, true);
}
else
{
continue;
}
yield return(null);
}
yield break;
}