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);
}
}
}
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;
}
}
}
