Beispiel #1
0
        /// <summary>
        /// Searches the data for a point on an edge closest to the given coordinate.
        /// </summary>
        /// <param name="graph"></param>
        /// <param name="vehicle"></param>
        /// <param name="coordinate"></param>
        /// <param name="delta"></param>
        /// <param name="matcher"></param>
        /// <param name="pointTags"></param>
        /// <param name="interpreter"></param>
        /// <param name="verticesOnly"></param>
        /// <param name="parameters"></param>
        public SearchClosestResult <TEdgeData> SearchClosest(IRoutingAlgorithmData <TEdgeData> graph, IRoutingInterpreter interpreter, Vehicle vehicle,
                                                             GeoCoordinate coordinate, float delta, IEdgeMatcher matcher, TagsCollectionBase pointTags, bool verticesOnly, Dictionary <string, object> parameters)
        {
            Meter distanceEpsilon = .1; // 10cm is the tolerance to distinguish points.

            var closestWithMatch = new SearchClosestResult <TEdgeData>(double.MaxValue, 0);
            GeoCoordinateBox closestWithMatchBox = null;
            var closestWithoutMatch = new SearchClosestResult <TEdgeData>(double.MaxValue, 0);
            GeoCoordinateBox closestWithoutMatchBox = null;

            double searchBoxSize = delta;
            // create the search box.
            var searchBox = new GeoCoordinateBox(new GeoCoordinate(
                                                     coordinate.Latitude - searchBoxSize, coordinate.Longitude - searchBoxSize),
                                                 new GeoCoordinate(
                                                     coordinate.Latitude + searchBoxSize, coordinate.Longitude + searchBoxSize));

            // get the arcs from the data source.
            var edges = graph.GetEdges(searchBox);

            if (!verticesOnly)
            { // find both closest arcs and vertices.
                // loop over all.
                while (edges.MoveNext())
                {
                    //if (!graph.TagsIndex.Contains(edges.EdgeData.Tags))
                    //{ // skip this edge, no valid tags found.
                    //    continue;
                    //}

                    // test the two points.
                    float  fromLatitude, fromLongitude;
                    float  toLatitude, toLongitude;
                    double distance;
                    if (graph.GetVertex(edges.Vertex1, out fromLatitude, out fromLongitude) &&
                        graph.GetVertex(edges.Vertex2, out toLatitude, out toLongitude))
                    { // return the vertex.
                        var vertex1Coordinate = new GeoCoordinate(fromLatitude, fromLongitude);
                        var vertex2Coordinate = new GeoCoordinate(toLatitude, toLongitude);

                        if (edges.EdgeData.ShapeInBox)
                        { // ok, check if it is needed to even check this edge.
                            var edgeBox        = new GeoCoordinateBox(vertex1Coordinate, vertex2Coordinate);
                            var edgeBoxOverlap = false;
                            if (closestWithoutMatchBox == null ||
                                closestWithoutMatchBox.Overlaps(edgeBox))
                            { // edge box overlap.
                                edgeBoxOverlap = true;
                            }
                            else if (closestWithMatchBox == null ||
                                     closestWithMatchBox.Overlaps(edgeBox))
                            { // edge box overlap.
                                edgeBoxOverlap = true;
                            }
                            if (!edgeBoxOverlap)
                            { // no overlap, impossible this edge is a candidate.
                                continue;
                            }
                        }

                        var arcTags        = graph.TagsIndex.Get(edges.EdgeData.Tags);
                        var canBeTraversed = vehicle.CanTraverse(arcTags);
                        if (canBeTraversed)
                        { // the edge can be traversed.
                            distance = coordinate.DistanceEstimate(vertex1Coordinate).Value;
                            if (distance < distanceEpsilon.Value)
                            { // the distance is smaller than the tolerance value.
                                var diff = coordinate - vertex1Coordinate;
                                closestWithoutMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
                                                                              new GeoCoordinate(coordinate - diff));
                                closestWithoutMatch = new SearchClosestResult <TEdgeData>(
                                    distance, edges.Vertex1);
                                if (matcher == null ||
                                    (pointTags == null || pointTags.Count == 0) ||
                                    matcher.MatchWithEdge(vehicle, pointTags, arcTags))
                                {
                                    closestWithMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
                                                                               new GeoCoordinate(coordinate - diff));
                                    closestWithMatch = new SearchClosestResult <TEdgeData>(
                                        distance, edges.Vertex1);
                                    break;
                                }
                            }

                            if (distance < closestWithoutMatch.Distance)
                            { // the distance is smaller for the without match.
                                var diff = coordinate - vertex1Coordinate;
                                closestWithoutMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
                                                                              new GeoCoordinate(coordinate - diff));
                                closestWithoutMatch = new SearchClosestResult <TEdgeData>(
                                    distance, edges.Vertex1);
                            }
                            if (distance < closestWithMatch.Distance)
                            { // the distance is smaller for the with match.
                                if (matcher == null ||
                                    (pointTags == null || pointTags.Count == 0) ||
                                    matcher.MatchWithEdge(vehicle, pointTags, graph.TagsIndex.Get(edges.EdgeData.Tags)))
                                {
                                    var diff = coordinate - vertex1Coordinate;
                                    closestWithMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
                                                                               new GeoCoordinate(coordinate - diff));
                                    closestWithMatch = new SearchClosestResult <TEdgeData>(
                                        distance, edges.Vertex1);
                                }
                            }

                            distance = coordinate.DistanceEstimate(vertex2Coordinate).Value;
                            if (distance < closestWithoutMatch.Distance)
                            { // the distance is smaller for the without match.
                                var diff = coordinate - vertex2Coordinate;
                                closestWithoutMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
                                                                              new GeoCoordinate(coordinate - diff));
                                closestWithoutMatch = new SearchClosestResult <TEdgeData>(
                                    distance, edges.Vertex2);
                            }
                            if (distance < closestWithMatch.Distance)
                            { // the distance is smaller for the with match.
                                if (matcher == null ||
                                    (pointTags == null || pointTags.Count == 0) ||
                                    matcher.MatchWithEdge(vehicle, pointTags, arcTags))
                                {
                                    var diff = coordinate - vertex2Coordinate;
                                    closestWithMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
                                                                               new GeoCoordinate(coordinate - diff));
                                    closestWithMatch = new SearchClosestResult <TEdgeData>(
                                        distance, edges.Vertex2);
                                }
                            }
                            // search along the line.
                            var coordinatesArray         = new ICoordinate[0];
                            var distanceTotal            = 0.0;
                            var arcValueValueCoordinates = edges.Intermediates;
                            if (arcValueValueCoordinates != null)
                            { // calculate distance along all coordinates.
                                coordinatesArray = arcValueValueCoordinates.ToArray();
                            }

                            // loop over all edges that are represented by this arc (counting intermediate coordinates).
                            var previous = vertex1Coordinate;
                            GeoCoordinateLine line;
                            var distanceToSegment = 0.0;
                            if (arcValueValueCoordinates != null)
                            {
                                for (int idx = 0; idx < coordinatesArray.Length; idx++)
                                {
                                    var current = new GeoCoordinate(
                                        coordinatesArray[idx].Latitude, coordinatesArray[idx].Longitude);
                                    var edgeBox        = new GeoCoordinateBox(previous, current);
                                    var edgeBoxOverlap = false;
                                    if (closestWithoutMatchBox == null ||
                                        closestWithoutMatchBox.Overlaps(edgeBox))
                                    { // edge box overlap.
                                        edgeBoxOverlap = true;
                                    }
                                    else if (closestWithMatchBox == null ||
                                             closestWithMatchBox.Overlaps(edgeBox))
                                    { // edge box overlap.
                                        edgeBoxOverlap = true;
                                    }
                                    if (edgeBoxOverlap)
                                    { // overlap, possible this edge is a candidate.
                                        line     = new GeoCoordinateLine(previous, current, true, true);
                                        distance = line.DistanceReal(coordinate).Value;

                                        if (distance < closestWithoutMatch.Distance)
                                        { // the distance is smaller.
                                            var projectedPoint = line.ProjectOn(coordinate);

                                            // calculate the position.
                                            if (projectedPoint != null)
                                            {     // calculate the distance.
                                                if (distanceTotal == 0)
                                                { // calculate total distance.
                                                    var pCoordinate = vertex1Coordinate;
                                                    for (int cIdx = 0; cIdx < coordinatesArray.Length; cIdx++)
                                                    {
                                                        var cCoordinate = new GeoCoordinate(coordinatesArray[cIdx].Latitude, coordinatesArray[cIdx].Longitude);
                                                        distanceTotal = distanceTotal + cCoordinate.DistanceReal(pCoordinate).Value;
                                                        pCoordinate   = cCoordinate;
                                                    }
                                                    distanceTotal = distanceTotal + vertex2Coordinate.DistanceReal(pCoordinate).Value;
                                                }

                                                var distancePoint = previous.DistanceReal(new GeoCoordinate(projectedPoint)).Value + distanceToSegment;
                                                var position      = distancePoint / distanceTotal;

                                                var diff = coordinate - new GeoCoordinate(projectedPoint);
                                                closestWithoutMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
                                                                                              new GeoCoordinate(coordinate - diff));
                                                closestWithoutMatch = new SearchClosestResult <TEdgeData>(
                                                    distance, edges.Vertex1, edges.Vertex2, position, edges.EdgeData, coordinatesArray);
                                            }
                                        }
                                        if (distance < closestWithMatch.Distance)
                                        {
                                            var projectedPoint = line.ProjectOn(coordinate);

                                            // calculate the position.
                                            if (projectedPoint != null)
                                            {     // calculate the distance
                                                if (distanceTotal == 0)
                                                { // calculate total distance.
                                                    var pCoordinate = vertex1Coordinate;
                                                    for (int cIdx = 0; cIdx < coordinatesArray.Length; cIdx++)
                                                    {
                                                        var cCoordinate = new GeoCoordinate(coordinatesArray[cIdx].Latitude, coordinatesArray[cIdx].Longitude);
                                                        distanceTotal = distanceTotal + cCoordinate.DistanceReal(pCoordinate).Value;
                                                        pCoordinate   = cCoordinate;
                                                    }
                                                    distanceTotal = distanceTotal + vertex2Coordinate.DistanceReal(pCoordinate).Value;
                                                }

                                                var distancePoint = previous.DistanceReal(new GeoCoordinate(projectedPoint)).Value + distanceToSegment;
                                                var position      = distancePoint / distanceTotal;

                                                if (matcher == null ||
                                                    (pointTags == null || pointTags.Count == 0) ||
                                                    matcher.MatchWithEdge(vehicle, pointTags, arcTags))
                                                {
                                                    var diff = coordinate - new GeoCoordinate(projectedPoint);
                                                    closestWithMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
                                                                                               new GeoCoordinate(coordinate - diff));
                                                    closestWithMatch = new SearchClosestResult <TEdgeData>(
                                                        distance, edges.Vertex1, edges.Vertex2, position, edges.EdgeData, coordinatesArray);
                                                }
                                            }
                                        }
                                    }

                                    // add current segment distance to distanceToSegment for the next segment.
                                    distanceToSegment = distanceToSegment + previous.DistanceEstimate(current).Value;

                                    // set previous.
                                    previous = current;
                                }
                            }

                            // check the last segment.
                            line = new GeoCoordinateLine(previous, vertex2Coordinate, true, true);

                            distance = line.DistanceReal(coordinate).Value;

                            if (distance < closestWithoutMatch.Distance)
                            { // the distance is smaller.
                                var projectedPoint = line.ProjectOn(coordinate);

                                // calculate the position.
                                if (projectedPoint != null)
                                {     // calculate the distance
                                    if (distanceTotal == 0)
                                    { // calculate total distance.
                                        var pCoordinate = vertex1Coordinate;
                                        for (int cIdx = 0; cIdx < coordinatesArray.Length; cIdx++)
                                        {
                                            var cCoordinate = new GeoCoordinate(coordinatesArray[cIdx].Latitude, coordinatesArray[cIdx].Longitude);
                                            distanceTotal = distanceTotal + cCoordinate.DistanceReal(pCoordinate).Value;
                                            pCoordinate   = cCoordinate;
                                        }
                                        distanceTotal = distanceTotal + vertex2Coordinate.DistanceReal(pCoordinate).Value;
                                    }

                                    double distancePoint = previous.DistanceReal(new GeoCoordinate(projectedPoint)).Value + distanceToSegment;
                                    double position      = distancePoint / distanceTotal;

                                    var diff = coordinate - new GeoCoordinate(projectedPoint);
                                    closestWithoutMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
                                                                                  new GeoCoordinate(coordinate - diff));
                                    closestWithoutMatch = new SearchClosestResult <TEdgeData>(
                                        distance, edges.Vertex1, edges.Vertex2, position, edges.EdgeData, coordinatesArray);
                                }
                            }
                            if (distance < closestWithMatch.Distance)
                            {
                                var projectedPoint = line.ProjectOn(coordinate);

                                // calculate the position.
                                if (projectedPoint != null)
                                {     // calculate the distance
                                    if (distanceTotal == 0)
                                    { // calculate total distance.
                                        var pCoordinate = vertex1Coordinate;
                                        for (int cIdx = 0; cIdx < coordinatesArray.Length; cIdx++)
                                        {
                                            var cCoordinate = new GeoCoordinate(coordinatesArray[cIdx].Latitude, coordinatesArray[cIdx].Longitude);
                                            distanceTotal = distanceTotal + cCoordinate.DistanceReal(pCoordinate).Value;
                                            pCoordinate   = cCoordinate;
                                        }
                                        distanceTotal = distanceTotal + vertex2Coordinate.DistanceReal(pCoordinate).Value;
                                    }

                                    double distancePoint = previous.DistanceReal(new GeoCoordinate(projectedPoint)).Value + distanceToSegment;
                                    double position      = distancePoint / distanceTotal;

                                    if (matcher == null ||
                                        (pointTags == null || pointTags.Count == 0) ||
                                        matcher.MatchWithEdge(vehicle, pointTags, arcTags))
                                    {
                                        var diff = coordinate - new GeoCoordinate(projectedPoint);
                                        closestWithMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
                                                                                   new GeoCoordinate(coordinate - diff));
                                        closestWithMatch = new SearchClosestResult <TEdgeData>(
                                            distance, edges.Vertex1, edges.Vertex2, position, edges.EdgeData, coordinatesArray);
                                    }
                                }
                            }
                        }
                    }
                }
            }
            else
            { // only find closest vertices.
                // loop over all.
                while (edges.MoveNext())
                {
                    float fromLatitude, fromLongitude;
                    float toLatitude, toLongitude;
                    if (graph.GetVertex(edges.Vertex1, out fromLatitude, out fromLongitude) &&
                        graph.GetVertex(edges.Vertex2, out toLatitude, out toLongitude))
                    {
                        var    vertexCoordinate = new GeoCoordinate(fromLatitude, fromLongitude);
                        double distance         = coordinate.DistanceReal(vertexCoordinate).Value;
                        if (distance < closestWithoutMatch.Distance)
                        { // the distance found is closer.
                            var diff = coordinate - vertexCoordinate;
                            closestWithoutMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
                                                                          new GeoCoordinate(coordinate - diff));
                            closestWithoutMatch = new SearchClosestResult <TEdgeData>(
                                distance, edges.Vertex1);
                        }

                        vertexCoordinate = new GeoCoordinate(toLatitude, toLongitude);
                        distance         = coordinate.DistanceReal(vertexCoordinate).Value;
                        if (distance < closestWithoutMatch.Distance)
                        { // the distance found is closer.
                            var diff = coordinate - vertexCoordinate;
                            closestWithoutMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
                                                                          new GeoCoordinate(coordinate - diff));
                            closestWithoutMatch = new SearchClosestResult <TEdgeData>(
                                distance, edges.Vertex2);
                        }

                        var arcValueValueCoordinates = edges.Intermediates;
                        if (arcValueValueCoordinates != null)
                        { // search over intermediate points.
                            var arcValueValueCoordinatesArray = arcValueValueCoordinates.ToArray();
                            for (int idx = 0; idx < arcValueValueCoordinatesArray.Length; idx++)
                            {
                                vertexCoordinate = new GeoCoordinate(
                                    arcValueValueCoordinatesArray[idx].Latitude,
                                    arcValueValueCoordinatesArray[idx].Longitude);
                                distance = coordinate.DistanceReal(vertexCoordinate).Value;
                                if (distance < closestWithoutMatch.Distance)
                                { // the distance found is closer.
                                    var diff = coordinate - vertexCoordinate;
                                    closestWithoutMatchBox = new GeoCoordinateBox(new GeoCoordinate(coordinate + diff),
                                                                                  new GeoCoordinate(coordinate - diff));
                                    closestWithoutMatch = new SearchClosestResult <TEdgeData>(
                                        distance, edges.Vertex1, edges.Vertex2, idx, edges.EdgeData, arcValueValueCoordinatesArray);
                                }
                            }
                        }
                    }
                }
            }

            // return the best result.
            if (closestWithMatch.Distance < double.MaxValue)
            {
                return(closestWithMatch);
            }
            return(closestWithoutMatch);
        }
