Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #8 from bartoszpankratz/master
Added A* search algorithm implementation
- Loading branch information
Showing
9 changed files
with
269 additions
and
68 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,114 @@ | ||
| """ | ||
| get_distance(A::Int, B::Int, | ||
| nodes::Dict{Int,T} , | ||
| vertices_to_nodes::Dict{Int,Int}) where T<:Union{OpenStreetMapX.ENU,OpenStreetMapX.ECEF} | ||
| Auxiliary function - takes two vertices of graph and return the distance between them. | ||
| Used to compute straight line distance heuristic for A* algorithm. | ||
| **Arguments** | ||
| * `A` : start vertex | ||
| * `B` : end vertex | ||
| * `nodes` : dictionary of .osm nodes ID's and correspoding points coordinates | ||
| * `vertices_to_nodes` : dictionary mapping graph vertices to .osm file nodes | ||
| """ | ||
| function get_distance(A::Int, B::Int, | ||
| nodes::Dict{Int,T}, | ||
| vertices_to_nodes::Dict{Int,Int}) where T<:Union{OpenStreetMapX.ENU,OpenStreetMapX.ECEF} | ||
| A,B = vertices_to_nodes[A], vertices_to_nodes[B] | ||
| OpenStreetMapX.distance(nodes[A],nodes[B]) | ||
| end | ||
|
|
||
|
|
||
| function extract_a_star_route(parents::Vector{Int},s::Int, u::Int) | ||
| route = Int[] | ||
| index = u | ||
| push!(route,index) | ||
| while index != s | ||
| index = parents[index] | ||
| push!(route, index) | ||
| end | ||
| reverse!(route) | ||
| end | ||
|
|
||
| """ | ||
| a_star_algorithm(g::AbstractGraph{U}, | ||
| s::Integer, | ||
| t::Integer, | ||
| distmx::AbstractMatrix{T}=LightGraphs.weights(g), | ||
| heuristic::Function = n -> zero(T)) where {T, U} | ||
| High level function - implementation of A star search algorithm: | ||
| (https://en.wikipedia.org/wiki/A*_search_algorithm). | ||
| Based on the implementation in LightGraphs library, | ||
| however significantly improved in terms of performance. | ||
| **Arguments** | ||
| * `g` : graph object | ||
| * `S` : start vertex | ||
| * `t` : end vertex | ||
| * `distmx` : distance matrix | ||
| * `heuristic` : search heuristic function; by default returns zero | ||
| """ | ||
| function a_star_algorithm(g::LightGraphs.AbstractGraph{U}, # the g | ||
| s::Integer, # the start vertex | ||
| t::Integer, # the end vertex | ||
| distmx::AbstractMatrix{T}=LightGraphs.weights(g), | ||
| heuristic::Function = n -> zero(T)) where {T, U} | ||
| checkbounds(distmx, Base.OneTo(nv(g)), Base.OneTo(nv(g))) | ||
| frontier = DataStructures.PriorityQueue{Tuple{T, U},T}() | ||
| frontier[(zero(T), U(s))] = zero(T) | ||
| nvg = nv(g) | ||
| visited = zeros(Bool, nvg) | ||
| dists = fill(typemax(T), nvg) | ||
| parents = zeros(U, nvg) | ||
| colormap = zeros(UInt8, nvg) | ||
| colormap[s] = 1 | ||
| @inbounds while !isempty(frontier) | ||
| (cost_so_far, u) = dequeue!(frontier) | ||
| u == t && (return OpenStreetMapX.extract_a_star_route(parents,s,u), cost_so_far) | ||
| for v in LightGraphs.outneighbors(g, u) | ||
| if get(colormap, v, 0) < 2 | ||
| dist = distmx[u, v] | ||
| colormap[v] = 1 | ||
| path_cost = cost_so_far + dist | ||
| if !visited[v] | ||
| visited[v] = true | ||
| parents[v] = u | ||
| dists[v] = path_cost | ||
| enqueue!(frontier, | ||
| (path_cost, v), | ||
| path_cost + heuristic(v,t)) | ||
| elseif path_cost < dists[v] | ||
| parents[v] = u | ||
| dists[v] = path_cost | ||
| frontier[path_cost, v] = path_cost + heuristic(v,t) | ||
| end | ||
| end | ||
| end | ||
| colormap[u] = 2 | ||
| end | ||
| Vector{U}(), Inf | ||
| end | ||
|
|
||
| """ | ||
| a_star_algorithm(m::OpenStreetMapX.MapData, | ||
| s::Integer, | ||
| t::Integer) | ||
| A star search algorithm with straight line distance heuristic | ||
| **Arguments** | ||
| * `m` : MapData object | ||
| * `S` : start vertex | ||
| * `t` : end vertex | ||
| * `distmx` : distance matrix | ||
| """ | ||
| function a_star_algorithm(m::OpenStreetMapX.MapData, | ||
| s::Integer, | ||
| t::Integer) | ||
| heuristic(u,v) = OpenStreetMapX.get_distance(u, v, m.nodes, m.n) | ||
| OpenStreetMapX.a_star_algorithm(m.g,s,t,m.w,heuristic) | ||
| end |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.