/// <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;
}
/// <summary>Creates a shallow copy of the PathEdge.</summary>
/// <returns>A shallow copy of the PathEdge.</returns>
public PathEdge Clone()
{
PathEdge clone = new PathEdge(Path, this.NodeSrc, this.NodeDest);
clone.Distance = this.Distance;
return clone;
}
public List<PathEdge> gridToList(PathEdge[] edgeGrid, PathNode srcNode, PathNode destNode)
{
List<PathEdge> edges = new List<PathEdge>();
while (destNode != srcNode)
{
if (edgeGrid[destNode.Index] == null)
return null;
edges.Add(edgeGrid[destNode.Index]);
destNode = Nodes[edgeGrid[destNode.Index].NodeSrc];
}
edges.Reverse();
return edges;
}
/// <summary>Resets all values to their defaults.</summary>
public void clear()
{
NodeSrc = -1;
NodeDest = -1;
Distance = 0d;
NumIntersections = 0;
Previous = null;
Next = null;
}