Beispiel #2
0
        /// <summary>
        /// Does dykstra calculation(s) with several options.
        /// </summary>
        /// <param name="graph"></param>
        /// <param name="interpreter"></param>
        /// <param name="vehicle"></param>
        /// <param name="sourceList"></param>
        /// <param name="targetList"></param>
        /// <param name="weight"></param>
        /// <param name="stopAtFirst"></param>
        /// <param name="returnAtWeight"></param>
        /// <param name="forward"></param>
        /// <param name="parameters"></param>
        /// <returns></returns>
        private PathSegment <long>[] DoCalculation(IRoutingAlgorithmData <Edge> graph, IRoutingInterpreter interpreter, Vehicle vehicle,
                                                   PathSegmentVisitList sourceList, PathSegmentVisitList[] targetList, double weight,
                                                   bool stopAtFirst, bool returnAtWeight, bool forward, Dictionary <string, object> parameters)
        {
            // intialize dykstra data structures.
            var heap   = new BinaryHeap <DykstraVisit>(100);
            var visits = new Dictionary <long, DykstraVisit>();

            // initialize a dictionary of speeds per profile.
            var speeds = new Dictionary <uint, Speed>();

            // make copies of the target and source visitlist.
            var source       = sourceList.Clone() as PathSegmentVisitList;
            var targets      = new PathSegmentVisitList[targetList.Length];
            var targetsCount = new int[targetList.Length];

            for (int targetIdx = 0; targetIdx < targetList.Length; targetIdx++)
            {
                targets[targetIdx]      = targetList[targetIdx].Clone() as PathSegmentVisitList;
                targetsCount[targetIdx] = targetList[targetIdx].Count;
            }

            //  initialize the result data structures.
            var segmentsAtWeight = new List <PathSegment <long> >();
            var segmentsToTarget = new PathSegment <long> [targets.Length]; // the resulting target segments.

            var labels = new Dictionary <long, IList <RoutingLabel> >();

            foreach (long vertex in source.GetVertices())
            {
                labels[vertex] = new List <RoutingLabel>();

                var path = source.GetPathTo(vertex);
                heap.Push(new DykstraVisit(path), (float)path.Weight);
            }

            // set the from node as the current node and put it in the correct data structures.
            // initialize the source's neighbors.
            var current = heap.Pop();

            while (current != null && visits.ContainsKey(current.Vertex))
            { // keep dequeuing.
                current = heap.Pop();
            }

            if (current == null)
            {
                return(null);
            }

            // test each target for the source.
            // test each source for any of the targets.
            var pathsFromSource = new Dictionary <long, PathSegment <long> >();

            foreach (long sourceVertex in source.GetVertices())
            { // get the path to the vertex.
                // get the source path.
                var sourcePath = source.GetPathTo(sourceVertex);
                sourcePath = sourcePath.From;
                while (sourcePath != null)
                { // add the path to the paths from source.
                    // add to visits.
                    var visit = new DykstraVisit(sourcePath);
                    visits[visit.Vertex] = visit;

                    pathsFromSource[sourcePath.VertexId] = sourcePath;
                    sourcePath = sourcePath.From;
                }
            }
            // loop over all targets, check for source.
            for (int idx = 0; idx < targets.Length; idx++)
            { // loop over each vertex in the targets.
                foreach (long targetVertex in new List <long>(targets[idx].GetVertices()))
                {
                    // get the target path.
                    var targetPath = targets[idx].GetPathTo(targetVertex);

                    targetPath = targetPath.From;
                    while (targetPath != null)
                    { // add the path to the paths from source.
                        PathSegment <long> pathFromSource;
                        if (pathsFromSource.TryGetValue(targetPath.VertexId, out pathFromSource))
                        { // a path is found.
                            // get the existing path if any.
                            var existing = segmentsToTarget[idx];
                            if (existing == null)
                            { // a path did not exist yet!
                                segmentsToTarget[idx] = targetPath.Reverse().ConcatenateAfter(pathFromSource);
                                targets[idx].Remove(targetVertex);
                            }
                            else if (existing.Weight > targetPath.Weight + pathFromSource.Weight)
                            { // a new path is found with a lower weight.
                                segmentsToTarget[idx] = targetPath.Reverse().ConcatenateAfter(pathFromSource);
                            }
                        }
                        targetPath = targetPath.From;
                    }
                }
            }
            if (targets.Length > 0 && targets.All(x => x.Count == 0))
            { // routing is finished!
                return(segmentsToTarget.ToArray());
            }

            if (stopAtFirst)
            { // only one entry is needed.
                var oneFound = false;
                for (int idx = 0; idx < targets.Length; idx++)
                {
                    if (targets[idx].Count < targetsCount[idx])
                    {
                        oneFound = true;
                        break;
                    }
                }

                if (oneFound)
                { // targets found, return the shortest!
                    PathSegment <long> shortest = null;
                    foreach (PathSegment <long> foundTarget in segmentsToTarget)
                    {
                        if (shortest == null)
                        {
                            shortest = foundTarget;
                        }
                        else if (foundTarget != null &&
                                 shortest.Weight > foundTarget.Weight)
                        {
                            shortest = foundTarget;
                        }
                    }
                    segmentsToTarget    = new PathSegment <long> [1];
                    segmentsToTarget[0] = shortest;
                    return(segmentsToTarget);
                }
                else
                { // not targets found yet!
                    segmentsToTarget = new PathSegment <long> [1];
                }
            }

            // test for identical start/end point.
            for (int idx = 0; idx < targets.Length; idx++)
            {
                var target = targets[idx];
                if (returnAtWeight)
                { // add all the reached vertices larger than weight to the results.
                    if (current.Weight > weight)
                    {
                        var toPath = target.GetPathTo(current.Vertex);
                        toPath.Reverse();
                        toPath = toPath.ConcatenateAfter(current.ToPath(visits));
                        segmentsAtWeight.Add(toPath);
                    }
                }
                else if (target.Contains(current.Vertex))
                { // the current is a target!
                    var toPath = target.GetPathTo(current.Vertex);
                    toPath = toPath.Reverse();
                    toPath = toPath.ConcatenateAfter(current.ToPath(visits));

                    if (stopAtFirst)
                    { // stop at the first occurrence.
                        segmentsToTarget[0] = toPath;
                        return(segmentsToTarget);
                    }
                    else
                    { // normal one-to-many; add to the result.
                        // check if routing is finished.
                        if (segmentsToTarget[idx] == null)
                        { // make sure only the first route is set.
                            segmentsToTarget[idx] = toPath;
                            if (targets.All(x => x.Count == 0))
                            { // routing is finished!
                                return(segmentsToTarget.ToArray());
                            }
                        }
                        else if (segmentsToTarget[idx].Weight > toPath.Weight)
                        { // check if the second, third or later is shorter.
                            segmentsToTarget[idx] = toPath;
                        }
                    }
                }
            }

            // start OsmSharp.Routing.
            var edges = graph.GetEdges(Convert.ToUInt32(current.Vertex));

            visits[current.Vertex] = current;

            // loop until target is found and the route is the shortest!
            var noSpeed = new Speed()
            {
                Direction = null, MeterPerSecond = 0
            };

            while (true)
            {
                // get the current labels list (if needed).
                IList <RoutingLabel> currentLabels = null;
                if (interpreter.Constraints != null)
                { // there are constraints, get the labels.
                    currentLabels = labels[current.Vertex];
                    labels.Remove(current.Vertex);
                }

                // check turn-restrictions.
                //List<uint[]> restrictions = null;
                bool isRestricted = false;
                //if (current.From != null &&
                //    current.From.Vertex > 0 &&
                //    graph.TryGetRestrictionAsStart(vehicle, (uint)current.From.Vertex, out restrictions))
                //{ // there are restrictions!
                //    // search for a restriction that ends in the currently selected vertex.
                //    for(int idx = 0; idx < restrictions.Count; idx++)
                //    {
                //        var restriction = restrictions[idx];
                //        if(restriction[restriction.Length - 1] == current.VertexId)
                //        { // oeps, do not consider the neighbours of this vertex.
                //            isRestricted = true;
                //            break;
                //        }

                //        for(int restrictedIdx = 0; restrictedIdx < restriction.Length; restrictedIdx++)
                //        { // make sure the restricted vertices can be choosen multiple times.
                //            // restrictedVertices.Add(restriction[restrictedIdx]);
                //            visitList.SetRestricted(restriction[restrictedIdx]);
                //        }
                //    }
                //}
                if (!isRestricted)
                {
                    // update the visited nodes.
                    while (edges.MoveNext())
                    {
                        var edge      = edges;
                        var neighbour = edge.Neighbour;

                        if (current.From == neighbour)
                        { // don't go back!
                            continue;
                        }

                        if (visits.ContainsKey(neighbour))
                        { // has already been choosen.
                            continue;
                        }

                        //// prevent u-turns.
                        //if(current.From != null)
                        //{ // a possible u-turn.
                        //    if(current.From.VertexId == neighbour.Neighbour)
                        //    { // a u-turn, don't do this please!
                        //        continue;
                        //    }
                        //}

                        // get the speed from cache or calculate.
                        var edgeData = edge.EdgeData;
                        var speed    = noSpeed;
                        if (!speeds.TryGetValue(edgeData.Tags, out speed))
                        { // speed not there, calculate speed.
                            var tags = graph.TagsIndex.Get(edgeData.Tags);
                            speed = noSpeed;
                            if (vehicle.CanTraverse(tags))
                            { // can traverse, speed not null!
                                speed = new Speed()
                                {
                                    MeterPerSecond = ((OsmSharp.Units.Speed.MeterPerSecond)vehicle.ProbableSpeed(tags)).Value,
                                    Direction      = vehicle.IsOneWay(tags)
                                };
                            }
                            speeds.Add(edgeData.Tags, speed);
                        }

                        // check the tags against the interpreter.
                        if (speed.MeterPerSecond > 0 && (!speed.Direction.HasValue || speed.Direction.Value == edgeData.Forward))
                        {     // it's ok; the edge can be traversed by the given vehicle.
                            if ((current.From == 0 || interpreter.CanBeTraversed(current.From, current.Vertex, neighbour)))
                            { // the neighbour is forward and is not settled yet!
                                bool restrictionsOk = true;
                                //if (restrictions != null)
                                //{ // search for a restriction that ends in the currently selected neighbour and check if it's via-vertex matches.
                                //    for (int idx = 0; idx < restrictions.Count; idx++)
                                //    {
                                //        var restriction = restrictions[idx];
                                //        if (restriction[restriction.Length - 1] == neighbour.Neighbour)
                                //        { // oeps, do not consider the neighbours of this vertex.
                                //            if (restriction[restriction.Length - 2] == current.VertexId)
                                //            { // damn this route-part is restricted!
                                //                restrictionsOk = false;
                                //                break;
                                //            }
                                //        }
                                //    }
                                //}

                                // check the labels (if needed).
                                bool constraintsOk = true;
                                if (restrictionsOk && interpreter.Constraints != null)
                                { // check if the label is ok.
                                    var neighbourLabel = interpreter.Constraints.GetLabelFor(
                                        graph.TagsIndex.Get(edgeData.Tags));

                                    // only test labels if there is a change.
                                    if (currentLabels.Count == 0 || !neighbourLabel.Equals(currentLabels[currentLabels.Count - 1]))
                                    { // labels are different, test them!
                                        constraintsOk = interpreter.Constraints.ForwardSequenceAllowed(currentLabels,
                                                                                                       neighbourLabel);

                                        if (constraintsOk)
                                        { // update the labels.
                                            var neighbourLabels = new List <RoutingLabel>(currentLabels);
                                            neighbourLabels.Add(neighbourLabel);

                                            labels[neighbour] = neighbourLabels;
                                        }
                                    }
                                    else
                                    { // set the same label(s).
                                        labels[neighbour] = currentLabels;
                                    }
                                }

                                if (constraintsOk && restrictionsOk)
                                { // all constraints are validated or there are none.
                                    // calculate neighbors weight.
                                    double totalWeight = current.Weight + (edgeData.Distance / speed.MeterPerSecond);
                                    //double totalWeight = current.Weight + edgeData.Distance;

                                    // update the visit list.
                                    var neighbourVisit = new DykstraVisit(neighbour, current.Vertex, (float)totalWeight);// new PathSegment<long>(neighbour, totalWeight, current);
                                    heap.Push(neighbourVisit, neighbourVisit.Weight);
                                }
                            }
                        }
                    }
                }

                // while the visit list is not empty.
                current = null;
                if (heap.Count > 0)
                { // choose the next vertex.
                    current = heap.Pop();
                    while (current != null && visits.ContainsKey(current.Vertex))
                    { // keep dequeuing.
                        current = heap.Pop();
                    }
                }
                while (current != null && current.Weight > weight)
                {
                    if (returnAtWeight)
                    { // add all the reached vertices larger than weight to the results.
                        segmentsAtWeight.Add(current.ToPath(visits));
                    }

                    // choose the next vertex.
                    current = heap.Pop();
                    while (current != null && visits.ContainsKey(current.Vertex))
                    { // keep dequeuing.
                        current = heap.Pop();
                    }
                }
                if (current != null)
                { // we visit this one, set visit.
                    visits[current.Vertex] = current;
                }
                else
                { // route is not found, there are no vertices left
                    // or the search went outside of the max bounds.
                    break;
                }

                // check target.
                for (int idx = 0; idx < targets.Length; idx++)
                {
                    PathSegmentVisitList target = targets[idx];
                    if (target.Contains(current.Vertex))
                    { // the current is a target!
                        var toPath = target.GetPathTo(current.Vertex);
                        toPath = toPath.Reverse();
                        toPath = toPath.ConcatenateAfter(current.ToPath(visits));

                        if (stopAtFirst)
                        { // stop at the first occurrence.
                            segmentsToTarget[0] = toPath;
                            return(segmentsToTarget);
                        }
                        else
                        { // normal one-to-many; add to the result.
                            // check if routing is finished.
                            if (segmentsToTarget[idx] == null)
                            { // make sure only the first route is set.
                                segmentsToTarget[idx] = toPath;
                            }
                            else if (segmentsToTarget[idx].Weight > toPath.Weight)
                            { // check if the second, third or later is shorter.
                                segmentsToTarget[idx] = toPath;
                            }

                            // remove this vertex from this target's paths.
                            target.Remove(current.Vertex);

                            // if this target is empty it's optimal route has been found.
                            if (target.Count == 0)
                            {     // now the shortest route has been found for sure!
                                if (targets.All(x => x.Count == 0))
                                { // routing is finished!
                                    // OsmSharp.Logging.Log.TraceEvent("Dykstra", TraceEventType.Information, string.Format("Finished with {0} visits.", visits.Count));
                                    return(segmentsToTarget.ToArray());
                                }
                            }
                        }
                    }
                }

                // get the neighbors of the current node.
                edges = graph.GetEdges(Convert.ToUInt32(current.Vertex));
            }

            // return the result.
            if (!returnAtWeight)
            {
                // OsmSharp.Logging.Log.TraceEvent("Dykstra", TraceEventType.Information, string.Format("Finished with {0} visits.", visits.Count));
                return(segmentsToTarget.ToArray());
            }
            // OsmSharp.Logging.Log.TraceEvent("Dykstra", TraceEventType.Information, string.Format("Finished with {0} visits.", visits.Count));
            return(segmentsAtWeight.ToArray());
        }
