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 nodesVisisted = new HashSet <DataNode <TPoint2D> >();
var queue = new Queue <DataNode <TPoint2D> >();
//initialization
queue.Enqueue(sourceNode);
while (queue.Count > 0)
{
//select node with lowest distance
var closestNode = queue.Dequeue();
//target node found
if (targetNodes.Contains(closestNode))
{
LinkedList <TPoint2D> points = null;
var closesDataNodeDistance = 0;
if (input.CreatePath)
{
points = new LinkedList <TPoint2D>();
//Build path. We don't include first point
var prev = closestNode.Data;
while (prevNode.ContainsKey(prev))
{
points.AddFirst(prev);
prev = prevNode[prev];
closesDataNodeDistance++;
}
}
else
{
//only find path distance
var prev = closestNode.Data;
while (prevNode.ContainsKey(prev))
{
prev = prevNode[prev];
closesDataNodeDistance++;
}
}
output.SetPath(closestNode.Data, new ShortcutPath(points, closesDataNodeDistance));
targetNodes.Remove(closestNode);
if (!targetNodes.Any())
{
break;
}
}
foreach (var node in closestNode.OutEdges.Keys)
{
var neighbor = (DataNode <TPoint2D>)node;
if (neighbor.Data.Index > maxIndex)
{
continue;
}
if (!nodesVisisted.Contains(neighbor))
{
prevNode[neighbor.Data] = closestNode.Data;
queue.Enqueue(neighbor);
nodesVisisted.Add(neighbor);
}
}
}
}
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);
}
}
}
}
}
