C++ Intersection::begin方法代码示例

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);
//.........这里部分代码省略.........

//.........这里部分代码省略.........
                   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);
}