Beispiel #3
0
        /// <summary>
        /// Builds the scene.
        /// </summary>
        /// <param name="map"></param>
        /// <param name="zoomFactor"></param>
        /// <param name="center"></param>
        /// <param name="view"></param>
        private void BuildScene(Map map, float zoomFactor, GeoCoordinate center, View2D view)
        {
            // get the indexed object at this zoom.
            HashSet <ArcId> interpretedObjects;

            if (!_interpretedObjects.TryGetValue((int)zoomFactor, out interpretedObjects))
            {
                interpretedObjects = new HashSet <ArcId>();
                _interpretedObjects.Add((int)zoomFactor, interpretedObjects);
            }

            // build the boundingbox.
            var viewBox = view.OuterBox;
            var box     = new GeoCoordinateBox(map.Projection.ToGeoCoordinates(viewBox.Min[0], viewBox.Min[1]),
                                               map.Projection.ToGeoCoordinates(viewBox.Max[0], viewBox.Max[1]));

            foreach (var requestedBox in _requestedBoxes)
            {
                if (requestedBox.Contains(box))
                {
                    return;
                }
            }
            _requestedBoxes.Add(box);

            //// set the scene backcolor.
            //SimpleColor? color = _styleInterpreter.GetCanvasColor ();
            //_scene.BackColor = color.HasValue
            //                               ? color.Value.Value
            //                               : SimpleColor.FromArgb (0, 255, 255, 255).Value;

            // get data.
            foreach (var arc in _dataSource.GetEdges(box))
            {
                // translate each object into scene object.
                var arcId = new ArcId()
                {
                    Vertex1 = arc.Vertex1,
                    Vertex2 = arc.Vertex2
                };
                if (!interpretedObjects.Contains(arcId))
                {
                    interpretedObjects.Add(arcId);

                    // create nodes.
                    float latitude, longitude;
                    _dataSource.GetVertex(arcId.Vertex1, out latitude, out longitude);
                    var node1 = new Node();
                    node1.Id        = arcId.Vertex1;
                    node1.Latitude  = latitude;
                    node1.Longitude = longitude;
                    _dataSource.GetVertex(arcId.Vertex2, out latitude, out longitude);
                    var node2 = new Node();
                    node2.Id        = arcId.Vertex2;
                    node2.Latitude  = latitude;
                    node2.Longitude = longitude;

                    // create way.
                    var way = CompleteWay.Create(-1);
                    if (arc.EdgeData.Forward)
                    {
                        way.Nodes.Add(node1);
                        way.Nodes.Add(node2);
                    }
                    else
                    {
                        way.Nodes.Add(node2);
                        way.Nodes.Add(node1);
                    }
                    way.Tags.AddOrReplace(_dataSource.TagsIndex.Get(arc.EdgeData.Tags));

                    _styleInterpreter.Translate(_scene, map.Projection, way);
                    interpretedObjects.Add(arcId);
                }
            }
        }