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);
                    }
                }
            }
        }
Exemplo n.º 3
0
        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);
                        }
                    }
                }
            }
        }