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