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);
}
}
}
}
}
