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