/
nodes.jl
146 lines (121 loc) · 4.91 KB
/
nodes.jl
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
"""
add_new_node!(nodes::Dict{Int,T},loc::T, start_id::Int = reinterpret((Int), hash(loc))) where T <: (Union{OpenStreetMapX.LLA,OpenStreetMapX.ENU})
Add a New Node
"""
function add_new_node!(nodes::Dict{Int,T},loc::T, start_id::Int = reinterpret((Int), hash(loc))) where T <: (Union{OpenStreetMapX.LLA,OpenStreetMapX.ENU})
id = start_id
while id <= typemax(Int)
if !haskey(nodes, id)
nodes[id] = loc
return id
end
id += 1
end
msg = "Unable to add new node to map, $(typemax(Int)) nodes is the current limit."
throw(error(msg))
end
"""
nearest_node(nodes::Dict{Int,T}, loc::T) where T<:(Union{OpenStreetMapX.ENU,OpenStreetMapX.ECEF})
Find the nearest node to a given location `loc`
"""
function nearest_node(nodes::Dict{Int,T}, loc::T) where T<:(Union{OpenStreetMapX.ENU,OpenStreetMapX.ECEF})
min_dist = Inf
best_ind = 0
for (key, node) in nodes
dist = OpenStreetMapX.distance(node, loc)
if dist < min_dist
min_dist = dist
best_ind = key
end
end
return best_ind
end
"""
nearest_node(m::MapData, loc::ENU, vs_only::Bool=true)
Find the nearest node to a given location `loc`
"""
function nearest_node(m::MapData, loc::ENU, vs_only::Bool=true)
vs_only ? nearest_node(m.nodes,loc, keys(m.v)) : nearest_node(m.nodes,loc)
end
"""
nearest_node(nodes::Dict{Int,T}, loc::T, node_list::AbstractSet{Int}) where T<:(Union{OpenStreetMapX.ENU,OpenStreetMapX.ECEF})
Find the nearest node in a list of nodes
"""
function nearest_node(nodes::Dict{Int,T}, loc::T, node_list::AbstractSet{Int}) where T<:(Union{OpenStreetMapX.ENU,OpenStreetMapX.ECEF})
min_dist = Inf
best_ind = 0
for ind in node_list
dist = OpenStreetMapX.distance(nodes[ind], loc)
if dist < min_dist
min_dist = dist
best_ind = ind
end
end
return best_ind
end
### Find nearest node serving as a vertex in a routing network ###
#nearest_node(nodes::Dict{Int,T}, loc::T, m::OpenStreetMapX.MapData) where T<:(Union{OpenStreetMapX.ENU,OpenStreetMapX.ECEF}) = OpenStreetMapX.nearest_node(nodes,loc,collect(keys(m.v)))
#nearest_node(loc::T, m::OpenStreetMapX.MapData) where T<:(Union{OpenStreetMapX.ENU,OpenStreetMapX.ECEF}) = OpenStreetMapX.nearest_node(m.nodes,loc,collect(keys(m.v)))
"""
nodes_within_range(nodes::Dict{Int,T}, loc::T, range::Float64 = Inf) where T<:(Union{OpenStreetMapX.ENU,OpenStreetMapX.ECEF})
Find all nodes within range of a location
"""
function nodes_within_range(nodes::Dict{Int,T}, loc::T, range::Float64 = Inf) where T<:(Union{OpenStreetMapX.ENU,OpenStreetMapX.ECEF})
if range == Inf
return keys(nodes)
end
indices = Int[]
for (key, node) in nodes
dist = OpenStreetMapX.distance(node, loc)
if dist < range
push!(indices, key)
end
end
return indices
end
"""
nodes_within_range(nodes::Dict{Int,T}, loc::T, node_list::AbstractSet{Int}, range::Float64 = Inf) where T<:(Union{OpenStreetMapX.ENU,OpenStreetMapX.ECEF})
Find nodes within range of a location using a subset of nodes
"""
function nodes_within_range(nodes::Dict{Int,T}, loc::T, node_list::AbstractSet{Int}, range::Float64 = Inf) where T<:(Union{OpenStreetMapX.ENU,OpenStreetMapX.ECEF})
if range == Inf
return collect(node_list)
end
indices = Int[]
for ind in node_list
dist = OpenStreetMapX.distance(nodes[ind], loc)
if dist < range
push!(indices, ind)
end
end
return indices
end
"""
nodes_within_range(nodes::Dict{Int,T},loc::T, m::OpenStreetMapX.MapData, range::Float64 = Inf) where T <:(Union{OpenStreetMapX.ENU,OpenStreetMapX.ECEF})
Find vertices of a routing network within range of a location
"""
nodes_within_range(nodes::Dict{Int,T},loc::T, m::OpenStreetMapX.MapData, range::Float64 = Inf) where T <:(Union{OpenStreetMapX.ENU,OpenStreetMapX.ECEF}) = OpenStreetMapX.nodes_within_range(nodes,loc,keys(m.v),range)
"""
centroid(nodes::Dict{Int,T}, node_list::Vector{Int}) where T<:(Union{OpenStreetMapX.LLA,OpenStreetMapX.ENU})
Compute Centroid of List of Nodes
"""
function centroid(nodes::Dict{Int,T}, node_list::Vector{Int}) where T<:(Union{OpenStreetMapX.LLA,OpenStreetMapX.ENU})
sum_1 = 0
sum_2 = 0
sum_3 = 0
if typeof(nodes) == Dict{Int,OpenStreetMapX.LLA}
for k = 1:length(node_list)
sum_1 += nodes[node_list[k]].lat
sum_2 += nodes[node_list[k]].lon
sum_3 += nodes[node_list[k]].alt
end
return OpenStreetMapX.LLA(sum_1/length(node_list),sum_2/length(node_list),sum_3/length(node_list))
else
for k = 1:length(node_list)
sum_1 += nodes[node_list[k]].east
sum_2 += nodes[node_list[k]].north
sum_3 += nodes[node_list[k]].up
end
return OpenStreetMapX.ENU(sum_1/length(node_list),sum_2/length(node_list),sum_3/length(node_list))
end
end