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