public List <Vector3> FindPath(Vector3 from, Vector3 to)
{
ClearPath();
foreach (NavSeqAStar s in g_Pathfinders)
{
s.ClearPath();
}
if (DynamicWeight)
{
HeuristicWeight = 1.0f;
}
RaycastHit hit;
g_PathList = new List <Vector3>();
if (Physics.Raycast(new Ray(transform.position + (transform.up * 0.2f), -1 * transform.up), out hit))
{
// Ensure we are on a grid
g_Grid = hit.collider.GetComponent <NavGrid>();
if (CleanPathOnRestart)
{
g_Grid.CleanAll();
}
g_FromNode = g_FromNode == null?g_Grid.GetOccupiedNode(this) : g_FromNode;
if (g_FromNode == null)
{
g_NPCController.Debug("NavAStar --> Agent is currently navigating in between nodes, try again please");
return(g_PathList);
}
/* Initialize all heuristics here */
InitializeFinder(g_FromNode, to, g_Grid);
foreach (NavSeqAStar s in g_Pathfinders)
{
s.InitializeFinder(g_FromNode, to, g_Grid);
}
while (g_OpenList.Count > 0)
{
// get next optimal node
NavNode n = g_Fringe.Values[0];
// Then Heuristic-based search
foreach (NavSeqAStar nav in g_Pathfinders)
{
float va = nav.g_gVal.Count == 0 ? float.MaxValue : nav.g_gVal[nav.g_Fringe.Values[0]];
if (va < (nav.g_SecondaryHeuristicsWeight * g_gVal[g_Fringe.Values[0]]))
{
if (IsCurrentStateGoal(new System.Object[] { nav.g_Fringe.Values[0] }))
{
g_Winner = "Heuristic won: " + nav.Name();
n = nav.g_Fringe.Values[0];
n.SetHighlightTile(true, Color.green, 1f);
g_GoalNode = n;
g_PathList = ConstructPath(n, nav.g_Parents);
g_CurrentPathValue = nav.g_gVal[n];
g_NPCController.Debug(g_Winner);
goto exit_pathfinding;
}
else
{
NavNode s = nav.g_Fringe.Values[0];
nav.ExpandNode(s);
nav.g_ClosedList.Add(s);
}
}
else if (IsCurrentStateGoal(new System.Object[] { n }))
{
g_Winner = "Search won: " + nav.Name();
n.SetHighlightTile(true, Color.green, 1f);
g_GoalNode = n;
g_PathList = ConstructPath(g_GoalNode, g_Parents);
g_CurrentPathValue = g_gVal[n];
g_NPCController.Debug(g_Winner);
goto exit_pathfinding;
}
else
{
ExpandNode(n);
g_ClosedList.Add(n);
}
}
}
}
else
{
g_NPCController.Debug("NavAStar --> Pathfinder not on grid");
}
exit_pathfinding:
g_FromNode = null;
return(g_PathList);
}