List <Vector3> AStarPath(Vector3 s, Vector3 f)
{
AAPrism startPrism = null, endPrism = null;
List <NavNode> list = new List <NavNode>(navNodes); //copy the list so we can modify it non-destructively
foreach (var prism in a) //find the containing prism for start and finish
{
if (prism.bounds.Contains(s)) //TODO optimize with sort and end loop
{
prism.Display(Color.green);
startPrism = prism;
}
if (prism.bounds.Contains(f))
{
prism.Display(Color.red);
endPrism = prism;
}
}
//TODO add nullchecking on prisms to see if off grid
if (startPrism == endPrism) //if they are in the same prism then we can travel direct
{
return(new List <Vector3> {
s, f
});
}
NavNode startNode = new NavNode(s); //add start and finish to list
NavNode endNode = new NavNode(f);
list.Add(startNode);
list.Add(endNode);
foreach (var location in startPrism.GetAllPoints()) //update neighbours
{
NavNode node = getNodeAt(list, location);
node.AddNeighbour(startNode);
startNode.AddNeighbour(node);
}
foreach (var location in endPrism.GetAllPoints())
{
NavNode node = getNodeAt(list, location);
node.AddNeighbour(endNode);
endNode.AddNeighbour(node);
}
foreach (var item in list) //prepare to pathfind
{
item.H = (item.position - endNode.position).sqrMagnitude; //TODO use regular mag to stop exponent growth?
item.parent = null;
item.state = NodeState.Untested;
}
List <Vector3> path = new List <Vector3>();
if (PathSearch(startNode, endNode)) //pathfind
{
Debug.Log("Path found!");
path = ReturnPath(endNode);
for (int i = 0; i < path.Count - 1; i++)
{
DebugExtension.DebugPoint(path[i], Color.magenta, .5f);
Debug.DrawLine(path[i], path[i + 1], Color.cyan);
}
SmoothPath(path);
for (int i = 0; i < path.Count - 1; i++)
{
DebugExtension.DebugPoint(path[i], Color.magenta, .5f);
Debug.DrawLine(path[i], path[i + 1], Color.yellow);
}
}
else
{
Debug.Log("Failed to find path!");
}
return(path);
}
