1 package org.opentrafficsim.road.gtu.lane.tactical;
2
3 import java.io.Serializable;
4 import java.util.ArrayList;
5 import java.util.HashSet;
6 import java.util.Iterator;
7 import java.util.List;
8 import java.util.Map;
9 import java.util.Set;
10
11 import org.djunits.value.vdouble.scalar.Length;
12 import org.djunits.value.vdouble.scalar.Time;
13 import org.opentrafficsim.core.geometry.OTSGeometryException;
14 import org.opentrafficsim.core.geometry.OTSLine3D;
15 import org.opentrafficsim.core.gtu.GTUDirectionality;
16 import org.opentrafficsim.core.gtu.GTUException;
17 import org.opentrafficsim.core.gtu.behavioralcharacteristics.BehavioralCharacteristics;
18 import org.opentrafficsim.core.gtu.behavioralcharacteristics.ParameterException;
19 import org.opentrafficsim.core.gtu.behavioralcharacteristics.ParameterTypes;
20 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
21 import org.opentrafficsim.core.network.LateralDirectionality;
22 import org.opentrafficsim.core.network.Link;
23 import org.opentrafficsim.core.network.LinkDirection;
24 import org.opentrafficsim.core.network.NetworkException;
25 import org.opentrafficsim.core.network.Node;
26 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
27 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
28 import org.opentrafficsim.road.gtu.lane.plan.operational.LaneBasedOperationalPlan;
29 import org.opentrafficsim.road.gtu.lane.plan.operational.LaneOperationalPlanBuilder;
30 import org.opentrafficsim.road.gtu.lane.plan.operational.LaneOperationalPlanBuilder.LaneChange;
31 import org.opentrafficsim.road.gtu.lane.plan.operational.SimpleOperationalPlan;
32 import org.opentrafficsim.road.gtu.lane.tactical.following.CarFollowingModel;
33 import org.opentrafficsim.road.network.lane.CrossSectionElement;
34 import org.opentrafficsim.road.network.lane.CrossSectionLink;
35 import org.opentrafficsim.road.network.lane.DirectedLanePosition;
36 import org.opentrafficsim.road.network.lane.Lane;
37 import org.opentrafficsim.road.network.lane.LaneDirection;
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52 public abstract class AbstractLaneBasedTacticalPlanner implements LaneBasedTacticalPlanner, Serializable
53 {
54
55 private static final long serialVersionUID = 20151125L;
56
57
58 private CarFollowingModel carFollowingModel;
59
60
61 private final LanePerception lanePerception;
62
63
64 private final LaneBasedGTU gtu;
65
66
67
68
69
70
71
72 public AbstractLaneBasedTacticalPlanner(final CarFollowingModel carFollowingModel, final LaneBasedGTU gtu,
73 final LanePerception lanePerception)
74 {
75 setCarFollowingModel(carFollowingModel);
76 this.gtu = gtu;
77 this.lanePerception = lanePerception;
78 }
79
80
81 @Override
82 public final LaneBasedGTU getGtu()
83 {
84 return this.gtu;
85 }
86
87
88
89
90
91
92
93
94
95
96 public static LanePathInfo buildLanePathInfo(final LaneBasedGTU gtu, final Length maxHeadway)
97 throws GTUException, NetworkException
98 {
99 DirectedLanePosition dlp = gtu.getReferencePosition();
100 return buildLanePathInfo(gtu, maxHeadway, dlp.getLane(), dlp.getPosition(), dlp.getGtuDirection());
101 }
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117 public static LanePathInfo buildLanePathInfo(final LaneBasedGTU gtu, final Length maxHeadway, final Lane startLane,
118 final Length position, final GTUDirectionality startDirectionality) throws GTUException, NetworkException
119 {
120 List<LaneDirection> laneListForward = new ArrayList<>();
121 Lane lane = startLane;
122 GTUDirectionality lastGtuDir = startDirectionality;
123 Length startPosition = position;
124 Lane lastLane = lane;
125 laneListForward.add(new LaneDirection(lastLane, lastGtuDir));
126 Length distanceToEndOfLane;
127 OTSLine3D path;
128 try
129 {
130 if (lastGtuDir.equals(GTUDirectionality.DIR_PLUS))
131 {
132 distanceToEndOfLane = lane.getLength().minus(position);
133 path = lane.getCenterLine().extract(position, lane.getLength());
134 }
135 else
136 {
137 distanceToEndOfLane = position;
138 path = lane.getCenterLine().extract(Length.ZERO, position).reverse();
139 }
140 }
141 catch (OTSGeometryException exception)
142 {
143
144
145
146
147
148
149 path = null;
150 distanceToEndOfLane = Length.ZERO;
151 laneListForward.clear();
152 startPosition = Length.ZERO;
153 }
154
155 while (distanceToEndOfLane.lt(maxHeadway))
156 {
157 Map<Lane, GTUDirectionality> lanes = lastGtuDir.equals(GTUDirectionality.DIR_PLUS)
158 ? lane.nextLanes(gtu.getGTUType()) : lane.prevLanes(gtu.getGTUType());
159 if (lanes.size() == 0)
160 {
161
162 return new LanePathInfo(path, laneListForward, startPosition);
163 }
164 else if (lanes.size() == 1)
165 {
166
167
168 LinkDirection ld = null;
169 ld = gtu.getStrategicalPlanner().nextLinkDirection(lane.getParentLink(), lastGtuDir, gtu.getGTUType());
170 lane = lanes.keySet().iterator().next();
171 if (ld != null && !lane.getParentLink().equals(ld.getLink()))
172 {
173
174 return new LanePathInfo(path, laneListForward, startPosition);
175 }
176 }
177 else
178 {
179
180
181 LinkDirection ld;
182 try
183 {
184 ld = gtu.getStrategicalPlanner().nextLinkDirection(lane.getParentLink(),
185 lastGtuDir, gtu.getGTUType());
186 }
187 catch (NetworkException ne)
188 {
189
190
191 return new LanePathInfo(path, laneListForward, startPosition);
192 }
193 Link nextLink = ld.getLink();
194 Lane newLane = null;
195 for (Lane nextLane : lanes.keySet())
196 {
197 if (nextLane.getParentLink().equals(nextLink))
198 {
199 newLane = nextLane;
200 break;
201 }
202 }
203 if (newLane == null)
204 {
205
206
207 return new LanePathInfo(path, laneListForward, startPosition);
208 }
209
210 lane = newLane;
211 }
212
213
214 try
215 {
216 if (lastGtuDir.equals(GTUDirectionality.DIR_PLUS))
217 {
218 if (lastLane.getParentLink().getEndNode().equals(lane.getParentLink().getStartNode()))
219 {
220
221 path = LaneOperationalPlanBuilder.concatenateNull(path, lane.getCenterLine());
222
223 lastGtuDir = GTUDirectionality.DIR_PLUS;
224 }
225 else
226 {
227
228 path = LaneOperationalPlanBuilder.concatenateNull(path, lane.getCenterLine().reverse());
229
230 lastGtuDir = GTUDirectionality.DIR_MINUS;
231 }
232 }
233 else
234 {
235 if (lastLane.getParentLink().getStartNode().equals(lane.getParentLink().getStartNode()))
236 {
237
238 path = LaneOperationalPlanBuilder.concatenateNull(path, lane.getCenterLine());
239
240 lastGtuDir = GTUDirectionality.DIR_PLUS;
241 }
242 else
243 {
244
245 path = LaneOperationalPlanBuilder.concatenateNull(path, lane.getCenterLine().reverse());
246
247 lastGtuDir = GTUDirectionality.DIR_MINUS;
248 }
249 }
250 lastLane = lane;
251 }
252 catch (OTSGeometryException exception)
253 {
254 throw new GTUException(exception);
255 }
256
257 laneListForward.add(new LaneDirection(lastLane, lastGtuDir));
258 distanceToEndOfLane = distanceToEndOfLane.plus(lastLane.getLength());
259
260 }
261 return new LanePathInfo(path, laneListForward, startPosition);
262 }
263
264
265
266
267
268
269
270
271
272
273
274 public static NextSplitInfo determineNextSplit(final LaneBasedGTU gtu, final Length maxHeadway)
275 throws GTUException, NetworkException
276 {
277 Node nextSplitNode = null;
278 Set<Lane> correctCurrentLanes = new HashSet<>();
279 DirectedLanePosition dlp = gtu.getReferencePosition();
280 Lane referenceLane = dlp.getLane();
281 double refFrac = dlp.getPosition().si / referenceLane.getLength().si;
282 Link lastLink = referenceLane.getParentLink();
283 GTUDirectionality lastGtuDir = dlp.getGtuDirection();
284 GTUDirectionality referenceLaneDirectionality = lastGtuDir;
285 Length lengthForward;
286 Length position = dlp.getPosition();
287 Node lastNode;
288 if (lastGtuDir.equals(GTUDirectionality.DIR_PLUS))
289 {
290 lengthForward = referenceLane.getLength().minus(position);
291 lastNode = referenceLane.getParentLink().getEndNode();
292 }
293 else
294 {
295 lengthForward = gtu.position(referenceLane, gtu.getReference());
296 lastNode = referenceLane.getParentLink().getStartNode();
297 }
298
299
300 while (lengthForward.lt(maxHeadway) && nextSplitNode == null)
301 {
302
303 Set<Link> links = lastNode.getLinks().toSet();
304 Iterator<Link> linkIterator = links.iterator();
305 while (linkIterator.hasNext())
306 {
307 Link link = linkIterator.next();
308 if (link.equals(lastLink) || !link.getLinkType().isCompatible(gtu.getGTUType())
309 || (link.getDirectionality(gtu.getGTUType()).isForward() && link.getEndNode().equals(lastNode))
310 || (link.getDirectionality(gtu.getGTUType()).isBackward() && link.getStartNode().equals(lastNode)))
311 {
312 linkIterator.remove();
313 }
314 }
315
316
317 boolean laneChange = false;
318 if (links.size() == 1)
319 {
320 for (CrossSectionElement cse : ((CrossSectionLink) lastLink).getCrossSectionElementList())
321 {
322 if (cse instanceof Lane && lastGtuDir.isPlus())
323 {
324 Lane lane = (Lane) cse;
325 if (lane.nextLanes(gtu.getGTUType()).size() == 0)
326 {
327 laneChange = true;
328 }
329 }
330 if (cse instanceof Lane && lastGtuDir.isMinus())
331 {
332 Lane lane = (Lane) cse;
333 if (lane.prevLanes(gtu.getGTUType()).size() == 0)
334 {
335 laneChange = true;
336 }
337 }
338 }
339 }
340
341
342 if (laneChange)
343 {
344 nextSplitNode = lastNode;
345
346
347 for (CrossSectionElement cse : referenceLane.getParentLink().getCrossSectionElementList())
348 {
349 if (cse instanceof Lane)
350 {
351 Lane l = (Lane) cse;
352
353 if (noLaneDrop(gtu, maxHeadway, l, l.getLength().multiplyBy(refFrac), referenceLaneDirectionality))
354 {
355 correctCurrentLanes.add(l);
356 }
357 }
358 }
359 return new NextSplitInfo(nextSplitNode, correctCurrentLanes);
360 }
361
362
363 if (links.size() > 1)
364 {
365 nextSplitNode = lastNode;
366 LinkDirection ld = gtu.getStrategicalPlanner().nextLinkDirection(nextSplitNode, lastLink, gtu.getGTUType());
367
368
369 for (CrossSectionElement cse : referenceLane.getParentLink().getCrossSectionElementList())
370 {
371 if (cse instanceof Lane)
372 {
373 Lane l = (Lane) cse;
374
375 if (connectsToPath(gtu, maxHeadway, l, l.getLength().multiplyBy(refFrac), referenceLaneDirectionality,
376 ld.getLink()))
377 {
378 correctCurrentLanes.add(l);
379 }
380 }
381 }
382 if (correctCurrentLanes.size() > 0)
383 {
384 return new NextSplitInfo(nextSplitNode, correctCurrentLanes);
385 }
386
387 Set<Lane> correctLanes = new HashSet<>();
388 Set<Lane> wrongLanes = new HashSet<>();
389 for (CrossSectionElement cse : ((CrossSectionLink) lastLink).getCrossSectionElementList())
390 {
391 if (cse instanceof Lane)
392 {
393 Lane l = (Lane) cse;
394 if (connectsToPath(gtu, maxHeadway.plus(l.getLength()), l, Length.ZERO, lastGtuDir, ld.getLink()))
395 {
396 correctLanes.add(l);
397 }
398 else
399 {
400 wrongLanes.add(l);
401 }
402 }
403 }
404 for (Lane wrongLane : wrongLanes)
405 {
406 for (Lane adjLane : wrongLane.accessibleAdjacentLanes(LateralDirectionality.LEFT, gtu.getGTUType()))
407 {
408 if (correctLanes.contains(adjLane))
409 {
410 return new NextSplitInfo(nextSplitNode, correctCurrentLanes, LateralDirectionality.LEFT);
411 }
412 }
413 for (Lane adjLane : wrongLane.accessibleAdjacentLanes(LateralDirectionality.RIGHT, gtu.getGTUType()))
414 {
415 if (correctLanes.contains(adjLane))
416 {
417 return new NextSplitInfo(nextSplitNode, correctCurrentLanes, LateralDirectionality.RIGHT);
418 }
419 }
420 }
421 return new NextSplitInfo(nextSplitNode, correctCurrentLanes, null);
422 }
423
424 if (links.size() == 0)
425 {
426 return new NextSplitInfo(null, correctCurrentLanes);
427 }
428
429
430 Link link = links.iterator().next();
431
432
433 if (lastGtuDir.equals(GTUDirectionality.DIR_PLUS))
434 {
435 if (lastLink.getEndNode().equals(link.getStartNode()))
436 {
437
438 lastGtuDir = GTUDirectionality.DIR_PLUS;
439 lastNode = link.getEndNode();
440 }
441 else
442 {
443
444 lastGtuDir = GTUDirectionality.DIR_MINUS;
445 lastNode = link.getEndNode();
446 }
447 }
448 else
449 {
450 if (lastLink.getStartNode().equals(link.getStartNode()))
451 {
452
453 lastNode = link.getStartNode();
454 lastGtuDir = GTUDirectionality.DIR_PLUS;
455 }
456 else
457 {
458
459 lastNode = link.getStartNode();
460 lastGtuDir = GTUDirectionality.DIR_MINUS;
461 }
462 }
463 lastLink = links.iterator().next();
464 lengthForward = lengthForward.plus(lastLink.getLength());
465 }
466
467 return new NextSplitInfo(null, correctCurrentLanes);
468 }
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483 protected static boolean connectsToPath(final LaneBasedGTU gtu, final Length maxHeadway, final Lane startLane,
484 final Length startLanePosition, final GTUDirectionality startDirectionality, final Link linkAfterSplit)
485 throws GTUException, NetworkException
486 {
487 List<LaneDirection> laneDirections =
488 buildLanePathInfo(gtu, maxHeadway, startLane, startLanePosition, startDirectionality).getLaneDirectionList();
489 for (LaneDirection laneDirection : laneDirections)
490 {
491 if (laneDirection.getLane().getParentLink().equals(linkAfterSplit))
492 {
493 return true;
494 }
495 }
496 return false;
497 }
498
499
500
501
502
503
504
505
506
507
508
509
510
511 protected static boolean noLaneDrop(final LaneBasedGTU gtu, final Length maxHeadway, final Lane startLane,
512 final Length startLanePosition, final GTUDirectionality startDirectionality) throws GTUException, NetworkException
513 {
514 LanePathInfo lpi = buildLanePathInfo(gtu, maxHeadway, startLane, startLanePosition, startDirectionality);
515 if (lpi.getPath().getLength().lt(maxHeadway))
516 {
517 return false;
518 }
519 return true;
520 }
521
522
523
524
525
526
527
528
529
530 protected static List<LinkDirection> buildLinkListForward(final LaneBasedGTU gtu, final Length maxHeadway)
531 throws GTUException, NetworkException
532 {
533 List<LinkDirection> linkList = new ArrayList<>();
534 DirectedLanePosition dlp = gtu.getReferencePosition();
535 Lane referenceLane = dlp.getLane();
536 Link lastLink = referenceLane.getParentLink();
537 GTUDirectionality lastGtuDir = dlp.getGtuDirection();
538 linkList.add(new LinkDirection(lastLink, lastGtuDir));
539 Length lengthForward;
540 Length position = dlp.getPosition();
541 Node lastNode;
542 if (lastGtuDir.equals(GTUDirectionality.DIR_PLUS))
543 {
544 lengthForward = referenceLane.getLength().minus(position);
545 lastNode = referenceLane.getParentLink().getEndNode();
546 }
547 else
548 {
549 lengthForward = gtu.position(referenceLane, gtu.getReference());
550 lastNode = referenceLane.getParentLink().getStartNode();
551 }
552
553
554 while (lengthForward.lt(maxHeadway))
555 {
556
557 Set<Link> links = lastNode.getLinks().toSet();
558 Iterator<Link> linkIterator = links.iterator();
559 while (linkIterator.hasNext())
560 {
561 Link link = linkIterator.next();
562 if (link.equals(lastLink) || !link.getLinkType().isCompatible(gtu.getGTUType()))
563 {
564 linkIterator.remove();
565 }
566 }
567
568 if (links.size() == 0)
569 {
570 return linkList;
571 }
572
573 Link link;
574 if (links.size() > 1)
575 {
576 LinkDirection ld = gtu.getStrategicalPlanner().nextLinkDirection(lastLink, lastGtuDir, gtu.getGTUType());
577 link = ld.getLink();
578 }
579 else
580 {
581 link = links.iterator().next();
582 }
583
584
585 if (lastGtuDir.equals(GTUDirectionality.DIR_PLUS))
586 {
587 if (lastLink.getEndNode().equals(link.getStartNode()))
588 {
589
590 lastGtuDir = GTUDirectionality.DIR_PLUS;
591 lastNode = lastLink.getEndNode();
592 }
593 else
594 {
595
596 lastGtuDir = GTUDirectionality.DIR_MINUS;
597 lastNode = lastLink.getEndNode();
598 }
599 }
600 else
601 {
602 if (lastLink.getStartNode().equals(link.getStartNode()))
603 {
604
605 lastNode = lastLink.getStartNode();
606 lastGtuDir = GTUDirectionality.DIR_PLUS;
607 }
608 else
609 {
610
611 lastNode = lastLink.getStartNode();
612 lastGtuDir = GTUDirectionality.DIR_MINUS;
613 }
614 }
615 lastLink = link;
616 linkList.add(new LinkDirection(lastLink, lastGtuDir));
617 lengthForward = lengthForward.plus(lastLink.getLength());
618 }
619 return linkList;
620 }
621
622
623 @Override
624 public final CarFollowingModel getCarFollowingModel()
625 {
626 return this.carFollowingModel;
627 }
628
629
630
631
632
633 public final void setCarFollowingModel(final CarFollowingModel carFollowingModel)
634 {
635 this.carFollowingModel = carFollowingModel;
636 }
637
638
639 @Override
640 public final LanePerception getPerception()
641 {
642 return this.lanePerception;
643 }
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658 public static LaneBasedOperationalPlan buildPlanFromSimplePlan(final LaneBasedGTU gtu, final Time startTime,
659 final BehavioralCharacteristics bc, final SimpleOperationalPlan simplePlan, final LaneChange laneChange)
660 throws ParameterException, GTUException, NetworkException, OperationalPlanException
661 {
662 Length forwardHeadway = bc.getParameter(ParameterTypes.LOOKAHEAD);
663 List<Lane> lanes = null;
664
665 if (simplePlan.isLaneChange())
666 {
667 gtu.changeLaneInstantaneously(simplePlan.getLaneChangeDirection());
668 }
669 if (true)
670 {
671 LanePathInfo lanePathInfo = buildLanePathInfo(gtu, forwardHeadway);
672 lanes = lanePathInfo.getLanes();
673 Length startPosition = lanePathInfo.getReferencePosition();
674 try
675 {
676 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, lanes, startPosition, startTime, gtu.getSpeed(),
677 simplePlan.getAcceleration(), bc.getParameter(ParameterTypes.DT));
678 }
679 catch (OTSGeometryException exception)
680 {
681 throw new OperationalPlanException(exception);
682 }
683 }
684
685 if (!laneChange.isChangingLane())
686 {
687 lanes = buildLanePathInfo(gtu, forwardHeadway).getLanes();
688 }
689 else
690 {
691
692
693
694
695 Map<Lane, Length> positions = gtu.positions(gtu.getReference());
696 LateralDirectionality lat = laneChange.isChangingLeft() ? LateralDirectionality.LEFT : LateralDirectionality.RIGHT;
697 DirectedLanePosition dlp = gtu.getReferencePosition();
698 Iterator<Lane> iterator = dlp.getLane().accessibleAdjacentLanes(lat, gtu.getGTUType()).iterator();
699 Lane adjLane = iterator.hasNext() ? iterator.next() : null;
700 if (adjLane != null && positions.containsKey(adjLane))
701 {
702
703 lanes = buildLanePathInfo(gtu, forwardHeadway).getLanes();
704 }
705 else
706 {
707
708 for (Lane lane : positions.keySet())
709 {
710 if (lane.accessibleAdjacentLanes(lat, gtu.getGTUType()).contains(dlp.getLane()))
711 {
712 lanes = buildLanePathInfo(gtu, forwardHeadway, lane, positions.get(lane), dlp.getGtuDirection())
713 .getLanes();
714 }
715 }
716 }
717 if (lanes == null)
718 {
719 throw new RuntimeException("From lane could not be determined during lane change.");
720 }
721 }
722 if ((!simplePlan.isLaneChange() && !laneChange.isChangingLane())
723 || (gtu.getSpeed().si == 0.0 && simplePlan.getAcceleration().si <= 0.0))
724 {
725 Length firstLanePosition = gtu.getReferencePosition().getPosition();
726 try
727 {
728 return LaneOperationalPlanBuilder.buildAccelerationPlan(gtu, lanes, firstLanePosition, startTime,
729 gtu.getSpeed(), simplePlan.getAcceleration(), bc.getParameter(ParameterTypes.DT));
730 }
731 catch (OTSGeometryException exception)
732 {
733 throw new OperationalPlanException(exception);
734 }
735 }
736
737 try
738 {
739 return LaneOperationalPlanBuilder.buildAccelerationLaneChangePlan(gtu, lanes, simplePlan.getLaneChangeDirection(),
740 gtu.getLocation(), startTime, gtu.getSpeed(), simplePlan.getAcceleration(),
741 bc.getParameter(ParameterTypes.DT), laneChange);
742 }
743 catch (OTSGeometryException exception)
744 {
745 throw new OperationalPlanException(exception);
746 }
747 }
748 }