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