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;
}
MSLaneChangerSublane::StateAndDist
MSLaneChangerSublane::checkChangeHelper(MSVehicle* vehicle, int laneOffset) {
StateAndDist result = StateAndDist(0, 0, 0);
if (mayChange(laneOffset)) {
const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes();
result.state = checkChangeSublane(laneOffset, preb, result.latDist);
result.dir = laneOffset;
if ((result.state & LCA_WANTS_LANECHANGE) != 0 && (result.state & LCA_URGENT) != 0 && (result.state & LCA_BLOCKED) != 0) {
(myCandi + laneOffset)->lastBlocked = vehicle;
if ((myCandi + laneOffset)->firstBlocked == 0) {
(myCandi + laneOffset)->firstBlocked = vehicle;
}
}
}
return result;
}
bool
MSLaneChanger::change() {
// Find change-candidate. If it is on an allowed lane, try to change
// to the right (there is a rule in Germany that you have to change
// to the right, unless you are overtaking). If change to the right
// isn't possible, check if there is a possibility to overtake (on the
// left.
// If candidate isn't on an allowed lane, changing to an allowed has
// priority.
myCandi = findCandidate();
MSVehicle* vehicle = veh(myCandi);
#ifdef DEBUG_VEHICLE_GUI_SELECTION
if (gDebugSelectedVehicle == vehicle->getID()) {
int bla = 0;
}
#endif
if (vehicle->getLaneChangeModel().isChangingLanes()) {
return continueChange(vehicle, myCandi);
}
if (!myAllowsChanging || vehicle->getLaneChangeModel().alreadyChanged()) {
registerUnchanged(vehicle);
return false;
}
std::pair<MSVehicle* const, SUMOReal> leader = getRealLeader(myCandi);
if (myChanger.size() == 1 || vehicle->getLaneChangeModel().isOpposite()) {
if (changeOpposite(leader)) {
return true;
}
registerUnchanged(vehicle);
return false;
}
#ifndef NO_TRACI
if (vehicle->isRemoteControlled()) {
return false; // !!! temporary; just because it broke, here
}
#endif
vehicle->updateBestLanes(); // needed?
for (int i = 0; i < (int) myChanger.size(); ++i) {
vehicle->adaptBestLanesOccupation(i, myChanger[i].dens);
}
const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes();
// check whether the vehicle wants and is able to change to right lane
int state1 = 0;
if (mayChange(-1)) {
state1 = checkChangeWithinEdge(-1, leader, preb);
bool changingAllowed1 = (state1 & LCA_BLOCKED) == 0;
// change if the vehicle wants to and is allowed to change
if ((state1 & LCA_RIGHT) != 0 && changingAllowed1) {
vehicle->getLaneChangeModel().setOwnState(state1);
startChange(vehicle, myCandi, -1);
return true;
}
if ((state1 & LCA_RIGHT) != 0 && (state1 & LCA_URGENT) != 0) {
(myCandi - 1)->lastBlocked = vehicle;
if ((myCandi - 1)->firstBlocked == 0) {
(myCandi - 1)->firstBlocked = vehicle;
}
}
}
// check whether the vehicle wants and is able to change to left lane
int state2 = 0;
if (mayChange(1)) {
state2 = checkChangeWithinEdge(1, leader, preb);
bool changingAllowed2 = (state2 & LCA_BLOCKED) == 0;
// change if the vehicle wants to and is allowed to change
if ((state2 & LCA_LEFT) != 0 && changingAllowed2) {
vehicle->getLaneChangeModel().setOwnState(state2);
startChange(vehicle, myCandi, 1);
return true;
}
if ((state2 & LCA_LEFT) != 0 && (state2 & LCA_URGENT) != 0) {
(myCandi + 1)->lastBlocked = vehicle;
if ((myCandi + 1)->firstBlocked == 0) {
(myCandi + 1)->firstBlocked = vehicle;
}
}
}
if ((state1 & (LCA_URGENT)) != 0 && (state2 & (LCA_URGENT)) != 0) {
// ... wants to go to the left AND to the right
// just let them go to the right lane...
state2 = 0;
}
vehicle->getLaneChangeModel().setOwnState(state2 | state1);
// only emergency vehicles should change to the opposite side on a
// multi-lane road
if (vehicle->getVehicleType().getVehicleClass() == SVC_EMERGENCY
&& changeOpposite(leader)) {
return true;
} else {
registerUnchanged(vehicle);
return false;
}
}
bool
MSLaneChanger::change() {
// Find change-candidate. If it is on an allowed lane, try to change
// to the right (there is a rule in Germany that you have to change
// to the right, unless you are overtaking). If change to the right
// isn't possible, check if there is a possibility to overtake (on the
// left.
// If candidate isn't on an allowed lane, changing to an allowed has
// priority.
myCandi = findCandidate();
MSVehicle* vehicle = veh(myCandi);
#ifdef DEBUG_VEHICLE_GUI_SELECTION
if (gDebugSelectedVehicle == vehicle->getID()) {
int bla = 0;
}
#endif
if (vehicle->getLaneChangeModel().isChangingLanes()) {
return continueChange(vehicle, myCandi);
}
if (!myAllowsChanging || vehicle->getLaneChangeModel().alreadyChanged()) {
registerUnchanged(vehicle);
return false;
}
std::pair<MSVehicle* const, SUMOReal> leader = getRealLeader(myCandi);
if (myChanger.size() == 1) {
if (changeOpposite(leader)) {
return true;
}
registerUnchanged(vehicle);
return false;
}
#ifndef NO_TRACI
if (vehicle->isRemoteControlled()) {
return false; // !!! temporary; just because it broke, here
}
#endif
vehicle->updateBestLanes(); // needed?
for (int i = 0; i < (int) myChanger.size(); ++i) {
vehicle->adaptBestLanesOccupation(i, myChanger[i].dens);
}
const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes();
// check whether the vehicle wants and is able to change to right lane
int state1 = 0;
if (mayChange(-1)) {
state1 = checkChangeWithinEdge(-1, leader, preb);
bool changingAllowed1 = (state1 & LCA_BLOCKED) == 0;
// change if the vehicle wants to and is allowed to change
if ((state1 & LCA_RIGHT) != 0 && changingAllowed1) {
vehicle->getLaneChangeModel().setOwnState(state1);
startChange(vehicle, myCandi, -1);
return true;
}
if ((state1 & LCA_RIGHT) != 0 && (state1 & LCA_URGENT) != 0) {
(myCandi - 1)->lastBlocked = vehicle;
if ((myCandi - 1)->firstBlocked == 0) {
(myCandi - 1)->firstBlocked = vehicle;
}
}
}
// check whether the vehicle wants and is able to change to left lane
int state2 = 0;
if (mayChange(1)) {
state2 = checkChangeWithinEdge(1, leader, preb);
bool changingAllowed2 = (state2 & LCA_BLOCKED) == 0;
// change if the vehicle wants to and is allowed to change
if ((state2 & LCA_LEFT) != 0 && changingAllowed2) {
vehicle->getLaneChangeModel().setOwnState(state2);
startChange(vehicle, myCandi, 1);
return true;
}
if ((state2 & LCA_LEFT) != 0 && (state2 & LCA_URGENT) != 0) {
(myCandi + 1)->lastBlocked = vehicle;
if ((myCandi + 1)->firstBlocked == 0) {
(myCandi + 1)->firstBlocked = vehicle;
}
}
}
if ((state1 & (LCA_URGENT)) != 0 && (state2 & (LCA_URGENT)) != 0) {
// ... wants to go to the left AND to the right
// just let them go to the right lane...
state2 = 0;
}
vehicle->getLaneChangeModel().setOwnState(state2 | state1);
// check whether the vehicles should be swapped
if (myAllowsSwap && ((state1 & (LCA_URGENT)) != 0 || (state2 & (LCA_URGENT)) != 0)) {
// get the direction ...
ChangerIt target;
int direction = 0;
if ((state1 & (LCA_URGENT)) != 0) {
// ... wants to go right
target = myCandi - 1;
direction = -1;
}
if ((state2 & (LCA_URGENT)) != 0) {
// ... wants to go left
target = myCandi + 1;
direction = 1;
}
MSVehicle* prohibitor = target->lead;
if (target->hoppedVeh != 0) {
SUMOReal hoppedPos = target->hoppedVeh->getPositionOnLane();
if (prohibitor == 0 || (hoppedPos > vehicle->getPositionOnLane() && prohibitor->getPositionOnLane() > hoppedPos)) {
prohibitor = 0;// !!! vehicles should not jump over more than one lanetarget->hoppedVeh;
}
}
if (prohibitor != 0
&&
((prohibitor->getLaneChangeModel().getOwnState() & (LCA_URGENT/*|LCA_SPEEDGAIN*/)) != 0
&&
(prohibitor->getLaneChangeModel().getOwnState() & (LCA_LEFT | LCA_RIGHT))
!=
(vehicle->getLaneChangeModel().getOwnState() & (LCA_LEFT | LCA_RIGHT))
)
) {
// check for position and speed
if (prohibitor->getVehicleType().getLengthWithGap() == vehicle->getVehicleType().getLengthWithGap()) {
// ok, may be swapped
// remove vehicle to swap with
MSLane::VehCont::iterator i = find(target->lane->myTmpVehicles.begin(), target->lane->myTmpVehicles.end(), prohibitor);
if (i != target->lane->myTmpVehicles.end()) {
assert(*i == prohibitor);
target->lane->myTmpVehicles.erase(i);
startChange(vehicle, myCandi, direction);
startChange(prohibitor, target, -direction);
std::swap(vehicle->myState, prohibitor->myState);
myCandi->lead = prohibitor;
target->lead = vehicle;
return true;
}
}
}
}
if (!changeOpposite(leader)) {
registerUnchanged(vehicle);
return false;
} else {
return true;
}
}
