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