public void InitNode(int index, int cost, AStarNode predecessor)
{
this.routeCost = cost;
this.predecessor = predecessor;
this.index = index;
}
public (int, int[]) AStar(int x, int y, SortedList <int, List <int> > constraints, int beginTime, bool reverseSearch)
{
if (reverseSearch)
{
var tmp = x;
x = y;
y = tmp;
}
var queue = aStarQueue;
EnqueueNode(x, 0, 0, null);
while (queue.Count != 0)
{
var currNode = queue.Dequeue();
queueCacheA[currNode.index] = null;
var currRouteCost = currNode.routeCost;
// If goal node.
if (currNode.index == y)
{
var result = new int[currNode.routeCost + 1];
result[currNode.routeCost] = currNode.index;
for (int i = currNode.routeCost; i > 0; i--)
{
currNode = currNode.predecessor;
result[i - 1] = currNode.index;
}
if (reverseSearch)
{
ReverseRoute(result);
}
CleanUpQueueCache();
return(currRouteCost, result);
}
int heuristicCost;
int neighborRouteCost = currNode.routeCost + 1;
int targetOffsetTime = 0;
// Stay at node for another time step, if not constrained
//TODO: Only before a blocked vertex?
if (constraints != null)
{
targetOffsetTime = reverseSearch == false ? beginTime + neighborRouteCost : beginTime - neighborRouteCost;
if (constraints.ContainsKey(targetOffsetTime))
{
foreach (var constraint in constraints[targetOffsetTime])
{
if (constraint == currNode.index)
{
goto Neighbors;
}
}
}
heuristicCost = neighborRouteCost + distancesCache[currNode.index][y];
EnqueueNode(currNode.index, neighborRouteCost, heuristicCost, currNode);
}
Neighbors:
// Add all neighbors into queue, if edge is not constrained.
foreach (var neighbor in vertices[currNode.index].edges)
{
int neighborIdx = neighbor.x == currNode.index ? neighbor.y : neighbor.x;
// No return edges.
if (currNode.predecessor != null)
{
if (neighborIdx == currNode.predecessor.index)
{
continue;
}
else if (currNode.predecessor.index == currNode.index)
{
var tmp = currNode.predecessor;
while (tmp.predecessor != null && tmp.index == currNode.index)
{
tmp = tmp.predecessor;
}
if (tmp.index == neighborIdx)
{
continue;
}
}
}
heuristicCost = neighborRouteCost + distancesCache[neighborIdx][y];
// Check if constraints are not violated.
if (constraints != null && constraints.ContainsKey(targetOffsetTime))
{
foreach (var constrainedVertexIdx in constraints[targetOffsetTime])
{
if (constrainedVertexIdx == neighborIdx)
{
goto SkipEnqueue;
}
}
}
EnqueueNode(neighborIdx, neighborRouteCost, heuristicCost, currNode);
SkipEnqueue:
;
}
}
throw new ArgumentException("Shortest path not found, check the arguments.");
void EnqueueNode(int nodeIndex, int routeCost, int heuristicCost, AStarNode prevNode)
{
AStarNode nextNode = queueCacheA[nodeIndex];
// newCost invariants - route with longer optimal length will always have higher newCost
// - of routes with the same optimal cost the one furthest from beginnig is preferred
float newCost = (heuristicCost << 10) - routeCost;
if (nextNode != null) // update value if better cost and node is not requeued already
{
if (nextNode.routeCost > routeCost && nextNode.index != nextNode.predecessor.index)
{
nextNode.routeCost = routeCost;
nextNode.predecessor = prevNode;
queue.UpdatePriority(nextNode, newCost);
}
}
else // first visiting the vertex or requeue the current vertex
{
nextNode = nodeFactory.GetNode(nodeIndex, routeCost, prevNode);
queueCacheA[nodeIndex] = nextNode;
queue.Enqueue(nextNode, newCost);
}
}
void CleanUpQueueCache()
{
queue.Clear();
Array.Copy(emptyArr, queueCacheA, queueCacheA.Length);
nodeFactory.ResetIndex();
}