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