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