bool
MSLaneChangerSublane::startChangeSublane(MSVehicle* vehicle, ChangerIt& from, SUMOReal latDist) {
//gDebugFlag4 = vehicle->getID() == "Togliatti_80_26";
// 1) update vehicles lateral position according to latDist and target lane
vehicle->myState.myPosLat += latDist;
vehicle->myCachedPosition = Position::INVALID;
// 2) distinguish several cases
// a) vehicle moves completely within the same lane
// b) vehicle intersects another lane
// - vehicle must be moved to the lane where it's midpoint is (either old or new)
// - shadow vehicle must be created/moved to the other lane if the vehicle intersects it
// 3) updated dens of all lanes that hold the vehicle or its shadow
const int direction = vehicle->getLateralPositionOnLane() < 0 ? -1 : 1;
ChangerIt to = from;
if (mayChange(direction)) {
to = from + direction;
} else {
/// XXX assert(false);
}
const bool changedToNewLane = to != from && fabs(vehicle->getLateralPositionOnLane()) > 0.5 * vehicle->getLane()->getWidth() && mayChange(direction);
if (changedToNewLane) {
vehicle->myState.myPosLat -= direction * 0.5 * (from->lane->getWidth() + to->lane->getWidth());
to->lane->myTmpVehicles.insert(to->lane->myTmpVehicles.begin(), vehicle);
to->dens += vehicle->getVehicleType().getLengthWithGap();
vehicle->getLaneChangeModel().startLaneChangeManeuver(from->lane, to->lane, direction);
to->ahead.addLeader(vehicle, false, 0);
} else {
registerUnchanged(vehicle);
from->ahead.addLeader(vehicle, false, 0);
}
MSLane* oldShadowLane = vehicle->getLaneChangeModel().getShadowLane();
vehicle->getLaneChangeModel().updateShadowLane();
MSLane* shadowLane = vehicle->getLaneChangeModel().getShadowLane();
if (shadowLane != 0 && shadowLane != oldShadowLane) {
assert(to != from);
const SUMOReal latOffset = vehicle->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge();
(myChanger.begin() + shadowLane->getIndex())->ahead.addLeader(vehicle, false, latOffset);
}
if (gDebugFlag4) std::cout << SIMTIME << " startChangeSublane shadowLane"
<< " old=" << Named::getIDSecure(oldShadowLane)
<< " new=" << Named::getIDSecure(vehicle->getLaneChangeModel().getShadowLane()) << "\n";
// compute new angle of the vehicle from the x- and y-distances travelled within last time step
// (should happen last because primaryLaneChanged() also triggers angle computation)
// this part of the angle comes from the orientation of our current lane
SUMOReal laneAngle = vehicle->getLane()->getShape().rotationAtOffset(vehicle->getLane()->interpolateLanePosToGeometryPos(vehicle->getPositionOnLane())) ;
// this part of the angle comes from the vehicle's lateral movement
SUMOReal changeAngle = 0;
// avoid flicker
if (fabs(latDist) > NUMERICAL_EPS) {
// avoid extreme angles by using vehicle length as a proxy for turning radius
changeAngle = atan2(latDist, SPEED2DIST(MAX2(vehicle->getVehicleType().getLength(), vehicle->getSpeed())));
}
vehicle->setAngle(laneAngle + changeAngle);
return changedToNewLane;
}
void
MSLaneChangerSublane::updateChanger(bool vehHasChanged) {
MSLaneChanger::updateChanger(vehHasChanged);
if (!vehHasChanged) {
MSVehicle* lead = myCandi->lead;
//std::cout << SIMTIME << " updateChanger lane=" << myCandi->lane->getID() << " lead=" << Named::getIDSecure(lead) << "\n";
myCandi->ahead.addLeader(lead, false, 0);
MSLane* shadowLane = lead->getLaneChangeModel().getShadowLane();
if (shadowLane != 0) {
const SUMOReal latOffset = lead->getLane()->getRightSideOnEdge() - shadowLane->getRightSideOnEdge();
//std::cout << SIMTIME << " updateChanger shadowLane=" << shadowLane->getID() << " lead=" << Named::getIDSecure(lead) << "\n";
(myChanger.begin() + shadowLane->getIndex())->ahead.addLeader(lead, false, latOffset);
}
}
//std::cout << SIMTIME << " updateChanger: lane=" << myCandi->lane->getID() << " lead=" << Named::getIDSecure(myCandi->lead) << " ahead=" << myCandi->ahead.toString() << " vehHasChanged=" << vehHasChanged << "\n";
//for (ChangerIt ce = myChanger.begin(); ce != myChanger.end(); ++ce) {
// std::cout << " lane=" << ce->lane->getID() << " vehicles=" << toString(ce->lane->myVehicles) << "\n";
//}
}
bool
MSEdge::insertVehicle(SUMOVehicle& v, SUMOTime time, const bool checkOnly, const bool forceCheck) const {
// when vaporizing, no vehicles are inserted, but checking needs to be successful to trigger removal
if (isVaporizing()) {
return checkOnly;
}
if (isTaz() && checkOnly) {
return true;
}
const SUMOVehicleParameter& pars = v.getParameter();
const MSVehicleType& type = v.getVehicleType();
if (pars.departSpeedProcedure == DEPART_SPEED_GIVEN && pars.departSpeed > getVehicleMaxSpeed(&v)) {
if (type.getSpeedDeviation() > 0) {
v.setChosenSpeedFactor(type.computeChosenSpeedDeviation(0, pars.departSpeed / (type.getSpeedFactor() * getSpeedLimit())));
if (v.getChosenSpeedFactor() > type.getSpeedFactor() * (2 * type.getSpeedDeviation() + 1)) {
// only warn for significant deviation
WRITE_WARNING("Choosing new speed factor " + toString(v.getChosenSpeedFactor()) + " for vehicle '" + pars.id + "' to match departure speed.");
}
} else {
throw ProcessError("Departure speed for vehicle '" + pars.id +
"' is too high for the departure edge '" + getID() + "'.");
}
}
if (!checkOnly) {
std::string msg;
if (!v.hasValidRoute(msg)) {
if (MSGlobals::gCheckRoutes) {
throw ProcessError("Vehicle '" + v.getID() + "' has no valid route. " + msg);
} else if (v.getEdge()->getPurpose() == MSEdge::EDGEFUNCTION_DISTRICT) {
WRITE_WARNING("Removing vehicle '" + pars.id + "' which has no valid route.");
MSNet::getInstance()->getInsertionControl().descheduleDeparture(&v);
return false;
}
}
}
if (MSGlobals::gUseMesoSim) {
SUMOReal pos = 0.0;
switch (pars.departPosProcedure) {
case DEPART_POS_GIVEN:
if (pars.departPos >= 0.) {
pos = pars.departPos;
} else {
pos = pars.departPos + getLength();
}
if (pos < 0 || pos > getLength()) {
WRITE_WARNING("Invalid departPos " + toString(pos) + " given for vehicle '" +
v.getID() + "'. Inserting at lane end instead.");
pos = getLength();
}
break;
case DEPART_POS_RANDOM:
case DEPART_POS_RANDOM_FREE:
pos = RandHelper::rand(getLength());
break;
default:
break;
}
bool result = false;
MESegment* segment = MSGlobals::gMesoNet->getSegmentForEdge(*this, pos);
MEVehicle* veh = static_cast<MEVehicle*>(&v);
if (pars.departPosProcedure == DEPART_POS_FREE) {
while (segment != 0 && !result) {
if (checkOnly) {
result = segment->hasSpaceFor(veh, time, true);
} else {
result = segment->initialise(veh, time);
}
segment = segment->getNextSegment();
}
} else {
if (checkOnly) {
result = segment->hasSpaceFor(veh, time, true);
} else {
result = segment->initialise(veh, time);
}
}
return result;
}
if (checkOnly) {
switch (v.getParameter().departLaneProcedure) {
case DEPART_LANE_GIVEN:
case DEPART_LANE_DEFAULT:
case DEPART_LANE_FIRST_ALLOWED: {
MSLane* insertionLane = getDepartLane(static_cast<MSVehicle&>(v));
if (insertionLane == 0) {
WRITE_WARNING("could not insert vehicle '" + v.getID() + "' on any lane of edge '" + getID() + "', time=" + time2string(MSNet::getInstance()->getCurrentTimeStep()));
return false;
}
const SUMOReal occupancy = insertionLane->getBruttoOccupancy();
return occupancy == (SUMOReal)0 || occupancy * myLength + v.getVehicleType().getLengthWithGap() <= myLength;
}
default:
for (std::vector<MSLane*>::const_iterator i = myLanes->begin(); i != myLanes->end(); ++i) {
const SUMOReal occupancy = (*i)->getBruttoOccupancy();
if (occupancy == (SUMOReal)0 || occupancy * myLength + v.getVehicleType().getLengthWithGap() <= myLength) {
return true;
}
}
}
return false;
}
MSLane* insertionLane = getDepartLane(static_cast<MSVehicle&>(v));
if (insertionLane == 0){
return false;
}
if (!forceCheck){
if (myLastFailedInsertionTime == time) {
if (myFailedInsertionMemory.count(insertionLane->getIndex())){
// A vehicle was already rejected for the proposed insertionLane in this timestep
return false;
}
} else {
// last rejection occured in a previous timestep, clear cache
myFailedInsertionMemory.clear();
}
}
bool success = insertionLane->insertVehicle(static_cast<MSVehicle&>(v));
if(!success){
myFailedInsertionMemory.insert(insertionLane->getIndex());
}
return success;
}
