bool
MSLane::push(MSVehicle* veh, const std::vector<int> &stripID, bool hasMainStrip) {
//assert (veh->getStrips().size() != 0);
MSVehicle::StripCont strips;
// Insert vehicle only if it's destination isn't reached.
// and it does not collide with previous
// check whether the vehicle has ended his route
// Add to mean data (edge/lane state dump)
if (! veh->moveRoutePointer(myEdge)) { // adjusts vehicles routeIterator
for (int i=0; i<stripID.size(); ++i) {
MSStrip *strip = getStrip(stripID[i]);
strip->push(veh);
strips.push_back(strip);
}
veh->enterLaneAtMove(this, SPEED2DIST(veh->getSpeed()) - veh->getPositionOnLane(), strips, hasMainStrip);
SUMOReal pspeed = veh->getSpeed();
SUMOReal oldPos = veh->getPositionOnLane() - SPEED2DIST(veh->getSpeed());
veh->workOnMoveReminders(oldPos, veh->getPositionOnLane(), pspeed);
return false;
} else {
if (hasMainStrip) {
veh->enterLaneAtMove(this, SPEED2DIST(veh->getSpeed()) - veh->getPositionOnLane(), strips, hasMainStrip);
veh->onRemovalFromNet(false);
MSNet::getInstance()->getVehicleControl().scheduleVehicleRemoval(veh);
}
return true;
}
}
std::pair<MSVehicle* const, SUMOReal>
MSLaneChanger::getRealFollower(const ChangerIt& target) const {
MSVehicle* neighFollow = veh(target);
// check whether the hopped vehicle got the follower
if (target->hoppedVeh != 0) {
SUMOReal hoppedPos = target->hoppedVeh->getPositionOnLane();
if (hoppedPos <= veh(myCandi)->getPositionOnLane() && (neighFollow == 0 || neighFollow->getPositionOnLane() > hoppedPos)) {
neighFollow = target->hoppedVeh;
}
}
if (neighFollow == 0) {
SUMOReal speed = target->lane->getSpeedLimit();
// in order to look back, we'd need the minimum braking ability of vehicles in the net...
// we'll assume it to be 4m/s^2
// !!!revisit
SUMOReal dist = speed * speed / (2.*4.) + SPEED2DIST(speed);
dist = MIN2(dist, (SUMOReal) 500.);
MSVehicle* candi = veh(myCandi);
SUMOReal seen = candi->getPositionOnLane() - candi->getVehicleType().getLength();
return target->lane->getFollowerOnConsecutive(dist, seen, candi->getSpeed(), candi->getPositionOnLane() - candi->getVehicleType().getLength(), 4.5);//!!! recheck
} else {
MSVehicle* candi = veh(myCandi);
return std::pair<MSVehicle* const, SUMOReal>(neighFollow, candi->getPositionOnLane() - candi->getVehicleType().getLength() - neighFollow->getPositionOnLane() - neighFollow->getVehicleType().getMinGap());
}
}
void
MSLCM_DK2004::informBlocker(MSAbstractLaneChangeModel::MSLCMessager& msgPass,
int& blocked,
int dir,
const std::pair<MSVehicle*, SUMOReal>& neighLead,
const std::pair<MSVehicle*, SUMOReal>& neighFollow) {
if ((blocked & LCA_BLOCKED_BY_FOLLOWER) != 0) {
assert(neighFollow.first != 0);
MSVehicle* nv = neighFollow.first;
SUMOReal decelGap =
neighFollow.second
+ SPEED2DIST(myVehicle.getSpeed()) * (SUMOReal) 2.0
- MAX2(nv->getSpeed() - (SUMOReal) ACCEL2DIST(nv->getCarFollowModel().getMaxDecel()) * (SUMOReal) 2.0, (SUMOReal) 0);
if (neighFollow.second > 0 && decelGap > 0 && decelGap >= nv->getCarFollowModel().getSecureGap(nv->getSpeed(), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel())) {
SUMOReal vsafe = myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), neighFollow.second, neighFollow.first->getSpeed(), neighFollow.first->getCarFollowModel().getMaxDecel());
msgPass.informNeighFollower(new Info(vsafe, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
} else {
SUMOReal vsafe = neighFollow.second <= 0 ? 0 : myCarFollowModel.followSpeed(&myVehicle, myVehicle.getSpeed(), neighFollow.second, neighFollow.first->getSpeed(), neighFollow.first->getCarFollowModel().getMaxDecel());
msgPass.informNeighFollower(new Info(vsafe, dir | LCA_AMBLOCKINGFOLLOWER_DONTBRAKE), &myVehicle);
}
}
if ((blocked & LCA_BLOCKED_BY_LEADER) != 0) {
if (neighLead.first != 0 && neighLead.second > 0) {
msgPass.informNeighLeader(new Info(0, dir | LCA_AMBLOCKINGLEADER), &myVehicle);
}
}
}
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;
}
SUMOReal
getMaxSpeedRegardingNextLanes(MSVehicle& veh, SUMOReal speed, SUMOReal pos) {
MSRouteIterator next = veh.getRoute().begin();
const MSCFModel &cfModel = veh.getCarFollowModel();
MSLane *currentLane = (*next)->getLanes()[0];
SUMOReal seen = currentLane->getLength() - pos;
SUMOReal dist = SPEED2DIST(speed) + cfModel.brakeGap(speed);
SUMOReal tspeed = speed;
while (seen<dist&&next!=veh.getRoute().end()-1) {
++next;
MSLane *nextLane = (*next)->getLanes()[0];
tspeed = MIN2(cfModel.ffeV(&veh, tspeed, seen, nextLane->getMaxSpeed()), nextLane->getMaxSpeed());
dist = SPEED2DIST(tspeed) + cfModel.brakeGap(tspeed);
seen += nextLane->getMaxSpeed();
}
return tspeed;
}
double
MSCFModel::brakeGapEuler(const double speed, const double decel, const double headwayTime) {
/* one possibility to speed this up is to calculate speedReduction * steps * (steps+1) / 2
for small values of steps (up to 10 maybe) and store them in an array */
const double speedReduction = ACCEL2SPEED(decel);
const int steps = int(speed / speedReduction);
return SPEED2DIST(steps * speed - speedReduction * steps * (steps + 1) / 2) + speed * headwayTime;
}
bool
MSCFModel_KraussOrig1::hasSafeGap(SUMOReal speed, SUMOReal gap, SUMOReal predSpeed, SUMOReal laneMaxSpeed) const throw() {
if (gap<0) {
return false;
}
SUMOReal vSafe = MIN2(_vsafe(gap, predSpeed), maxNextSpeed(speed));
SUMOReal vNext = MIN3(maxNextSpeed(speed), laneMaxSpeed, vSafe);
return (vNext>=getSpeedAfterMaxDecel(speed) && gap>= SPEED2DIST(speed));
}
//TODO: refactor out? only called by setCritical for getting the leader (last veh of lane)
MSVehicle*
MSLane::pop(SUMOTime) {
assert(! myVehicles.empty());
MSVehicle* first = myVehicles.back();
first->leaveLaneAtMove(SPEED2DIST(first->getSpeed())/* - first->pos()*/);
myVehicles.pop_back();
myVehicleLengthSum -= first->getVehicleType().getLength();
return first;
}
SUMOReal
MSCFModel_Wiedemann::krauss_vsafe(SUMOReal gap, SUMOReal predSpeed) const {
if (predSpeed == 0 && gap < 0.01) {
return 0;
}
const SUMOReal tauDecel = myDecel * myHeadwayTime;
const SUMOReal speedReduction = ACCEL2SPEED(myDecel);
const int predSteps = int(predSpeed / speedReduction);
const SUMOReal leaderContrib = 2. * myDecel * (gap + SPEED2DIST(predSteps * predSpeed - speedReduction * predSteps * (predSteps + 1) / 2));
return (SUMOReal)(-tauDecel + sqrt(tauDecel * tauDecel + leaderContrib));
}
double
MSCFModel::interactionGap(const MSVehicle* const veh, double vL) const {
// Resolve the vsafe equation to gap. Assume predecessor has
// speed != 0 and that vsafe will be the current speed plus acceleration,
// i.e that with this gap there will be no interaction.
const double vNext = MIN2(maxNextSpeed(veh->getSpeed(), veh), veh->getLane()->getVehicleMaxSpeed(veh));
const double gap = (vNext - vL) *
((veh->getSpeed() + vL) / (2.*myDecel) + myHeadwayTime) +
vL * myHeadwayTime;
// Don't allow timeHeadWay < deltaT situations.
return MAX2(gap, SPEED2DIST(vNext));
}
SUMOReal
MSCFModel_KraussOrig1::interactionGap(const MSVehicle * const veh, SUMOReal vL) const throw() {
// Resolve the vsafe equation to gap. Assume predecessor has
// speed != 0 and that vsafe will be the current speed plus acceleration,
// i.e that with this gap there will be no interaction.
SUMOReal vNext = MIN2(maxNextSpeed(veh->getSpeed()), veh->getLane().getMaxSpeed());
SUMOReal gap = (vNext - vL) *
((veh->getSpeed() + vL) * myInverseTwoDecel + myTau) +
vL * myTau;
// Don't allow timeHeadWay < deltaT situations.
return MAX2(gap, SPEED2DIST(vNext));
}
double
MSCFModel_Kerner::_v(const MSVehicle* const veh, double speed, double vfree, double gap, double predSpeed) const {
if (predSpeed == 0 && gap < 0.01) {
return 0;
}
// !!! in the following, the prior step is not considered!!!
double G = MAX2((double) 0, (double)(SPEED2DIST(myK * speed) + myPhi / myAccel * speed * (speed - predSpeed)));
double vcond = gap > G ? speed + ACCEL2SPEED(myAccel) : speed + MAX2(ACCEL2SPEED(-myDecel), MIN2(ACCEL2SPEED(myAccel), predSpeed - speed));
double vsafe = (double)(-1. * myTauDecel + sqrt(myTauDecel * myTauDecel + (predSpeed * predSpeed) + (2. * myDecel * gap)));
VehicleVariables* vars = (VehicleVariables*)veh->getCarFollowVariables();
double va = MAX2((double) 0, MIN3(vfree, vsafe, vcond)) + vars->rand;
double v = MAX2((double) 0, MIN4(vfree, va, speed + ACCEL2SPEED(myAccel), vsafe));
return v;
}
double
MSCFModel::followSpeedTransient(double duration, const MSVehicle* const /*veh*/, double /*speed*/, double gap2pred, double predSpeed, double predMaxDecel) const {
// minimium distance covered by the leader if braking
double leaderMinDist = gap2pred + distAfterTime(duration, predSpeed, -predMaxDecel);
// if ego would not brake it could drive with speed leaderMinDist / duration
// due to potentential ego braking it can safely drive faster
if (MSGlobals::gSemiImplicitEulerUpdate) {
// number of potential braking steps
int a = (int)ceil(duration / TS - TS);
// can we brake for the whole time?
const double bg = brakeGap(a * myDecel, myDecel, 0);
if (bg <= leaderMinDist) {
// braking continuously for duration
// distance reduction due to braking
double b = TS * getMaxDecel() * 0.5 * (a * a - a);
if (gDebugFlag2) std::cout << " followSpeedTransient"
<< " duration=" << duration
<< " gap=" << gap2pred
<< " leaderMinDist=" << leaderMinDist
<< " decel=" << getMaxDecel()
<< " a=" << a
<< " bg=" << bg
<< " b=" << b
<< " x=" << (b + leaderMinDist) / duration
<< "\n";
return (b + leaderMinDist) / duration;
} else {
// @todo improve efficiency
double bg = 0;
double speed = 0;
while (bg < leaderMinDist) {
speed += ACCEL2SPEED(myDecel);
bg += SPEED2DIST(speed);
}
speed -= DIST2SPEED(bg - leaderMinDist);
return speed;
}
} else {
// can we brake for the whole time?
const double fullBrakingSeconds = sqrt(leaderMinDist * 2 / myDecel);
if (fullBrakingSeconds >= duration) {
// braking continuously for duration
// average speed after braking for duration is x2 = x - 0.5 * duration * myDecel
// x2 * duration <= leaderMinDist must hold
return leaderMinDist / duration + duration * getMaxDecel() / 2;
} else {
return fullBrakingSeconds * myDecel;
}
}
}
double
MSCFModel::distAfterTime(double t, double speed, const double accel) {
if (accel >= 0.) {
return (speed + 0.5 * accel * t) * t;
}
const double decel = -accel;
if (speed <= decel * t) {
// braking to a full stop
return brakeGap(speed, decel, 0);
}
if (MSGlobals::gSemiImplicitEulerUpdate) {
// @todo improve efficiency
double result = 0;
while (t > 0) {
speed -= ACCEL2SPEED(decel);
result += MAX2(0.0, SPEED2DIST(speed));
t -= TS;
}
return result;
} else {
const double speed2 = speed - t * decel;
return 0.5 * (speed + speed2) * t;
}
}
double
MSCFModel::freeSpeed(const double currentSpeed, const double decel, const double dist, const double targetSpeed, const bool onInsertion, const double actionStepLength) {
// XXX: (Leo) This seems to be exclusively called with decel = myDecel (max deceleration) and is not overridden
// by any specific CFModel. That may cause undesirable hard braking (at junctions where the vehicle
// changes to a road with a lower speed limit).
if (MSGlobals::gSemiImplicitEulerUpdate) {
// adapt speed to succeeding lane, no reaction time is involved
// when breaking for y steps the following distance g is covered
// (drive with v in the final step)
// g = (y^2 + y) * 0.5 * b + y * v
// y = ((((sqrt((b + 2.0*v)*(b + 2.0*v) + 8.0*b*g)) - b)*0.5 - v)/b)
const double v = SPEED2DIST(targetSpeed);
if (dist < v) {
return targetSpeed;
}
const double b = ACCEL2DIST(decel);
const double y = MAX2(0.0, ((sqrt((b + 2.0 * v) * (b + 2.0 * v) + 8.0 * b * dist) - b) * 0.5 - v) / b);
const double yFull = floor(y);
const double exactGap = (yFull * yFull + yFull) * 0.5 * b + yFull * v + (y > yFull ? v : 0.0);
const double fullSpeedGain = (yFull + (onInsertion ? 1. : 0.)) * ACCEL2SPEED(decel);
return DIST2SPEED(MAX2(0.0, dist - exactGap) / (yFull + 1)) + fullSpeedGain + targetSpeed;
} else {
// ballistic update (Leo)
// calculate maximum next speed vN that is adjustable to vT=targetSpeed after a distance d=dist
// and given a maximal deceleration b=decel, denote the current speed by v0.
// the distance covered by a trajectory that attains vN in the next action step (length=dt) and decelerates afterwards
// with b is given as
// d = 0.5*dt*(v0+vN) + (t-dt)*vN - 0.5*b*(t-dt)^2, (1)
// where time t of arrival at d with speed vT is
// t = dt + (vN-vT)/b. (2)
// We insert (2) into (1) to obtain
// d = 0.5*dt*(v0+vN) + vN*(vN-vT)/b - 0.5*b*((vN-vT)/b)^2
// 0 = (dt*b*v0 - vT*vT - 2*b*d) + dt*b*vN + vN*vN
// and solve for vN
assert(currentSpeed >= 0);
assert(targetSpeed >= 0);
const double dt = onInsertion ? 0 : actionStepLength; // handles case that vehicle is inserted just now (at the end of move)
const double v0 = currentSpeed;
const double vT = targetSpeed;
const double b = decel;
const double d = dist - NUMERICAL_EPS; // prevent returning a value > targetSpeed due to rounding errors
// Solvability for positive vN (if d is small relative to v0):
// 1) If 0.5*(v0+vT)*dt > d, we set vN=vT.
// (In case vT<v0, this implies that on the interpolated trajectory there are points beyond d where
// the interpolated velocity is larger than vT, but at least on the temporal discretization grid, vT is not exceeded)
// 2) We ignore the (possible) constraint vN >= v0 - b*dt, which could lead to a problem if v0 - t*b > vT.
// (finalizeSpeed() is responsible for assuring that the next velocity is chosen in accordance with maximal decelerations)
// If implied accel a leads to v0 + a*asl < vT, choose acceleration s.th. v0 + a*asl = vT
if (0.5 * (v0 + vT)*dt >= d) {
// Attain vT after time asl
return v0 + TS * (vT - v0) / actionStepLength;
} else {
const double q = ((dt * v0 - 2 * d) * b - vT * vT); // (q < 0 is fulfilled because of (#))
const double p = 0.5 * b * dt;
const double vN = -p + sqrt(p * p - q); // target speed at time t0+asl
return v0 + TS * (vN - v0) / actionStepLength;
}
}
}
bool
MSLane::isEmissionSuccess(MSVehicle* aVehicle,
SUMOReal speed, SUMOReal pos,
bool patchSpeed, size_t startStripId) throw(ProcessError) {
// and the speed is not too high (vehicle should decelerate)
// try to get a leader on consecutive lanes
// we have to do this even if we have found a leader on our lane because it may
// be driving into another direction
//std::cerr<<"EMISSION speed:"<<speed<<std::endl;
std::cerr<<"EMISSION vehicle:"<<aVehicle->getID()<<std::endl;
size_t endStripId = startStripId + aVehicle->getWidth() - 1;
assert(startStripId >=0 && endStripId < myStrips.size());
aVehicle->getBestLanes(true, this);
const MSCFModel &cfModel = aVehicle->getCarFollowModel();
const std::vector<MSLane*> &bestLaneConts = aVehicle->getBestLanesContinuation(this);
std::vector<MSLane*>::const_iterator ri = bestLaneConts.begin();
SUMOReal seen = getLength() - pos;
SUMOReal dist = cfModel.brakeGap(speed);
const MSRoute &r = aVehicle->getRoute();
MSRouteIterator ce = r.begin();
MSLane *currentLane = this;
MSLane *nextLane = this;
while (seen<dist&&ri!=bestLaneConts.end()&&nextLane!=0/*&&ce!=r.end()*/) {
// get the next link used...
MSLinkCont::const_iterator link = currentLane->succLinkSec(*aVehicle, 1, *currentLane, bestLaneConts);
// ...and the next used lane (including internal)
if (!currentLane->isLinkEnd(link) && (*link)->havePriority() && (*link)->getState()!=MSLink::LINKSTATE_TL_RED) { // red may have priority?
#ifdef HAVE_INTERNAL_LANES
bool nextInternal = false;
nextLane = (*link)->getViaLane();
if (nextLane==0) {
nextLane = (*link)->getLane();
} else {
nextInternal = true;
}
#else
nextLane = (*link)->getLane();
#endif
} else {
nextLane = 0;
}
// check how next lane effects the journey
if (nextLane!=0) {
SUMOReal gap = 0;
//TODO: fix get & set partial occupator to strip level
MSVehicle * leader = 0;//currentLane->getPartialOccupator();
if (leader!=0) {
gap = getPartialOccupatorEnd();
} else {
// check leader on next lane
leader = nextLane->getLastVehicle(aVehicle->getStrips());
if (leader!=0) {
gap = seen+leader->getPositionOnLane()-leader->getVehicleType().getLength();
}
}
if (leader!=0) {
SUMOReal nspeed = gap>=0 ? cfModel.ffeV(aVehicle, speed, gap, leader->getSpeed()) : 0;
if (nspeed<speed) {
if (patchSpeed) {
speed = MIN2(nspeed, speed);
dist = cfModel.brakeGap(speed);
} else {
// we may not drive with the given velocity - we crash into the leader
return false;
}
}
}
// check next lane's maximum velocity
SUMOReal nspeed = nextLane->getMaxSpeed();
if (nspeed<speed) {
// patch speed if needed
if (patchSpeed) {
speed = MIN2(cfModel.ffeV(aVehicle, speed, seen, nspeed), speed);
dist = cfModel.brakeGap(speed);
} else {
// we may not drive with the given velocity - we would be too fast on the next lane
return false;
}
}
// check traffic on next junctions
const SUMOTime arrivalTime = MSNet::getInstance()->getCurrentTimeStep() + TIME2STEPS(seen / speed);
#ifdef HAVE_INTERNAL_LANES
const SUMOTime leaveTime = (*link)->getViaLane()==0 ? arrivalTime + TIME2STEPS((*link)->getLength() * speed) : arrivalTime + TIME2STEPS((*link)->getViaLane()->getLength() * speed);
#else
const SUMOTime leaveTime = arrivalTime + TIME2STEPS((*link)->getLength() * speed);
#endif
if ((*link)->hasApproachingFoe(arrivalTime, leaveTime)) {
SUMOReal nspeed = cfModel.ffeV(aVehicle, speed, seen, 0);
if (nspeed<speed) {
if (patchSpeed) {
speed = MIN2(nspeed, speed);
dist = cfModel.brakeGap(speed);
} else {
// we may not drive with the given velocity - we crash into the leader
return false;
}
}
} else {
// we can only drive to the end of the current lane...
SUMOReal nspeed = cfModel.ffeV(aVehicle, speed, seen, 0);
if (nspeed<speed) {
if (patchSpeed) {
speed = MIN2(nspeed, speed);
dist = cfModel.brakeGap(speed);
} else {
// we may not drive with the given velocity - we crash into the leader
return false;
}
}
}
seen += nextLane->getLength();
++ce;
++ri;
currentLane = nextLane;
}
}
if (seen<dist) {
SUMOReal nspeed = cfModel.ffeV(aVehicle, speed, seen, 0);
if (nspeed<speed) {
if (patchSpeed) {
speed = MIN2(nspeed, speed);
dist = cfModel.brakeGap(speed);
} else {
// we may not drive with the given velocity - we crash into the leader
MsgHandler::getErrorInstance()->inform("Vehicle '" + aVehicle->getID() + "' will not be able to emit using given velocity!");
// !!! we probably should do something else...
return false;
}
}
}
// get the pointer to the vehicle next in front of the given position
MSVehicle *pred;
std::vector<MSVehicle *> predCont;
std::vector<MSVehicle *>::iterator predIt, it;
for (unsigned int i=startStripId; i<=endStripId; ++i) {
predCont.push_back(myStrips.at(i)->getPredAtPos(pos));
}
predIt = predCont.begin();
SUMOReal currMin = -1;
if (*predIt != 0) {
currMin = (*predIt)->getPositionOnLane();
} else {
// signals no leader in front
predIt = predCont.end();
}
for (it = predCont.begin(); it != predCont.end(); ++it) {
if (*it == 0) continue;
if ((*it)->getPositionOnLane() < currMin) {
predIt = it;
currMin = (*it)->getPositionOnLane();
}
}
if (predIt != predCont.end()) {
// ok, there is one (a leader)
MSVehicle* leader = *predIt;
SUMOReal frontGapNeeded = aVehicle->getCarFollowModel().getSecureGap(speed, leader->getCarFollowModel().getSpeedAfterMaxDecel(leader->getSpeed()));
SUMOReal gap = MSVehicle::gap(leader->getPositionOnLane(), leader->getVehicleType().getLength(), pos);
if (gap<frontGapNeeded) {
// too close to the leader on this lane
return false;
}
}
// FIXME: implement look back
// check back vehicle
if (false/*predIt!=myVehicles.begin()*/) {
// there is direct follower on this lane
MSVehicle *follower = *(predIt-1);
SUMOReal backGapNeeded = follower->getCarFollowModel().getSecureGap(follower->getSpeed(), aVehicle->getCarFollowModel().getSpeedAfterMaxDecel(speed));
SUMOReal gap = MSVehicle::gap(pos, aVehicle->getVehicleType().getLength(), follower->getPositionOnLane());
if (gap<backGapNeeded) {
// too close to the follower on this lane
return false;
}
} else if (false) {
// check approaching vehicle (consecutive follower)
SUMOReal lspeed = getMaxSpeed();
// in order to look back, we'd need the minimum braking ability of vehicles in the net...
// we'll assume it to be 4m/s^2
// !!!revisit
SUMOReal dist = lspeed * lspeed * SUMOReal(1./2.*4.) + SPEED2DIST(lspeed);
std::pair<const MSVehicle * const, SUMOReal> approaching = getFollowerOnConsecutive(dist, 0, speed, pos - aVehicle->getVehicleType().getLength());
if (approaching.first!=0) {
const MSVehicle *const follower = approaching.first;
SUMOReal backGapNeeded = follower->getCarFollowModel().getSecureGap(follower->getSpeed(), aVehicle->getCarFollowModel().getSpeedAfterMaxDecel(speed));
SUMOReal gap = approaching.second - pos - aVehicle->getVehicleType().getLength();
if (gap<backGapNeeded) {
// too close to the consecutive follower
return false;
}
}
// check for in-lapping vehicle
MSVehicle* leader = getPartialOccupator();
if (leader!=0) {
SUMOReal frontGapNeeded = aVehicle->getCarFollowModel().getSecureGap(speed, leader->getCarFollowModel().getSpeedAfterMaxDecel(leader->getSpeed()));
SUMOReal gap = getPartialOccupatorEnd() - pos;
if (gap<=frontGapNeeded) {
// too close to the leader on this lane
return false;
}
}
}
// may got negative while adaptation
if (speed<0) {
return false;
}
// enter
//XXX: later change to enterStripAtEmit()?
//if (speed < 0.0001) speed += 10.0;
StripCont strips;
strips.resize(aVehicle->getWidth());
StripCont::iterator start = myStrips.begin() + startStripId;
std::copy(start, start + aVehicle->getWidth(), strips.begin());
aVehicle->enterLaneAtEmit(this, pos, speed, strips);
bool wasInactive = getVehicleNumber()==0;
if (true/*predIt==myVehicles.end()*/) {
// vehicle will be the first on the lane
//std::cerr<<"startStripId:"<<startStripId<<", NumStrips:"<<strips.size()<<", VehWidth:"<<aVehicle->getWidth()<<std::endl;
for (size_t i=startStripId; i<startStripId+strips.size(); ++i) {
this->getStrip(i)->pushIntoStrip(aVehicle);
this->getStrip(i)->setVehLenSum(this->getStrip(i)->getVehLenSum() +
aVehicle->getVehicleType().getLength());
}
aVehicle->printDebugMsg("Emitting");
printDebugMsg();
} else {
//this->getStrip(0).insert(0, aVehicle);
}
//myVehicleLengthSum += aVehicle->getVehicleType().getLength();
if (wasInactive) {
MSNet::getInstance()->getEdgeControl().gotActive(this);
}
return true;
}
double
HelpersEnergy::compute(const SUMOEmissionClass /* c */, const PollutantsInterface::EmissionType e, const double v, const double a, const double slope, const std::map<int, double>* param) const {
if (e != PollutantsInterface::ELEC) {
return 0.;
}
if (param == 0) {
param = &myDefaultParameter;
}
//@ToDo: All formulas below work with the logic of the euler update (refs #860).
// Approximation order could be improved. Refs. #2592.
const double lastV = v - ACCEL2SPEED(a);
const double mass = param->find(SUMO_ATTR_VEHICLEMASS)->second;
// calculate potential energy difference
double energyDiff = mass * 9.81 * sin(DEG2RAD(slope)) * SPEED2DIST(v);
// kinetic energy difference of vehicle
energyDiff += 0.5 * mass * (v * v - lastV * lastV);
// add rotational energy diff of internal rotating elements
energyDiff += param->find(SUMO_ATTR_INTERNALMOMENTOFINERTIA)->second * (v * v - lastV * lastV);
// Energy loss through Air resistance [Ws]
// Calculate energy losses:
// EnergyLoss,Air = 1/2 * rho_air [kg/m^3] * myFrontSurfaceArea [m^2] * myAirDragCoefficient [-] * v_Veh^2 [m/s] * s [m]
// ... with rho_air [kg/m^3] = 1,2041 kg/m^3 (at T = 20C)
// ... with s [m] = v_Veh [m/s] * TS [s]
energyDiff += 0.5 * 1.2041 * param->find(SUMO_ATTR_FRONTSURFACEAREA)->second * param->find(SUMO_ATTR_AIRDRAGCOEFFICIENT)->second * v * v * SPEED2DIST(v);
// Energy loss through Roll resistance [Ws]
// ... (fabs(veh.getSpeed())>=0.01) = 0, if vehicle isn't moving
// EnergyLoss,Tire = c_R [-] * F_N [N] * s [m]
// ... with c_R = ~0.012 (car tire on asphalt)
// ... with F_N [N] = myMass [kg] * g [m/s^2]
energyDiff += param->find(SUMO_ATTR_ROLLDRAGCOEFFICIENT)->second * 9.81 * mass * SPEED2DIST(v);
// Energy loss through friction by radial force [Ws]
// If angle of vehicle was changed
const double angleDiff = param->find(SUMO_ATTR_ANGLE)->second;
if (angleDiff != 0.) {
// Compute new radio
double radius = SPEED2DIST(v) / fabs(angleDiff);
// Check if radius is in the interval [0.0001 - 10000] (To avoid overflow and division by zero)
if (radius < 0.0001) {
radius = 0.0001;
} else if (radius > 10000) {
radius = 10000;
}
// EnergyLoss,internalFrictionRadialForce = c [m] * F_rad [N];
// Energy loss through friction by radial force [Ws]
energyDiff += param->find(SUMO_ATTR_RADIALDRAGCOEFFICIENT)->second * mass * v * v / radius;
}
// EnergyLoss,constantConsumers
// Energy loss through constant loads (e.g. A/C) [Ws]
energyDiff += param->find(SUMO_ATTR_CONSTANTPOWERINTAKE)->second;
//E_Bat = E_kin_pot + EnergyLoss;
if (energyDiff > 0) {
// Assumption: Efficiency of myPropulsionEfficiency when accelerating
energyDiff /= param->find(SUMO_ATTR_PROPULSIONEFFICIENCY)->second;
} else {
// Assumption: Efficiency of myRecuperationEfficiency when recuperating
energyDiff *= param->find(SUMO_ATTR_RECUPERATIONEFFICIENCY)->second;
}
// convert from [Ws] to [Wh] (3600s / 1h):
return energyDiff / 3600.;
}
SUMOReal
MSLCM_LC2013::informLeader(MSAbstractLaneChangeModel::MSLCMessager& msgPass,
int blocked,
int dir,
const std::pair<MSVehicle*, SUMOReal>& neighLead,
SUMOReal remainingSeconds) {
SUMOReal plannedSpeed = MIN2(myVehicle.getSpeed(),
myVehicle.getCarFollowModel().stopSpeed(&myVehicle, myVehicle.getSpeed(), myLeftSpace - myLeadingBlockerLength));
for (std::vector<SUMOReal>::const_iterator i = myVSafes.begin(); i != myVSafes.end(); ++i) {
SUMOReal v = (*i);
if (v >= myVehicle.getSpeed() - ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxDecel())) {
plannedSpeed = MIN2(plannedSpeed, v);
}
}
if ((blocked & LCA_BLOCKED_BY_LEADER) != 0) {
assert(neighLead.first != 0);
MSVehicle* nv = neighLead.first;
// decide whether we want to overtake the leader or follow it
const SUMOReal dv = plannedSpeed - nv->getSpeed();
const SUMOReal overtakeDist = (neighLead.second // drive to back of follower
+ nv->getVehicleType().getLengthWithGap() // drive to front of follower
+ myVehicle.getVehicleType().getLength() // ego back reaches follower front
+ nv->getCarFollowModel().getSecureGap( // save gap to follower
nv->getSpeed(), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel()));
if (dv < 0
// overtaking on the right on an uncongested highway is forbidden (noOvertakeLCLeft)
|| (dir == LCA_MLEFT && !myVehicle.congested())
// not enough space to overtake?
|| myLeftSpace < overtakeDist
// not enough time to overtake?
|| dv * remainingSeconds < overtakeDist) {
// cannot overtake
msgPass.informNeighLeader(new Info(-1, dir | LCA_AMBLOCKINGLEADER), &myVehicle);
// slow down smoothly to follow leader
const SUMOReal targetSpeed = myCarFollowModel.followSpeed(
&myVehicle, myVehicle.getSpeed(), neighLead.second, nv->getSpeed(), nv->getCarFollowModel().getMaxDecel());
if (targetSpeed < myVehicle.getSpeed()) {
// slow down smoothly to follow leader
const SUMOReal decel = ACCEL2SPEED(MIN2(myVehicle.getCarFollowModel().getMaxDecel(),
MAX2(MIN_FALLBEHIND, (myVehicle.getSpeed() - targetSpeed) / remainingSeconds)));
const SUMOReal nextSpeed = MIN2(plannedSpeed, myVehicle.getSpeed() - decel);
myVSafes.push_back(nextSpeed);
return nextSpeed;
} else {
// leader is fast enough anyway
myVSafes.push_back(targetSpeed);
return plannedSpeed;
}
} else {
// overtaking, leader should not accelerate
msgPass.informNeighLeader(new Info(nv->getSpeed(), dir | LCA_AMBLOCKINGLEADER), &myVehicle);
return -1;
}
} else if (neighLead.first != 0) { // (remainUnblocked)
// we are not blocked now. make sure we stay far enough from the leader
MSVehicle* nv = neighLead.first;
const SUMOReal nextNVSpeed = nv->getSpeed() - HELP_OVERTAKE; // conservative
const SUMOReal dv = SPEED2DIST(myVehicle.getSpeed() - nextNVSpeed);
const SUMOReal targetSpeed = myCarFollowModel.followSpeed(
&myVehicle, myVehicle.getSpeed(), neighLead.second - dv, nextNVSpeed, nv->getCarFollowModel().getMaxDecel());
myVSafes.push_back(targetSpeed);
return MIN2(targetSpeed, plannedSpeed);
} else {
// not overtaking
return plannedSpeed;
}
}
SUMOReal MSDevice_Battery::getPropEnergy(SUMOVehicle& veh) {
// calculate current kinetic energy
SUMOReal height_cur = veh.getPositionOnLane() / veh.getLane()->getLength() * (veh.getLane()->getShape().back().z() - veh.getLane()->getShape().front().z());
// kinetic energy of vehicle with current velocity
SUMOReal currentEnergy = 0.5 * getMass() * veh.getSpeed() * veh.getSpeed();
// add current potential energy of vehicle at current position
currentEnergy += getMass() * 9.81 * height_cur;
// Calculate the radius of the vehicle's current path if is distintc (r = ds / dphi)
SUMOReal radius = 0;
// If angle of vehicle was changed
if (getLastAngle() != veh.getAngle()) {
// Compute new radio
radius = SPEED2DIST(veh.getSpeed()) / fabs(GeomHelper::angleDiff(getLastAngle(), veh.getAngle()));
// Check if radius is in the interval [0.0001 - 10000] (To avoid overflow and division by zero)
if (radius < 0.0001) {
radius = 0.0001;
} else if (radius > 10000) {
radius = 10000;
}
}
// add current rotational energy of internal rotating elements
currentEnergy += getInternalMomentOfInertia() * veh.getSpeed() * veh.getSpeed();
// kinetic + potential + rotational energy gain [Ws] (MODIFICATED LAST ANGLE)
SUMOReal EnergyLoss = (currentEnergy - getLastEnergy());
// save current total energy for next time step
setLastEnergy(currentEnergy);
// Calculate energy losses:
// EnergyLoss,Air = 1/2 * rho_air [kg/m^3] * FrontSurfaceArea [m^2] * AirDragCoefficient [-] * v_Veh^2 [m/s] * s [m]
// ... with rho_air [kg/m^3] = 1,2041 kg/m^3 (at T = 20°C)
// ... with s [m] = v_Veh [m/s] * 1 [s]
EnergyLoss += 0.5 * 1.2041 * getFrontSurfaceArea() * getAirDragCoefficient() * fabs(veh.getSpeed() * veh.getSpeed() * veh.getSpeed());
// Energy loss through Air resistance [Ws]
// EnergyLoss,Tire = c_R [-] * F_N [N] * s [m]
// ... with c_R = ~0.012 (car tire on asphalt)
// ... with F_N [N] = Mass [kg] * g [m/s^2]
EnergyLoss += getRollDragCoefficient() * 9.81 * getMass() * fabs(veh.getSpeed());
// Energy loss through Roll resistance [Ws]
// ... (fabs(veh.getSpeed())>=0.01) = 0, if vehicle isn't moving
// EnergyLoss,internalFrictionRadialForce = c [m] * F_rad [N];
if (getLastAngle() != veh.getAngle()) {
// Energy loss through friction by radial force [Ws]
EnergyLoss += getRadialDragCoefficient() * getMass() * veh.getSpeed() * veh.getSpeed() / radius;
}
// EnergyLoss,constantConsumers
// Energy loss through constant loads (e.g. A/C) [Ws]
EnergyLoss += getConstantPowerIntake();
//E_Bat = E_kin_pot + EnergyLoss;
if (EnergyLoss > 0) {
// Assumption: Efficiency of PropulsionEfficiency when accelerating
EnergyLoss = EnergyLoss / getPropulsionEfficiency();
} else {
// Assumption: Efficiency of RecuperationEfficiency when recuperating
EnergyLoss = EnergyLoss * getRecuperationEfficiency();
}
// convert from [Ws] to [kWh] (3600s / 1h):
EnergyLoss = EnergyLoss / 3600 ; // EnergyLoss[Ws] * 1[h]/3600[s] * 1[k]/1000
// Return calculated energy
return(EnergyLoss);
}