int
MSLaneChanger::checkChangeWithinEdge(
int laneOffset,
const std::pair<MSVehicle* const, SUMOReal>& leader,
const std::vector<MSVehicle::LaneQ>& preb) const {
std::pair<MSVehicle* const, SUMOReal> neighLead = getRealLeader(myCandi + laneOffset);
std::pair<MSVehicle*, SUMOReal> neighFollow = getRealFollower(myCandi + laneOffset);
if (neighLead.first != 0 && neighLead.first == neighFollow.first) {
// vehicles should not be leader and follower at the same time to avoid
// contradictory behavior
neighFollow.first = 0;
}
ChangerIt target = myCandi + laneOffset;
return checkChange(laneOffset, target->lane, leader, neighLead, neighFollow, preb);
}
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 (gSelected.isSelected(GLO_VEHICLE, static_cast<const GUIVehicle*>(vehicle)->getGlID())) {
int bla = 0;
}
#endif
const std::vector<MSVehicle::LaneQ>& preb = vehicle->getBestLanes();
assert(preb.size() == myChanger.size());
for (int i = 0; i < (int) myChanger.size(); ++i) {
((std::vector<MSVehicle::LaneQ>&) preb)[i].occupation = myChanger[i].dens + preb[i].nextOccupation;
}
vehicle->getLaneChangeModel().prepareStep();
std::pair<MSVehicle* const, SUMOReal> leader = getRealThisLeader(myCandi);
// check whether the vehicle wants and is able to change to right lane
int state1 = 0;
if (myCandi != myChanger.begin() && (myCandi - 1)->lane->allowsVehicleClass(veh(myCandi)->getVehicleType().getVehicleClass())) {
std::pair<MSVehicle* const, SUMOReal> rLead = getRealLeader(myCandi - 1);
std::pair<MSVehicle* const, SUMOReal> rFollow = getRealFollower(myCandi - 1);
state1 = change2right(leader, rLead, rFollow, preb);
if ((state1 & LCA_URGENT) != 0 || (state1 & LCA_SPEEDGAIN) != 0) {
state1 |= LCA_RIGHT;
}
bool changingAllowed1 = (state1 & LCA_BLOCKED) == 0;
// change if the vehicle wants to and is allowed to change
if ((state1 & LCA_RIGHT) != 0 && changingAllowed1) {
#ifndef NO_TRACI
// inform lane change model about this change
vehicle->getLaneChangeModel().fulfillChangeRequest(MSVehicle::REQUEST_RIGHT);
#endif
(myCandi - 1)->hoppedVeh = vehicle;
(myCandi - 1)->lane->myTmpVehicles.push_front(vehicle);
vehicle->leaveLane(MSMoveReminder::NOTIFICATION_LANE_CHANGE);
myCandi->lane->leftByLaneChange(vehicle);
vehicle->enterLaneAtLaneChange((myCandi - 1)->lane);
(myCandi - 1)->lane->enteredByLaneChange(vehicle);
vehicle->myLastLaneChangeOffset = 0;
vehicle->getLaneChangeModel().changed();
(myCandi - 1)->dens += (myCandi - 1)->hoppedVeh->getVehicleType().getLengthWithGap();
return true;
}
if ((state1 & LCA_RIGHT) != 0 && (state1 & LCA_URGENT) != 0) {
(myCandi - 1)->lastBlocked = vehicle;
}
}
// check whether the vehicle wants and is able to change to left lane
int state2 = 0;
if ((myCandi + 1) != myChanger.end() && (myCandi + 1)->lane->allowsVehicleClass(veh(myCandi)->getVehicleType().getVehicleClass())) {
std::pair<MSVehicle* const, SUMOReal> lLead = getRealLeader(myCandi + 1);
std::pair<MSVehicle* const, SUMOReal> lFollow = getRealFollower(myCandi + 1);
state2 = change2left(leader, lLead, lFollow, preb);
if ((state2 & LCA_URGENT) != 0 || (state2 & LCA_SPEEDGAIN) != 0) {
state2 |= LCA_LEFT;
}
bool changingAllowed2 = (state2 & LCA_BLOCKED) == 0;
//vehicle->getLaneChangeModel().setOwnState(state2|state1);
// change if the vehicle wants to and is allowed to change
if ((state2 & LCA_LEFT) != 0 && changingAllowed2) {
#ifndef NO_TRACI
// inform lane change model about this change
vehicle->getLaneChangeModel().fulfillChangeRequest(MSVehicle::REQUEST_LEFT);
#endif
(myCandi + 1)->hoppedVeh = veh(myCandi);
(myCandi + 1)->lane->myTmpVehicles.push_front(veh(myCandi));
vehicle->leaveLane(MSMoveReminder::NOTIFICATION_LANE_CHANGE);
myCandi->lane->leftByLaneChange(vehicle);
vehicle->enterLaneAtLaneChange((myCandi + 1)->lane);
(myCandi + 1)->lane->enteredByLaneChange(vehicle);
vehicle->myLastLaneChangeOffset = 0;
vehicle->getLaneChangeModel().changed();
(myCandi + 1)->dens += (myCandi + 1)->hoppedVeh->getVehicleType().getLengthWithGap();
return true;
}
if ((state2 & LCA_LEFT) != 0 && (state2 & LCA_URGENT) != 0) {
(myCandi + 1)->lastBlocked = vehicle;
}
}
vehicle->getLaneChangeModel().setOwnState(state2 | state1);
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(state1);
}
// check whether the vehicles should be swapped
if (myAllowsSwap && ((state1 & (LCA_URGENT)) != 0 || (state2 & (LCA_URGENT)) != 0)) {
// get the direction ...
ChangerIt target;
if ((state1 & (LCA_URGENT)) != 0) {
// ... wants to go right
target = myCandi - 1;
}
if ((state2 & (LCA_URGENT)) != 0) {
// ... wants to go left
target = myCandi + 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() == 0) {
// 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()) {
MSVehicle* bla = *i;
assert(bla == prohibitor);
target->lane->myTmpVehicles.erase(i);
// set this vehicle
target->hoppedVeh = vehicle;
target->lane->myTmpVehicles.push_front(vehicle);
myCandi->hoppedVeh = prohibitor;
myCandi->lane->myTmpVehicles.push_front(prohibitor);
// leave lane and detectors
vehicle->leaveLane(MSMoveReminder::NOTIFICATION_LANE_CHANGE);
prohibitor->leaveLane(MSMoveReminder::NOTIFICATION_LANE_CHANGE);
// patch position and speed
SUMOReal p1 = vehicle->getPositionOnLane();
vehicle->myState.myPos = prohibitor->myState.myPos;
prohibitor->myState.myPos = p1;
p1 = vehicle->getSpeed();
vehicle->myState.mySpeed = prohibitor->myState.mySpeed;
prohibitor->myState.mySpeed = p1;
// enter lane and detectors
vehicle->enterLaneAtLaneChange(target->lane);
prohibitor->enterLaneAtLaneChange(myCandi->lane);
// mark lane change
vehicle->getLaneChangeModel().changed();
vehicle->myLastLaneChangeOffset = 0;
prohibitor->getLaneChangeModel().changed();
prohibitor->myLastLaneChangeOffset = 0;
(myCandi)->dens += prohibitor->getVehicleType().getLengthWithGap();
(target)->dens += vehicle->getVehicleType().getLengthWithGap();
return true;
}
}
}
}
// Candidate didn't change lane.
myCandi->lane->myTmpVehicles.push_front(veh(myCandi));
vehicle->myLastLaneChangeOffset += DELTA_T;
(myCandi)->dens += vehicle->getVehicleType().getLengthWithGap();
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 || 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;
}
}