public List <Vector3> ConstructPath(NavNode goal, Dictionary <NavNode, NavNode> parents) { List <Vector3> path = new List <Vector3>(parents.Count + 1); List <NavNode> pathToPrint = new List <NavNode>(parents.Count + 1); path.Insert(0, goal.Position); pathToPrint.Insert(0, goal); bool done = false; NavNode curr = goal; g_PathLengthNodes++; while (!done) { if (parents[curr].Equals(curr)) { done = true; } curr.SetHighlightTile(true, curr == goal ? Color.green : Color.yellow, 0.8f); curr = parents[curr]; path.Insert(0, curr.Position); pathToPrint.Insert(0, curr); g_PathLengthNodes++; if (g_PathLengthNodes > Mathf.Pow(Mathf.Max(g_Grid.transform.localScale.x, g_Grid.transform.localScale.z), 3.0f)) { throw new System.Exception("Impossible to construct path"); } } if (g_Grid != null) { g_Grid.WritePathToFile(pathToPrint); } return(path); }
public void DryRunAlgorithm() { if (DryRunAlgo) { RaycastHit hit; List <Vector3> pathList = new List <Vector3>(); if (Physics.Raycast(new Ray(transform.position + (transform.up * 0.2f), -1 * transform.up), out hit)) { g_Grid = hit.collider.GetComponent <NavGrid>(); int succeed = 0; while (succeed < DryRunTests) { int x = Mathf.RoundToInt(UnityEngine.Random.Range(0, g_Grid.GridDimensions.x - 1)), y = Mathf.RoundToInt(UnityEngine.Random.Range(0, g_Grid.GridDimensions.y - 1)); g_FromNode = g_Grid.GetGridNode(x, y); if (g_FromNode != null) { NavNode to = null; do { x = Mathf.RoundToInt(UnityEngine.Random.Range(0, g_Grid.GridDimensions.x - 1)); y = Mathf.RoundToInt(UnityEngine.Random.Range(0, g_Grid.GridDimensions.x - 1)); to = g_Grid.GetGridNode(x, y); } while (to == null); g_FromNode.SetHighlightTile(true, Color.black, 1f); FindPath(g_FromNode.Position, to.Position); succeed++; } } } g_NPCController.Debug("Finished algorithm dry run"); } DryRunAlgo = false; g_FromNode = null; }
public void AddINPCPathfinderNode(INPCPathfinder ipf, NavNode node) { if (g_WalkedOnNodes.ContainsKey(ipf) && g_WalkedOnNodes[ipf] != node) { if (Application.isPlaying) { g_WalkedOnNodes[ipf].SetHighlightTile(false, Color.grey, 0.5f); } g_WalkedOnNodes.Remove(ipf); } else if (!g_WalkedOnNodes.ContainsKey(ipf)) { g_WalkedOnNodes.Add(ipf, node); if (Application.isPlaying) { node.SetHighlightTile(true, Color.red, 0.5f); } } }
public List <Vector3> ConstructPath(NavNode goal, Dictionary <NavNode, NavNode> parents) { List <Vector3> path = new List <Vector3>(parents.Count + 1); List <NavNode> pathToPrint = new List <NavNode>(parents.Count + 1); path.Insert(0, goal.Position); pathToPrint.Insert(0, goal); bool done = false; NavNode curr = goal; g_PathLengthNodes++; while (!done) { if (parents.ContainsKey(curr)) { if (parents[curr].Equals(curr)) { done = true; } curr.SetHighlightTile(true, curr == goal ? Color.green : Color.yellow, 0.8f); curr = parents[curr]; path.Insert(0, curr.Position); pathToPrint.Insert(0, curr); g_PathLengthNodes++; } else { done = true; } } if (g_Grid != null) { g_Grid.WritePathToFile(pathToPrint); } return(path); }
public void Benchmark() { if (BenchmarkAll) { List <float> times = new List <float>(); for (int r = 0; r < 5; r++) { float globalBenchMarkTime = Time.realtimeSinceStartup; Dictionary <string, int> hCounter = new Dictionary <string, int>(); Dictionary <string, float> timeAvg = new Dictionary <string, float>(); RaycastHit hit; List <Vector3> pathList = new List <Vector3>(); if (Physics.Raycast(new Ray(transform.position + (transform.up * 0.2f), -1 * transform.up), out hit)) { g_Grid = hit.collider.GetComponent <NavGrid>(); int succeed = 0; while (succeed < DryRunTests) { int x = Mathf.RoundToInt(UnityEngine.Random.Range(0, g_Grid.GridDimensions.x - 1)), y = Mathf.RoundToInt(UnityEngine.Random.Range(0, g_Grid.GridDimensions.y - 1)); g_FromNode = g_Grid.GetGridNode(x, y); if (g_FromNode != null) { NavNode to = null; do { x = Mathf.RoundToInt(UnityEngine.Random.Range(0, g_Grid.GridDimensions.x - 1)); y = Mathf.RoundToInt(UnityEngine.Random.Range(0, g_Grid.GridDimensions.x - 1)); to = g_Grid.GetGridNode(x, y); } while (to == null); NavNode from = g_FromNode; // benchmark each path for (int i = 0; i < 10; ++i) { string log = ""; float w2 = 1f; if (i == 0) { foreach (NavSeqAStar n in g_Pathfinders) { n.g_SecondaryHeuristicsWeight = w2; } } else if (i == 1) { foreach (NavSeqAStar n in g_Pathfinders) { w2 += 0.25f; n.g_SecondaryHeuristicsWeight = w2; } } else if (i == 2) { foreach (NavSeqAStar n in g_Pathfinders) { w2 += 0.5f; n.g_SecondaryHeuristicsWeight = w2; } } else if (i == 3) { foreach (NavSeqAStar n in g_Pathfinders) { w2 += 0.75f; n.g_SecondaryHeuristicsWeight = w2; } } else { foreach (NavSeqAStar n in g_Pathfinders) { w2 += 1f; n.g_SecondaryHeuristicsWeight = w2; } } float now = Time.realtimeSinceStartup; from.SetHighlightTile(true, Color.black, 1f); FindPath(from.Position, to.Position); if (hCounter.ContainsKey(g_Winner)) { hCounter[g_Winner] += 1; } else { hCounter.Add(g_Winner, 1); } log += "W2: " + w2 + "\n"; float t = (Time.realtimeSinceStartup - now); log += "Time: " + t + "ms \n"; if (timeAvg.ContainsKey(g_Winner)) { timeAvg[g_Winner] += t; } else { timeAvg.Add(g_Winner, t); } UnityEngine.Debug.Log(log); g_CurrentPathValue = 0f; g_PathLengthNodes = 0; } foreach (string s in hCounter.Keys) { Debug.Log(s + " average time: " + timeAvg[s] / hCounter[s] + "\n"); } succeed++; timeAvg.Clear(); hCounter.Clear(); } } } times.Add((Time.realtimeSinceStartup - globalBenchMarkTime)); } foreach (float f in times) { Debug.Log(f); } } DryRunAlgo = false; g_FromNode = null; }
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); }