#include <config.h>
#include <cassert>
#include <utils/options/OptionsCont.h>
#include <utils/common/MsgHandler.h>
#include <utils/common/ToString.h>
#include "NBNetBuilder.h"
#include "NBNodeCont.h"
#include "NBNode.h"
#include "NBEdge.h"
#include "NBAlgorithms_Ramps.h"
#define OFFRAMP_LOOKBACK 500
#define MIN_SPLIT_LENGTH POSITION_EPS
#define DEBUGNODEID ""
#define DEBUGCOND(obj) ((obj != 0 && (obj)->getID() == DEBUGNODEID))
const std::string NBRampsComputer::ADDED_ON_RAMP_EDGE("-AddedOnRampEdge");
NBRampsComputer::NBRampsComputer() { }
void
NBRampsComputer::computeRamps(NBNetBuilder& nb, OptionsCont& oc, bool mayAddOrRemove) {
const bool guessAndAdd = oc.getBool("ramps.guess") && mayAddOrRemove;
const double minHighwaySpeed = oc.getFloat("ramps.min-highway-speed");
const double maxRampSpeed = oc.getFloat("ramps.max-ramp-speed");
const double rampLength = oc.getFloat("ramps.ramp-length");
const double minWeaveLength = oc.getFloat("ramps.min-weave-length");
const bool dontSplit = oc.getBool("ramps.no-split");
NBNodeCont& nc = nb.getNodeCont();
NBEdgeCont& ec = nb.getEdgeCont();
NBDistrictCont& dc = nb.getDistrictCont();
std::set<NBEdge*> incremented;
std::set<std::string> noramps;
if (oc.isSet("ramps.unset")) {
std::vector<std::string> edges = oc.getStringVector("ramps.unset");
noramps.insert(edges.begin(), edges.end());
}
for (const EdgeSet& round : ec.getRoundabouts()) {
for (NBEdge* const edge : round) {
noramps.insert(edge->getID());
}
}
nb.getPTStopCont().addEdges2Keep(oc, noramps);
nb.getParkingCont().addEdges2Keep(oc, noramps);
if (guessAndAdd || oc.getBool("ramps.guess-acceleration-lanes")) {
for (const auto& it : ec) {
it.second->markOffRamp(false);
}
std::set<NBNode*, ComparatorIdLess> potOnRamps;
std::set<NBNode*, ComparatorIdLess> potOffRamps;
for (const auto& i : nc) {
NBNode* cur = i.second;
#ifdef DEBUG_RAMPS
if (DEBUGCOND(cur)) {
std::cout << "check ramps cur=" << cur->getID() << "\n";
}
#endif
if (mayNeedOnRamp(cur, minHighwaySpeed, maxRampSpeed, noramps, minWeaveLength)) {
potOnRamps.insert(cur);
}
if (mayNeedOffRamp(cur, minHighwaySpeed, maxRampSpeed, noramps)) {
potOffRamps.insert(cur);
}
}
for (std::set<NBNode*, ComparatorIdLess>::const_iterator i = potOnRamps.begin(); i != potOnRamps.end(); ++i) {
buildOnRamp(*i, nc, ec, dc, rampLength, dontSplit || !guessAndAdd, guessAndAdd);
}
for (std::set<NBNode*, ComparatorIdLess>::const_iterator i = potOffRamps.begin(); i != potOffRamps.end(); ++i) {
buildOffRamp(*i, nc, ec, dc, rampLength, dontSplit || !guessAndAdd, guessAndAdd, potOnRamps);
}
}
if (oc.isSet("ramps.set") && mayAddOrRemove) {
std::vector<std::string> edges = oc.getStringVector("ramps.set");
std::set<NBNode*, ComparatorIdLess> potOnRamps;
for (const std::string& i : edges) {
NBEdge* e = ec.retrieve(i);
if (noramps.count(i) != 0) {
WRITE_WARNINGF(TL("Can not build ramp on edge '%' - the edge is unsuitable."), i);
continue;
}
if (e == nullptr) {
WRITE_WARNINGF(TL("Can not build on ramp on edge '%' - the edge is not known."), i);
continue;
}
NBNode* from = e->getFromNode();
if (from->getIncomingEdges().size() == 2 && from->getOutgoingEdges().size() == 1) {
buildOnRamp(from, nc, ec, dc, rampLength, dontSplit, true);
potOnRamps.insert(from);
}
e = ec.retrieve(i);
if (e == nullptr) {
WRITE_WARNINGF(TL("Can not build off ramp on edge '%' - the edge is not known."), i);
continue;
}
NBNode* to = e->getToNode();
if (to->getIncomingEdges().size() == 1 && to->getOutgoingEdges().size() == 2) {
buildOffRamp(to, nc, ec, dc, rampLength, dontSplit, true, potOnRamps);
}
}
}
}
bool
NBRampsComputer::mayNeedOnRamp(NBNode* cur, double minHighwaySpeed, double maxRampSpeed, const std::set<std::string>& noramps, double minWeaveLength) {
if (cur->getOutgoingEdges().size() != 1 || cur->getIncomingEdges().size() != 2) {
return false;
}
NBEdge* potHighway, *potRamp, *cont;
getOnRampEdges(cur, &potHighway, &potRamp, &cont);
#ifdef DEBUG_RAMPS
if (DEBUGCOND(cur)) {
std::cout << "check on ramp hw=" << potHighway->getID() << " ramp=" << potRamp->getID() << " cont=" << cont->getID() << std::endl;
}
#endif
if (fulfillsRampConstraints(potHighway, potRamp, cont, minHighwaySpeed, maxRampSpeed, noramps)) {
double seen = cont->getLength();
while (seen < minWeaveLength) {
if (cont->getToNode()->getOutgoingEdges().size() > 1) {
return false;
} else if (cont->getToNode()->getOutgoingEdges().size() == 0) {
return true;
}
cont = cont->getToNode()->getOutgoingEdges().front();
seen += cont->getLength();
}
return true;
} else {
return false;
}
}
bool
NBRampsComputer::mayNeedOffRamp(NBNode* cur, double minHighwaySpeed, double maxRampSpeed, const std::set<std::string>& noramps) {
if (cur->getIncomingEdges().size() != 1 || cur->getOutgoingEdges().size() != 2) {
return false;
}
NBEdge* potHighway, *potRamp, *prev;
getOffRampEdges(cur, &potHighway, &potRamp, &prev);
#ifdef DEBUG_RAMPS
if (DEBUGCOND(cur)) {
std::cout << "check off ramp hw=" << potHighway->getID() << " ramp=" << potRamp->getID() << " prev=" << prev->getID() << std::endl;
}
#endif
return fulfillsRampConstraints(potHighway, potRamp, prev, minHighwaySpeed, maxRampSpeed, noramps);
}
void
NBRampsComputer::buildOnRamp(NBNode* cur, NBNodeCont& nc, NBEdgeCont& ec, NBDistrictCont& dc, double rampLength, bool dontSplit, bool addLanes) {
NBEdge* potHighway, *potRamp, *cont;
getOnRampEdges(cur, &potHighway, &potRamp, &cont);
#ifdef DEBUG_RAMPS
if (DEBUGCOND(cur)) {
std::cout << "buildOnRamp cur=" << cur->getID() << " hw=" << potHighway->getID() << " ramp=" << potRamp->getID() << " cont=" << cont->getID() << "\n";
}
#endif
const int firstLaneNumber = cont->getNumLanes();
const int toAdd = (potRamp->getNumLanes() + potHighway->getNumLanes()) - firstLaneNumber;
NBEdge* first = cont;
NBEdge* last = cont;
NBEdge* curr = cont;
std::set<NBEdge*> incremented;
if (addLanes && toAdd > 0 && std::find(incremented.begin(), incremented.end(), cont) == incremented.end()) {
double currLength = 0;
while (curr != nullptr && currLength + curr->getGeometry().length() - POSITION_EPS < rampLength) {
if (find(incremented.begin(), incremented.end(), curr) == incremented.end()) {
curr->incLaneNo(toAdd);
if (curr->getStep() < NBEdge::EdgeBuildingStep::LANES2LANES_USER || !ec.hasPostProcessConnection(curr->getID())) {
curr->invalidateConnections(true);
}
incremented.insert(curr);
moveRampRight(curr, toAdd);
currLength += curr->getGeometry().length();
last = curr;
for (int i = 0; i < curr->getNumLanes() - potHighway->getNumLanes(); ++i) {
curr->setAcceleration(i, true);
}
}
NBNode* nextN = curr->getToNode();
if (nextN->getOutgoingEdges().size() == 1 && nextN->getIncomingEdges().size() == 1) {
curr = nextN->getOutgoingEdges()[0];
if (curr->getNumLanes() != firstLaneNumber) {
curr = nullptr;
} else if (curr->isTurningDirectionAt(last)) {
curr = nullptr;
} else if (curr == potHighway || curr == potRamp) {
curr = nullptr;
}
} else {
curr = nullptr;
}
}
if (curr != nullptr && !dontSplit && currLength + MIN_SPLIT_LENGTH < rampLength && curr->getNumLanes() == firstLaneNumber && std::find(incremented.begin(), incremented.end(), curr) == incremented.end()) {
bool wasFirst = first == curr;
std::string newNodeID = getUnusedID(curr->getID() + "-AddedOnRampNode", nc);
std::string newEdgeID = getUnusedID(curr->getID() + ADDED_ON_RAMP_EDGE, ec);
NBNode* rn = new NBNode(newNodeID, curr->getGeometry().positionAtOffset(rampLength - currLength));
nc.insert(rn);
std::string name = curr->getID();
const double currShift = myShiftedEdges[curr];
if (!ec.splitAt(dc, curr, rn, newEdgeID, curr->getID(), curr->getNumLanes() + toAdd, curr->getNumLanes())) {
WRITE_WARNING("Could not build on-ramp for edge '" + curr->getID() + "' for unknown reason");
return;
}
curr = ec.retrieve(newEdgeID);
myShiftedEdges[curr] = currShift;
incremented.insert(curr);
last = curr;
moveRampRight(curr, toAdd);
if (wasFirst) {
first = curr;
}
for (int i = 0; i < curr->getNumLanes() - potHighway->getNumLanes(); ++i) {
curr->setAcceleration(i, true);
}
}
if (curr == cont && dontSplit && addLanes) {
WRITE_WARNING("Could not build on-ramp for edge '" + curr->getID() + "' due to option '--ramps.no-split'");
return;
}
} else {
for (int i = 0; i < firstLaneNumber - potHighway->getNumLanes(); ++i) {
cont->setAcceleration(i, true);
}
}
if (addLanes) {
if (potHighway->getStep() < NBEdge::EdgeBuildingStep::LANES2LANES_USER) {
if (!potHighway->addLane2LaneConnections(0, first, potRamp->getNumLanes(), MIN2(first->getNumLanes() - potRamp->getNumLanes(), potHighway->getNumLanes()), NBEdge::Lane2LaneInfoType::VALIDATED, true)) {
throw ProcessError(TL("Could not set connection!"));
}
}
if (potRamp->getStep() < NBEdge::EdgeBuildingStep::LANES2LANES_USER) {
if (!potRamp->addLane2LaneConnections(0, first, 0, potRamp->getNumLanes(), NBEdge::Lane2LaneInfoType::VALIDATED, true)) {
throw ProcessError(TL("Could not set connection!"));
}
}
patchRampGeometry(potRamp, first, potHighway, false);
}
}
void
NBRampsComputer::buildOffRamp(NBNode* cur, NBNodeCont& nc, NBEdgeCont& ec, NBDistrictCont& dc, double rampLength, bool dontSplit, bool addLanes,
const std::set<NBNode*, ComparatorIdLess>& potOnRamps) {
NBEdge* potHighway, *potRamp, *prev;
getOffRampEdges(cur, &potHighway, &potRamp, &prev);
#ifdef DEBUG_RAMPS
if (DEBUGCOND(cur)) {
std::cout << "buildOffRamp cur=" << cur->getID() << " hw=" << potHighway->getID() << " ramp=" << potRamp->getID() << " prev=" << prev->getID() << "\n";
}
#endif
const int firstLaneNumber = prev->getNumLanes();
const int toAdd = (potRamp->getNumLanes() + potHighway->getNumLanes()) - firstLaneNumber;
NBEdge* first = prev;
NBEdge* last = prev;
NBEdge* curr = prev;
std::set<NBEdge*> incremented;
if (addLanes && toAdd > 0 && std::find(incremented.begin(), incremented.end(), prev) == incremented.end()) {
double currLength = 0;
while (curr != nullptr && currLength + curr->getGeometry().length() - POSITION_EPS < rampLength) {
if (find(incremented.begin(), incremented.end(), curr) == incremented.end()) {
curr->incLaneNo(toAdd);
if (curr->getStep() < NBEdge::EdgeBuildingStep::LANES2LANES_USER || !ec.hasPostProcessConnection(curr->getID())) {
curr->invalidateConnections(true);
}
incremented.insert(curr);
moveRampRight(curr, toAdd);
currLength += curr->getGeometry().length();
last = curr;
}
NBNode* prevN = curr->getFromNode();
if (prevN->getIncomingEdges().size() == 1 && prevN->getOutgoingEdges().size() == 1) {
curr = prevN->getIncomingEdges()[0];
if (curr->getStep() < NBEdge::EdgeBuildingStep::LANES2LANES_USER || !ec.hasPostProcessConnection(curr->getID())) {
curr->invalidateConnections();
}
if (curr->getNumLanes() != firstLaneNumber) {
curr = nullptr;
} else if (last->isTurningDirectionAt(curr)) {
curr = nullptr;
} else if (curr == potHighway || curr == potRamp) {
curr = nullptr;
}
} else {
curr = nullptr;
}
}
if (curr != nullptr && !dontSplit && currLength + MIN_SPLIT_LENGTH < rampLength && curr->getNumLanes() == firstLaneNumber && std::find(incremented.begin(), incremented.end(), curr) == incremented.end()) {
bool wasFirst = first == curr;
Position pos = curr->getGeometry().positionAtOffset(curr->getGeometry().length() - (rampLength - currLength));
std::string newNodeID = getUnusedID(curr->getID() + "-AddedOffRampNode", nc);
std::string newEdgeID = getUnusedID(curr->getID() + "-AddedOffRampEdge", ec);
NBNode* rn = new NBNode(newNodeID, pos);
nc.insert(rn);
std::string name = curr->getID();
const double currShift = myShiftedEdges[curr];
if (!ec.splitAt(dc, curr, rn, curr->getID(), newEdgeID, curr->getNumLanes(), curr->getNumLanes() + toAdd)) {
WRITE_WARNING("Could not build off-ramp for edge '" + curr->getID() + "' for unknown reason");
return;
}
curr = ec.retrieve(newEdgeID);
myShiftedEdges[curr] = currShift;
incremented.insert(curr);
last = curr;
moveRampRight(curr, toAdd);
if (wasFirst) {
first = curr;
}
}
if (curr == prev && dontSplit && addLanes) {
WRITE_WARNING("Could not build off-ramp for edge '" + curr->getID() + "' due to option '--ramps.no-split'");
return;
}
}
NBEdge* toMark = first;
toMark->markOffRamp(true);
double markedLength = toMark->getLoadedLength();
while (markedLength < OFFRAMP_LOOKBACK) {
if (toMark != first && toMark->getToNode()->getOutgoingEdges().size() != 1) {
break;
}
NBNode* from = toMark->getFromNode();
if (from->getIncomingEdges().size() == 1) {
toMark = from->getIncomingEdges()[0];
} else if (potOnRamps.count(from) == 1) {
NBEdge* potOnRamp, *cont;
getOnRampEdges(from, &toMark, &potOnRamp, &cont);
} else {
break;
}
toMark->markOffRamp(true);
markedLength += toMark->getLoadedLength();
}
if (addLanes) {
if (first->getStep() < NBEdge::EdgeBuildingStep::LANES2LANES_USER) {
if (!first->addLane2LaneConnections(potRamp->getNumLanes(), potHighway, 0, MIN2(first->getNumLanes() - 1, potHighway->getNumLanes()), NBEdge::Lane2LaneInfoType::VALIDATED, true)) {
throw ProcessError(TL("Could not set connection!"));
}
if (!first->addLane2LaneConnections(0, potRamp, 0, potRamp->getNumLanes(), NBEdge::Lane2LaneInfoType::VALIDATED, false)) {
throw ProcessError(TL("Could not set connection!"));
}
}
patchRampGeometry(potRamp, first, potHighway, true);
}
}
void
NBRampsComputer::moveRampRight(NBEdge* ramp, int addedLanes) {
if (ramp->getLaneSpreadFunction() != LaneSpreadFunction::CENTER) {
return;
}
try {
PositionVector g = ramp->getGeometry();
double offset = (0.5 * addedLanes *
(ramp->getLaneWidth() == NBEdge::UNSPECIFIED_WIDTH ? SUMO_const_laneWidth : ramp->getLaneWidth()));
if (myShiftedEdges.count(ramp) != 0) {
offset -= myShiftedEdges[ramp];
}
g.move2side(offset);
ramp->setGeometry(g);
myShiftedEdges[ramp] = offset;
} catch (InvalidArgument&) {
WRITE_WARNINGF(TL("For edge '%': could not compute shape."), ramp->getID());
}
}
bool
NBRampsComputer::determinedBySpeed(NBEdge** potHighway, NBEdge** potRamp) {
if (fabs((*potHighway)->getSpeed() - (*potRamp)->getSpeed()) < .1) {
return false;
}
if ((*potHighway)->getSpeed() < (*potRamp)->getSpeed()) {
std::swap(*potHighway, *potRamp);
}
return true;
}
bool
NBRampsComputer::determinedByLaneNumber(NBEdge** potHighway, NBEdge** potRamp) {
if ((*potHighway)->getNumLanes() == (*potRamp)->getNumLanes()) {
return false;
}
if ((*potHighway)->getNumLanes() < (*potRamp)->getNumLanes()) {
std::swap(*potHighway, *potRamp);
}
return true;
}
void
NBRampsComputer::getOnRampEdges(NBNode* n, NBEdge** potHighway, NBEdge** potRamp, NBEdge** other) {
*other = n->getOutgoingEdges()[0];
const std::vector<NBEdge*>& edges = n->getIncomingEdges();
assert(edges.size() == 2);
*potHighway = edges[0];
*potRamp = edges[1];
if (NBContHelper::relative_incoming_edge_sorter(*other)(*potRamp, *potHighway)) {
std::swap(*potHighway, *potRamp);
}
}
void
NBRampsComputer::getOffRampEdges(NBNode* n, NBEdge** potHighway, NBEdge** potRamp, NBEdge** other) {
*other = n->getIncomingEdges()[0];
const std::vector<NBEdge*>& edges = n->getOutgoingEdges();
*potHighway = edges[0];
*potRamp = edges[1];
assert(edges.size() == 2);
const std::vector<NBEdge*>& edges2 = n->getEdges();
#ifdef DEBUG_RAMPS
if (DEBUGCOND(n)) {
std::cout << " edges=" << toString(edges) << " edges2=" << toString(edges2) << "\n";
}
#endif
std::vector<NBEdge*>::const_iterator i = std::find(edges2.begin(), edges2.end(), *other);
NBContHelper::nextCW(edges2, i);
if ((*i) == *potRamp) {
std::swap(*potHighway, *potRamp);
}
}
bool
NBRampsComputer::fulfillsRampConstraints(
NBEdge* potHighway, NBEdge* potRamp, NBEdge* other, double minHighwaySpeed, double maxRampSpeed,
const std::set<std::string>& noramps) {
if (hasWrongMode(potHighway) || hasWrongMode(potRamp) || hasWrongMode(other)) {
return false;
}
if (NBNode::isTrafficLight(potRamp->getToNode()->getType())) {
return false;
}
if (potHighway->isMacroscopicConnector() || potRamp->isMacroscopicConnector() || other->isMacroscopicConnector()) {
return false;
}
if (potHighway->getNumLanes() + potRamp->getNumLanes() < other->getNumLanes()) {
return false;
}
double maxSpeed = MAX3(potHighway->getSpeed(), other->getSpeed(), potRamp->getSpeed());
if (maxSpeed < minHighwaySpeed) {
return false;
}
if (other->getToNode() == potHighway->getFromNode()) {
if (other->isTurningDirectionAt(potHighway) ||
other->isTurningDirectionAt(potRamp)) {
return false;
}
} else {
if (other->isTurningDirectionAt(potHighway) ||
other->isTurningDirectionAt(potRamp)) {
return false;
}
}
const NBNode* node = ((potHighway->getToNode() == potRamp->getToNode() && potHighway->getToNode() == other->getFromNode())
? potHighway->getToNode() : potHighway->getFromNode());
double angle = fabs(NBHelpers::relAngle(potHighway->getAngleAtNode(node), other->getAngleAtNode(node)));
if (angle >= 60) {
return false;
}
angle = fabs(NBHelpers::relAngle(potRamp->getAngleAtNode(node), other->getAngleAtNode(node)));
if (angle >= 60) {
return false;
}
if (maxRampSpeed > 0 && maxRampSpeed < potRamp->getSpeed()) {
return false;
}
if (noramps.find(other->getID()) != noramps.end()) {
return false;
}
return true;
}
bool
NBRampsComputer::hasWrongMode(NBEdge* edge) {
if ((edge->getPermissions() & SVC_PASSENGER) == 0) {
return true;
}
for (int i = 0; i < (int)edge->getNumLanes(); ++i) {
if ((edge->getPermissions(i) & ~(SVC_PEDESTRIAN | SVC_BICYCLE)) == 0) {
return true;
}
}
return false;
}
void
NBRampsComputer::patchRampGeometry(NBEdge* potRamp, NBEdge* first, NBEdge* potHighway, bool onRamp) {
if (first->getLaneSpreadFunction() == LaneSpreadFunction::CENTER && first->hasDefaultGeometryEndpoints()) {
const NBNode* n = onRamp ? potHighway->getToNode() : potHighway->getFromNode();
if (potHighway->hasDefaultGeometryEndpointAtNode(n)) {
PositionVector p2 = first->getGeometry();
try {
p2.move2side((first->getNumLanes() - potHighway->getNumLanes()) * first->getLaneWidth(0) * 0.5);
first->setGeometry(p2);
} catch (InvalidArgument&) {}
}
}
PositionVector p = potRamp->getGeometry();
double offset = 0;
int firstIndex = MAX2(0, MIN2(potRamp->getNumLanes(), first->getNumLanes()) - 1);
if (potRamp->getLaneSpreadFunction() == LaneSpreadFunction::RIGHT) {
offset = -first->getLaneWidth(firstIndex) / 2;
} else {
if (firstIndex % 2 == 1) {
offset = -first->getLaneWidth(firstIndex / 2) / 2;
}
firstIndex /= 2;
}
first->resetLaneShapes();
PositionVector l = first->getLaneShape(firstIndex);
try {
l.move2side(offset);
} catch (InvalidArgument&) {}
if (onRamp) {
p[0] = l[-1];
} else {
p.pop_back();
p.push_back(l[0]);
}
potRamp->setGeometry(p);
}