private void Update() { if (((prevStartPos - StartPoint).sqrMagnitude > 0.1) || (prevGoalPos - GoalPoint).sqrMagnitude > 0.1) { currentTask?.Stop(); path = null; Debug.Log("Start founding path)"); System.GC.Collect(); memoryStart = System.GC.GetTotalMemory(true); sw = new Stopwatch(); sw.Start(); currentTask = PathFinding.GetPathAsync(StartPoint, GoalPoint); prevStartPos = StartPoint; prevGoalPos = GoalPoint; } if (currentTask != null && currentTask.Status == PathTaskStatus.Completed) { path = currentTask.Path; currentTask = null; sw.Stop(); System.GC.Collect(); var memoryEnd = System.GC.GetTotalMemory(true); Debug.Log("Path finded for " + sw.Elapsed + ", RAM used " + (memoryEnd - memoryStart) / 1024 / 1024 + "MB"); //approximate value } }
public SinglePathTask GetPathAsync(Vector2 startPosition, Vector2 goalPosition, double accuracy = 1) { var thrId = Interlocked.Increment(ref _threadIdGenerator); _runnedThreads.Add(thrId); var pathTask = new SinglePathTask(thrId, this); TaskFactory.Add(() => GetPathInternalTask(startPosition, goalPosition, accuracy, pathTask)); return(pathTask); }
private void Start() { EventManager.OnMapScanned.Add(10000, () => { Debug.Log("Starting founding pathing)"); System.GC.Collect(); var memoryStart = System.GC.GetTotalMemory(true); Stopwatch sw = new Stopwatch(); sw.Start(); currentTask = PathFinding.GetPathAsync(StartPoint, GoalPoint); path = currentTask.WaitForResult(); sw.Stop(); System.GC.Collect(); var memoryEnd = System.GC.GetTotalMemory(true); Debug.Log("Path finded for " + sw.Elapsed + ", RAM used " + (memoryEnd - memoryStart) / 1024 / 1024 + "MB"); //approximate value }); prevStartPos = StartPoint; prevGoalPos = GoalPoint; }
private void GetPathInternalTask(Vector2 startPosition, Vector2 goalPosition, double accuracy, SinglePathTask pathTask) { var startCell = Map.Instance.GetCell(startPosition); var goalCell = Map.Instance.GetCell(goalPosition); if (startCell == null) { Debug.LogWarning("Start position " + startPosition + " not scanned"); pathTask.Fail(); return; } if (goalCell == null) { Debug.LogWarning("Goal position " + goalPosition + " not scanned"); pathTask.Fail(); return; } //if (startCell.Passability == Cell.MIN_PASSABILITY) //{ // foreach (var item in startCell.Neighbours) // { // if (item.Key.Passability > Cell.MIN_PASSABILITY) // { // startCell = item.Key; // break; // } // } // if (startCell == null) // { // _runnedThreads.Remove(threadId); // if (callback != null) // callback(param, null); // Debug.Log("start cell in obstacle"); // return; // } //} if (goalCell.Passability == Cell.MIN_PASSABILITY) { foreach (var item in goalCell.Neighbours) { if (item.Key.Passability > Cell.MIN_PASSABILITY) { goalCell = item.Key; break; } } if (goalCell.Passability == Cell.MIN_PASSABILITY) { Debug.LogWarning("goal cell is obstacle. Cell:" + goalCell.Position); pathTask.Fail(); return; } } Dictionary <Cell, Node> closed = new Dictionary <Cell, Node>(); Dictionary <Cell, Node> open = new Dictionary <Cell, Node>(); List <Node> sortedOpen = new List <Node>();//todo: change to sorted list try { var PutToOpen = new Action <Node>(node => { for (int i = 0; i < open.Count; i++) { if (sortedOpen[i].f < node.f) { continue; } sortedOpen.Insert(i, node); open.Add(node.Cell, node); return; } sortedOpen.Insert(open.Count, node); open.Add(node.Cell, node); }); var PopFromOpen = new Func <Node>(() => { var result = sortedOpen[0]; sortedOpen.RemoveAt(0); open.Remove(result.Cell); return(result); }); var start = new Node(startCell) { g = 0d, h = Utils.GetDistance(goalCell.Position, startCell.Position) * accuracy }; start.f = start.g + start.h; PutToOpen(start); while (open.Count != 0) { if (pathTask.Status == PathTaskStatus.Canceled) { return; } //if (closed.Count % 100 == 0) // Thread.Sleep(10); var x = PopFromOpen(); if (x.Cell.Equals(goalCell)) { pathTask.Complete(ConstructPath(x)); return; } closed.Add(x.Cell, x); foreach (var yy in x.Cell.Neighbours) { if (yy.Key.Passability == Cell.MIN_PASSABILITY) { continue; } //если текущий сосед содержится в списке просмотренных вершин, то пропустить его if (closed.ContainsKey(yy.Key)) { continue; } bool tentativeIsBetter = true; //var tentativeGScore = x.g + 1d;//1d-расстояние между х и соседом var tentativeGScore = x.g + yy.Value / (yy.Key.Passability / Cell.MAX_PASSABILITY_F); //Получаем y из open Node y; if (open.TryGetValue(yy.Key, out y)) { if (tentativeGScore < y.g) { open.Remove(yy.Key); sortedOpen.Remove(y); } else { tentativeIsBetter = false; } } else { y = new Node(yy.Key); } if (tentativeIsBetter) { y.Parent = x; y.g = tentativeGScore; y.h = Utils.GetDistance(y.Cell.Position, goalCell.Position) * accuracy; y.f = y.g + y.h; PutToOpen(y); } } } Debug.LogWarning("Goal not founded: StartPos: " + startCell.Position + "\tGoalPos: " + goalCell.Position); pathTask.Fail(); } catch (Exception ex) { pathTask.Fail(); throw new Exception(ex.Message, ex); } finally { open.Clear(); sortedOpen.Clear(); closed.Clear(); } }