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