#include <routingkit/osm_graph_builder.h>
#include <routingkit/geo_dist.h>
#include <routingkit/timer.h>
#include <routingkit/sort.h>
#include <routingkit/inverse_vector.h>
#include <routingkit/permutation.h>
#include <routingkit/graph_util.h>
#include <routingkit/bit_vector.h>
#include <routingkit/filter.h>
#include <routingkit/id_mapper.h>
#include <routingkit/osm_decoder.h>
#include <vector>
#include <cstdint>
#include <string>
#include <stdio.h>
#include <memory>
namespace RoutingKit{
OSMRoutingIDMapping load_osm_id_mapping_from_pbf(
const std::string&file_name,
std::function<bool(uint64_t, const TagMap&)>is_routing_node,
std::function<bool(uint64_t, const TagMap&)>is_way_used_for_routing,
std::function<void(const std::string&)>log_message,
bool all_modelling_nodes_are_routing_nodes
){
OSMRoutingIDMapping map;
long long timer=0;
if(log_message){
log_message("Scanning OSM PBF data to determine IDs");
if(all_modelling_nodes_are_routing_nodes)
log_message("All modelling nodes are routing nodes");
else
log_message("Not all modelling nodes are routing nodes");
timer = -get_micro_time();
}
std::function<void(uint64_t,double,double,const TagMap&)>node_callback;
if(is_routing_node){
node_callback = [&](uint64_t osm_node_id, double lat, double lon, const TagMap&tags){
if(is_routing_node(osm_node_id, tags)){
map.is_modelling_node.make_large_enough_for(osm_node_id);
map.is_modelling_node.set(osm_node_id);
map.is_routing_node.make_large_enough_for(osm_node_id);
map.is_routing_node.set(osm_node_id);
}
};
}
std::function<void(uint64_t,const std::vector<std::uint64_t>& osm_node_id_list,const TagMap&)>way_callback;
if(!all_modelling_nodes_are_routing_nodes){
way_callback = [&](uint64_t osm_way_id, const std::vector<std::uint64_t>& osm_node_id_list, const TagMap&tags) {
if(osm_node_id_list.size() >= 2 && is_way_used_for_routing(osm_way_id, tags)){
map.is_routing_way.make_large_enough_for(osm_way_id);
map.is_routing_way.set(osm_way_id);
for(std::uint64_t osm_node_id : osm_node_id_list) {
map.is_modelling_node.make_large_enough_for(osm_node_id);
if(map.is_modelling_node.is_set(osm_node_id)) {
map.is_routing_node.make_large_enough_for(osm_node_id);
map.is_routing_node.set(osm_node_id);
} else {
map.is_modelling_node.set(osm_node_id);
}
}
assert(map.is_modelling_node.is_set(osm_node_id_list.front()));
assert(map.is_modelling_node.is_set(osm_node_id_list.back()));
map.is_routing_node.make_large_enough_for(osm_node_id_list.front());
map.is_routing_node.set(osm_node_id_list.front());
map.is_routing_node.make_large_enough_for(osm_node_id_list.back());
map.is_routing_node.set(osm_node_id_list.back());
}
};
}else{
way_callback = [&](uint64_t osm_way_id, const std::vector<std::uint64_t>& osm_node_id_list, const TagMap&tags) {
if(osm_node_id_list.size() >= 2 && is_way_used_for_routing(osm_way_id, tags)){
map.is_routing_way.make_large_enough_for(osm_way_id);
map.is_routing_way.set(osm_way_id);
for(std::uint64_t osm_node_id : osm_node_id_list) {
map.is_modelling_node.make_large_enough_for(osm_node_id);
map.is_routing_node.make_large_enough_for(osm_node_id);
map.is_routing_node.set(osm_node_id);
map.is_modelling_node.set(osm_node_id);
}
assert(map.is_modelling_node.is_set(osm_node_id_list.front()));
assert(map.is_modelling_node.is_set(osm_node_id_list.back()));
}
};
}
unordered_read_osm_pbf(file_name, node_callback, way_callback, nullptr, log_message);
if(log_message){
timer += get_micro_time();
log_message("Finished scan, needed "+std::to_string(timer)+" musec.");
log_message("OSM ID range goes up to "+std::to_string(map.is_routing_node.size()) +" for routing nodes.");
log_message("OSM ID range goes up to "+std::to_string(map.is_modelling_node.size()) +" for modelling nodes.");
log_message("OSM ID range goes up to "+std::to_string(map.is_routing_way.size()) +" for routing ways.");
log_message("Found "+std::to_string(map.is_routing_node.population_count()) +" routing nodes.");
log_message("Found "+std::to_string(map.is_modelling_node.population_count()) +" modelling nodes.");
log_message("Found "+std::to_string(map.is_routing_way.population_count()) +" routing ways.");
}
return map;
}
OSMRoutingGraph load_osm_routing_graph_from_pbf(
const std::string&pbf_file,
const OSMRoutingIDMapping&mapping,
std::function<OSMWayDirectionCategory(uint64_t, unsigned, const TagMap&)>way_callback,
std::function<
void(
uint64_t osm_relation_id,
const std::vector<OSMRelationMember>&member_list,
const TagMap&tags,
std::function<void(OSMTurnRestriction)>
)
>turn_restriction_decoder,
std::function<void(const std::string&)>log_message,
bool file_is_ordered_even_though_file_header_says_that_it_is_unordered,
OSMRoadGeometry geometry_to_be_extracted
){
assert((mapping.is_modelling_node | mapping.is_routing_node) == mapping.is_modelling_node);
if(!way_callback){
way_callback = [](uint64_t, unsigned, const TagMap&){ return OSMWayDirectionCategory::open_in_both; };
}
if(turn_restriction_decoder && geometry_to_be_extracted == OSMRoadGeometry::none){
geometry_to_be_extracted = OSMRoadGeometry::first_and_last;
}
std::vector<unsigned>tail;
OSMRoutingGraph routing_graph;
long long timer=0;
if(log_message){
log_message("Start computing ID mappings");
timer = -get_micro_time();
}
IDMapper modelling_node(mapping.is_modelling_node);
IDMapper routing_node(mapping.is_routing_node);
IDMapper routing_way(mapping.is_routing_way);
if(log_message){
timer += get_micro_time();
log_message("Finished, needed "+std::to_string(timer)+" musec.");
}
auto on_new_arc = [&](
unsigned x, unsigned y, unsigned dist, unsigned routing_way_id, bool is_antiparallel_to_way,
const std::vector<float>&modelling_node_latitude,
const std::vector<float>&modelling_node_longitude)
{
tail.push_back(x);
routing_graph.head.push_back(y);
routing_graph.geo_distance.push_back(dist);
routing_graph.way.push_back(routing_way_id);
routing_graph.is_arc_antiparallel_to_way.push_back(is_antiparallel_to_way);
if(geometry_to_be_extracted == OSMRoadGeometry::uncompressed){
routing_graph.first_modelling_node.push_back(routing_graph.modelling_node_latitude.size());
routing_graph.modelling_node_latitude.insert(
routing_graph.modelling_node_latitude.end(),
modelling_node_latitude.begin(), modelling_node_latitude.end()
);
routing_graph.modelling_node_longitude.insert(
routing_graph.modelling_node_longitude.end(),
modelling_node_longitude.begin(), modelling_node_longitude.end()
);
}else if(geometry_to_be_extracted == OSMRoadGeometry::first_and_last){
routing_graph.first_modelling_node.push_back(routing_graph.modelling_node_latitude.size());
if(modelling_node_latitude.size() == 1){
routing_graph.modelling_node_latitude.push_back(modelling_node_latitude.front());
routing_graph.modelling_node_longitude.push_back(modelling_node_longitude.front());
}else if(!modelling_node_latitude.empty()){
routing_graph.modelling_node_latitude.push_back(modelling_node_latitude.front());
routing_graph.modelling_node_longitude.push_back(modelling_node_longitude.front());
routing_graph.modelling_node_latitude.push_back(modelling_node_latitude.back());
routing_graph.modelling_node_longitude.push_back(modelling_node_longitude.back());
}
}
};
std::vector<OSMTurnRestriction>osm_turn_restrictions;
std::function<void(uint64_t osm_relation_id, const std::vector<OSMRelationMember>&member_list, const TagMap&tags)>relation_callback = nullptr;
if(turn_restriction_decoder){
relation_callback = [&](
uint64_t osm_relation_id, const std::vector<OSMRelationMember>&member_list, const TagMap&tags
){
turn_restriction_decoder(
osm_relation_id, member_list, tags,
[&](OSMTurnRestriction restriction){
osm_turn_restrictions.push_back(restriction);
}
);
};
}
std::vector<float>latitude(modelling_node.local_id_count());
std::vector<float>longitude(modelling_node.local_id_count());
std::vector<float>modelling_node_latitude;
std::vector<float>modelling_node_longitude;
if(log_message){
log_message("Scanning OSM PBF data to load routing arcs");
timer = -get_micro_time();
}
ordered_read_osm_pbf(
pbf_file,
[&](uint64_t osm_node_id, double lat, double lon, const TagMap&tags){
unsigned modelling_id = modelling_node.to_local(osm_node_id, invalid_id);
if(modelling_id != invalid_id){
latitude[modelling_id] = lat;
longitude[modelling_id] = lon;
}
},
[&](uint64_t osm_way_id, const std::vector<std::uint64_t> & node_list, const TagMap&tags) {
unsigned routing_way_id = routing_way.to_local(osm_way_id, invalid_id);
if(routing_way_id != invalid_id){
OSMWayDirectionCategory dir = way_callback(osm_way_id, routing_way_id, tags);
if(dir != OSMWayDirectionCategory::closed){
unsigned modelling_id_of_previous_modelling_node = modelling_node.to_local(node_list[0]);
unsigned routing_id_of_last_routing_node = routing_node.to_local(node_list[0]);
double dist_since_last_routing_node = 0;
for(unsigned i=1; i<node_list.size(); ++i){
unsigned modelling_id_of_current_node = modelling_node.to_local(node_list[i]);
dist_since_last_routing_node += geo_dist(
latitude[modelling_id_of_current_node], longitude[modelling_id_of_current_node],
latitude[modelling_id_of_previous_modelling_node], longitude[modelling_id_of_previous_modelling_node]
);
if(geometry_to_be_extracted == OSMRoadGeometry::uncompressed || geometry_to_be_extracted == OSMRoadGeometry::first_and_last){
modelling_node_latitude.push_back(latitude[modelling_id_of_current_node]);
modelling_node_longitude.push_back(longitude[modelling_id_of_current_node]);
}
modelling_id_of_previous_modelling_node = modelling_id_of_current_node;
unsigned routing_id_of_current_node = routing_node.to_local(node_list[i], invalid_id);
if(routing_id_of_current_node != invalid_id){
if(geometry_to_be_extracted == OSMRoadGeometry::uncompressed || geometry_to_be_extracted == OSMRoadGeometry::first_and_last){
modelling_node_latitude.pop_back();
modelling_node_longitude.pop_back();
}
switch(dir){
case OSMWayDirectionCategory::only_open_forwards:
on_new_arc(routing_id_of_last_routing_node, routing_id_of_current_node, dist_since_last_routing_node, routing_way_id, false, modelling_node_latitude, modelling_node_longitude);
break;
case OSMWayDirectionCategory::open_in_both:
on_new_arc(routing_id_of_last_routing_node, routing_id_of_current_node, dist_since_last_routing_node, routing_way_id, false, modelling_node_latitude, modelling_node_longitude);
case OSMWayDirectionCategory::only_open_backwards:
std::reverse(modelling_node_latitude.begin(), modelling_node_latitude.end());
std::reverse(modelling_node_longitude.begin(), modelling_node_longitude.end());
on_new_arc(routing_id_of_current_node, routing_id_of_last_routing_node, dist_since_last_routing_node, routing_way_id, true, modelling_node_latitude, modelling_node_longitude);
break;
default:
assert(false);
}
dist_since_last_routing_node = 0;
modelling_node_latitude.clear();
modelling_node_longitude.clear();
routing_id_of_last_routing_node = routing_id_of_current_node;
}
}
}
}
},
relation_callback,
log_message,
file_is_ordered_even_though_file_header_says_that_it_is_unordered
);
if(log_message){
timer += get_micro_time();
log_message("Finished scan, needed "+std::to_string(timer)+" musec.");
log_message("Found "+std::to_string(tail.size())+" arcs.");
log_message("Start sorting arcs by tail");
timer = -get_micro_time();
}
{
unsigned node_count = routing_node.local_id_count();
auto p = compute_inverse_sort_permutation_first_by_tail_then_by_head_and_apply_sort_to_tail(node_count, tail, routing_graph.head);
routing_graph.head = apply_inverse_permutation(p, std::move(routing_graph.head));
routing_graph.geo_distance = apply_inverse_permutation(p, std::move(routing_graph.geo_distance));
routing_graph.way = apply_inverse_permutation(p, std::move(routing_graph.way));
routing_graph.is_arc_antiparallel_to_way = apply_inverse_permutation(p, std::move(routing_graph.is_arc_antiparallel_to_way));
routing_graph.first_out = invert_vector(tail, node_count);
if(geometry_to_be_extracted == OSMRoadGeometry::uncompressed || geometry_to_be_extracted == OSMRoadGeometry::first_and_last){
routing_graph.first_modelling_node.push_back(routing_graph.modelling_node_latitude.size());
std::vector<unsigned>first_modelling_node;
std::vector<float>modelling_node_latitude;
std::vector<float>modelling_node_longitude;
first_modelling_node.reserve(routing_graph.first_modelling_node.size());
modelling_node_latitude.reserve(routing_graph.modelling_node_latitude.size());
modelling_node_longitude.reserve(routing_graph.modelling_node_longitude.size());
auto new_arc_id_to_old_arc_id = invert_permutation(p);
for(auto old_arc_id : new_arc_id_to_old_arc_id){
first_modelling_node.push_back(modelling_node_latitude.size());
int first = routing_graph.first_modelling_node[old_arc_id];
int last = routing_graph.first_modelling_node[old_arc_id + 1];
modelling_node_latitude.insert(
modelling_node_latitude.end(),
routing_graph.modelling_node_latitude.begin() + first,
routing_graph.modelling_node_latitude.begin() + last
);
modelling_node_longitude.insert(
modelling_node_longitude.end(),
routing_graph.modelling_node_longitude.begin() + first,
routing_graph.modelling_node_longitude.begin() + last
);
}
first_modelling_node.push_back(modelling_node_latitude.size());
routing_graph.first_modelling_node = std::move(first_modelling_node);
routing_graph.modelling_node_latitude = std::move(modelling_node_latitude);
routing_graph.modelling_node_longitude = std::move(modelling_node_longitude);
}
}
if(log_message){
timer += get_micro_time();
log_message("Finished sorting, needed "+std::to_string(timer)+" musec.");
log_message("Start computing modelling to routing node mapping");
timer = -get_micro_time();
}
BitVector modelling_node_is_routing_node(modelling_node.local_id_count(), false);
for(unsigned r=0; r < routing_node.local_id_count(); ++r)
modelling_node_is_routing_node.set(modelling_node.to_local(routing_node.to_global(r)));
if(log_message){
timer += get_micro_time();
log_message("Finished, needed "+std::to_string(timer)+" musec.");
log_message("Start reducing geographic positions to routing nodes");
timer = -get_micro_time();
}
routing_graph.latitude = keep_element_of_vector_if(modelling_node_is_routing_node, latitude);
routing_graph.longitude = keep_element_of_vector_if(modelling_node_is_routing_node, longitude);
if(log_message){
timer += get_micro_time();
log_message("Finished, needed "+std::to_string(timer)+" musec.");
log_message("Found "+std::to_string(osm_turn_restrictions.size())+" OSM turn restrictions.");
}
if(!osm_turn_restrictions.empty()){
auto&first_out = routing_graph.first_out;
auto&head = routing_graph.head;
auto&way = routing_graph.way;
const unsigned node_count = first_out.size()-1;
const unsigned arc_count = head.size();
const unsigned way_count = routing_way.local_id_count();
{
if(log_message){
log_message("Start mapping IDs in turn restrictions");
timer = -get_micro_time();
}
auto
in = osm_turn_restrictions.begin(),
out = osm_turn_restrictions.begin(),
end = osm_turn_restrictions.end();
while(in != end){
bool has_error = false;
unsigned local_from_way = routing_way.to_local(in->from_way, invalid_id);
if(local_from_way == invalid_id){
has_error = true;
}
unsigned local_to_way = routing_way.to_local(in->to_way, invalid_id);
if(local_to_way == invalid_id){
has_error = true;
}
unsigned local_via_node = invalid_id;
if(in->via_node != (std::uint64_t)-1){
local_via_node = routing_node.to_local(in->via_node, invalid_id);
if(local_via_node == invalid_id){
has_error = true;
}
}
if(has_error){
++in;
} else {
out->osm_relation_id = in->osm_relation_id;
out->category = in->category;
out->direction = in->direction;
out->from_way = local_from_way;
out->to_way = local_to_way;
out->via_node = local_via_node;
++in;
++out;
}
}
osm_turn_restrictions.erase(out, end);
if(log_message){
timer += get_micro_time();
log_message("Finished, needed "+std::to_string(timer)+" musec.");
}
}
if(log_message){
log_message("After removing restrictions with not exported ways or node only "+std::to_string(osm_turn_restrictions.size())+" restrictions remain.");
}
std::vector<unsigned>forbidden_from;
std::vector<unsigned>forbidden_to;
auto add_forbidden_turn = [&](unsigned from_arc, unsigned to_arc){
assert(from_arc < arc_count);
assert(to_arc < arc_count);
forbidden_from.push_back(from_arc);
forbidden_to.push_back(to_arc);
};
{
if(log_message){
log_message("Sorting arcs by way");
timer = -get_micro_time();
}
auto index_to_arc = compute_sort_permutation_using_key(way, way_count, [](unsigned x){return x;});
auto first_index_of_way = invert_vector(apply_permutation(index_to_arc, way), way_count);
if(log_message){
timer += get_micro_time();
log_message("Finished, needed "+std::to_string(timer)+" musec.");
}
{
if(log_message){
log_message("Start filling in missing via node IDs in turn restrictions and handling mandatory turns with from = to");
timer = -get_micro_time();
}
std::vector<bool> is_head_of_from_way(node_count, false);
std::vector<unsigned> incomming_arc_of_way_into_node(node_count, invalid_id);
unsigned repaired_count = 0;
unsigned no_via_count = 0;
unsigned multiple_via_count = 0;
unsigned go_straight_count = 0;
auto
in = osm_turn_restrictions.begin(),
out = osm_turn_restrictions.begin(),
end = osm_turn_restrictions.end();
while(in != end){
if(in->via_node != invalid_id) {
*out = *in;
++out;
++in;
} else if(in->from_way == in->to_way && in->category == OSMTurnRestrictionCategory::mandatory){
for(auto i=first_index_of_way[in->from_way]; i!=first_index_of_way[in->from_way+1]; ++i){
auto arc = index_to_arc[i];
incomming_arc_of_way_into_node[head[arc]] = arc;
}
for(auto i=first_index_of_way[in->from_way]; i!=first_index_of_way[in->from_way+1]; ++i){
auto out_arc = index_to_arc[i];
auto via_node = tail[out_arc];
auto in_arc = incomming_arc_of_way_into_node[via_node];
if(in_arc != invalid_id)
for(unsigned not_out_arc=first_out[via_node]; not_out_arc!=first_out[via_node+1]; ++not_out_arc)
if(not_out_arc != out_arc)
add_forbidden_turn(in_arc, not_out_arc);
}
for(auto i=first_index_of_way[in->from_way]; i!=first_index_of_way[in->from_way+1]; ++i){
auto arc = index_to_arc[i];
incomming_arc_of_way_into_node[head[arc]] = invalid_id;
}
++in;
++go_straight_count;
} else {
unsigned via_node = invalid_id;
unsigned second_via_node = invalid_id;
for(auto i=first_index_of_way[in->from_way]; i!=first_index_of_way[in->from_way+1]; ++i){
auto arc = index_to_arc[i];
is_head_of_from_way[head[arc]] = true;
}
for(auto i=first_index_of_way[in->to_way]; i!=first_index_of_way[in->to_way+1]; ++i){
auto arc = index_to_arc[i];
if(is_head_of_from_way[tail[arc]]){
if(via_node != invalid_id){
second_via_node = tail[arc];
} else {
via_node = tail[arc];
}
}
}
for(auto i=first_index_of_way[in->from_way]; i!=first_index_of_way[in->from_way+1]; ++i){
auto arc = index_to_arc[i];
is_head_of_from_way[head[arc]] = false;
}
if(via_node == invalid_id){
if(log_message)
log_message(
"Turn restriction with OSM-relation-ID \""+std::to_string(in->osm_relation_id)+"\" "
"with OSM-way-from-ID \""+std::to_string(routing_way.to_global(in->from_way))+"\" "
"and OSM-way to-ID \""+std::to_string(routing_way.to_global(in->to_way))+"\" "
"does not have a via-node "
"and their ways do not cross, ingoring restriction"
);
++no_via_count;
++in;
}else if(second_via_node != invalid_id){
if(log_message)
log_message(
"Turn restriction with OSM-relation-ID \""+std::to_string(in->osm_relation_id)+"\" "
"with OSM-way-from-ID \""+std::to_string(routing_way.to_global(in->from_way))+"\" "
"and OSM-way-to-ID \""+std::to_string(routing_way.to_global(in->to_way))+"\" "
"does not have a via-node "
"and there are multiple ambiguous candidates, namely OSM-node-IDs \""+std::to_string(routing_node.to_global(via_node))+"\" "
"and \""+std::to_string(routing_node.to_global(second_via_node))+"\" (and maybe more), ingoring restriction"
);
++multiple_via_count;
++in;
}else{
++repaired_count;
out->osm_relation_id = in->osm_relation_id;
out->direction = in->direction;
out->category = in->category;
out->from_way = in->from_way;
out->to_way = in->to_way;
out->via_node = via_node;
++out;
++in;
}
}
}
if(log_message){
log_message("There were "+std::to_string(go_straight_count)+" go-straight turns that were expanded into "+std::to_string(forbidden_from.size())+" forbidden turns.");
log_message("There were "+std::to_string(repaired_count)+" normal turns without via-node for which a via-node could be derived.");
log_message("There were "+std::to_string(no_via_count)+" normal turns without via-node discarded because no potential via-node was found.");
log_message("There were "+std::to_string(no_via_count)+" normal turns without via-node discarded because multiple potential via-node were found.");
}
osm_turn_restrictions.erase(out, end);
if(log_message){
timer += get_micro_time();
log_message("Finished, needed "+std::to_string(timer)+" musec.");
}
}
}
{
if(log_message){
log_message("Building forbidden turns");
timer = -get_micro_time();
}
const unsigned node_count = routing_graph.first_out.size()-1;
const unsigned arc_count = routing_graph.head.size();
(void)arc_count;
auto in_arc = compute_sort_permutation_using_key(routing_graph.head, node_count, [](unsigned x){return x;});
auto first_in = invert_vector(apply_permutation(in_arc, routing_graph.head), node_count);
for(auto x:osm_turn_restrictions){
assert(x.via_node != invalid_id);
assert(x.from_way != invalid_id);
assert(x.to_way != invalid_id);
std::vector<unsigned>from_candidates;
for(unsigned i=first_in[x.via_node]; i!=first_in[x.via_node+1]; ++i){
assert(i < arc_count);
unsigned arc = in_arc[i];
assert(arc < arc_count);
if(routing_graph.way[arc] == x.from_way){
from_candidates.push_back(arc);
}
}
if(from_candidates.empty()){
continue;
}
#ifndef NDEBUG
for(auto from:from_candidates)
assert(head[from] == x.via_node);
#endif
std::vector<unsigned>to_candidates;
for(unsigned arc=routing_graph.first_out[x.via_node]; arc!=routing_graph.first_out[x.via_node+1]; ++arc){
assert(arc < arc_count);
if(routing_graph.way[arc] == x.to_way){
to_candidates.push_back(arc);
}
}
if(to_candidates.empty()){
continue;
}
#ifndef NDEBUG
for(auto to:to_candidates)
assert(tail[to] == x.via_node);
#endif
unsigned from, to;
if(from_candidates.size() == 1 && to_candidates.size() == 1){
from = from_candidates[0];
to = to_candidates[0];
} else {
float via_lat = routing_graph.latitude[x.via_node];
float via_lon = routing_graph.longitude[x.via_node];
const float pi = 3.14159265359f;
auto mod_2pi = [&](float angle) -> float {
while(angle < 0.0f){
angle += 2.0f*pi;
}
while(angle > 2.0f*pi){
angle -= 2.0f*pi;
}
return angle;
};
unsigned matching_candidate_count = 0;
for(unsigned from_cand: from_candidates){
float from_lat, from_lon;
if(routing_graph.first_modelling_node[from_cand] == routing_graph.first_modelling_node[from_cand+1]){
from_lat = routing_graph.latitude[tail[from_cand]];
from_lon = routing_graph.longitude[tail[from_cand]];
}else{
from_lat = routing_graph.modelling_node_latitude[routing_graph.first_modelling_node[from_cand+1]-1];
from_lon = routing_graph.modelling_node_longitude[routing_graph.first_modelling_node[from_cand+1]-1];
}
float from_angle = atan2(via_lat-from_lat, via_lon-from_lon);
for(unsigned to_cand: to_candidates){
float to_lat, to_lon;
if(routing_graph.first_modelling_node[to_cand] == routing_graph.first_modelling_node[to_cand+1]){
to_lat = routing_graph.latitude[routing_graph.head[to_cand]];
to_lon = routing_graph.longitude[routing_graph.head[to_cand]];
}else{
to_lat = routing_graph.modelling_node_latitude[routing_graph.first_modelling_node[to_cand]];
to_lon = routing_graph.modelling_node_longitude[routing_graph.first_modelling_node[to_cand]];
}
float to_angle = atan2(to_lat-via_lat, to_lon-via_lon);
float angle_diff = mod_2pi(to_angle - from_angle);
switch(x.direction){
case OSMTurnDirection::left_turn:
if(pi*1.0/4.0 < angle_diff && angle_diff < pi*3.0/4.0){
++matching_candidate_count;
from = from_cand;
to = to_cand;
}
break;
case OSMTurnDirection::right_turn:
if(pi*5.0/4.0 < angle_diff && angle_diff < pi*7.0/4.0){
++matching_candidate_count;
from = from_cand;
to = to_cand;
}
break;
case OSMTurnDirection::straight_on:
if(angle_diff < pi/3.0f || 5.0f*pi/3.0f < angle_diff){
++matching_candidate_count;
from = from_cand;
to = to_cand;
}
break;
case OSMTurnDirection::u_turn:
if(2.0f*pi/3.0f < angle_diff && angle_diff < 4.0f*pi/3.0f){
++matching_candidate_count;
from = from_cand;
to = to_cand;
}
break;
}
}
}
if(matching_candidate_count == 0){
if(log_message){
log_message(
"OSM turn restriction relation ID \""+std::to_string(x.osm_relation_id)+"\" "
"is a turn restriction where it is impossible to infer the restriction without "
"using the turn direction information. However, no restriction candidate is consistent "
"with the turn direction. -> ignoring restriction"
);
}
continue;
}
if(matching_candidate_count >= 2){
if(log_message){
log_message(
"OSM turn restriction relation ID \""+std::to_string(x.osm_relation_id)+"\" "
"is a turn restriction where it is impossible to infer the restriction without "
"using the turn direction information. However, "+std::to_string(matching_candidate_count)+" restriction candidates are consistent "
"with the turn direction -> ignoring all candidates restriction"
);
}
continue;
}
}
if(x.category == OSMTurnRestrictionCategory::prohibitive){
add_forbidden_turn(from, to);
} else {
for(unsigned i=first_out[x.via_node]; i!=first_out[x.via_node+1]; ++i){
if(i != to){
add_forbidden_turn(from, i);
}
}
}
}
if(log_message){
timer += get_micro_time();
log_message("Finished, needed "+std::to_string(timer)+" musec.");
}
}
{
if(log_message){
log_message("Sorting forbidden turns");
timer = -get_micro_time();
}
auto p = compute_inverse_sort_permutation_first_by_tail_then_by_head_and_apply_sort_to_tail(arc_count, forbidden_from, forbidden_to);
forbidden_to = apply_inverse_permutation(p, move(forbidden_to));
assert(is_sorted_using_less(forbidden_from));
BitVector is_duplicate = make_bit_vector(
forbidden_from.size(),
[&](unsigned x){
if(x == 0)
return false;
else
return forbidden_from[x-1] == forbidden_from[x] && forbidden_to[x-1] == forbidden_to[x];
}
);
inplace_remove_element_from_vector_if(is_duplicate, forbidden_from);
inplace_remove_element_from_vector_if(is_duplicate, forbidden_to);
assert(is_sorted_using_less(forbidden_from));
if(log_message){
timer += get_micro_time();
log_message("Finished, needed "+std::to_string(timer)+" musec.");
}
}
routing_graph.forbidden_turn_from_arc = std::move(forbidden_from);
routing_graph.forbidden_turn_to_arc = std::move(forbidden_to);
}
if(log_message){
log_message("Extracted "+std::to_string(routing_graph.forbidden_turn_from_arc.size())+" forbidden turns.");
}
return routing_graph; }
}