/// <summary>Starting with the src node, searches for the shortest path to all of the specified destinations.</summary>
/// <param name="src">The node to start from.</param>
/// <param name="dest">An array of nodes to search for.</param>
/// <param name="index">The index of each of the provided nodes in the returned PathEdge array.</param>
/// <returns>The shortest path to all of the specified destinations.</returns>
public PathEdge[] search(PathNode src, int[] dest, out int[] index)
{
int numFound = 0;
int stepsSinceLast = -1;
index = new int[dest.Length];
for (int i = 0; i < index.Length; i++)
{
index[i] = -1;
}
double[] costs = new double[Nodes.Length];
MinPQCost queue = new MinPQCost(costs, null);
queue.Enqueue(src.Index);
PathEdge[] frontier = new PathEdge[Nodes.Length];
PathEdge[] shortest = new PathEdge[Nodes.Length];
// TODO: specify threshold
while (queue.Count > 0 && (numFound == 0 || stepsSinceLast < 100))
{
int nextClosest = queue.Dequeue();
shortest[nextClosest] = frontier[nextClosest];
for (int i = 0; i < dest.Length; i++)
{
if (nextClosest == dest[i])
{
numFound++;
stepsSinceLast = 0;
index[i] = nextClosest;
if (numFound == dest.Length)
break;
}
}
if (numFound < dest.Length)
{
foreach (PathEdge edge in Nodes[nextClosest])
{
if (edge.NumIntersections != 0)
continue;
#if DEBUG
if (edge.NumIntersections < 0)
throw new Exception("NumIntersections should not be less than 0.");
#endif
double newCost = costs[nextClosest] + edge.Distance;
if (frontier[edge.NodeDest] == null)
{
costs[edge.NodeDest] = newCost;
queue.Enqueue(edge.NodeDest);
frontier[edge.NodeDest] = edge;
}
else if (newCost < costs[edge.NodeDest] && shortest[edge.NodeDest] == null)
{
costs[edge.NodeDest] = newCost;
queue.costShrunk(edge.NodeDest); // update position in queue
frontier[edge.NodeDest] = edge;
}
}
}
stepsSinceLast++;
}
return shortest;
}
public PathEdge[] search(PathNode src, PathNode dest)
{
int destIndex = (dest == null) ? -1 : dest.Index;
double[] costs = new double[Nodes.Length];
double[] heur = new double[Nodes.Length];
MinPQCost queue = new MinPQCost(costs, heur);
queue.Enqueue(src.Index);
PathEdge[] frontier = new PathEdge[Nodes.Length];
PathEdge[] shortest = new PathEdge[Nodes.Length];
while (queue.Count > 0)
{
int nextClosest = queue.Dequeue();
shortest[nextClosest] = frontier[nextClosest];
if (nextClosest == destIndex)
break;
foreach (PathEdge edge in Nodes[nextClosest])
{
if (edge.NumIntersections != 0)
continue;
#if DEBUG
if (edge.NumIntersections < 0)
throw new Exception("NumIntersections should not be less than 0.");
#endif
double newCost = costs[nextClosest] + edge.Distance;
if (frontier[edge.NodeDest] == null)
{
costs[edge.NodeDest] = newCost;
heur[edge.NodeDest] = VectorD.Distance(Nodes[edge.NodeDest].Position, dest.Position);
queue.Enqueue(edge.NodeDest);
frontier[edge.NodeDest] = edge;
}
else if (newCost < costs[edge.NodeDest] && shortest[edge.NodeDest] == null)
{
costs[edge.NodeDest] = newCost;
queue.costShrunk(edge.NodeDest); // update position in queue
frontier[edge.NodeDest] = edge;
}
}
}
return shortest;
}