public virtual void Search(ref HashSet <SpaceTimeNode> reservationTable) { var startNode = new SpaceTimeNode(Start.X, Start.Y, 0); _gScore[startNode] = 0; _open.Enqueue(startNode, Heuristic(Start)); SpaceTimeNode current; while (_open.Count > 0) { current = _open.Dequeue(); _closed.Add(current); // current tile is reserved, we can't be here if (!current.Equals(Start) && (reservationTable.Contains(current) || reservationTable.Contains(current.Next()))) { continue; } if (current.Equals(Target)) { Path.AddRange(ReconstructPath(current)); for (var t = 0; t < Path.Count; t++) { reservationTable.Add(new SpaceTimeNode(Path[t].X, Path[t].Y, t)); } return; } foreach (var neighbor in _graph.GetNeighbours(current)) { if (_closed.Contains(neighbor) || reservationTable.Contains(neighbor) || reservationTable.Contains(neighbor.Next())) { continue; } var tentative_gScore = _gScore.GetValueOrDefault(current, double.PositiveInfinity) + Cost(current, neighbor); if (tentative_gScore < _gScore.GetValueOrDefault(neighbor, double.PositiveInfinity)) { _cameFrom[neighbor] = current; _gScore[neighbor] = tentative_gScore; var fScore = tentative_gScore + Heuristic(_graph.GetTile(neighbor)); if (!_open.TryUpdatePriority(neighbor, fScore)) { _open.Enqueue(neighbor, fScore); } } } } }
private double Cost(SpaceTimeNode p, SpaceTimeNode q) { if (q == p.Next()) { // wait node return(1); } return(Metrics.Octile(_graph.GetTile(p), _graph.GetTile(q))); }
private List <AbstractTile> ReconstructPath(SpaceTimeNode current) { var path = new List <AbstractTile>() { _graph.GetTile(current) }; while (_cameFrom.TryGetValue(current, out var prev)) { current = prev; path.Add(_graph.GetTile(current)); } path.Reverse(); return(path); }
private double Cost(SpaceTimeNode p, SpaceTimeNode q) { var pTile = _graph.GetTile(p); if (p.T == _w) { return(_rra.AbstractDist(pTile)); } if (q == p.Next()) { if (q.Equals(Target)) { return(0); } return(1); } var qTile = _graph.GetTile(q); return(Metrics.Octile(pTile, qTile)); }
private void Search(ref HashSet <SpaceTimeNode> reservationTable) { _cameFrom = new Dictionary <SpaceTimeNode, SpaceTimeNode>(); _closed = new HashSet <SpaceTimeNode>(); _gScore = new Dictionary <SpaceTimeNode, double>(); _open = new SimplePriorityQueue <SpaceTimeNode, double>(); var pos = new SpaceTimeNode(Position.X, Position.Y, 0); _gScore[pos] = 0; _open.Enqueue(pos, _rra.AbstractDist(Position)); SpaceTimeNode current; while (_open.Count > 0) { current = _open.Dequeue(); _closed.Add(current); // current tile is reserved, we can't be here if (!current.Equals(Position) && (reservationTable.Contains(new SpaceTimeNode(current.X, current.Y, current.T + _pathIndex)) || reservationTable.Contains(new SpaceTimeNode(current.X, current.Y, current.T + _pathIndex + 1)))) { continue; } if (current.T == _w || current.Equals(Target)) { Path.AddRange(ReconstructPath(current)); for (var t = _pathIndex; t < Path.Count; t++) { reservationTable.Add(new SpaceTimeNode(Path[t].X, Path[t].Y, t)); } return; } foreach (var neighbor in _graph.GetNeighbours(current)) { if (_closed.Contains(neighbor) || reservationTable.Contains(new SpaceTimeNode(neighbor.X, neighbor.Y, neighbor.T + _pathIndex)) || reservationTable.Contains(new SpaceTimeNode(neighbor.X, neighbor.Y, neighbor.T + _pathIndex + 1))) { continue; } var tentative_gScore = _gScore.GetValueOrDefault(current, double.PositiveInfinity) + Cost(current, neighbor); if (tentative_gScore < _gScore.GetValueOrDefault(neighbor, double.PositiveInfinity)) { _cameFrom[neighbor] = current; _gScore[neighbor] = tentative_gScore; var fScore = tentative_gScore + _rra.AbstractDist(_graph.GetTile(neighbor)); if (!_open.TryUpdatePriority(neighbor, fScore)) { _open.Enqueue(neighbor, fScore); } } } } }