public override List <Vector2Int> FindPath(Vector2Int start, Vector2Int goal) { if (_graph == null) { return(null); } _start = start; _goal = goal; JumpCostTime = 0; var startNode = new PathingNode(_start) { F = 0, G = 0, Opened = true }; _open.Enqueue(startNode, startNode.F); this.OnNodeAddOpenSet?.Invoke(startNode); while (_open.Count != 0) { PathingNode node = _open.Dequeue(); node.Closed = true; this.OnNodeVisited?.Invoke(node); if (node.Location == _goal) { return(Trace(node)); } IdentitySuccessors(node); } return(null); }
public override List <Vector2Int> FindPath(Vector2Int start, Vector2Int goal) { if (_graph == null) { return(null); } _start = start; _goal = goal; var startNode = new PathingNode(_start) { F = 0, G = 0, Opened = true }; _open.Enqueue(startNode, startNode.F); this.OnNodeAddOpenSet?.Invoke(startNode); while (_open.Count != 0) { PathingNode curNode = _open.Dequeue(); curNode.Closed = true; this.OnNodeVisited?.Invoke(curNode); if (curNode.Location == _goal) { return(Trace(curNode)); } foreach (var neighbour in _graph.Neighbours(curNode)) { if (neighbour.Closed) { continue; } double d = Utils.Heuristic_Diagonal(Math.Abs(neighbour.Location.x - curNode.Location.x), Math.Abs(neighbour.Location.y - curNode.Location.y)); double ng = curNode.G + d; if (!neighbour.Opened || ng < neighbour.G) { neighbour.G = ng; if (!neighbour.H.HasValue) { neighbour.H = Utils.Heuristic_Diagonal(Math.Abs(neighbour.Location.x - _goal.x), Math.Abs(neighbour.Location.y - _goal.y)); } neighbour.F = (neighbour.G + neighbour.H.Value); neighbour.Parent = curNode; if (!neighbour.Opened) { _open.Enqueue(neighbour, neighbour.F); neighbour.Opened = true; OnNodeAddOpenSet?.Invoke(neighbour); } else { _open.UpdatePriority(neighbour, neighbour.F); } } } } return(null); }