diff --git a/src/routing.jl b/src/routing.jl index 69471ce..9a8b166 100644 --- a/src/routing.jl +++ b/src/routing.jl @@ -6,15 +6,7 @@ ### Get list of vertices (highway nodes) in specified levels of classes ### # For all highways -function highwayVertices(highways::Dict{Int,Highway}) - vertices = Set{Int}() - - for highway in values(highways) - union!(vertices, highway.nodes) - end - - return vertices -end +highwayVertices(highways::Dict{Int,Highway}) = foldl(union!, Set{Int}(), values(highways)) # For classified highways function highwayVertices(highways::Dict{Int,Highway}, classes::Dict{Int,Int}) @@ -42,14 +34,14 @@ end ### Form transportation network graph of map ### function createGraph(nodes, highways::Dict{Int,Highway}, classes, levels, reverse::Bool=false) - v = Dict{Int,Graphs.KeyVertex{Int}}() # Vertices w = Float64[] # Weights g_classes = Int[] # Road classes - g = Graphs.inclist(Graphs.KeyVertex{Int}, is_directed=true) # Graph - - verts = highwayVertices(highways, classes, levels) - for vert in verts - v[vert] = Graphs.add_vertex!(g, vert) + vertices = Int[highwayVertices(highways, classes, levels)...] + g = Graphs.inclist(vertices, is_directed=true) + + index = Dict{Int,Int}() + for (i,node) in enumerate(vertices) + index[node] = i end for (key, class) in classes @@ -58,22 +50,15 @@ function createGraph(nodes, highways::Dict{Int,Highway}, classes, levels, revers if length(highway.nodes) > 1 # Add edges to graph and compute weights for n = 2:length(highway.nodes) - if reverse - node0 = highway.nodes[n] - node1 = highway.nodes[n-1] - else - node0 = highway.nodes[n-1] - node1 = highway.nodes[n] - end - edge = Graphs.make_edge(g, v[node0], v[node1]) + node0 = reverse ? highway.nodes[n] : highway.nodes[n-1] + node1 = reverse ? highway.nodes[n-1] : highway.nodes[n] + edge = Graphs.make_edge(g, node0, node1) Graphs.add_edge!(g, edge) weight = distance(nodes, node0, node1) push!(w, weight) push!(g_classes, class) - node_set = Set(node0, node1) - if !highway.oneway - edge = Graphs.make_edge(g, v[node1], v[node0]) + edge = Graphs.make_edge(g, node1, node0) Graphs.add_edge!(g, edge) push!(w, weight) push!(g_classes, class) @@ -83,45 +68,39 @@ function createGraph(nodes, highways::Dict{Int,Highway}, classes, levels, revers end end - return Network(g, v, w, g_classes) + return Network(g, vertices, index, w, g_classes) end ### Form transportation network graph of map ### function createGraph(segments::Vector{Segment}, intersections, reverse::Bool=false) - v = Dict{Int,Graphs.KeyVertex{Int}}() # Vertices w = Float64[] # Weights class = Int[] # Road class - g = Graphs.inclist(Graphs.KeyVertex{Int}, is_directed=true) # Graph - - for vert in keys(intersections) - v[vert] = Graphs.add_vertex!(g, vert) + vertices = Int[keys(intersections)...] + g = Graphs.inclist(vertices, is_directed=true) + + index = Dict{Int,Int}() + for (i,node) in enumerate(vertices) + index[node] = i end for segment in segments # Add edges to graph and compute weights - if reverse - node0 = segment.node1 - node1 = segment.node0 - else - node0 = segment.node0 - node1 = segment.node1 - end - edge = Graphs.make_edge(g, v[node0], v[node1]) + node0 = reverse ? segment.node1 : segment.node0 + node1 = reverse ? segment.node0 : segment.node1 + edge = Graphs.make_edge(g, node0, node1) Graphs.add_edge!(g, edge) weight = segment.dist push!(w, weight) push!(class, segment.class) - node_set = Set(node0, node1) - if !segment.oneway - edge = Graphs.make_edge(g, v[node1], v[node0]) + edge = Graphs.make_edge(g, node1, node0) Graphs.add_edge!(g, edge) push!(w, weight) push!(class, segment.class) end end - return Network(g, v, w, class) + return Network(g, vertices, index, w, class) end @@ -143,52 +122,52 @@ end ### Get distance between two nodes ### # ENU Coordinates -function distance(nodes::Dict{Int,ENU}, node0, node1) - loc0 = nodes[node0] - loc1 = nodes[node1] +function distance(nodes::Dict{Int,ENU}, node0::Int, node1::Int) + loc0 = nodes[node0]::ENU + loc1 = nodes[node1]::ENU return distance(loc0, loc1) end function distance(loc0::ENU, loc1::ENU) - x0 = loc0.east - y0 = loc0.north - z0 = loc0.up + x0 = loc0.east::Float64 + y0 = loc0.north::Float64 + z0 = loc0.up::Float64 - x1 = loc1.east - y1 = loc1.north - z1 = loc1.up + x1 = loc1.east::Float64 + y1 = loc1.north::Float64 + z1 = loc1.up::Float64 return distance(x0, y0, z0, x1, y1, z1) end # ECEF Coordinates -function distance(nodes::Dict{Int,ECEF}, node0, node1) - loc0 = nodes[node0] - loc1 = nodes[node1] +function distance(nodes::Dict{Int,ECEF}, node0::Int, node1::Int) + loc0 = nodes[node0]::ECEF + loc1 = nodes[node1]::ECEF return distance(loc0, loc1) end function distance(loc0::ECEF, loc1::ECEF) - x0 = loc0.x - y0 = loc0.y - z0 = loc0.z + x0 = loc0.x::Float64 + y0 = loc0.y::Float64 + z0 = loc0.z::Float64 - x1 = loc1.x - y1 = loc1.y - z1 = loc1.z + x1 = loc1.x::Float64 + y1 = loc1.y::Float64 + z1 = loc1.z::Float64 return distance(x0, y0, z0, x1, y1, z1) end # Cartesian coordinates -function distance(x0, y0, z0, x1, y1, z1) +function distance(x0::Float64, y0::Float64, z0::Float64, x1::Float64, y1::Float64, z1::Float64) return sqrt((x1-x0)^2 + (y1-y0)^2 + (z1-z0)^2) end ### Compute the distance of a route ### -function distance(nodes, route) +function distance{T<:Union(ENU,ECEF)}(nodes::Dict{Int64,T}, route::Vector{Int}) dist = 0 for n = 2:length(route) dist += distance(nodes, route[n-1], route[n]) @@ -199,34 +178,32 @@ end ### Shortest Paths ### # Dijkstra's Algorithm -function dijkstra(g, w, start_vertex) +function dijkstra(g, w::Vector{Float64}, start_vertex::Int) return Graphs.dijkstra_shortest_paths(g, w, start_vertex) end # Bellman Ford's Algorithm -function bellmanFord(g, w, start_vertices) +function bellmanFord(g, w::Vector{Float64}, start_vertices::Vector{Int}) return Graphs.bellman_ford_shortest_paths(g, w, start_vertices) end # Extract route from Dijkstra results object -function extractRoute(dijkstra, start_index, finish_index) + function extractRoute(dijkstra, network::Network, node::Int) route = Int[] - - distance = dijkstra.dists[finish_index] - - if distance != Inf - index = finish_index - push!(route, index) - while index != start_index - index = dijkstra.parents[index].index - push!(route, index) + vertices = network.v + index = network.index[node] + distance = dijkstra.dists[index] + if dijkstra.hasparent[index] || dijkstra.parents[index] == vertices[index] + while dijkstra.hasparent[index] + push!(route, vertices[index]) + index = network.index[dijkstra.parents[index]] end + push!(route, vertices[index]) + reverse!(route) end - reverse!(route) - return route, distance -end + end ### Generate an ordered list of edges traversed in route function routeEdges(network::Network, route::Vector{Int}) @@ -237,8 +214,8 @@ function routeEdges(network::Network, route::Vector{Int}) s = route[n] t = route[n+1] - for e_candidate in Graphs.out_edges(network.v[s],network.g) - if t == e_candidate.target.key + for e_candidate in Graphs.out_edges(s,network.g) + if t == e_candidate.target e[n] = e_candidate.index break end @@ -249,28 +226,10 @@ function routeEdges(network::Network, route::Vector{Int}) end ### Shortest Route ### -function shortestRoute(network, node0, node1) - start_vertex = network.v[node0] - - dijkstra_result = dijkstra(network.g, network.w, start_vertex) - - start_index = network.v[node0].index - finish_index = network.v[node1].index - route_indices, distance = extractRoute(dijkstra_result, start_index, finish_index) - - route_nodes = getRouteNodes(network, route_indices) - - return route_nodes, distance -end - -function getRouteNodes(network, route_indices) - route_nodes = Array(Int, length(route_indices)) - v = Graphs.vertices(network.g) - for n = 1:length(route_indices) - route_nodes[n] = v[route_indices[n]].key - end - - return route_nodes +function shortestRoute(network::Network, node0::Int, node1::Int) + dijkstra_result = dijkstra(network.g, network.w, node0) + distance = dijkstra_result.dists[network.index[node1]] + return extractRoute(dijkstra_result, network, node1) end function networkTravelTimes(network, class_speeds) @@ -283,75 +242,54 @@ function networkTravelTimes(network, class_speeds) end ### Fastest Route ### -function fastestRoute(network, node0, node1, class_speeds=SPEED_ROADS_URBAN) - start_vertex = network.v[node0] - +function fastestRoute(network::Network, node0::Int, node1::Int, class_speeds=SPEED_ROADS_URBAN) # Modify weights to be times rather than distances w = networkTravelTimes(network, class_speeds) - dijkstra_result = dijkstra(network.g, w, start_vertex) - - start_index = network.v[node0].index - finish_index = network.v[node1].index - route_indices, route_time = extractRoute(dijkstra_result, start_index, finish_index) - - route_nodes = getRouteNodes(network, route_indices) - - return route_nodes, route_time + dijkstra_result = dijkstra(network.g, w, node0) + distance = dijkstra_result.dists[network.index[node1]] + return extractRoute(dijkstra_result, network, node1) end -function filterVertices(vertices, weights, limit) +function filterVertices(vertices::Vector{Int}, weights::Vector{Float64}, limit::Float64) if limit == Inf @assert length(vertices) == length(weights) - return keys(vertices), weights + return vertices, weights end - indices = Int[] + nodes = Int[] distances = Float64[] - for vertex in vertices - distance = weights[vertex.index] + for (i,vertex) in enumerate(vertices) + distance = weights[i] if distance < limit - push!(indices, vertex.key) + push!(nodes, vertex) push!(distances, distance) end end - return indices, distances + return nodes, distances end # Extract nodes from BellmanFordStates object within an (optional) limit # based on driving distance -function nodesWithinDrivingDistance(network::Network, start_indices, limit=Inf) - start_vertices = [network.v[i] for i in start_indices] - bellmanford = bellmanFord(network.g, network.w, start_vertices) - return filterVertices(values(network.v), bellmanford.dists, limit) +function nodesWithinDrivingDistance(network::Network, start_nodes::Vector{Int}, limit::Float64=Inf) + bellmanford = bellmanFord(network.g, network.w, start_nodes) + return filterVertices(network.v, bellmanford.dists, float(limit)) end -function nodesWithinDrivingDistance(network::Network, - loc::ENU, - limit=Inf, - loc_range=100.0) - return nodesWithinDrivingDistance(network, - nodesWithinRange(network.v, loc, loc_range), - limit) -end +nodesWithinDrivingDistance(network::Network, start_node::Int, limit::Float64=Inf) = + nodesWithinDrivingDistance(network, [start_node], limit) + +nodesWithinDrivingDistance(network::Network, loc::ENU, limit::Float64=Inf, loc_range::Float64=100.0) = + nodesWithinDrivingDistance(network, nodesWithinRange(network.v, loc, loc_range), limit) -# Extract nodes from BellmanFordStates object within a (optional) limit, -# based on driving time -function nodesWithinDrivingTime(network, - start_indices, - limit=Inf, - class_speeds=SPEED_ROADS_URBAN) +# Extract nodes from BellmanFordStates object within a (optional) limit, based on driving time +function nodesWithinDrivingTime(network, start_nodes::Vector{Int}, limit::Float64=Inf, class_speeds=SPEED_ROADS_URBAN) # Modify weights to be times rather than distances w = networkTravelTimes(network, class_speeds) - start_vertices = [network.v[i] for i in start_indices] - bellmanford = bellmanFord(network.g, w, start_vertices) - return filterVertices(values(network.v), bellmanford.dists, limit) + bellmanford = bellmanFord(network.g, w, start_nodes) + return filterVertices(network.v, bellmanford.dists, limit) end -function nodesWithinDrivingTime(network::Network, - loc::ENU, - limit=Inf, - class_speeds=SPEED_ROADS_URBAN, - loc_range=100.0) - return nodesWithinDrivingTime(network, - nodesWithinRange(network.v, loc, loc_range), - limit) -end \ No newline at end of file +nodesWithinDrivingTime(network, start_node::Int, limit::Float64=Inf, class_speeds=SPEED_ROADS_URBAN) = + nodesWithinDrivingTime(network, [start_node], limit, class_speeds) + +nodesWithinDrivingTime(network::Network, loc::ENU, limit=Inf, class_speeds=SPEED_ROADS_URBAN, loc_range=100.0) = + nodesWithinDrivingTime(network, nodesWithinRange(network.v, loc, loc_range), limit) diff --git a/src/types.jl b/src/types.jl index c80ae9f..324668e 100644 --- a/src/types.jl +++ b/src/types.jl @@ -61,9 +61,10 @@ end # Transporation network graph data and helpers to increase routing speed type Network g # Graph object - v::Dict{Int,Graphs.KeyVertex{Int}} # (node id) => (graph vertex) + v::Vector{Int} # OSM IDs + index::Dict{Int,Int} # (osm id) => (node index [in v]) w::Vector{Float64} # Edge weights, indexed by edge id - class::Vector{Int} # Road class of each edge + class::Vector{Int} # Road class of each edge end ################### diff --git a/test/routes.jl b/test/routes.jl index 4414629..b3833eb 100644 --- a/test/routes.jl +++ b/test/routes.jl @@ -25,8 +25,8 @@ network = createGraph(nodesENU, hwys, roads, Set(1:8)) loc_start = OpenStreetMap.ENU(-5000, 5500, 0) loc_end = OpenStreetMap.ENU(5500, -4000, 0) -node0 = nearestNode(nodesENU, loc_start, collect(keys(network.v))) -node1 = nearestNode(nodesENU, loc_end, collect(keys(network.v))) +node0 = nearestNode(nodesENU, loc_start, network.v) +node1 = nearestNode(nodesENU, loc_end, network.v) @test_approx_eq nodesENU[node0].east -197.70015977531895 @test_approx_eq_eps nodesENU[node0].north 129.2258444276026 1e-8 @@ -68,20 +68,20 @@ edges = OpenStreetMap.getEdges(network) for k = 1:Graphs.num_edges(network.g) @test edges[k].index == k end -@test edges[10].source.index == 128 -@test edges[10].target.index == 154 -@test edges[20].source.index == 111 -@test edges[30].source.index == 106 -@test edges[40].source.index == 9 -@test edges[50].source.index == 22 +@test network.index[edges[10].source] == 128 +@test network.index[edges[10].target] == 154 +@test network.index[edges[20].source] == 111 +@test network.index[edges[30].source] == 106 +@test network.index[edges[40].source] == 9 +@test network.index[edges[50].source] == 22 # Form transportation network from segments intersections = findIntersections(hwys) segments = segmentHighways(nodesENU, hwys, intersections, roads, Set(1:8)) segment_network = createGraph(segments, intersections) -node0 = nearestNode(nodesENU, loc_start, collect(keys(segment_network.v))) -node1 = nearestNode(nodesENU, loc_end, collect(keys(segment_network.v))) +node0 = nearestNode(nodesENU, loc_start, segment_network.v) +node1 = nearestNode(nodesENU, loc_end, segment_network.v) # Shortest route _, shortest_segment_distance = shortestRoute(segment_network, node0, node1) @@ -109,7 +109,7 @@ _, r_fastest_segment_time = fastestRoute(r_segment_network, node1, node0) # Nodes within Range loc0 = nodesENU[node0] -filteredENU = filter((k,v)->haskey(network.v,k), nodesENU) +filteredENU = filter((k,v)-> k in network.v, nodesENU) local_indices = nodesWithinRange(filteredENU, loc0, 100.0) @test length(local_indices) == 4 for index in [61317384, 23, 61317383, 17]