osrm-backend/src/extractor/guidance/turn_lane_matcher.cpp

#include "extractor/guidance/turn_lane_matcher.hpp"
#include "util/bearing.hpp"

#include <boost/assert.hpp>
#include <boost/numeric/conversion/cast.hpp>

#include <functional>

using osrm::util::angularDeviation;

namespace osrm
{
namespace extractor
{
namespace guidance
{
namespace lanes
{

// Translate Turn Tags into a Matching Direction Modifier
DirectionModifier::Enum getMatchingModifier(const TurnLaneType::Mask tag)
{
    const constexpr TurnLaneType::Mask tag_by_modifier[] = {TurnLaneType::uturn,
                                                            TurnLaneType::sharp_right,
                                                            TurnLaneType::right,
                                                            TurnLaneType::slight_right,
                                                            TurnLaneType::straight,
                                                            TurnLaneType::slight_left,
                                                            TurnLaneType::left,
                                                            TurnLaneType::sharp_left,
                                                            TurnLaneType::merge_to_left,
                                                            TurnLaneType::merge_to_right};
    const auto index =
        std::distance(tag_by_modifier, std::find(tag_by_modifier, tag_by_modifier + 10, tag));

    BOOST_ASSERT(index <= 10);

    const constexpr DirectionModifier::Enum modifiers[11] = {
        DirectionModifier::UTurn,
        DirectionModifier::SharpRight,
        DirectionModifier::Right,
        DirectionModifier::SlightRight,
        DirectionModifier::Straight,
        DirectionModifier::SlightLeft,
        DirectionModifier::Left,
        DirectionModifier::SharpLeft,
        DirectionModifier::Straight,
        DirectionModifier::Straight,
        DirectionModifier::UTurn}; // fallback for invalid tags

    return modifiers[index];
}

// check whether a match of a given tag and a turn instruction can be seen as valid
bool isValidMatch(const TurnLaneType::Mask tag, const TurnInstruction instruction)
{
    using extractor::guidance::hasLeftModifier;
    using extractor::guidance::hasRightModifier;
    const auto isMirroredModifier = [](const TurnInstruction instruction) {
        return instruction.type == TurnType::Merge;
    };

    if (tag == TurnLaneType::uturn)
    {
        return hasLeftModifier(instruction) ||
               instruction.direction_modifier == DirectionModifier::UTurn;
    }
    else if (tag == TurnLaneType::sharp_right || tag == TurnLaneType::right ||
             tag == TurnLaneType::slight_right)
    {
        if (isMirroredModifier(instruction))
            return hasLeftModifier(instruction);
        else
            // needs to be adjusted for left side driving
            return leavesRoundabout(instruction) || hasRightModifier(instruction);
    }
    else if (tag == TurnLaneType::straight)
    {
        return instruction.direction_modifier == DirectionModifier::Straight ||
               instruction.type == TurnType::Suppressed || instruction.type == TurnType::NewName ||
               instruction.type == TurnType::StayOnRoundabout || entersRoundabout(instruction) ||
               (instruction.type ==
                    TurnType::Fork && // Forks can be experienced, even for straight segments
                (instruction.direction_modifier == DirectionModifier::SlightLeft ||
                 instruction.direction_modifier == DirectionModifier::SlightRight)) ||
               (instruction.type ==
                    TurnType::Continue && // Forks can be experienced, even for straight segments
                (instruction.direction_modifier == DirectionModifier::SlightLeft ||
                 instruction.direction_modifier == DirectionModifier::SlightRight)) ||
               instruction.type == TurnType::UseLane;
    }
    else if (tag == TurnLaneType::slight_left || tag == TurnLaneType::left ||
             tag == TurnLaneType::sharp_left)
    {
        if (isMirroredModifier(instruction))
            return hasRightModifier(instruction);
        else
        {
            // Needs to be fixed for left side driving
            return (instruction.type == TurnType::StayOnRoundabout) || hasLeftModifier(instruction);
        }
    }
    return false;
}

double getMatchingQuality(const TurnLaneType::Mask tag, const ConnectedRoad &road)
{
    const constexpr double idealized_turn_angles[] = {0, 35, 90, 135, 180, 225, 270, 315};
    const auto modifier = getMatchingModifier(tag);
    BOOST_ASSERT(static_cast<std::size_t>(modifier) <
                 sizeof(idealized_turn_angles) / sizeof(*idealized_turn_angles));
    const auto idealized_angle = idealized_turn_angles[modifier];
    return angularDeviation(idealized_angle, road.angle);
}

// Every tag is somewhat idealized in form of the expected angle. A through lane should go straight
// (or follow a 180 degree turn angle between in/out segments.) The following function tries to find
// the best possible match for every tag in a given intersection, considering a few corner cases
// introduced to OSRM handling u-turns
typename Intersection::const_iterator findBestMatch(const TurnLaneType::Mask tag,
                                                    const Intersection &intersection)
{
    return std::min_element(intersection.begin(),
                            intersection.end(),
                            [tag](const ConnectedRoad &lhs, const ConnectedRoad &rhs) {
                                // prefer valid matches
                                if (isValidMatch(tag, lhs.instruction) !=
                                    isValidMatch(tag, rhs.instruction))
                                    return isValidMatch(tag, lhs.instruction);

                                // if the entry allowed flags don't match, we select the one with
                                // entry allowed set to true
                                if (lhs.entry_allowed != rhs.entry_allowed)
                                    return lhs.entry_allowed;

                                return getMatchingQuality(tag, lhs) < getMatchingQuality(tag, rhs);
                            });
}

// Reverse is a special case, because it requires access to the leftmost tag. It has its own
// matching function as a result of that. The leftmost tag is required, since u-turns are disabled
// by default in OSRM. Therefor we cannot check whether a turn is allowed, since it could be
// possible that it is forbidden. In addition, the best u-turn angle does not necessarily represent
// the u-turn, since it could be a sharp-left turn instead on a road with a middle island.
typename Intersection::const_iterator findBestMatchForReverse(const TurnLaneType::Mask neighbor_tag,
                                                              const Intersection &intersection)
{
    const auto neighbor_itr = findBestMatch(neighbor_tag, intersection);
    if (neighbor_itr + 1 == intersection.cend())
        return intersection.begin();

    const TurnLaneType::Mask tag = TurnLaneType::uturn;
    return std::min_element(
        intersection.begin() + std::distance(intersection.begin(), neighbor_itr),
        intersection.end(),
        [tag](const ConnectedRoad &lhs, const ConnectedRoad &rhs) {
            // prefer valid matches
            if (isValidMatch(tag, lhs.instruction) != isValidMatch(tag, rhs.instruction))
                return isValidMatch(tag, lhs.instruction);

            // if the entry allowed flags don't match, we select the one with
            // entry allowed set to true
            if (lhs.entry_allowed != rhs.entry_allowed)
                return lhs.entry_allowed;

            return getMatchingQuality(tag, lhs) < getMatchingQuality(tag, rhs);
        });
}

// a match is trivial if all turns can be associated with their best match in a valid way and the
// matches occur in order
bool canMatchTrivially(const Intersection &intersection, const LaneDataVector &lane_data)
{
    std::size_t road_index = 1, lane = 0;
    if (!lane_data.empty() && lane_data.front().tag == TurnLaneType::uturn)
    {
        // the very first is a u-turn to the right
        if (intersection[0].entry_allowed)
            lane = 1;
    }
    for (; road_index < intersection.size() && lane < lane_data.size(); ++road_index)
    {
        if (intersection[road_index].entry_allowed)
        {
            BOOST_ASSERT(lane_data[lane].from != INVALID_LANEID);
            if (!isValidMatch(lane_data[lane].tag, intersection[road_index].instruction))
                return false;

            if (findBestMatch(lane_data[lane].tag, intersection) !=
                intersection.begin() + road_index)
                return false;

            ++lane;
        }
    }
    return lane == lane_data.size() ||
           (lane + 1 == lane_data.size() && lane_data.back().tag == TurnLaneType::uturn);
}

Intersection triviallyMatchLanesToTurns(Intersection intersection,
                                        const LaneDataVector &lane_data,
                                        const util::NodeBasedDynamicGraph &node_based_graph,
                                        const LaneDescriptionID lane_string_id,
                                        util::guidance::LaneDataIdMap &lane_data_to_id)
{
    std::size_t road_index = 1, lane = 0;

    const auto matchRoad = [&](ConnectedRoad &road, const TurnLaneData &data) {
        util::guidance::LaneTupleIdPair key{{LaneID(data.to - data.from + 1), data.from},
                                            lane_string_id};

        auto lane_data_id = boost::numeric_cast<LaneDataID>(lane_data_to_id.size());
        const auto it = lane_data_to_id.find(key);

        if (it == lane_data_to_id.end())
            lane_data_to_id.insert({key, lane_data_id});
        else
            lane_data_id = it->second;

        // set lane id instead after the switch:
        road.lane_data_id = lane_data_id;
    };

    if (!lane_data.empty() && lane_data.front().tag == TurnLaneType::uturn)
    {
        // the very first is a u-turn to the right
        if (intersection[0].entry_allowed)
        {
            std::size_t u_turn = 0;
            if (node_based_graph.GetEdgeData(intersection[0].eid).reversed)
            {
                if (intersection.size() <= 1 || !intersection[1].entry_allowed ||
                    intersection[1].instruction.direction_modifier != DirectionModifier::SharpRight)
                {
                    // cannot match u-turn in a valid way
                    return intersection;
                }
                u_turn = 1;
                road_index = 2;
            }
            intersection[u_turn].entry_allowed = true;
            intersection[u_turn].instruction.type = TurnType::Turn;
            intersection[u_turn].instruction.direction_modifier = DirectionModifier::UTurn;

            matchRoad(intersection[u_turn], lane_data.back());
            // continue with the first lane
            lane = 1;
        }
        else
            return intersection;
    }

    for (; road_index < intersection.size() && lane < lane_data.size(); ++road_index)
    {
        if (intersection[road_index].entry_allowed)
        {
            BOOST_ASSERT(lane_data[lane].from != INVALID_LANEID);
            BOOST_ASSERT(isValidMatch(lane_data[lane].tag, intersection[road_index].instruction));
            BOOST_ASSERT(findBestMatch(lane_data[lane].tag, intersection) ==
                         intersection.begin() + road_index);

            if (TurnType::Suppressed == intersection[road_index].instruction.type &&
                !lane_data[lane].suppress_assignment)
                intersection[road_index].instruction.type = TurnType::UseLane;

            matchRoad(intersection[road_index], lane_data[lane]);
            ++lane;
        }
    }

    // handle reverse tag, if present
    if (lane + 1 == lane_data.size() && lane_data.back().tag == TurnLaneType::uturn)
    {
        std::size_t u_turn = 0;
        if (node_based_graph.GetEdgeData(intersection[0].eid).reversed)
        {
            if (!intersection.back().entry_allowed ||
                intersection.back().instruction.direction_modifier != DirectionModifier::SharpLeft)
            {
                // cannot match u-turn in a valid way
                return intersection;
            }
            u_turn = intersection.size() - 1;
        }
        intersection[u_turn].entry_allowed = true;
        intersection[u_turn].instruction.type = TurnType::Turn;
        intersection[u_turn].instruction.direction_modifier = DirectionModifier::UTurn;

        matchRoad(intersection[u_turn], lane_data.back());
    }
    return intersection;
}

} // namespace lane_matching
} // namespace guidance
} // namespace extractor
} // namespace osrm