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";
//}
}
