public override void Compute(SPFInput input, out SPFOutput output) { output = new SPFOutput(input); var source = input.Source; var target = input.Targets.First(); var regionSet = (ShortcutIntervalSet)input.ShortcutSet; var trajectory = input.Trajectory; var distances = new IntervalsRangeTree(regionSet, trajectory, target); for (var i = target.Index; i >= source.Index; i--) { distances.Insert(trajectory[i]); } var subtreeData = distances.GetSubtreeData(source); var step = subtreeData.NextStep; if (step == null) { return; } LinkedList <TPoint2D> path = null; if (input.CreatePath) { path = new LinkedList <TPoint2D>(); while (step != null) { var point = step.Point; path.AddLast(point); step = step.NextStep; } } output.SetPath(target, new ShortcutPath(path, subtreeData.Distance)); }
public override void Compute(SPFInput input, out SPFOutput output) { output = new SPFOutput(input); var graph = (ShortcutGraph)input.ShortcutSet; var sourceNode = graph.GetNode(input.Source); var targetNodes = new HashSet <DataNode <TPoint2D> >(); var maxIndex = 0; foreach (var point in input.Targets) { targetNodes.Add(graph.GetNode(point)); maxIndex = Math.Max(maxIndex, point.Index); } var prevNode = new Dictionary <TPoint2D, TPoint2D>(); var graphToHeap = new Dictionary <DataNode <TPoint2D>, IHeapNode <int, DataNode <TPoint2D> > >(); var nodeHeap = options.ChosenHeapFactory.CreateHeap(graph); //initialization var sourceHeapNode = nodeHeap.Add(0, sourceNode); graphToHeap.Add(sourceNode, sourceHeapNode); while (!nodeHeap.IsEmpty) { //select node with lowest distance var closestHeapNode = nodeHeap.Pop(); var closesDataNodeDistance = closestHeapNode.Key; var closesDataNode = closestHeapNode.Value; //target node found if (targetNodes.Contains(closesDataNode)) { LinkedList <TPoint2D> points = null; if (input.CreatePath) { points = new LinkedList <TPoint2D>(); //Build path. We don't include first point var prev = closesDataNode.Data; while (prevNode.ContainsKey(prev)) { points.AddFirst(prev); prev = prevNode[prev]; } } output.SetPath(closesDataNode.Data, new ShortcutPath(points, closesDataNodeDistance)); targetNodes.Remove(closesDataNode); if (!targetNodes.Any()) { break; } } //increment distances of adjacent nodes foreach (var node in closesDataNode.OutEdges.Keys) { var neighbor = (DataNode <TPoint2D>)node; if (neighbor.Data.Index > maxIndex) { continue; } var weightedEdge = (WeightedEdge)closesDataNode.OutEdges[neighbor]; var altDistance = closesDataNodeDistance; if (weightedEdge != null) { altDistance += weightedEdge.Data; } else { altDistance += 1; } IHeapNode <int, DataNode <TPoint2D> > heapNode; var seenBefore = graphToHeap.TryGetValue(neighbor, out heapNode); if (seenBefore && altDistance < heapNode.Key) { prevNode[neighbor.Data] = closesDataNode.Data; nodeHeap.Update(heapNode, altDistance); } else if (!seenBefore) { prevNode[neighbor.Data] = closesDataNode.Data; heapNode = nodeHeap.Add(altDistance, neighbor); graphToHeap.Add(neighbor, heapNode); } } } }
public override void Compute(SPFInput input, out SPFOutput output) { output = new SPFOutput(input); var source = input.Source; var target = input.Targets.First(); var intervals = (ShortcutIntervalSet)input.ShortcutSet; var prevNode = new Dictionary <TPoint2D, TPoint2D>(); var pointsVisited = new HashSet <TPoint2D>(); var queue = new Queue <TPoint2D>(); //initialization queue.Enqueue(source); while (queue.Count > 0) { //select node with lowest distance var closestPoint = queue.Dequeue(); //target node found if (closestPoint == target) { LinkedList <TPoint2D> points = null; var closesDataNodeDistance = 0; if (input.CreatePath) { points = new LinkedList <TPoint2D>(); //Build path. We don't include first point var prev = closestPoint; while (prevNode.ContainsKey(prev)) { points.AddFirst(prev); prev = prevNode[prev]; closesDataNodeDistance++; } } else { //only find path distance var prev = closestPoint; while (prevNode.ContainsKey(prev)) { prev = prevNode[prev]; closesDataNodeDistance++; } } output.SetPath(closestPoint, new ShortcutPath(points, closesDataNodeDistance)); } foreach (var region in intervals.IntervalMap[closestPoint]) { if (region.Start.Index > target.Index) { break; } for (var i = region.Start.Index; i <= region.End.Index; i++) { var neighbor = intervals.Trajectory[i]; if (neighbor.Index > target.Index) { break; } if (!pointsVisited.Contains(neighbor)) { prevNode[neighbor] = closestPoint; queue.Enqueue(neighbor); pointsVisited.Add(neighbor); } } } } }