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