1 package org.opentrafficsim.road.gtu.lane.tactical.util.lmrs;
2
3 import static org.opentrafficsim.road.gtu.lane.tactical.util.lmrs.Synchronization.canBeAhead;
4 import static org.opentrafficsim.road.gtu.lane.tactical.util.lmrs.Synchronization.gentleUrgency;
5 import static org.opentrafficsim.road.gtu.lane.tactical.util.lmrs.Synchronization.getFollower;
6 import static org.opentrafficsim.road.gtu.lane.tactical.util.lmrs.Synchronization.getMergeDistance;
7 import static org.opentrafficsim.road.gtu.lane.tactical.util.lmrs.Synchronization.headwayWithLcSpace;
8 import static org.opentrafficsim.road.gtu.lane.tactical.util.lmrs.Synchronization.removeAllUpstreamOfConflicts;
9 import static org.opentrafficsim.road.gtu.lane.tactical.util.lmrs.Synchronization.requiredBufferSpace;
10 import static org.opentrafficsim.road.gtu.lane.tactical.util.lmrs.Synchronization.stopForEnd;
11 import static org.opentrafficsim.road.gtu.lane.tactical.util.lmrs.Synchronization.tagAlongAcceleration;
12
13 import java.util.LinkedHashMap;
14 import java.util.Map;
15 import java.util.SortedSet;
16
17 import org.djunits.unit.AccelerationUnit;
18 import org.djunits.value.vdouble.scalar.Acceleration;
19 import org.djunits.value.vdouble.scalar.Duration;
20 import org.djunits.value.vdouble.scalar.Length;
21 import org.djunits.value.vdouble.scalar.Speed;
22 import org.djutils.exceptions.Try;
23 import org.opentrafficsim.base.parameters.ParameterException;
24 import org.opentrafficsim.base.parameters.ParameterTypes;
25 import org.opentrafficsim.base.parameters.Parameters;
26 import org.opentrafficsim.core.gtu.GTUException;
27 import org.opentrafficsim.core.gtu.perception.EgoPerception;
28 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
29 import org.opentrafficsim.core.network.LateralDirectionality;
30 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
31 import org.opentrafficsim.road.gtu.lane.perception.InfrastructureLaneChangeInfo;
32 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
33 import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable;
34 import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
35 import org.opentrafficsim.road.gtu.lane.perception.SortedSetPerceptionIterable;
36 import org.opentrafficsim.road.gtu.lane.perception.categories.InfrastructurePerception;
37 import org.opentrafficsim.road.gtu.lane.perception.categories.IntersectionPerception;
38 import org.opentrafficsim.road.gtu.lane.perception.categories.neighbors.NeighborsPerception;
39 import org.opentrafficsim.road.gtu.lane.perception.headway.Headway;
40 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayConflict;
41 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
42 import org.opentrafficsim.road.gtu.lane.plan.operational.LaneChange;
43 import org.opentrafficsim.road.gtu.lane.tactical.following.CarFollowingModel;
44 import org.opentrafficsim.road.gtu.lane.tactical.util.CarFollowingUtil;
45 import org.opentrafficsim.road.network.OTSRoadNetwork;
46 import org.opentrafficsim.road.network.speed.SpeedLimitInfo;
47
48
49
50
51
52
53
54
55
56
57
58
59 public interface Synchronization extends LmrsParameters
60 {
61
62
63 Synchronizationu/lane/tactical/util/lmrs/Synchronization.html#Synchronization">Synchronization DEADEND = new Synchronization()
64 {
65 @Override
66 public Acceleration synchronize(final LanePerception perception, final Parameters params, final SpeedLimitInfo sli,
67 final CarFollowingModel cfm, final double desire, final LateralDirectionality lat, final LmrsData lmrsData,
68 final LaneChange laneChange, final LateralDirectionality initiatedLaneChange)
69 throws ParameterException, OperationalPlanException
70 {
71 Acceleration a = Acceleration.POSITIVE_INFINITY;
72
73 Length remainingDist = null;
74 int nCur = 0;
75 for (InfrastructureLaneChangeInfo ili : perception.getPerceptionCategory(InfrastructurePerception.class)
76 .getInfrastructureLaneChangeInfo(RelativeLane.CURRENT))
77 {
78 if (remainingDist == null || remainingDist.gt(ili.getRemainingDistance()))
79 {
80 remainingDist = ili.getRemainingDistance();
81 nCur = ili.getRequiredNumberOfLaneChanges();
82 }
83 }
84 if (remainingDist != null)
85 {
86 Speed speed = perception.getPerceptionCategory(EgoPerception.class).getSpeed();
87 Acceleration bCrit = params.getParameter(ParameterTypes.BCRIT);
88 remainingDist = remainingDist.minus(params.getParameter(ParameterTypes.S0));
89 if (!laneChange.isChangingLane() && initiatedLaneChange.isNone())
90 {
91
92
93
94
95
96 remainingDist = remainingDist.minus(laneChange.getMinimumLaneChangeDistance().times(nCur));
97 }
98 if (remainingDist.le0())
99 {
100 if (speed.gt0())
101 {
102 a = Acceleration.min(a, bCrit.neg());
103 }
104 else
105 {
106 a = Acceleration.min(a, Acceleration.ZERO);
107 }
108 }
109 else
110 {
111 Acceleration bMin = new Acceleration(.5 * speed.si * speed.si / remainingDist.si, AccelerationUnit.SI);
112 if (bMin.ge(bCrit))
113 {
114 a = Acceleration.min(a, bMin.neg());
115 }
116 }
117 }
118 return a;
119 }
120
121 };
122
123
124 Synchronizationu/lane/tactical/util/lmrs/Synchronization.html#Synchronization">Synchronization PASSIVE = new Synchronization()
125 {
126 @Override
127 public Acceleration synchronize(final LanePerception perception, final Parameters params, final SpeedLimitInfo sli,
128 final CarFollowingModel cfm, final double desire, final LateralDirectionality lat, final LmrsData lmrsData,
129 final LaneChange laneChange, final LateralDirectionality initiatedLaneChange)
130 throws ParameterException, OperationalPlanException
131 {
132 Acceleration a = Acceleration.POSITIVE_INFINITY;
133 double dCoop = params.getParameter(DCOOP);
134 RelativeLanelane/perception/RelativeLane.html#RelativeLane">RelativeLane relativeLane = new RelativeLane(lat, 1);
135
136 PerceptionCollectable<HeadwayGTU, LaneBasedGTU> set = removeAllUpstreamOfConflicts(removeAllUpstreamOfConflicts(
137 perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(relativeLane), perception,
138 relativeLane), perception, RelativeLane.CURRENT);
139 HeadwayGTU leader = null;
140 if (set != null)
141 {
142 if (desire >= dCoop && !set.isEmpty())
143 {
144 leader = set.first();
145 }
146 else
147 {
148 for (HeadwayGTU gtu : set)
149 {
150 if (gtu.getSpeed().gt0())
151 {
152 leader = gtu;
153 break;
154 }
155 }
156 }
157 }
158 Speed ownSpeed = perception.getPerceptionCategory(EgoPerception.class).getSpeed();
159 if (leader != null)
160 {
161 Length headway = headwayWithLcSpace(leader, params, laneChange);
162 Acceleration aSingle =
163 LmrsUtil.singleAcceleration(headway, ownSpeed, leader.getSpeed(), desire, params, sli, cfm);
164 a = Acceleration.min(a, aSingle);
165 a = gentleUrgency(a, desire, params);
166
167 a = Acceleration.min(a, DEADEND.synchronize(perception, params, sli, cfm, desire, lat, lmrsData, laneChange,
168 initiatedLaneChange));
169 }
170
171 PerceptionCollectable<HeadwayGTU, LaneBasedGTU> leaders =
172 perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(RelativeLane.CURRENT);
173 if (!leaders.isEmpty() && leaders.first().getSpeed().lt(params.getParameter(ParameterTypes.VCONG)))
174 {
175 Length headway = leaders.first().getDistance().minus(laneChange.getMinimumLaneChangeDistance());
176 Acceleration aSingle =
177 LmrsUtil.singleAcceleration(headway, ownSpeed, leaders.first().getSpeed(), desire, params, sli, cfm);
178 aSingle = gentleUrgency(aSingle, desire, params);
179 a = Acceleration.min(a, aSingle);
180 }
181
182
183 Length xMerge =
184 getMergeDistance(perception, lat).minus(perception.getPerceptionCategory(EgoPerception.class).getLength());
185 if (xMerge.gt0())
186 {
187 Acceleration aMerge = LmrsUtil.singleAcceleration(xMerge, ownSpeed, Speed.ZERO, desire, params, sli, cfm);
188 a = Acceleration.max(a, aMerge);
189 }
190 return a;
191 }
192
193 };
194
195
196
197
198
199
200
201 Synchronizationlane/tactical/util/lmrs/Synchronization.html#Synchronization">Synchronization ALIGN_GAP = new Synchronization()
202 {
203 @Override
204 public Acceleration synchronize(final LanePerception perception, final Parameters params, final SpeedLimitInfo sli,
205 final CarFollowingModel cfm, final double desire, final LateralDirectionality lat, final LmrsData lmrsData,
206 final LaneChange laneChange, final LateralDirectionality initiatedLaneChange)
207 throws ParameterException, OperationalPlanException
208 {
209 Acceleration a = Acceleration.POSITIVE_INFINITY;
210 EgoPerception<?, ?> ego = perception.getPerceptionCategory(EgoPerception.class);
211 Speed ownSpeed = ego.getSpeed();
212 RelativeLanelane/perception/RelativeLane.html#RelativeLane">RelativeLane relativeLane = new RelativeLane(lat, 1);
213 PerceptionCollectable<HeadwayGTU, LaneBasedGTU> leaders =
214 perception.getPerceptionCategory(NeighborsPerception.class).getLeaders(relativeLane);
215 if (!leaders.isEmpty())
216 {
217 HeadwayGTU leader = leaders.first();
218 Length gap = leader.getDistance();
219 LmrsUtil.setDesiredHeadway(params, desire);
220 PerceptionCollectable<HeadwayGTU, LaneBasedGTU> followers =
221 perception.getPerceptionCategory(NeighborsPerception.class).getFollowers(relativeLane);
222 if (!followers.isEmpty())
223 {
224 HeadwayGTU follower = followers.first();
225 Length netGap = leader.getDistance().plus(follower.getDistance()).times(0.5);
226 gap = Length.max(gap, leader.getDistance().minus(netGap).plus(cfm.desiredHeadway(params, ownSpeed)));
227 }
228 a = CarFollowingUtil.followSingleLeader(cfm, params, ownSpeed, sli, gap, leader.getSpeed());
229 LmrsUtil.resetDesiredHeadway(params);
230
231 a = gentleUrgency(a, desire, params);
232
233
234
235 }
236
237 a = Acceleration.min(a,
238 DEADEND.synchronize(perception, params, sli, cfm, desire, lat, lmrsData, laneChange, initiatedLaneChange));
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256 Length xMerge = getMergeDistance(perception, lat);
257 if (xMerge.gt0())
258 {
259 Acceleration aMerge = LmrsUtil.singleAcceleration(xMerge, ownSpeed, Speed.ZERO, desire, params, sli, cfm);
260 a = Acceleration.max(a, aMerge);
261 }
262 return a;
263 }
264 };
265
266
267 Synchronizationtactical/util/lmrs/Synchronization.html#Synchronization">Synchronization PASSIVE_MOVING = new Synchronization()
268 {
269 @Override
270 public Acceleration synchronize(final LanePerception perception, final Parameters params, final SpeedLimitInfo sli,
271 final CarFollowingModel cfm, final double desire, final LateralDirectionality lat, final LmrsData lmrsData,
272 final LaneChange laneChange, final LateralDirectionality initiatedLaneChange)
273 throws ParameterException, OperationalPlanException
274 {
275 double dCoop = params.getParameter(DCOOP);
276 Speed ownSpeed = perception.getPerceptionCategory(EgoPerception.class).getSpeed();
277 if (desire < dCoop && ownSpeed.si < params.getParameter(ParameterTypes.LOOKAHEAD).si
278 / params.getParameter(ParameterTypes.T0).si)
279 {
280 return Acceleration.POSITIVE_INFINITY;
281 }
282 return PASSIVE.synchronize(perception, params, sli, cfm, desire, lat, lmrsData, laneChange, initiatedLaneChange);
283 }
284 };
285
286
287 Synchronizationtu/lane/tactical/util/lmrs/Synchronization.html#Synchronization">Synchronization ACTIVE = new Synchronization()
288 {
289 @Override
290 public Acceleration synchronize(final LanePerception perception, final Parameters params, final SpeedLimitInfo sli,
291 final CarFollowingModel cfm, final double desire, final LateralDirectionality lat, final LmrsData lmrsData,
292 final LaneChange laneChange, final LateralDirectionality initiatedLaneChange)
293 throws ParameterException, OperationalPlanException
294 {
295
296 Acceleration b = params.getParameter(ParameterTypes.B);
297 Duration tMin = params.getParameter(ParameterTypes.TMIN);
298 Duration tMax = params.getParameter(ParameterTypes.TMAX);
299 Speed vCong = params.getParameter(ParameterTypes.VCONG);
300 Length x0 = params.getParameter(ParameterTypes.LOOKAHEAD);
301 Duration t0 = params.getParameter(ParameterTypes.T0);
302 Duration lc = params.getParameter(ParameterTypes.LCDUR);
303 Speed tagSpeed = x0.divide(t0);
304 double dCoop = params.getParameter(DCOOP);
305 EgoPerception<?, ?> ego = perception.getPerceptionCategory(EgoPerception.class);
306 Speed ownSpeed = ego.getSpeed();
307 Length ownLength = ego.getLength();
308 Length dx;
309 try
310 {
311 dx = perception.getGtu().getFront().getDx();
312 }
313 catch (GTUException exception)
314 {
315 throw new OperationalPlanException(exception);
316 }
317
318
319 InfrastructurePerception infra = perception.getPerceptionCategory(InfrastructurePerception.class);
320 SortedSet<InfrastructureLaneChangeInfo> info = infra.getInfrastructureLaneChangeInfo(RelativeLane.CURRENT);
321
322
323 Length xMerge = getMergeDistance(perception, lat);
324 int nCur = 0;
325 Length xCur = Length.POSITIVE_INFINITY;
326 for (InfrastructureLaneChangeInfo lcInfo : info)
327 {
328 int nCurTmp = lcInfo.getRequiredNumberOfLaneChanges();
329
330 Length xCurTmp = lcInfo.getRemainingDistance().minus(ownLength.times(2.0 * nCurTmp)).minus(dx);
331 if (xCurTmp.lt(xCur))
332 {
333 nCur = nCurTmp;
334 xCur = xCurTmp;
335 }
336 }
337
338
339
340 Length xMergeSync = xCur.minus(Length.instantiateSI(.5 * ownSpeed.si * ownSpeed.si / b.si));
341 xMergeSync = Length.min(xMerge, xMergeSync);
342
343
344 NeighborsPerception neighbors = perception.getPerceptionCategory(NeighborsPerception.class);
345 RelativeLaneoad/gtu/lane/perception/RelativeLane.html#RelativeLane">RelativeLane lane = new RelativeLane(lat, 1);
346 PerceptionCollectable<HeadwayGTU, LaneBasedGTU> leaders =
347 removeAllUpstreamOfConflicts(removeAllUpstreamOfConflicts(neighbors.getLeaders(lane), perception, lane),
348 perception, RelativeLane.CURRENT);
349 HeadwayGTU syncVehicle = lmrsData.getSyncVehicle(leaders);
350 if (syncVehicle != null && ((syncVehicle.getSpeed().lt(vCong) && syncVehicle.getDistance().lt(xMergeSync))
351 || syncVehicle.getDistance().gt(xCur)))
352 {
353 syncVehicle = null;
354 }
355
356
357 if (leaders != null && syncVehicle == null)
358 {
359 Length maxDistance = Length.min(x0, xCur);
360 for (HeadwayGTU leader : leaders)
361 {
362 if (leader.getDistance().lt(maxDistance))
363 {
364 if ((leader.getDistance().gt(xMergeSync) || leader.getSpeed().gt(vCong))
365 && tagAlongAcceleration(leader, ownSpeed, ownLength, tagSpeed, desire, params, sli, cfm)
366 .gt(b.neg()))
367 {
368 syncVehicle = leader;
369 break;
370 }
371 }
372 else
373 {
374 break;
375 }
376 }
377 }
378
379
380 HeadwayGTU up;
381 PerceptionCollectable<HeadwayGTU, LaneBasedGTU> followers =
382 removeAllUpstreamOfConflicts(removeAllUpstreamOfConflicts(neighbors.getFollowers(lane), perception, lane),
383 perception, RelativeLane.CURRENT);
384 HeadwayGTU follower = followers == null || followers.isEmpty() ? null
385 : followers.first().moved(
386 followers.first().getDistance().plus(ownLength).plus(followers.first().getLength()).neg(),
387 followers.first().getSpeed(), followers.first().getAcceleration());
388 boolean upOk;
389 if (syncVehicle == null)
390 {
391 up = null;
392 upOk = false;
393 }
394 else
395 {
396 up = getFollower(syncVehicle, leaders, follower, ownLength);
397 upOk = up == null ? false
398 : tagAlongAcceleration(up, ownSpeed, ownLength, tagSpeed, desire, params, sli, cfm).gt(b.neg());
399 }
400 while (syncVehicle != null
401 && up != null && (upOk || (!canBeAhead(up, xCur, nCur, ownSpeed, ownLength, tagSpeed, dCoop, b, tMin, tMax,
402 x0, t0, lc, desire) && desire > dCoop))
403 && (up.getDistance().gt(xMergeSync) || up.getSpeed().gt(vCong)))
404 {
405 if (up.equals(follower))
406 {
407
408 syncVehicle = null;
409 up = null;
410 break;
411 }
412 syncVehicle = up;
413 up = getFollower(syncVehicle, leaders, follower, ownLength);
414 upOk = up == null ? false
415 : tagAlongAcceleration(up, ownSpeed, ownLength, tagSpeed, desire, params, sli, cfm).gt(b.neg());
416 }
417 lmrsData.setSyncVehicle(syncVehicle);
418
419
420 Acceleration a = Acceleration.POSITIVE_INFINITY;
421 if (syncVehicle != null)
422 {
423 a = gentleUrgency(tagAlongAcceleration(syncVehicle, ownSpeed, ownLength, tagSpeed, desire, params, sli, cfm),
424 desire, params);
425 }
426 else if (nCur > 0 && (follower != null || (leaders != null && !leaders.isEmpty())))
427 {
428
429 if (follower != null && !canBeAhead(follower, xCur, nCur, ownSpeed, ownLength, tagSpeed, dCoop, b, tMin, tMax,
430 x0, t0, lc, desire))
431 {
432
433 double c = requiredBufferSpace(ownSpeed, nCur, x0, t0, lc, dCoop).si;
434 double t = (xCur.si - follower.getDistance().si - c) / follower.getSpeed().si;
435 double xGap = ownSpeed.si * (tMin.si + desire * (tMax.si - tMin.si));
436 Acceleration acc = Acceleration.instantiateSI(2 * (xCur.si - c - ownSpeed.si * t - xGap) / (t * t));
437 if (follower.getSpeed().eq0() || acc.si < -ownSpeed.si / t || t < 0)
438 {
439
440
441 a = stopForEnd(xCur, xMerge, params, ownSpeed, cfm, sli);
442 }
443 else
444 {
445 a = gentleUrgency(acc, desire, params);
446 }
447 }
448 else if (!LmrsUtil.acceptLaneChange(perception, params, sli, cfm, desire, ownSpeed, Acceleration.ZERO, lat,
449 lmrsData.getGapAcceptance(), laneChange))
450 {
451 a = stopForEnd(xCur, xMerge, params, ownSpeed, cfm, sli);
452
453 if (leaders != null && !leaders.isEmpty())
454 {
455 double c = requiredBufferSpace(ownSpeed, nCur, x0, t0, lc, dCoop).si;
456 double t = (xCur.si - leaders.first().getDistance().si - c) / leaders.first().getSpeed().si;
457 double xGap = ownSpeed.si * (tMin.si + desire * (tMax.si - tMin.si));
458 Acceleration acc = Acceleration.instantiateSI(2 * (xCur.si - c - ownSpeed.si * t - xGap) / (t * t));
459 if (!(leaders.first().getSpeed().eq0() || acc.si < -ownSpeed.si / t || t < 0))
460 {
461 a = Acceleration.max(a, acc);
462 }
463 }
464 }
465 }
466
467
468 if (nCur > 1)
469 {
470 if (xMerge.gt0())
471 {
472
473 Speed vMerge = xCur.lt(xMerge) ? Speed.ZERO
474 : xCur.minus(xMerge).divide(t0.times((1 - dCoop) * (nCur - 1)).plus(lc));
475 vMerge = Speed.max(vMerge, x0.divide(t0));
476 a = Acceleration.min(a, CarFollowingUtil.approachTargetSpeed(cfm, params, ownSpeed, sli, xMerge, vMerge));
477 }
478 else
479 {
480
481 Length c = requiredBufferSpace(ownSpeed, nCur, x0, t0, lc, dCoop);
482 if (xCur.lt(c))
483 {
484 a = Acceleration.min(a, b.neg());
485 }
486 }
487 }
488 return a;
489 }
490 };
491
492
493
494
495
496
497
498
499 public static Length getMergeDistance(final LanePerception perception, final LateralDirectionality lat)
500 throws OperationalPlanException
501 {
502 InfrastructurePerception infra = perception.getPerceptionCategory(InfrastructurePerception.class);
503 Length dx = Try.assign(() -> perception.getGtu().getFront().getDx(), "Could not obtain GTU.");
504 Length xMergeRef = infra.getLegalLaneChangePossibility(RelativeLane.CURRENT, lat);
505 if (xMergeRef.gt0() && xMergeRef.lt(dx))
506 {
507 return Length.ZERO;
508 }
509 Length xMerge = xMergeRef.minus(dx);
510 return xMerge.lt0() ? xMerge.neg() : Length.ZERO;
511 }
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528 Acceleration synchronize(LanePerception perception, Parameters params, SpeedLimitInfo sli, CarFollowingModel cfm,
529 double desire, LateralDirectionality lat, LmrsData lmrsData, LaneChange laneChange,
530 LateralDirectionality initiatedLaneChange) throws ParameterException, OperationalPlanException;
531
532
533
534
535
536
537
538
539
540 static Length headwayWithLcSpace(final Headway headway, final Parameters parameters, final LaneChange laneChange)
541 throws ParameterException
542 {
543 if (headway.getSpeed().gt(parameters.getParameter(ParameterTypes.VCONG)))
544 {
545 return headway.getDistance();
546 }
547 return headway.getDistance().minus(laneChange.getMinimumLaneChangeDistance());
548 }
549
550
551
552
553
554
555
556
557
558 static PerceptionCollectable<HeadwayGTU, LaneBasedGTU> removeAllUpstreamOfConflicts(
559 final PerceptionCollectable<HeadwayGTU, LaneBasedGTU> set, final LanePerception perception,
560 final RelativeLane relativeLane) throws OperationalPlanException
561 {
562
563
564
565
566
567 SortedSetPerceptionIterable<HeadwayGTU> out;
568 try
569 {
570 out = new SortedSetPerceptionIterable<HeadwayGTU>(
571 (OTSRoadNetwork) perception.getGtu().getReferencePosition().getLane().getParentLink().getNetwork());
572 }
573 catch (GTUException exception)
574 {
575 throw new OperationalPlanException(exception);
576 }
577 if (set == null)
578 {
579 return out;
580 }
581 IntersectionPerception intersection = perception.getPerceptionCategoryOrNull(IntersectionPerception.class);
582 if (intersection == null)
583 {
584 return set;
585 }
586 Map<String, HeadwayGTU> map = new LinkedHashMap<>();
587 for (HeadwayGTU gtu : set)
588 {
589 map.put(gtu.getId(), gtu);
590 }
591 Iterable<HeadwayConflict> conflicts = intersection.getConflicts(relativeLane);
592 if (conflicts != null)
593 {
594 for (HeadwayConflict conflict : conflicts)
595 {
596 if (conflict.isCrossing() || conflict.isMerge())
597 {
598 for (HeadwayGTU conflictGtu : conflict.getUpstreamConflictingGTUs())
599 {
600 if (map.containsKey(conflictGtu.getId()))
601 {
602 map.remove(conflictGtu.getId());
603 }
604 }
605 }
606 }
607 }
608 out.addAll(map.values());
609 return out;
610
611 }
612
613
614
615
616
617
618
619
620
621
622 static Acceleration gentleUrgency(final Acceleration a, final double desire, final Parameters params)
623 throws ParameterException
624 {
625 Acceleration b = params.getParameter(ParameterTypes.B);
626 if (a.si > -b.si)
627 {
628 return a;
629 }
630 double dCoop = params.getParameter(DCOOP);
631 if (desire < dCoop)
632 {
633 return b.neg();
634 }
635 Acceleration bCrit = params.getParameter(ParameterTypes.BCRIT);
636 double f = (desire - dCoop) / (1.0 - dCoop);
637 Acceleration lim = Acceleration.interpolate(b.neg(), bCrit.neg(), f);
638 return Acceleration.max(a, lim);
639 }
640
641
642
643
644
645
646
647
648
649 static HeadwayGTUtu/lane/perception/headway/HeadwayGTU.html#HeadwayGTU">HeadwayGTU getFollower(final HeadwayGTU gtu, final PerceptionCollectable<HeadwayGTU, LaneBasedGTU> leaders,
650 final HeadwayGTU follower, final Length ownLength)
651 {
652 HeadwayGTU last = null;
653 for (HeadwayGTU leader : leaders)
654 {
655 if (leader.equals(gtu))
656 {
657 return last == null ? follower : last;
658 }
659 last = leader;
660 }
661 return null;
662 }
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677 @SuppressWarnings("checkstyle:parameternumber")
678 static Acceleration tagAlongAcceleration(final HeadwayGTU leader, final Speed followerSpeed, final Length followerLength,
679 final Speed tagSpeed, final double desire, final Parameters params, final SpeedLimitInfo sli,
680 final CarFollowingModel cfm) throws ParameterException
681 {
682 double dCoop = params.getParameter(DCOOP);
683 double tagV = followerSpeed.lt(tagSpeed) ? 1.0 - followerSpeed.si / tagSpeed.si : 0.0;
684 double tagD = desire <= dCoop ? 1.0 : 1.0 - (desire - dCoop) / (1.0 - dCoop);
685 double tagExtent = tagV < tagD ? tagV : tagD;
686
687
688
689
690
691
692
693
694
695
696
697 Length headwayAdjustment = params.getParameter(ParameterTypes.S0)
698 .plus(Length.min(followerLength, leader.getLength()).times(0.5)).times(tagExtent);
699 Acceleration a = LmrsUtil.singleAcceleration(leader.getDistance().plus(headwayAdjustment), followerSpeed,
700 leader.getSpeed(), desire, params, sli, cfm);
701 return a;
702 }
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723 static boolean canBeAhead(final HeadwayGTU adjacentVehicle, final Length xCur, final int nCur, final Speed ownSpeed,
724 final Length ownLength, final Speed tagSpeed, final double dCoop, final Acceleration b, final Duration tMin,
725 final Duration tMax, final Length x0, final Duration t0, final Duration lc, final double desire)
726 throws ParameterException
727 {
728
729
730 boolean tmp = LmrsUtil
731 .singleAcceleration(adjacentVehicle.getDistance().neg().minus(adjacentVehicle.getLength()).minus(ownLength),
732 adjacentVehicle.getSpeed(), ownSpeed, desire, adjacentVehicle.getParameters(),
733 adjacentVehicle.getSpeedLimitInfo(), adjacentVehicle.getCarFollowingModel())
734 .gt(b.neg());
735 if (adjacentVehicle.getDistance().lt(ownLength.neg())
736 && ((desire > dCoop && tmp) || (ownSpeed.lt(tagSpeed) && adjacentVehicle.getSpeed().lt(tagSpeed))))
737 {
738 return true;
739 }
740
741
742
743
744
745
746
747
748
749
750
751
752
753 Length c = requiredBufferSpace(ownSpeed, nCur, x0, t0, lc, dCoop);
754 double t = (xCur.si - c.si) / ownSpeed.si;
755 double xGap = adjacentVehicle.getSpeed().si * (tMin.si + desire * (tMax.si - tMin.si));
756 return 0.0 < t && t < (xCur.si - adjacentVehicle.getDistance().si - ownLength.si - adjacentVehicle.getLength().si - c.si
757 - xGap) / adjacentVehicle.getSpeed().si;
758 }
759
760
761
762
763
764
765
766
767
768
769
770 static Length requiredBufferSpace(final Speed speed, final int nCur, final Length x0, final Duration t0, final Duration lc,
771 final double dCoop)
772 {
773 Length xCrit = speed.times(t0);
774 xCrit = Length.max(xCrit, x0);
775 return speed.times(lc).plus(xCrit.times((nCur - 1.0) * (1.0 - dCoop)));
776 }
777
778
779
780
781
782
783
784
785
786
787
788
789 static Acceleration stopForEnd(final Length xCur, final Length xMerge, final Parameters params, final Speed ownSpeed,
790 final CarFollowingModel cfm, final SpeedLimitInfo sli) throws ParameterException
791 {
792 if (xCur.lt0())
793 {
794
795 return Acceleration.max(params.getParameter(ParameterTypes.BCRIT).neg(),
796 CarFollowingUtil.stop(cfm, params, ownSpeed, sli, xMerge));
797 }
798 LmrsUtil.setDesiredHeadway(params, 1.0);
799 Acceleration a = CarFollowingUtil.stop(cfm, params, ownSpeed, sli, xCur);
800 if (a.lt0())
801 {
802
803 a = Acceleration.min(a, params.getParameter(ParameterTypes.B).neg());
804
805 if (xMerge.gt0())
806 {
807 a = Acceleration.max(a, CarFollowingUtil.stop(cfm, params, ownSpeed, sli, xMerge));
808 }
809 }
810 else
811 {
812 a = Acceleration.POSITIVE_INFINITY;
813 }
814 LmrsUtil.resetDesiredHeadway(params);
815 return a;
816 }
817
818
819
820
821
822
823
824 static HeadwayGTUane/perception/headway/HeadwayGTU.html#HeadwayGTU">HeadwayGTU getTargetLeader(final HeadwayGTU gtu, final SortedSet<HeadwayGTU> leaders)
825 {
826 for (HeadwayGTU leader : leaders)
827 {
828 if (leader.getDistance().gt(gtu.getDistance()))
829 {
830 return leader;
831 }
832 }
833 return null;
834 }
835
836 }