Intersection TurnAnalysis::assignTurnTypes(const NodeID from_nid,
const EdgeID via_eid,
Intersection intersection) const
{
// Roundabouts are a main priority. If there is a roundabout instruction present, we process the
// turn as a roundabout
if (roundabout_handler.canProcess(from_nid, via_eid, intersection))
{
intersection = roundabout_handler(from_nid, via_eid, std::move(intersection));
}
else
{
// set initial defaults for normal turns and modifier based on angle
intersection = setTurnTypes(from_nid, via_eid, std::move(intersection));
if (motorway_handler.canProcess(from_nid, via_eid, intersection))
{
intersection = motorway_handler(from_nid, via_eid, std::move(intersection));
}
else
{
BOOST_ASSERT(turn_handler.canProcess(from_nid, via_eid, intersection));
intersection = turn_handler(from_nid, via_eid, std::move(intersection));
}
}
// Handle sliproads
intersection = sliproad_handler(from_nid, via_eid, std::move(intersection));
// Turn On Ramps Into Off Ramps, if we come from a motorway-like road
if (node_based_graph.GetEdgeData(via_eid).road_classification.IsMotorwayClass())
{
std::for_each(intersection.begin(), intersection.end(), [](ConnectedRoad &road) {
if (road.turn.instruction.type == TurnType::OnRamp)
road.turn.instruction.type = TurnType::OffRamp;
});
}
return intersection;
}
// "Sliproads" occur when we've got a link between two roads (MOTORWAY_LINK, etc), but
// the two roads are *also* directly connected shortly afterwards.
// In these cases, we tag the turn-type as "sliproad", and then in post-processing
// we emit a "turn", instead of "take the ramp"+"merge"
Intersection TurnAnalysis::handleSliproads(const EdgeID source_edge_id,
Intersection intersection) const
{
auto intersection_node_id = node_based_graph.GetTarget(source_edge_id);
const auto linkTest = [this](const ConnectedRoad &road) {
return // isLinkClass(
// node_based_graph.GetEdgeData(road.turn.eid).road_classification.road_class) &&
!node_based_graph.GetEdgeData(road.turn.eid).roundabout && road.entry_allowed &&
angularDeviation(road.turn.angle, STRAIGHT_ANGLE) <= 2 * NARROW_TURN_ANGLE;
};
bool hasNarrow =
std::find_if(intersection.begin(), intersection.end(), linkTest) != intersection.end();
if (!hasNarrow)
return intersection;
const auto source_edge_data = node_based_graph.GetEdgeData(source_edge_id);
// Find the continuation of the intersection we're on
auto next_road = std::find_if(
intersection.begin(),
intersection.end(),
[this, source_edge_data](const ConnectedRoad &road) {
const auto road_edge_data = node_based_graph.GetEdgeData(road.turn.eid);
// Test to see if the source edge and the one we're looking at are the same road
return road_edge_data.road_classification.road_class ==
source_edge_data.road_classification.road_class &&
road_edge_data.name_id != EMPTY_NAMEID &&
road_edge_data.name_id == source_edge_data.name_id && road.entry_allowed &&
angularDeviation(road.turn.angle, STRAIGHT_ANGLE) < FUZZY_ANGLE_DIFFERENCE;
});
const bool hasNext = next_road != intersection.end();
if (!hasNext)
{
return intersection;
}
// Threshold check, if the intersection is too far away, don't bother continuing
const auto &next_road_data = node_based_graph.GetEdgeData(next_road->turn.eid);
if (next_road_data.distance > MAX_SLIPROAD_THRESHOLD)
{
return intersection;
}
const auto next_road_next_intersection =
intersection_generator(intersection_node_id, next_road->turn.eid);
const auto next_intersection_node = node_based_graph.GetTarget(next_road->turn.eid);
std::unordered_set<NameID> target_road_names;
for (const auto &road : next_road_next_intersection)
{
const auto &target_data = node_based_graph.GetEdgeData(road.turn.eid);
target_road_names.insert(target_data.name_id);
}
for (auto &road : intersection)
{
if (linkTest(road))
{
auto target_intersection = intersection_generator(intersection_node_id, road.turn.eid);
for (const auto &candidate_road : target_intersection)
{
const auto &candidate_data = node_based_graph.GetEdgeData(candidate_road.turn.eid);
if (target_road_names.count(candidate_data.name_id) > 0 &&
node_based_graph.GetTarget(candidate_road.turn.eid) == next_intersection_node)
{
road.turn.instruction.type = TurnType::Sliproad;
break;
}
}
}
}
if (next_road->turn.instruction.type == TurnType::Fork)
{
const auto &next_data = node_based_graph.GetEdgeData(next_road->turn.eid);
if (next_data.name_id == source_edge_data.name_id)
{
if (angularDeviation(next_road->turn.angle, STRAIGHT_ANGLE) < 5)
next_road->turn.instruction.type = TurnType::Suppressed;
else
next_road->turn.instruction.type = TurnType::Continue;
next_road->turn.instruction.direction_modifier =
getTurnDirection(next_road->turn.angle);
}
else if (next_data.name_id != EMPTY_NAMEID)
{
next_road->turn.instruction.type = TurnType::NewName;
next_road->turn.instruction.direction_modifier =
getTurnDirection(next_road->turn.angle);
}
else
{
next_road->turn.instruction.type = TurnType::Suppressed;
}
}
return intersection;
}
/*
* Segregated Roads often merge onto a single intersection.
* While technically representing different roads, they are
* often looked at as a single road.
* Due to the merging, turn Angles seem off, wenn we compute them from the
* initial positions.
*
* b<b<b<b(1)<b<b<b
* aaaaa-b
* b>b>b>b(2)>b>b>b
*
* Would be seen as a slight turn going fro a to (2). A Sharp turn going from
* (1) to (2).
*
* In cases like these, we megre this segregated roads into a single road to
* end up with a case like:
*
* aaaaa-bbbbbb
*
* for the turn representation.
* Anything containing the first u-turn in a merge affects all other angles
* and is handled separately from all others.
*/
Intersection IntersectionGenerator::mergeSegregatedRoads(Intersection intersection) const
{
const auto getRight = [&](std::size_t index) {
return (index + intersection.size() - 1) % intersection.size();
};
const auto mergable = [&](std::size_t first, std::size_t second) -> bool {
const auto &first_data = node_based_graph.GetEdgeData(intersection[first].turn.eid);
const auto &second_data = node_based_graph.GetEdgeData(intersection[second].turn.eid);
return first_data.name_id != INVALID_NAME_ID && first_data.name_id == second_data.name_id &&
!first_data.roundabout && !second_data.roundabout &&
first_data.travel_mode == second_data.travel_mode &&
first_data.road_classification == second_data.road_classification &&
// compatible threshold
angularDeviation(intersection[first].turn.angle, intersection[second].turn.angle) <
60 &&
first_data.reversed != second_data.reversed;
};
const auto merge = [](const ConnectedRoad &first,
const ConnectedRoad &second) -> ConnectedRoad {
if (!first.entry_allowed)
{
ConnectedRoad result = second;
result.turn.angle = (first.turn.angle + second.turn.angle) / 2;
if (first.turn.angle - second.turn.angle > 180)
result.turn.angle += 180;
if (result.turn.angle > 360)
result.turn.angle -= 360;
return result;
}
else
{
BOOST_ASSERT(!second.entry_allowed);
ConnectedRoad result = first;
result.turn.angle = (first.turn.angle + second.turn.angle) / 2;
if (first.turn.angle - second.turn.angle > 180)
result.turn.angle += 180;
if (result.turn.angle > 360)
result.turn.angle -= 360;
return result;
}
};
if (intersection.size() <= 1)
return intersection;
const bool is_connected_to_roundabout = [this, &intersection]() {
for (const auto &road : intersection)
{
if (node_based_graph.GetEdgeData(road.turn.eid).roundabout)
return true;
}
return false;
}();
// check for merges including the basic u-turn
// these result in an adjustment of all other angles
if (mergable(0, intersection.size() - 1))
{
const double correction_factor =
(360 - intersection[intersection.size() - 1].turn.angle) / 2;
for (std::size_t i = 1; i + 1 < intersection.size(); ++i)
intersection[i].turn.angle += correction_factor;
// FIXME if we have a left-sided country, we need to switch this off and enable it below
intersection[0] = merge(intersection.front(), intersection.back());
intersection[0].turn.angle = 0;
if (is_connected_to_roundabout)
{
// We are merging a u-turn against the direction of a roundabout
//
// -----------> roundabout
// / \
// out in
//
// These cases have to be disabled, even if they are not forbidden specifically by a
// relation
intersection[0].entry_allowed = false;
}
intersection.pop_back();
}
else if (mergable(0, 1))
{
const double correction_factor = (intersection[1].turn.angle) / 2;
for (std::size_t i = 2; i < intersection.size(); ++i)
intersection[i].turn.angle += correction_factor;
intersection[0] = merge(intersection[0], intersection[1]);
intersection[0].turn.angle = 0;
intersection.erase(intersection.begin() + 1);
}
// a merge including the first u-turn requres an adjustment of the turn angles
// therefore these are handled prior to this step
for (std::size_t index = 2; index < intersection.size(); ++index)
{
if (mergable(index, getRight(index)))
{
intersection[getRight(index)] =
merge(intersection[getRight(index)], intersection[index]);
intersection.erase(intersection.begin() + index);
--index;
}
}
const auto ByAngle = [](const ConnectedRoad &first, const ConnectedRoad second) {
return first.turn.angle < second.turn.angle;
};
std::sort(std::begin(intersection), std::end(intersection), ByAngle);
return intersection;
}
void
MMO_Expression_::_traverseAlgebraics (Dependencies deps, Index derivativeIndex,
Dependencies derivativeDeps,
map<Index, Index> *states,
map<Index, Index> *discretes,
Index variableChange, DEP_Type type,
int value)
{
for (Index *idx = deps->begin (type); !deps->end (type);
idx = deps->next (type))
{
list<MMO_Equation> algEqs = _data->algebraics ()->equation (*idx);
list<MMO_Equation>::iterator algEq;
if (type == DEP_ALGEBRAIC_VECTOR_DEF)
{
derivativeDeps->insert (*idx, DEP_ALGEBRAIC_VECTOR);
}
int algValue = -1;
for (algEq = algEqs.begin (); algEq != algEqs.end (); algEq++)
{
Index algebraicIdx = *idx;
if (value >= 0 && idx->hasRange ())
{
algebraicIdx = idx->indexValue (value);
}
if (!(*algEq)->exp()->deps()->autonomous())
{
_deps->setAutonomous(false);
}
Index variableCh;
int f = (*algEq)->lhs ().factor ();
int c = (*algEq)->lhs ().operConstant ();
if (f != 0 && type == DEP_ALGEBRAIC_DEF)
{
if (f != 1 || c != 0)
{
variableCh.setFactor (f);
variableCh.setConstant (-c);
variableCh.setLow ((*algEq)->lhs ().low () * f + c);
variableCh.setHi ((*algEq)->lhs ().hi () * f + c);
}
}
Index algebraicIndex = (*algEq)->lhs ();
if (variableChange.isSet ())
{
algebraicIndex = (*algEq)->lhs ().applyVariableChange (
variableChange);
algebraicIdx = idx->applyVariableChange (variableChange);
algebraicIdx.setLow (variableChange.low ());
algebraicIdx.setHi (variableChange.hi ());
}
Intersection is = algebraicIndex.intersection (algebraicIdx);
if (is.type () != IDX_DISJOINT)
{
Index algKey = algebraicIdx;
Index equationIndex;
equationIndex.setOffset (_equationIndex++);
if (is.hasRange ())
{
equationIndex.setRange ();
equationIndex.setLow (is.low ());
equationIndex.setHi (is.hi ());
algKey.setRange ();
algKey.setHi (equationIndex.hi ());
algKey.setLow (equationIndex.low ());
}
else
{
equationIndex.setConstant (is.modelicaValue ());
if (is.type () == IDX_CONSTANT_BA)
{
algKey.setConstant (is.modelicaValue () + is.begin ());
algValue = is.modelicaValue ();
}
else if (is.type () == IDX_CONSTANT_AB)
{
algKey.setConstant (is.modelicaValue () + is.begin ());
algValue = is.modelicaValue ();
}
else
{
algKey.setConstant (is.modelicaValue ());
}
algKey.setLow (1);
algKey.setHi (1);
algKey.setFactor (0);
}
if (type == DEP_ALGEBRAIC_DEF)
{
derivativeDeps->insert (algKey, DEP_ALGEBRAIC);
}
_addAlgebriacDeps (algKey, (*algEq), equationIndex,
derivativeIndex, derivativeDeps, states,
discretes, variableCh, algValue);
}
}
}
}
std::size_t IntersectionHandler::countValid(const Intersection &intersection) const
{
return std::count_if(intersection.begin(), intersection.end(), [](const ConnectedRoad &road) {
return road.entry_allowed;
});
}
std::pair<util::guidance::EntryClass, util::guidance::BearingClass>
classifyIntersection(Intersection intersection)
{
if (intersection.empty())
return {};
std::sort(intersection.begin(),
intersection.end(),
[](const ConnectedRoad &left, const ConnectedRoad &right) {
return left.bearing < right.bearing;
});
util::guidance::EntryClass entry_class;
util::guidance::BearingClass bearing_class;
const bool canBeDiscretized = [&]() {
if (intersection.size() <= 1)
return true;
DiscreteBearing last_discrete_bearing = util::guidance::BearingClass::getDiscreteBearing(
std::round(intersection.back().bearing));
for (const auto road : intersection)
{
const DiscreteBearing discrete_bearing =
util::guidance::BearingClass::getDiscreteBearing(std::round(road.bearing));
if (discrete_bearing == last_discrete_bearing)
return false;
last_discrete_bearing = discrete_bearing;
}
return true;
}();
// finally transfer data to the entry/bearing classes
std::size_t number = 0;
if (canBeDiscretized)
{
if (util::guidance::BearingClass::getDiscreteBearing(intersection.back().bearing) <
util::guidance::BearingClass::getDiscreteBearing(intersection.front().bearing))
{
intersection.insert(intersection.begin(), intersection.back());
intersection.pop_back();
}
for (const auto &road : intersection)
{
if (road.entry_allowed)
entry_class.activate(number);
auto discrete_bearing_class =
util::guidance::BearingClass::getDiscreteBearing(std::round(road.bearing));
bearing_class.add(std::round(discrete_bearing_class *
util::guidance::BearingClass::discrete_step_size));
++number;
}
}
else
{
for (const auto &road : intersection)
{
if (road.entry_allowed)
entry_class.activate(number);
bearing_class.add(std::round(road.bearing));
++number;
}
}
return std::make_pair(entry_class, bearing_class);
}
