private void IdentitySuccessors(PathingNode node) { foreach (PathingNode neighbour in _graph.Neighbours(node)) { System.DateTime s = System.DateTime.Now; Vector2Int jumpPoint = Jump(neighbour.Location, node.Location); JumpCostTime += ((System.DateTime.Now - s).TotalMilliseconds); if (jumpPoint != Utils.InvalidGrid) { PathingNode jumpNode = _graph[jumpPoint]; if (jumpNode.Closed) { continue; } double d = Utils.Heuristic_Diagonal(Math.Abs(jumpPoint.x - node.Location.x), Math.Abs(jumpPoint.y - node.Location.y)); double ng = node.G + d; if (!jumpNode.Opened || ng < jumpNode.G) { jumpNode.G = ng; if (!jumpNode.H.HasValue) { jumpNode.H = Utils.Heuristic_Diagonal(Math.Abs(jumpPoint.x - _goal.x), Math.Abs(jumpPoint.y - _goal.y)); } jumpNode.F = jumpNode.G + jumpNode.H.Value; jumpNode.Parent = node; if (!jumpNode.Opened) { _open.Enqueue(jumpNode, jumpNode.F); jumpNode.Opened = true; OnNodeAddOpenSet?.Invoke(jumpNode); } else { _open.UpdatePriority(jumpNode, jumpNode.F); } } } } }
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); }