Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2005-2024 German Aerospace Center (DLR) and others.
4 : // This program and the accompanying materials are made available under the
5 : // terms of the Eclipse Public License 2.0 which is available at
6 : // https://www.eclipse.org/legal/epl-2.0/
7 : // This Source Code may also be made available under the following Secondary
8 : // Licenses when the conditions for such availability set forth in the Eclipse
9 : // Public License 2.0 are satisfied: GNU General Public License, version 2
10 : // or later which is available at
11 : // https://www.gnu.org/licenses/old-licenses/gpl-2.0-standalone.html
12 : // SPDX-License-Identifier: EPL-2.0 OR GPL-2.0-or-later
13 : /****************************************************************************/
14 : /// @file MSLCM_DK2008.cpp
15 : /// @author Daniel Krajzewicz
16 : /// @author Friedemann Wesner
17 : /// @author Sascha Krieg
18 : /// @author Michael Behrisch
19 : /// @author Jakob Erdmann
20 : /// @date Tue, 29.05.2005
21 : ///
22 : // A lane change model developed by D. Krajzewicz between 2004 and 2010
23 : /****************************************************************************/
24 : #include <config.h>
25 :
26 : #include <iostream>
27 : #include <utils/common/RandHelper.h>
28 : #include <microsim/MSEdge.h>
29 : #include <microsim/MSLane.h>
30 : #include <microsim/MSLink.h>
31 : #include <microsim/MSNet.h>
32 : #include "MSLCM_DK2008.h"
33 :
34 : // ===========================================================================
35 : // variable definitions
36 : // ===========================================================================
37 : // 80km/h will be the threshold for dividing between long/short foresight
38 : #define LOOK_FORWARD_SPEED_DIVIDER 14.
39 :
40 : #define LOOK_FORWARD_FAR 15.
41 : #define LOOK_FORWARD_NEAR 5.
42 :
43 :
44 :
45 : #define JAM_FACTOR 2.
46 : #define JAM_FACTOR2 1.
47 :
48 :
49 : // ===========================================================================
50 : // member method definitions
51 : // ===========================================================================
52 8 : MSLCM_DK2008::MSLCM_DK2008(MSVehicle& v)
53 : : MSAbstractLaneChangeModel(v, LaneChangeModel::DK2008),
54 8 : myChangeProbability(0),
55 8 : myLeadingBlockerLength(0), myLeftSpace(0) {}
56 :
57 16 : MSLCM_DK2008::~MSLCM_DK2008() {
58 8 : changed();
59 16 : }
60 :
61 : int
62 167 : MSLCM_DK2008::wantsChange(
63 : int laneOffset,
64 : MSAbstractLaneChangeModel::MSLCMessager& msgPass, int blocked,
65 : const std::pair<MSVehicle*, double>& leader,
66 : const std::pair<MSVehicle*, double>& /*follower*/,
67 : const std::pair<MSVehicle*, double>& neighLead,
68 : const std::pair<MSVehicle*, double>& neighFollow,
69 : const MSLane& neighLane,
70 : const std::vector<MSVehicle::LaneQ>& preb,
71 : MSVehicle** lastBlocked,
72 : MSVehicle** firstBlocked) {
73 : UNUSED_PARAMETER(firstBlocked);
74 167 : return (laneOffset == -1 ?
75 27 : wantsChangeToRight(msgPass, blocked, leader, neighLead, neighFollow, neighLane, preb, lastBlocked, firstBlocked)
76 140 : : wantsChangeToLeft(msgPass, blocked, leader, neighLead, neighFollow, neighLane, preb, lastBlocked, firstBlocked));
77 :
78 : }
79 :
80 :
81 : int
82 27 : MSLCM_DK2008::wantsChangeToRight(MSAbstractLaneChangeModel::MSLCMessager& msgPass,
83 : int blocked,
84 : const std::pair<MSVehicle*, double>& leader,
85 : const std::pair<MSVehicle*, double>& neighLead,
86 : const std::pair<MSVehicle*, double>& neighFollow,
87 : const MSLane& neighLane,
88 : const std::vector<MSVehicle::LaneQ>& preb,
89 : MSVehicle** lastBlocked,
90 : MSVehicle** firstBlocked) {
91 : UNUSED_PARAMETER(firstBlocked);
92 : MSVehicle::LaneQ curr, best;
93 : int bestLaneOffset = 0;
94 : double currentDist = 0;
95 : double neighDist = 0;
96 : double neighExtDist = 0;
97 : double currExtDist = 0;
98 : int currIdx = 0;
99 27 : const MSLane* prebLane = myVehicle.getLane();
100 27 : if (prebLane->getEdge().isInternal()) {
101 : // internal edges are not kept inside the bestLanes structure
102 0 : prebLane = prebLane->getLinkCont()[0]->getLane();
103 : }
104 54 : for (int p = 0; p < (int) preb.size(); ++p) {
105 54 : if (preb[p].lane == prebLane && p > 0) {
106 : curr = preb[p];
107 27 : bestLaneOffset = curr.bestLaneOffset;
108 27 : currentDist = curr.length;
109 27 : currExtDist = curr.lane->getLength();
110 27 : neighDist = preb[p - 1].length;
111 27 : neighExtDist = preb[p - 1].lane->getLength();
112 27 : best = preb[p + bestLaneOffset];
113 : currIdx = p;
114 : break;
115 : }
116 : }
117 :
118 : // keep information about being a leader/follower
119 27 : int ret = (myOwnState & 0xffff0000);
120 :
121 27 : if (leader.first != 0
122 0 : &&
123 0 : (myOwnState & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0
124 27 : &&
125 0 : (leader.first->getLaneChangeModel().getOwnState()&LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0) {
126 :
127 0 : myOwnState &= (0xffffffff - LCA_AMBLOCKINGFOLLOWER_DONTBRAKE);
128 0 : if (myVehicle.getSpeed() > SUMO_const_haltingSpeed) {
129 0 : myOwnState |= LCA_AMBACKBLOCKER;
130 : } else {
131 0 : ret |= LCA_AMBACKBLOCKER;
132 0 : myDontBrake = true;
133 : }
134 : }
135 :
136 : // process information about the last blocked vehicle
137 : // if this vehicle is blocking someone in front, we maybe decelerate to let him in
138 27 : if ((*lastBlocked) != nullptr) {
139 0 : double gap = (*lastBlocked)->getPositionOnLane() - (*lastBlocked)->getVehicleType().getLength() - myVehicle.getPositionOnLane() - myVehicle.getVehicleType().getMinGap();
140 0 : if (gap > 0.1) {
141 0 : if (myVehicle.getSpeed() < ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxDecel())) {
142 0 : if ((*lastBlocked)->getSpeed() < SUMO_const_haltingSpeed) {
143 0 : ret |= LCA_AMBACKBLOCKER_STANDING;
144 : } else {
145 0 : ret |= LCA_AMBACKBLOCKER;
146 : }
147 0 : myVSafes.push_back(getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), (double)(gap - 0.1), (*lastBlocked)->getSpeed(), (*lastBlocked)->getCarFollowModel().getMaxDecel()));
148 0 : (*lastBlocked) = nullptr;
149 : }
150 0 : return ret;
151 : }
152 : }
153 :
154 : // we try to estimate the distance which is necessary to get on a lane
155 : // we have to get on in order to keep our route
156 : // we assume we need something that depends on our velocity
157 : // and compare this with the free space on our wished lane
158 : //
159 : // if the free space is somehow less than the space we need, we should
160 : // definitely try to get to the desired lane
161 : //
162 : // this rule forces our vehicle to change the lane if a lane changing is necessary soon
163 27 : double rv = myVehicle.getSpeed() > LOOK_FORWARD_SPEED_DIVIDER
164 54 : ? myVehicle.getSpeed() * (double) LOOK_FORWARD_FAR
165 27 : : myVehicle.getSpeed() * (double) LOOK_FORWARD_NEAR;
166 27 : rv += myVehicle.getVehicleType().getLengthWithGap() * (double) 2.;
167 :
168 27 : double tdist = currentDist - myVehicle.getPositionOnLane() - best.occupation * (double) JAM_FACTOR2;
169 :
170 : // assert(best.length > curr.length);
171 : // XXX if (curr.length != best.length) && ...
172 27 : if (fabs(best.length - curr.length) > MIN2((double) .1, best.lane->getLength()) && bestLaneOffset < 0 && currentDistDisallows(tdist/*currentDist*/, bestLaneOffset, rv)) {
173 0 : informBlocker(msgPass, blocked, LCA_MRIGHT, neighLead, neighFollow);
174 0 : if (neighLead.second > 0 && neighLead.second > leader.second) {
175 0 : myVSafes.push_back(getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), neighLead.second, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()) - (double) 0.5);
176 : }
177 :
178 : // letting vehicles merge in at the end of the lane in case of counter-lane change, step#1, right
179 : // if there is a leader and he wants to change to left (we want to change to right)
180 0 : if (neighLead.first != 0 && (neighLead.first->getLaneChangeModel().getOwnState()&LCA_LEFT) != 0) {
181 : // save at least his length in myLeadingBlockerLength
182 0 : myLeadingBlockerLength = MAX2(neighLead.first->getVehicleType().getLengthWithGap(), myLeadingBlockerLength);
183 : // save the left space
184 0 : myLeftSpace = currentDist - myVehicle.getPositionOnLane();
185 : }
186 : //
187 :
188 0 : return ret | LCA_RIGHT | LCA_STRATEGIC | LCA_URGENT;
189 : }
190 :
191 :
192 : // the opposite lane-changing direction should be done than the one examined herein
193 : // we'll check whether we assume we could change anyhow and get back in time...
194 : //
195 : // this rule prevents the vehicle from moving in opposite direction of the best lane
196 : // unless the way till the end where the vehicle has to be on the best lane
197 : // is long enough
198 27 : double maxJam = MAX2(preb[currIdx - 1].occupation, preb[currIdx].occupation);
199 27 : double neighLeftPlace = MAX2((double) 0, neighDist - myVehicle.getPositionOnLane() - maxJam);
200 27 : if (bestLaneOffset >= 0 && (currentDistDisallows(neighLeftPlace, bestLaneOffset + 2, rv))) {
201 : // ...we will not change the lane if not
202 27 : return ret | LCA_STAY | LCA_STRATEGIC;
203 : }
204 :
205 :
206 : // if the current lane is the best and a lane-changing would cause a situation
207 : // of which we assume we will not be able to return to the lane we have to be on...
208 : //
209 : // this rule prevents the vehicle from leaving the current, best lane when it is
210 : // close to this lane's end
211 0 : if (currExtDist > neighExtDist && (neighLeftPlace * 2. < rv/*||currE[currIdx+1].length<currentDist*/)) {
212 0 : return ret | LCA_STAY | LCA_STRATEGIC;
213 : }
214 :
215 : // let's also regard the case where the vehicle is driving on a highway...
216 : // in this case, we do not want to get to the dead-end of an on-ramp
217 : //
218 : // THIS RULE APPLIES ONLY TO CHANGING TO THE RIGHT LANE
219 0 : if (bestLaneOffset == 0 && preb[currIdx - 1].bestLaneOffset != 0 && myVehicle.getLane()->getSpeedLimit() > 80. / 3.6) {
220 0 : return ret | LCA_STAY | LCA_STRATEGIC;
221 : }
222 : // --------
223 :
224 : // -------- make place on current lane if blocking follower
225 : if (amBlockingFollowerPlusNB()
226 0 : &&
227 0 : (currentDistAllows(neighDist, bestLaneOffset, rv) || neighDist >= currentDist)) {
228 :
229 0 : return ret | LCA_RIGHT | LCA_COOPERATIVE | LCA_URGENT;
230 : }
231 : // --------
232 :
233 :
234 : // -------- security checks for krauss
235 : // (vsafe fails when gap<0)
236 0 : if ((blocked & LCA_BLOCKED) != 0) {
237 : return ret;
238 : }
239 : // --------
240 :
241 : // -------- higher speed
242 0 : if ((congested(neighLead.first) && neighLead.second < 20) || predInteraction(leader)) { //!!!
243 0 : return ret;
244 : }
245 0 : double thisLaneVSafe = myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle);
246 0 : double neighLaneVSafe = neighLane.getVehicleMaxSpeed(&myVehicle);
247 0 : if (neighLead.first == 0) {
248 0 : neighLaneVSafe = MIN2(neighLaneVSafe, getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), neighDist, 0, 0));
249 : } else {
250 : // @todo: what if leader is below safe gap?!!!
251 0 : neighLaneVSafe = MIN2(neighLaneVSafe, getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), neighLead.second, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()));
252 : }
253 0 : if (leader.first == 0) {
254 0 : thisLaneVSafe = MIN2(thisLaneVSafe, getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), currentDist, 0, 0));
255 : } else {
256 : // @todo: what if leader is below safe gap?!!!
257 0 : thisLaneVSafe = MIN2(thisLaneVSafe, getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), leader.second, leader.first->getSpeed(), leader.first->getCarFollowModel().getMaxDecel()));
258 : }
259 :
260 0 : thisLaneVSafe = MIN2(thisLaneVSafe, myVehicle.getMaxSpeed());
261 0 : neighLaneVSafe = MIN2(neighLaneVSafe, myVehicle.getMaxSpeed());
262 0 : if (thisLaneVSafe - neighLaneVSafe > 5. / 3.6) {
263 : // ok, the current lane is faster than the right one...
264 0 : if (myChangeProbability < 0) {
265 0 : myChangeProbability *= pow(0.5, TS);
266 : }
267 : } else {
268 : // ok, the right lane is faster than the current
269 0 : myChangeProbability -= TS * ((neighLaneVSafe - thisLaneVSafe) / (myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle)));
270 : }
271 :
272 : // let's recheck the "Rechtsfahrgebot"
273 0 : double vmax = myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle);
274 0 : vmax -= (double)(5. / 2.6);
275 0 : if (neighLaneVSafe >= vmax) {
276 0 : myChangeProbability -= TS * ((neighLaneVSafe - vmax) / (vmax));
277 : }
278 :
279 0 : if (myChangeProbability < -2 && neighDist / MAX2((double) .1, myVehicle.getSpeed()) > 20.) { //./MAX2((double) .1, myVehicle.getSpeed())) { // -.1
280 0 : return ret | LCA_RIGHT | LCA_SPEEDGAIN;
281 : }
282 : // --------
283 :
284 : return ret;
285 : }
286 :
287 :
288 : int
289 140 : MSLCM_DK2008::wantsChangeToLeft(MSAbstractLaneChangeModel::MSLCMessager& msgPass,
290 : int blocked,
291 : const std::pair<MSVehicle*, double>& leader,
292 : const std::pair<MSVehicle*, double>& neighLead,
293 : const std::pair<MSVehicle*, double>& neighFollow,
294 : const MSLane& neighLane,
295 : const std::vector<MSVehicle::LaneQ>& preb,
296 : MSVehicle** lastBlocked,
297 : MSVehicle** firstBlocked) {
298 : UNUSED_PARAMETER(firstBlocked);
299 : MSVehicle::LaneQ curr, best;
300 : int bestLaneOffset = 0;
301 : double currentDist = 0;
302 : double neighDist = 0;
303 : double neighExtDist = 0;
304 : double currExtDist = 0;
305 : int currIdx = 0;
306 140 : const MSLane* prebLane = myVehicle.getLane();
307 140 : if (prebLane->getEdge().isInternal()) {
308 : // internal edges are not kept inside the bestLanes structure
309 0 : prebLane = prebLane->getLinkCont()[0]->getLane();
310 : }
311 167 : for (int p = 0; p < (int) preb.size(); ++p) {
312 167 : if (preb[p].lane == prebLane) {
313 : curr = preb[p];
314 140 : bestLaneOffset = curr.bestLaneOffset;
315 140 : currentDist = curr.length;
316 140 : currExtDist = curr.lane->getLength();
317 140 : neighDist = preb[p + 1].length;
318 140 : neighExtDist = preb[p + 1].lane->getLength();
319 140 : best = preb[p + bestLaneOffset];
320 : currIdx = p;
321 : break;
322 : }
323 : }
324 : // keep information about being a leader/follower
325 140 : int ret = (myOwnState & 0xffff0000);
326 :
327 : // ?!!!
328 140 : if (leader.first != 0
329 0 : &&
330 0 : (myOwnState & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0
331 140 : &&
332 0 : (leader.first->getLaneChangeModel().getOwnState()&LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0) {
333 :
334 0 : myOwnState &= (0xffffffff - LCA_AMBLOCKINGFOLLOWER_DONTBRAKE);
335 0 : if (myVehicle.getSpeed() > SUMO_const_haltingSpeed) {
336 0 : myOwnState |= LCA_AMBACKBLOCKER;
337 : } else {
338 0 : ret |= LCA_AMBACKBLOCKER;
339 0 : myDontBrake = true;
340 : }
341 : }
342 :
343 : // process information about the last blocked vehicle
344 : // if this vehicle is blocking someone in front, we maybe decelerate to let him in
345 140 : if ((*lastBlocked) != nullptr) {
346 0 : double gap = (*lastBlocked)->getPositionOnLane() - (*lastBlocked)->getVehicleType().getLength() - myVehicle.getPositionOnLane() - myVehicle.getVehicleType().getMinGap();
347 0 : if (gap > 0.1) {
348 0 : if (myVehicle.getSpeed() < ACCEL2SPEED(myVehicle.getCarFollowModel().getMaxDecel())) {
349 0 : if ((*lastBlocked)->getSpeed() < SUMO_const_haltingSpeed) {
350 0 : ret |= LCA_AMBACKBLOCKER_STANDING;
351 : } else {
352 0 : ret |= LCA_AMBACKBLOCKER;
353 : }
354 0 : myVSafes.push_back(getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), (double)(gap - 0.1), (*lastBlocked)->getSpeed(), (*lastBlocked)->getCarFollowModel().getMaxDecel()));
355 0 : (*lastBlocked) = nullptr;
356 : }
357 0 : return ret;
358 : }
359 : }
360 :
361 : // we try to estimate the distance which is necessary to get on a lane
362 : // we have to get on in order to keep our route
363 : // we assume we need something that depends on our velocity
364 : // and compare this with the free space on our wished lane
365 : //
366 : // if the free space is somehow less than the space we need, we should
367 : // definitely try to get to the desired lane
368 : //
369 : // this rule forces our vehicle to change the lane if a lane changing is necessary soon
370 140 : double lv = myVehicle.getSpeed() > LOOK_FORWARD_SPEED_DIVIDER
371 280 : ? myVehicle.getSpeed() * (double) LOOK_FORWARD_FAR
372 140 : : myVehicle.getSpeed() * (double) LOOK_FORWARD_NEAR;
373 140 : lv += myVehicle.getVehicleType().getLengthWithGap() * (double) 2.;
374 :
375 :
376 140 : double tdist = currentDist - myVehicle.getPositionOnLane() - best.occupation * (double) JAM_FACTOR2;
377 253 : if (fabs(best.length - curr.length) > MIN2((double) .1, best.lane->getLength()) && bestLaneOffset > 0
378 253 : &&
379 : currentDistDisallows(tdist/*currentDist*/, bestLaneOffset, lv)) {
380 4 : informBlocker(msgPass, blocked, LCA_MLEFT, neighLead, neighFollow);
381 4 : if (neighLead.second > 0 && neighLead.second > leader.second) {
382 0 : myVSafes.push_back(getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), neighLead.second, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()) - (double) 0.5);
383 : }
384 :
385 : // letting vehicles merge in at the end of the lane in case of counter-lane change, step#1, left
386 : // if there is a leader and he wants to change to right (we want to change to left)
387 4 : if (neighLead.first != 0 && (neighLead.first->getLaneChangeModel().getOwnState()&LCA_RIGHT) != 0) {
388 : // save at least his length in myLeadingBlockerLength
389 0 : myLeadingBlockerLength = MAX2(neighLead.first->getVehicleType().getLengthWithGap(), myLeadingBlockerLength);
390 : // save the left space
391 0 : myLeftSpace = currentDist - myVehicle.getPositionOnLane();
392 : }
393 : //
394 :
395 4 : return ret | LCA_LEFT | LCA_STRATEGIC | LCA_URGENT;
396 : }
397 :
398 : // the opposite lane-changing direction should be rather done, not
399 : // the one examined herein
400 : // we'll check whether we assume we could change anyhow and get back in time...
401 : //
402 : // this rule prevents the vehicle from moving in opposite direction of the best lane
403 : // unless the way till the end where the vehicle has to be on the best lane
404 : // is long enough
405 136 : double maxJam = MAX2(preb[currIdx + 1].occupation, preb[currIdx].occupation);
406 136 : double neighLeftPlace = MAX2((double) 0, neighDist - myVehicle.getPositionOnLane() - maxJam);
407 136 : if (bestLaneOffset <= 0 && (currentDistDisallows(neighLeftPlace, bestLaneOffset - 2, lv))) {
408 : // ...we will not change the lane if not
409 27 : return ret | LCA_STAY | LCA_STRATEGIC;
410 : }
411 :
412 :
413 : // if the current lane is the best and a lane-changing would cause a situation
414 : // of which we assume we will not be able to return to the lane we have to be on...
415 : //
416 : // this rule prevents the vehicle from leaving the current, best lane when it is
417 : // close to this lane's end
418 109 : if (currExtDist > neighExtDist && (neighLeftPlace * 2. < lv/*||currE[currIdx+1].length<currentDist*/)) {
419 : // ... let's not change the lane
420 0 : return ret | LCA_STAY | LCA_STRATEGIC;
421 : }
422 :
423 : /*
424 : // let's also regard the case where the vehicle is driving on a highway...
425 : // in this case, we do not want to get to the dead-end of an on-ramp
426 : if(bestLaneOffset==0&&myVehicle.getLane().getMaxSpeed()>80./3.6) {
427 : return ret;
428 : }
429 : */
430 :
431 :
432 : /*
433 : // if the current lane is the
434 : if(bestLaneOffset==0&&(neighDist==0||curr.seenVehicles2*JAM_FACTOR>=neighExtDist-curr.length)) {
435 : return ret;
436 : }
437 : */
438 : // --------
439 :
440 : // -------- make place on current lane if blocking follower
441 : if (amBlockingFollowerPlusNB()
442 109 : &&
443 0 : (currentDistAllows(neighDist, bestLaneOffset, lv) || neighDist >= currentDist)) {
444 :
445 0 : return ret | LCA_LEFT | LCA_COOPERATIVE | LCA_URGENT;
446 : }
447 : // --------
448 :
449 : // -------- security checks for krauss
450 : // (vsafe fails when gap<0)
451 109 : if ((blocked & LCA_BLOCKED) != 0) {
452 : return ret;
453 : }
454 :
455 : // -------- higher speed
456 109 : if ((congested(neighLead.first) && neighLead.second < 20) || predInteraction(leader)) { //!!!
457 0 : return ret;
458 : }
459 109 : double neighLaneVSafe = neighLane.getVehicleMaxSpeed(&myVehicle);
460 109 : double thisLaneVSafe = myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle);
461 109 : if (neighLead.first == 0) {
462 109 : neighLaneVSafe = MIN2(neighLaneVSafe, getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), neighDist, 0, 0)); // !!! warum nicht die Folgesgeschw.?
463 : } else {
464 : // @todo: what if leader is below safe gap?!!!
465 0 : neighLaneVSafe = MIN2(neighLaneVSafe, getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), neighLead.second, neighLead.first->getSpeed(), neighLead.first->getCarFollowModel().getMaxDecel()));
466 : }
467 109 : if (leader.first == 0) {
468 109 : thisLaneVSafe = MIN2(thisLaneVSafe, getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), currentDist, 0, 0));
469 : } else {
470 : // @todo: what if leader is below safe gap?!!!
471 0 : thisLaneVSafe = MIN2(thisLaneVSafe, getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), leader.second, leader.first->getSpeed(), leader.first->getCarFollowModel().getMaxDecel()));
472 : }
473 109 : thisLaneVSafe = MIN2(thisLaneVSafe, myVehicle.getMaxSpeed());
474 109 : neighLaneVSafe = MIN2(neighLaneVSafe, myVehicle.getMaxSpeed());
475 109 : if (thisLaneVSafe > neighLaneVSafe) {
476 : // this lane is better
477 0 : if (myChangeProbability > 0) {
478 0 : myChangeProbability *= pow(0.5, TS);
479 : }
480 : } else {
481 : // right lane is better
482 109 : myChangeProbability += TS * ((neighLaneVSafe - thisLaneVSafe) / (myVehicle.getLane()->getVehicleMaxSpeed(&myVehicle))); // !!! Fahrzeuggeschw.!
483 : }
484 109 : if (myChangeProbability > .2 && neighDist / MAX2((double) .1, myVehicle.getSpeed()) > 20.) { // .1
485 0 : return ret | LCA_LEFT | LCA_SPEEDGAIN | LCA_URGENT;
486 : }
487 : // --------
488 :
489 : return ret;
490 : }
491 :
492 :
493 : double
494 257 : MSLCM_DK2008::patchSpeed(const double min, const double wanted, const double max, const MSCFModel& cfModel) {
495 257 : int state = myOwnState;
496 :
497 : // letting vehicles merge in at the end of the lane in case of counter-lane change, step#2
498 : double MAGIC_offset = 1.;
499 : // if we want to change and have a blocking leader and there is enough room for him in front of us
500 257 : if (myLeadingBlockerLength != 0) {
501 0 : double space = myLeftSpace - myLeadingBlockerLength - MAGIC_offset - myVehicle.getVehicleType().getMinGap();
502 0 : if (space > 0) {
503 : // compute speed for decelerating towards a place which allows the blocking leader to merge in in front
504 0 : double safe = cfModel.stopSpeed(&myVehicle, myVehicle.getSpeed(), space, MSCFModel::CalcReason::LANE_CHANGE);
505 : // if we are approaching this place
506 0 : if (safe < wanted) {
507 : // return this speed as the speed to use
508 : return MAX2(min, safe);
509 : }
510 : }
511 : }
512 :
513 : // just to make sure to be notified about lane chaning end
514 257 : if (myVehicle.getLane()->getEdge().getLanes().size() == 1 || myVehicle.getLane()->getEdge().isInternal()) {
515 : // remove chaning information if on a road with a single lane
516 93 : changed();
517 93 : return wanted;
518 : }
519 :
520 : double nVSafe = wanted;
521 : bool gotOne = false;
522 164 : for (std::vector<double>::const_iterator i = myVSafes.begin(); i != myVSafes.end(); ++i) {
523 0 : double v = (*i);
524 0 : if (v >= min && v <= max) {
525 : nVSafe = MIN2(v, nVSafe);
526 : gotOne = true;
527 : }
528 : }
529 :
530 : // check whether the vehicle is blocked
531 164 : if ((state & LCA_WANTS_LANECHANGE) != 0) {
532 4 : if (gotOne && !myDontBrake) {
533 : return nVSafe;
534 : }
535 : // check whether the vehicle maybe has to be swapped with one of
536 : // the blocking vehicles
537 4 : if ((state & LCA_BLOCKED) != 0) {
538 0 : if ((state & LCA_BLOCKED_BY_LEADER) != 0) {
539 : // if interacting with leader and not too slow
540 0 : return (min + wanted) / (double) 2.0;
541 : }
542 0 : if ((state & LCA_BLOCKED_BY_FOLLOWER) != 0) {
543 0 : return (max + wanted) / (double) 2.0;
544 : }
545 0 : return (min + wanted) / (double) 2.0;
546 : }
547 : }
548 :
549 :
550 : // decelerate if being a blocking follower
551 : // (and does not have to change lanes)
552 164 : if ((state & LCA_AMBLOCKINGFOLLOWER) != 0) {
553 0 : if (fabs(max - myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle)) < 0.001 && min == 0) { // !!! was standing
554 : return 0;
555 : }
556 0 : return (min + wanted) / (double) 2.0;
557 : }
558 164 : if ((state & LCA_AMBACKBLOCKER) != 0) {
559 0 : if (max <= myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle) && min == 0) { // !!! was standing
560 : return min;
561 : }
562 : }
563 164 : if ((state & LCA_AMBACKBLOCKER_STANDING) != 0) {
564 : return min;
565 : }
566 : // accelerate if being a blocking leader or blocking follower not able to brake
567 : // (and does not have to change lanes)
568 164 : if ((state & LCA_AMBLOCKINGLEADER) != 0) {
569 0 : return (max + wanted) / (double) 2.0;
570 : }
571 164 : if ((state & LCA_AMBLOCKINGFOLLOWER_DONTBRAKE) != 0) {
572 0 : if (max <= myVehicle.getCarFollowModel().maxNextSpeed(myVehicle.getSpeed(), &myVehicle) && min == 0) { // !!! was standing
573 : return wanted;
574 : }
575 0 : return (min + wanted) / (double) 2.0;
576 : }
577 : return wanted;
578 : }
579 :
580 :
581 : void*
582 0 : MSLCM_DK2008::inform(void* info, MSVehicle* /*sender*/) {
583 : Info* pinfo = (Info*) info;
584 : //myOwnState &= 0xffffffff; // reset all bits of MyLCAEnum but only those
585 0 : myOwnState |= pinfo->second;
586 0 : delete pinfo;
587 0 : return (void*) true;
588 : }
589 :
590 :
591 : void
592 105 : MSLCM_DK2008::changed() {
593 105 : myOwnState = 0;
594 105 : myChangeProbability = 0;
595 105 : myLeadingBlockerLength = 0;
596 105 : myLeftSpace = 0;
597 : myVSafes.clear();
598 105 : myDontBrake = false;
599 105 : }
600 :
601 :
602 : void
603 4 : MSLCM_DK2008::informBlocker(MSAbstractLaneChangeModel::MSLCMessager& msgPass,
604 : int& blocked,
605 : int dir,
606 : const std::pair<MSVehicle*, double>& neighLead,
607 : const std::pair<MSVehicle*, double>& neighFollow) {
608 4 : if ((blocked & LCA_BLOCKED_BY_FOLLOWER) != 0) {
609 : assert(neighFollow.first != 0);
610 0 : MSVehicle* nv = neighFollow.first;
611 : double decelGap =
612 0 : neighFollow.second
613 0 : + SPEED2DIST(myVehicle.getSpeed()) * (double) 2.0
614 0 : - MAX2(nv->getSpeed() - (double) ACCEL2DIST(nv->getCarFollowModel().getMaxDecel()) * (double) 2.0, (double) 0);
615 0 : if (neighFollow.second > 0 && decelGap > 0 && decelGap >= nv->getCarFollowModel().getSecureGap(nv, &myVehicle,
616 0 : nv->getSpeed(), myVehicle.getSpeed(), myVehicle.getCarFollowModel().getMaxDecel())) {
617 0 : double vsafe = getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), neighFollow.second, neighFollow.first->getSpeed(), neighFollow.first->getCarFollowModel().getMaxDecel());
618 0 : msgPass.informNeighFollower(new Info(vsafe, dir | LCA_AMBLOCKINGFOLLOWER), &myVehicle);
619 : } else {
620 0 : double vsafe = neighFollow.second <= 0 ? 0 : getCarFollowModel().followSpeed(&myVehicle, myVehicle.getSpeed(), neighFollow.second, neighFollow.first->getSpeed(), neighFollow.first->getCarFollowModel().getMaxDecel());
621 0 : msgPass.informNeighFollower(new Info(vsafe, dir | LCA_AMBLOCKINGFOLLOWER_DONTBRAKE), &myVehicle);
622 : }
623 : }
624 4 : if ((blocked & LCA_BLOCKED_BY_LEADER) != 0) {
625 0 : if (neighLead.first != 0 && neighLead.second > 0) {
626 0 : msgPass.informNeighLeader(new Info(0, dir | LCA_AMBLOCKINGLEADER), &myVehicle);
627 : }
628 : }
629 4 : }
630 :
631 :
632 : void
633 249 : MSLCM_DK2008::prepareStep() {
634 249 : MSAbstractLaneChangeModel::prepareStep();
635 249 : myOwnState = 0;
636 249 : myLeadingBlockerLength = 0;
637 249 : myLeftSpace = 0;
638 : myVSafes.clear();
639 249 : myDontBrake = false;
640 : // truncate myChangeProbability to work around numerical instability between different builds
641 249 : myChangeProbability = ceil(myChangeProbability * 100000.0) * 0.00001;
642 249 : resetSpeedLat();
643 249 : }
644 :
645 :
646 : /****************************************************************************/
|
