Line data Source code
1 : /****************************************************************************/
2 : // Eclipse SUMO, Simulation of Urban MObility; see https://eclipse.dev/sumo
3 : // Copyright (C) 2002-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 MSLeaderInfo.cpp
15 : /// @author Jakob Erdmann
16 : /// @date Oct 2015
17 : ///
18 : // Information about vehicles ahead (may be multiple vehicles if
19 : // lateral-resolution is active)
20 : /****************************************************************************/
21 : #include <config.h>
22 :
23 : #include <cassert>
24 : #include <cmath>
25 : #include <utils/common/ToString.h>
26 : #include <microsim/MSGlobals.h>
27 : #include <microsim/MSVehicle.h>
28 : #include <microsim/lcmodels/MSAbstractLaneChangeModel.h>
29 : #include <microsim/MSNet.h>
30 : #include <microsim/MSLane.h>
31 : #include "MSLeaderInfo.h"
32 :
33 :
34 : // ===========================================================================
35 : // MSLeaderInfo member method definitions
36 : // ===========================================================================
37 1470825955 : MSLeaderInfo::MSLeaderInfo(const double laneWidth, const MSVehicle* ego, const double latOffset) :
38 1470825955 : myWidth(laneWidth),
39 1470825955 : myOffset(0),
40 1470825955 : myVehicles(MAX2(1, int(ceil(laneWidth / MSGlobals::gLateralResolution))), (MSVehicle*)nullptr),
41 1470825955 : myFreeSublanes((int)myVehicles.size()),
42 1470825955 : egoRightMost(-1),
43 1470825955 : egoLeftMost(-1),
44 1470825955 : myHasVehicles(false) {
45 1470825955 : if (ego != nullptr) {
46 230123286 : getSubLanes(ego, latOffset, egoRightMost, egoLeftMost);
47 : // filter out sublanes not of interest to ego
48 230123286 : myFreeSublanes -= egoRightMost;
49 230123286 : myFreeSublanes -= (int)myVehicles.size() - 1 - egoLeftMost;
50 : }
51 1470825955 : }
52 :
53 :
54 3492843338 : MSLeaderInfo::~MSLeaderInfo() { }
55 :
56 :
57 : int
58 1117438181 : MSLeaderInfo::addLeader(const MSVehicle* veh, bool beyond, double latOffset) {
59 1117438181 : if (veh == nullptr) {
60 0 : return myFreeSublanes;
61 : }
62 1117438181 : if (myVehicles.size() == 1) {
63 : // speedup for the simple case
64 542293390 : if (!beyond || myVehicles[0] == 0) {
65 542293390 : myVehicles[0] = veh;
66 542293390 : myFreeSublanes = 0;
67 542293390 : myHasVehicles = true;
68 : }
69 542293390 : return myFreeSublanes;
70 : }
71 : // map center-line based coordinates into [0, myWidth] coordinates
72 : int rightmost, leftmost;
73 575144791 : getSubLanes(veh, latOffset, rightmost, leftmost);
74 : //if (gDebugFlag1) std::cout << " addLeader veh=" << veh->getID() << " beyond=" << beyond << " latOffset=" << latOffset << " sublaneOffset=" << myOffset
75 : // << " rightmost=" << rightmost << " leftmost=" << leftmost
76 : // << " eRM=" << egoRightMost << " eLM=" << egoLeftMost
77 : // << " myFreeSublanes=" << myFreeSublanes << "\n";
78 2397485304 : for (int sublane = rightmost; sublane <= leftmost; ++sublane) {
79 17664320 : if ((egoRightMost < 0 || (egoRightMost <= sublane && sublane <= egoLeftMost))
80 1830777348 : && (!beyond || myVehicles[sublane] == 0)) {
81 1527429777 : if (myVehicles[sublane] == 0) {
82 909502073 : myFreeSublanes--;
83 : }
84 1527429777 : myVehicles[sublane] = veh;
85 1527429777 : myHasVehicles = true;
86 : }
87 : }
88 575144791 : return myFreeSublanes;
89 : }
90 :
91 :
92 : void
93 401697 : MSLeaderInfo::clear() {
94 401697 : myVehicles.assign(myVehicles.size(), (MSVehicle*)nullptr);
95 401697 : myFreeSublanes = (int)myVehicles.size();
96 401697 : if (egoRightMost >= 0) {
97 0 : myFreeSublanes -= egoRightMost;
98 0 : myFreeSublanes -= (int)myVehicles.size() - 1 - egoLeftMost;
99 : }
100 401697 : }
101 :
102 :
103 : void
104 4175770733 : MSLeaderInfo::getSubLanes(const MSVehicle* veh, double latOffset, int& rightmost, int& leftmost) const {
105 4175770733 : if (myVehicles.size() == 1) {
106 : // speedup for the simple case
107 820171995 : rightmost = 0;
108 820171995 : leftmost = 0;
109 820171995 : return;
110 : }
111 : // map center-line based coordinates into [0, myWidth] coordinates
112 3355598738 : const double vehCenter = veh->getLateralPositionOnLane() + 0.5 * myWidth + latOffset + myOffset * MSGlobals::gLateralResolution;
113 3355598738 : const double vehHalfWidth = 0.5 * veh->getVehicleType().getWidth();
114 3355598738 : double rightVehSide = vehCenter - vehHalfWidth;
115 3355598738 : double leftVehSide = vehCenter + vehHalfWidth;
116 : // Reserve some additional space if the vehicle is performing a maneuver continuation.
117 3355598738 : if (veh->getActionStepLength() != DELTA_T) {
118 239692374 : if (veh->getLaneChangeModel().getManeuverDist() < 0. || veh->getLaneChangeModel().getSpeedLat() < 0.) {
119 12524164 : const double maneuverDist = MIN2(veh->getVehicleType().getMaxSpeedLat() * veh->getActionStepLengthSecs(), -MIN2(0., veh->getLaneChangeModel().getManeuverDist()));
120 6262082 : rightVehSide -= maneuverDist;
121 : }
122 239692374 : if (veh->getLaneChangeModel().getManeuverDist() > 0. || veh->getLaneChangeModel().getSpeedLat() > 0.) {
123 16585108 : const double maneuverDist = MIN2(veh->getVehicleType().getMaxSpeedLat() * veh->getActionStepLengthSecs(), MAX2(0., veh->getLaneChangeModel().getManeuverDist()));
124 8292554 : leftVehSide += maneuverDist;
125 : }
126 : }
127 3355598738 : if (rightVehSide > myWidth || leftVehSide < 0) {
128 : // vehicle does not touch this lane
129 : // set the values so that an iteration
130 : // for (i = rightmost; i <= leftmost; i++) stops immediately
131 37668997 : rightmost = -1000;
132 37668997 : leftmost = -2000;
133 : } else {
134 3317929741 : rightmost = MAX2(0, (int)floor((rightVehSide + NUMERICAL_EPS) / MSGlobals::gLateralResolution));
135 6635776931 : leftmost = MIN2((int)myVehicles.size() - 1, (int)floor(MAX2(0.0, leftVehSide - NUMERICAL_EPS) / MSGlobals::gLateralResolution));
136 : }
137 : //if (veh->isSelected()) std::cout << SIMTIME << " veh=" << veh->getID()
138 : // << std::setprecision(2)
139 : // << " posLat=" << veh->getLateralPositionOnLane()
140 : // << " latOffset=" << latOffset
141 : // << " vehCenter=" << vehCenter
142 : // << " rightVehSide=" << rightVehSide
143 : // << " leftVehSide=" << leftVehSide
144 : // << " rightmost=" << rightmost
145 : // << " leftmost=" << leftmost
146 : // << " myOffset=" << myOffset
147 : // << std::setprecision(2)
148 : // << "\n";
149 : }
150 :
151 :
152 : void
153 394388146 : MSLeaderInfo::getSublaneBorders(int sublane, double latOffset, double& rightSide, double& leftSide) const {
154 : assert(sublane >= 0);
155 : assert(sublane < (int)myVehicles.size());
156 394388146 : const double res = MSGlobals::gLateralResolution > 0 ? MSGlobals::gLateralResolution : myWidth;
157 394388146 : rightSide = sublane * res + latOffset - myOffset * MSGlobals::gLateralResolution;
158 394388146 : leftSide = MIN2((sublane + 1) * res, myWidth) + latOffset - myOffset * MSGlobals::gLateralResolution;
159 394388146 : }
160 :
161 :
162 : const MSVehicle*
163 3581289908 : MSLeaderInfo::operator[](int sublane) const {
164 : assert(sublane >= 0);
165 : assert(sublane < (int)myVehicles.size());
166 3581289908 : return myVehicles[sublane];
167 : }
168 :
169 :
170 : std::string
171 0 : MSLeaderInfo::toString() const {
172 0 : std::ostringstream oss;
173 : oss.setf(std::ios::fixed, std::ios::floatfield);
174 : oss << std::setprecision(2);
175 0 : for (int i = 0; i < (int)myVehicles.size(); ++i) {
176 0 : oss << Named::getIDSecure(myVehicles[i]);
177 0 : if (i < (int)myVehicles.size() - 1) {
178 0 : oss << ", ";
179 : }
180 : }
181 0 : oss << " free=" << myFreeSublanes;
182 0 : return oss.str();
183 0 : }
184 :
185 :
186 : void
187 74572811 : MSLeaderInfo::setSublaneOffset(int offset) {
188 : assert(MSGlobals::gLateralResolution > 0);
189 74572811 : myOffset = offset;
190 74572811 : }
191 :
192 :
193 : bool
194 140043837 : MSLeaderInfo::hasStoppedVehicle() const {
195 140043837 : if (!myHasVehicles) {
196 : return false;
197 : }
198 635417248 : for (int i = 0; i < (int)myVehicles.size(); ++i) {
199 512892842 : if (myVehicles[i] != 0 && myVehicles[i]->isStopped()) {
200 : return true;
201 : }
202 : }
203 : return false;
204 : }
205 :
206 : void
207 143352 : MSLeaderInfo::removeOpposite(const MSLane* lane) {
208 679334 : for (int i = 0; i < (int)myVehicles.size(); ++i) {
209 535982 : const MSVehicle* veh = myVehicles[i];
210 834466 : if (veh != 0 &&
211 298484 : (veh->getLaneChangeModel().isOpposite()
212 292877 : || &lane->getEdge() != &veh->getLane()->getEdge())) {
213 9304 : myVehicles[i] = nullptr;
214 : }
215 : }
216 143352 : }
217 :
218 :
219 : // ===========================================================================
220 : // MSLeaderDistanceInfo member method definitions
221 : // ===========================================================================
222 587759103 : MSLeaderDistanceInfo::MSLeaderDistanceInfo(const double laneWidth, const MSVehicle* ego, const double latOffset) :
223 : MSLeaderInfo(laneWidth, ego, latOffset),
224 587759103 : myDistances(myVehicles.size(), std::numeric_limits<double>::max()) {
225 587759103 : }
226 :
227 :
228 192783322 : MSLeaderDistanceInfo::MSLeaderDistanceInfo(const CLeaderDist& cLeaderDist, const double laneWidth) :
229 : MSLeaderInfo(laneWidth, nullptr, 0.),
230 192783322 : myDistances(1, cLeaderDist.second) {
231 : assert(myVehicles.size() == 1);
232 192783322 : myVehicles[0] = cLeaderDist.first;
233 192783322 : myHasVehicles = cLeaderDist.first != nullptr;
234 192783322 : }
235 :
236 1317611591 : MSLeaderDistanceInfo::~MSLeaderDistanceInfo() { }
237 :
238 :
239 : int
240 513883517 : MSLeaderDistanceInfo::addLeader(const MSVehicle* veh, double gap, double latOffset, int sublane) {
241 : //if (SIMTIME == 31 && gDebugFlag1 && veh != 0 && veh->getID() == "cars.8") {
242 : // std::cout << " BREAKPOINT\n";
243 : //}
244 513883517 : if (veh == nullptr) {
245 1003977 : return myFreeSublanes;
246 : }
247 512879540 : if (myVehicles.size() == 1) {
248 : // speedup for the simple case
249 : sublane = 0;
250 : }
251 512879540 : if (sublane >= 0 && sublane < (int)myVehicles.size()) {
252 : // sublane is already given
253 511230166 : if (gap < myDistances[sublane]) {
254 381146896 : if (myVehicles[sublane] == 0) {
255 378427681 : myFreeSublanes--;
256 : }
257 381146896 : myVehicles[sublane] = veh;
258 381146896 : myDistances[sublane] = gap;
259 381146896 : myHasVehicles = true;
260 : }
261 511230166 : return myFreeSublanes;
262 : }
263 : int rightmost, leftmost;
264 1649374 : getSubLanes(veh, latOffset, rightmost, leftmost);
265 : //if (gDebugFlag1) std::cout << " addLeader veh=" << veh->getID() << " gap=" << gap << " latOffset=" << latOffset << " sublaneOffset=" << myOffset
266 : // << " rightmost=" << rightmost << " leftmost=" << leftmost
267 : // << " eRM=" << egoRightMost << " eLM=" << egoLeftMost
268 : // << " myFreeSublanes=" << myFreeSublanes << "\n";
269 5940222 : for (int sublaneIdx = rightmost; sublaneIdx <= leftmost; ++sublaneIdx) {
270 586854 : if ((egoRightMost < 0 || (egoRightMost <= sublaneIdx && sublaneIdx <= egoLeftMost))
271 4780957 : && gap < myDistances[sublaneIdx]) {
272 1849821 : if (myVehicles[sublaneIdx] == 0) {
273 1584249 : myFreeSublanes--;
274 : }
275 1849821 : myVehicles[sublaneIdx] = veh;
276 1849821 : myDistances[sublaneIdx] = gap;
277 1849821 : myHasVehicles = true;
278 : }
279 : }
280 1649374 : return myFreeSublanes;
281 : }
282 :
283 :
284 : void
285 1092869 : MSLeaderDistanceInfo::addLeaders(MSLeaderDistanceInfo& other) {
286 : const int maxSubLane = MIN2(numSublanes(), other.numSublanes());
287 6155319 : for (int i = 0; i < maxSubLane; i++) {
288 5062450 : addLeader(other[i].first, other[i].second, 0, i);
289 : //if ((myDistances[i] > 0 && myDistances[i] > other.myDistances[i])
290 : // || (other.myDistances[i] < 0 && myDistances[i] < other.myDistances[i])) {
291 : // addLeader(other[i].first, other[i].second, 0, i);
292 : //}
293 : }
294 1092869 : }
295 :
296 :
297 : void
298 0 : MSLeaderDistanceInfo::clear() {
299 0 : MSLeaderInfo::clear();
300 0 : myDistances.assign(myVehicles.size(), std::numeric_limits<double>::max());
301 0 : }
302 :
303 :
304 : CLeaderDist
305 4541137508 : MSLeaderDistanceInfo::operator[](int sublane) const {
306 : assert(sublane >= 0);
307 : assert(sublane < (int)myVehicles.size());
308 4541137508 : return std::make_pair(myVehicles[sublane], myDistances[sublane]);
309 : }
310 :
311 :
312 : std::string
313 0 : MSLeaderDistanceInfo::toString() const {
314 0 : std::ostringstream oss;
315 : oss.setf(std::ios::fixed, std::ios::floatfield);
316 : oss << std::setprecision(2);
317 0 : for (int i = 0; i < (int)myVehicles.size(); ++i) {
318 0 : oss << Named::getIDSecure(myVehicles[i]) << ":";
319 0 : if (myVehicles[i] == 0) {
320 0 : oss << "inf";
321 : } else {
322 0 : oss << myDistances[i];
323 : }
324 0 : if (i < (int)myVehicles.size() - 1) {
325 0 : oss << ", ";
326 : }
327 : }
328 0 : oss << " free=" << myFreeSublanes;
329 0 : return oss.str();
330 0 : }
331 :
332 : void
333 457197 : MSLeaderDistanceInfo::fixOppositeGaps(bool isFollower) {
334 2258654 : for (int i = 0; i < (int)myVehicles.size(); ++i) {
335 1801457 : if (myVehicles[i] != nullptr) {
336 1312872 : if (myVehicles[i]->getLaneChangeModel().isOpposite()) {
337 616380 : myDistances[i] -= myVehicles[i]->getVehicleType().getLength();
338 696492 : } else if (isFollower && myDistances[i] > POSITION_EPS) {
339 : // can ignore oncoming followers once they are past
340 228020 : myVehicles[i] = nullptr;
341 228020 : myDistances[i] = -1;
342 : }
343 : }
344 : }
345 457197 : }
346 :
347 :
348 : void
349 88992 : MSLeaderDistanceInfo::patchGaps(double amount) {
350 447351 : for (int i = 0; i < (int)myVehicles.size(); ++i) {
351 358359 : if (myVehicles[i] != nullptr) {
352 321451 : myDistances[i] += amount;
353 : }
354 : }
355 88992 : }
356 :
357 : CLeaderDist
358 1373511 : MSLeaderDistanceInfo::getClosest() const {
359 : double minGap = -1;
360 : const MSVehicle* veh = nullptr;
361 1373511 : if (hasVehicles()) {
362 : minGap = std::numeric_limits<double>::max();
363 1721186 : for (int i = 0; i < (int)myVehicles.size(); ++i) {
364 1027933 : if (myVehicles[i] != nullptr && myDistances[i] < minGap) {
365 : minGap = myDistances[i];
366 : veh = myVehicles[i];
367 : }
368 : }
369 : }
370 1373511 : return std::make_pair(veh, minGap);
371 : }
372 :
373 :
374 : void
375 11713359 : MSLeaderDistanceInfo::moveSamePosTo(const MSVehicle* ego, MSLeaderDistanceInfo& other) {
376 11713359 : const double pos = ego->getPositionOnLane();
377 61219647 : for (int i = 0; i < (int)myVehicles.size(); ++i) {
378 49506288 : if (myVehicles[i] != nullptr && myDistances[i] < 0 && myVehicles[i]->getPositionOnLane() == pos
379 50657223 : && &myVehicles[i]->getLane()->getEdge() == &ego->getLane()->getEdge()) {
380 1150722 : other.myVehicles[i] = myVehicles[i];
381 1150722 : other.myDistances[i] = myDistances[i];
382 1150722 : myVehicles[i] = nullptr;
383 1150722 : myDistances[i] = -1;
384 : }
385 : }
386 11713359 : }
387 :
388 : // ===========================================================================
389 : // MSCriticalFollowerDistanceInfo member method definitions
390 : // ===========================================================================
391 :
392 :
393 265040372 : MSCriticalFollowerDistanceInfo::MSCriticalFollowerDistanceInfo(const double laneWidth, const MSVehicle* ego, const double latOffset, const bool haveOppositeLeaders) :
394 : MSLeaderDistanceInfo(laneWidth, ego, latOffset),
395 530080744 : myMissingGaps(myVehicles.size(), -std::numeric_limits<double>::max()),
396 265040372 : myHaveOppositeLeaders(haveOppositeLeaders)
397 265040372 : { }
398 :
399 :
400 265040372 : MSCriticalFollowerDistanceInfo::~MSCriticalFollowerDistanceInfo() { }
401 :
402 :
403 : int
404 2578565088 : MSCriticalFollowerDistanceInfo::addFollower(const MSVehicle* veh, const MSVehicle* ego, double gap, double latOffset, int sublane) {
405 2578565088 : if (veh == nullptr) {
406 3710379 : return myFreeSublanes;
407 : }
408 2574854709 : const double requiredGap = (myHaveOppositeLeaders ? 0
409 2569120410 : : veh->getCarFollowModel().getSecureGap(veh, ego, veh->getSpeed(), ego->getSpeed(), ego->getCarFollowModel().getMaxDecel()));
410 2574854709 : const double missingGap = requiredGap - gap;
411 : /*
412 : if (ego->getID() == "disabled" || gDebugFlag1) {
413 : std::cout << " addFollower veh=" << veh->getID()
414 : << " ego=" << ego->getID()
415 : << " gap=" << gap
416 : << " reqGap=" << requiredGap
417 : << " missingGap=" << missingGap
418 : << " latOffset=" << latOffset
419 : << " sublane=" << sublane
420 : << "\n";
421 : if (sublane > 0) {
422 : std::cout
423 : << " dists[s]=" << myDistances[sublane]
424 : << " gaps[s]=" << myMissingGaps[sublane]
425 : << "\n";
426 : } else {
427 : std::cout << toString() << "\n";
428 : }
429 : }
430 : */
431 2574854709 : if (myVehicles.size() == 1) {
432 : // speedup for the simple case
433 : sublane = 0;
434 : }
435 2574854709 : if (sublane >= 0 && sublane < (int)myVehicles.size()) {
436 : // sublane is already given
437 : // overlapping vehicles are stored preferably
438 : // among those vehicles with missing gap, closer ones are preferred
439 192210681 : if ((missingGap > myMissingGaps[sublane]
440 85487660 : || (missingGap > 0 && gap < myDistances[sublane])
441 85482515 : || (gap < 0 && myDistances[sublane] > 0))
442 106728169 : && !(gap > 0 && myDistances[sublane] < 0)
443 298911584 : && !(myMissingGaps[sublane] > 0 && myDistances[sublane] < gap)
444 : ) {
445 106673686 : if (myVehicles[sublane] == 0) {
446 93919220 : myFreeSublanes--;
447 : }
448 106673686 : myVehicles[sublane] = veh;
449 106673686 : myDistances[sublane] = gap;
450 106673686 : myMissingGaps[sublane] = missingGap;
451 106673686 : myHasVehicles = true;
452 : }
453 192210681 : return myFreeSublanes;
454 : }
455 : int rightmost, leftmost;
456 2382644028 : getSubLanes(veh, latOffset, rightmost, leftmost);
457 11239432533 : for (int sublaneIdx = rightmost; sublaneIdx <= leftmost; ++sublaneIdx) {
458 1129025 : if ((egoRightMost < 0 || (egoRightMost <= sublaneIdx && sublaneIdx <= egoLeftMost))
459 : // overlapping vehicles are stored preferably
460 : // among those vehicles with missing gap, closer ones are preferred
461 8856725624 : && (missingGap > myMissingGaps[sublaneIdx]
462 38975245 : || (missingGap > 0 && gap < myDistances[sublaneIdx])
463 37862609 : || (gap < 0 && myDistances[sublaneIdx] > 0))
464 8818863027 : && !(gap > 0 && myDistances[sublaneIdx] < 0)
465 17675624042 : && !(myMissingGaps[sublaneIdx] > 0 && myDistances[sublaneIdx] < gap)
466 : ) {
467 8818821306 : if (myVehicles[sublaneIdx] == 0) {
468 664323522 : myFreeSublanes--;
469 : }
470 8818821306 : myVehicles[sublaneIdx] = veh;
471 8818821306 : myDistances[sublaneIdx] = gap;
472 8818821306 : myMissingGaps[sublaneIdx] = missingGap;
473 8818821306 : myHasVehicles = true;
474 : }
475 : }
476 2382644028 : return myFreeSublanes;
477 : }
478 :
479 :
480 : void
481 0 : MSCriticalFollowerDistanceInfo::clear() {
482 0 : MSLeaderDistanceInfo::clear();
483 0 : myMissingGaps.assign(myVehicles.size(), -std::numeric_limits<double>::max());
484 0 : }
485 :
486 :
487 : std::string
488 0 : MSCriticalFollowerDistanceInfo::toString() const {
489 0 : std::ostringstream oss;
490 : oss.setf(std::ios::fixed, std::ios::floatfield);
491 : oss << std::setprecision(2);
492 0 : for (int i = 0; i < (int)myVehicles.size(); ++i) {
493 0 : oss << Named::getIDSecure(myVehicles[i]) << ":";
494 0 : if (myVehicles[i] == 0) {
495 0 : oss << "inf:-inf";
496 : } else {
497 0 : oss << myDistances[i] << ":" << myMissingGaps[i];
498 : }
499 0 : if (i < (int)myVehicles.size() - 1) {
500 0 : oss << ", ";
501 : }
502 : }
503 0 : oss << " free=" << myFreeSublanes;
504 0 : return oss.str();
505 0 : }
506 :
507 :
508 : /****************************************************************************/
|
