1 package org.opentrafficsim.road.gtu.lane.perception.categories;
2
3 import java.util.Collection;
4 import java.util.Comparator;
5 import java.util.ConcurrentModificationException;
6 import java.util.HashMap;
7 import java.util.Iterator;
8 import java.util.LinkedHashSet;
9 import java.util.Map;
10 import java.util.Set;
11 import java.util.SortedMap;
12 import java.util.SortedSet;
13 import java.util.TreeMap;
14 import java.util.TreeSet;
15
16 import org.djunits.value.vdouble.scalar.Length;
17 import org.opentrafficsim.base.TimeStampedObject;
18 import org.opentrafficsim.base.parameters.ParameterException;
19 import org.opentrafficsim.base.parameters.ParameterTypeLength;
20 import org.opentrafficsim.base.parameters.ParameterTypes;
21 import org.opentrafficsim.core.gtu.GTUException;
22 import org.opentrafficsim.core.gtu.RelativePosition;
23 import org.opentrafficsim.core.gtu.Try;
24 import org.opentrafficsim.core.network.LateralDirectionality;
25 import org.opentrafficsim.core.network.NetworkException;
26 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
27 import org.opentrafficsim.road.gtu.lane.perception.DownstreamNeighborsIterable;
28 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
29 import org.opentrafficsim.road.gtu.lane.perception.LaneStructureRecord;
30 import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable;
31 import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
32 import org.opentrafficsim.road.gtu.lane.perception.UpstreamNeighborsIterable;
33 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
34
35 import nl.tudelft.simulation.language.Throw;
36
37
38
39
40
41
42
43
44
45
46
47
48 public class DirectNeighborsPerception extends LaneBasedAbstractPerceptionCategory implements NeighborsPerception
49 {
50
51
52 private static final long serialVersionUID = 20160811L;
53
54
55 protected static final ParameterTypeLength LOOKAHEAD = ParameterTypes.LOOKAHEAD;
56
57
58 protected static final ParameterTypeLength LOOKBACK = ParameterTypes.LOOKBACK;
59
60
61 private final Map<RelativeLane, TimeStampedObject<PerceptionCollectable<HeadwayGTU, LaneBasedGTU>>> followers =
62 new HashMap<>();
63
64
65 private final Map<RelativeLane, TimeStampedObject<PerceptionCollectable<HeadwayGTU, LaneBasedGTU>>> leaders =
66 new HashMap<>();
67
68
69 private final Map<LateralDirectionality, TimeStampedObject<SortedSet<HeadwayGTU>>> firstFollowers = new HashMap<>();
70
71
72 private final Map<LateralDirectionality, TimeStampedObject<SortedSet<HeadwayGTU>>> firstLeaders = new HashMap<>();
73
74
75 private final Map<LateralDirectionality, TimeStampedObject<Boolean>> gtuAlongside = new HashMap<>();
76
77
78 private final HeadwayGtuType headwayGtuType;
79
80
81
82
83
84 public DirectNeighborsPerception(final LanePerception perception, final HeadwayGtuType headwayGtuType)
85 {
86 super(perception);
87 this.headwayGtuType = headwayGtuType;
88 }
89
90
91 @Override
92 public final void updateAll() throws GTUException, NetworkException, ParameterException
93 {
94 this.firstLeaders.clear();
95 this.firstFollowers.clear();
96 this.gtuAlongside.clear();
97 if (getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.LEFT))
98 {
99 updateFirstLeaders(LateralDirectionality.LEFT);
100 updateFirstFollowers(LateralDirectionality.LEFT);
101 updateGtuAlongside(LateralDirectionality.LEFT);
102 }
103 if (getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.RIGHT))
104 {
105 updateFirstLeaders(LateralDirectionality.RIGHT);
106 updateFirstFollowers(LateralDirectionality.RIGHT);
107 updateGtuAlongside(LateralDirectionality.RIGHT);
108 }
109 this.leaders.clear();
110 this.followers.clear();
111 for (RelativeLane lane : getPerception().getLaneStructure().getExtendedCrossSection())
112 {
113 updateLeaders(lane);
114 updateFollowers(lane);
115 }
116 }
117
118
119 @Override
120 public final void updateFirstLeaders(final LateralDirectionality lat)
121 throws ParameterException, GTUException, NetworkException
122 {
123 checkLateralDirectionality(lat);
124 SortedSet<HeadwayGTU> headwaySet =
125 new SortedNeighborsSet(getFirstDownstreamGTUs(lat, RelativePosition.FRONT, RelativePosition.REAR),
126 this.headwayGtuType, getGtu(), true);
127 this.firstLeaders.put(lat, new TimeStampedObject<>(headwaySet, getTimestamp()));
128 }
129
130
131 @Override
132 public final void updateFirstFollowers(final LateralDirectionality lat)
133 throws GTUException, ParameterException, NetworkException
134 {
135 checkLateralDirectionality(lat);
136 SortedSet<HeadwayGTU> headwaySet = new SortedNeighborsSet(
137 getFirstUpstreamGTUs(lat, RelativePosition.REAR, RelativePosition.FRONT), this.headwayGtuType, getGtu(), false);
138 this.firstFollowers.put(lat, new TimeStampedObject<>(headwaySet, getTimestamp()));
139 }
140
141
142 @Override
143 public final void updateGtuAlongside(final LateralDirectionality lat) throws GTUException, ParameterException
144 {
145
146 checkLateralDirectionality(lat);
147
148 SortedSet<DistanceGTU> headwaySet = getFirstDownstreamGTUs(lat, RelativePosition.REAR, RelativePosition.FRONT);
149 if (!headwaySet.isEmpty() && headwaySet.first().getDistance().le0())
150 {
151 this.gtuAlongside.put(lat, new TimeStampedObject<>(true, getTimestamp()));
152 return;
153 }
154
155 headwaySet = getFirstUpstreamGTUs(lat, RelativePosition.FRONT, RelativePosition.REAR);
156 if (!headwaySet.isEmpty() && headwaySet.first().getDistance().le0())
157 {
158 this.gtuAlongside.put(lat, new TimeStampedObject<>(true, getTimestamp()));
159 return;
160 }
161
162 this.gtuAlongside.put(lat, new TimeStampedObject<>(false, getTimestamp()));
163
164 }
165
166
167
168
169
170
171
172
173
174
175
176 private SortedSet<DistanceGTU> getFirstDownstreamGTUs(final LateralDirectionality lat,
177 final RelativePosition.TYPE egoRelativePosition, final RelativePosition.TYPE otherRelativePosition)
178 throws GTUException, ParameterException
179 {
180 SortedSet<DistanceGTU> headwaySet = new TreeSet<>();
181 Set<LaneStructureRecord> currentSet = new LinkedHashSet<>();
182 Set<LaneStructureRecord> nextSet = new LinkedHashSet<>();
183 LaneStructureRecord record = getPerception().getLaneStructure().getFirstRecord(new RelativeLane(lat, 1));
184 Length dxSearch = getGtu().getRelativePositions().get(egoRelativePosition).getDx();
185 Length dxHeadway = getGtu().getFront().getDx();
186 branchUpstream(record, dxSearch, currentSet);
187
188 while (!currentSet.isEmpty())
189 {
190 Iterator<LaneStructureRecord> iterator = currentSet.iterator();
191 while (iterator.hasNext())
192 {
193 record = iterator.next();
194
195
196
197
198
199
200
201 LaneBasedGTU down = record.getLane().getGtuAhead(record.getStartDistance().neg().plus(dxSearch),
202 record.getDirection(), otherRelativePosition, getTimestamp());
203 if (down != null)
204 {
205
206
207
208
209 headwaySet.add(new DistanceGTU(down,
210 record.getStartDistance().plus(down.position(record.getLane(), down.getRear())).minus(dxHeadway)));
211 }
212 else
213 {
214
215 for (LaneStructureRecord next : record.getNext())
216 {
217 nextSet.add(next);
218 }
219 }
220 }
221 currentSet = nextSet;
222 nextSet = new LinkedHashSet<>();
223 }
224 return headwaySet;
225 }
226
227
228
229
230
231
232
233
234 private void branchUpstream(final LaneStructureRecord record, final Length dx, final Set<LaneStructureRecord> set)
235 {
236 Length pos = record.getStartDistance().neg().minus(dx);
237 if (pos.lt0() && !record.getPrev().isEmpty())
238 {
239 for (LaneStructureRecord prev : record.getPrev())
240 {
241 branchUpstream(prev, dx, set);
242 }
243 }
244 else
245 {
246 set.add(record);
247 }
248 }
249
250
251
252
253
254
255
256
257
258
259
260 private SortedSet<DistanceGTU> getFirstUpstreamGTUs(final LateralDirectionality lat,
261 final RelativePosition.TYPE egoRelativePosition, final RelativePosition.TYPE otherRelativePosition)
262 throws GTUException, ParameterException
263 {
264 SortedSet<DistanceGTU> headwaySet = new TreeSet<>();
265 Set<LaneStructureRecord> currentSet = new LinkedHashSet<>();
266 Set<LaneStructureRecord> prevSet = new LinkedHashSet<>();
267 LaneStructureRecord record = getPerception().getLaneStructure().getFirstRecord(new RelativeLane(lat, 1));
268 Length dxSearch = getGtu().getRelativePositions().get(egoRelativePosition).getDx();
269 Length dxHeadway = getGtu().getRear().getDx();
270 branchDownstream(record, dxSearch, currentSet);
271
272 while (!currentSet.isEmpty())
273 {
274 Iterator<LaneStructureRecord> iterator = currentSet.iterator();
275 while (iterator.hasNext())
276 {
277 record = iterator.next();
278
279
280
281
282
283
284
285 LaneBasedGTU up = record.getLane().getGtuBehind(record.getStartDistance().neg().plus(dxSearch),
286 record.getDirection(), otherRelativePosition, getTimestamp());
287 if (up != null)
288 {
289
290
291
292
293 headwaySet.add(new DistanceGTU(up, record.getStartDistance().neg()
294 .minus(up.position(record.getLane(), up.getFront())).plus(dxHeadway)));
295 }
296 else
297 {
298
299 for (LaneStructureRecord prev : record.getPrev())
300 {
301 prevSet.add(prev);
302 }
303 }
304 }
305 currentSet = prevSet;
306 prevSet = new LinkedHashSet<>();
307 }
308 return headwaySet;
309 }
310
311
312
313
314
315
316
317
318 private void branchDownstream(final LaneStructureRecord record, final Length dx, final Set<LaneStructureRecord> set)
319 {
320 Length pos = record.getStartDistance().neg().plus(dx);
321 if (pos.gt(record.getLane().getLength()))
322 {
323 for (LaneStructureRecord next : record.getNext())
324 {
325 branchDownstream(next, dx, set);
326 }
327 }
328 else
329 {
330 set.add(record);
331 }
332 }
333
334
335 @Override
336 public final void updateLeaders(final RelativeLane lane) throws ParameterException, GTUException, NetworkException
337 {
338 Throw.whenNull(lane, "Lane may not be null.");
339 LaneStructureRecord record = getPerception().getLaneStructure().getFirstRecord(lane);
340 Length pos = record.getStartDistance().neg();
341 pos = record.getDirection().isPlus() ? pos.plus(getGtu().getFront().getDx()) : pos.minus(getGtu().getFront().getDx());
342 boolean ignoreIfUpstream = true;
343 PerceptionCollectable<HeadwayGTU, LaneBasedGTU> it = new DownstreamNeighborsIterable(getGtu(), record,
344 Length.max(Length.ZERO, pos), getGtu().getParameters().getParameter(LOOKAHEAD), getGtu().getFront(),
345 this.headwayGtuType, getGtu(), lane, ignoreIfUpstream);
346 this.leaders.put(lane, new TimeStampedObject<>(it, getTimestamp()));
347 }
348
349
350 @Override
351 public final void updateFollowers(final RelativeLane lane) throws GTUException, NetworkException, ParameterException
352 {
353 Throw.whenNull(lane, "Lane may not be null.");
354 LaneStructureRecord record = getPerception().getLaneStructure().getFirstRecord(lane);
355 Length pos = record.getStartDistance().neg();
356 pos = record.getDirection().isPlus() ? pos.plus(getGtu().getFront().getDx()) : pos.minus(getGtu().getFront().getDx());
357 PerceptionCollectable<HeadwayGTU, LaneBasedGTU> it =
358 new UpstreamNeighborsIterable(getGtu(), record, Length.max(Length.ZERO, pos),
359 getGtu().getParameters().getParameter(LOOKBACK), getGtu().getRear(), this.headwayGtuType, lane);
360 this.followers.put(lane, new TimeStampedObject<>(it, getTimestamp()));
361 }
362
363
364 @Override
365 public final SortedSet<HeadwayGTU> getFirstLeaders(final LateralDirectionality lat)
366 throws ParameterException, NullPointerException, IllegalArgumentException
367 {
368 checkLateralDirectionality(lat);
369 return this.firstLeaders.get(lat).getObject();
370 }
371
372
373 @Override
374 public final SortedSet<HeadwayGTU> getFirstFollowers(final LateralDirectionality lat)
375 throws ParameterException, NullPointerException, IllegalArgumentException
376 {
377 checkLateralDirectionality(lat);
378 return getObjectOrNull(this.firstFollowers.get(lat));
379 }
380
381
382 @Override
383 public final boolean isGtuAlongside(final LateralDirectionality lat)
384 throws ParameterException, NullPointerException, IllegalArgumentException
385 {
386 checkLateralDirectionality(lat);
387 return getObjectOrNull(this.gtuAlongside.get(lat));
388 }
389
390
391 @Override
392 public final PerceptionCollectable<HeadwayGTU, LaneBasedGTU> getLeaders(final RelativeLane lane)
393 {
394 return getObjectOrNull(this.leaders.get(lane));
395 }
396
397
398 @Override
399 public final PerceptionCollectable<HeadwayGTU, LaneBasedGTU> getFollowers(final RelativeLane lane)
400 {
401 return getObjectOrNull(this.followers.get(lane));
402 }
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424 public final TimeStampedObject<SortedSet<HeadwayGTU>> getTimeStampedFirstLeaders(final LateralDirectionality lat)
425 throws ParameterException, NullPointerException, IllegalArgumentException
426 {
427 checkLateralDirectionality(lat);
428 return this.firstLeaders.get(lat);
429 }
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452 public final TimeStampedObject<SortedSet<HeadwayGTU>> getTimeStampedFirstFollowers(final LateralDirectionality lat)
453 throws ParameterException, NullPointerException, IllegalArgumentException
454 {
455 checkLateralDirectionality(lat);
456 return this.firstFollowers.get(lat);
457 }
458
459
460
461
462
463
464
465
466
467 public final TimeStampedObject<Boolean> isGtuAlongsideTimeStamped(final LateralDirectionality lat)
468 throws ParameterException, NullPointerException, IllegalArgumentException
469 {
470 checkLateralDirectionality(lat);
471 return this.gtuAlongside.get(lat);
472 }
473
474
475
476
477
478
479
480 public final TimeStampedObject<PerceptionCollectable<HeadwayGTU, LaneBasedGTU>> getTimeStampedLeaders(
481 final RelativeLane lane)
482 {
483 return this.leaders.get(lane);
484 }
485
486
487
488
489
490
491
492 public final TimeStampedObject<PerceptionCollectable<HeadwayGTU, LaneBasedGTU>> getTimeStampedFollowers(
493 final RelativeLane lane)
494 {
495 return this.followers.get(lane);
496 }
497
498
499
500
501
502
503
504
505 private void checkLateralDirectionality(final LateralDirectionality lat)
506 throws ParameterException, NullPointerException, IllegalArgumentException
507 {
508
509 Throw.whenNull(lat, "Lateral directionality may not be null.");
510 Throw.when(lat.equals(LateralDirectionality.NONE), IllegalArgumentException.class,
511 "Lateral directionality may not be NONE.");
512 Throw.when(
513 (lat.equals(LateralDirectionality.LEFT)
514 && !getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.LEFT))
515 || (lat.equals(LateralDirectionality.RIGHT)
516 && !getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.RIGHT)),
517 IllegalArgumentException.class, "Lateral directionality may only point to an existing adjacent lane.");
518 }
519
520
521 @Override
522 public final String toString()
523 {
524 return "DirectNeighborsPesrception";
525 }
526
527
528
529
530
531
532
533
534
535
536
537
538
539 private class DistanceGTU implements Comparable<DistanceGTU>
540 {
541
542
543 private LaneBasedGTU gtu;
544
545
546 private Length distance;
547
548
549
550
551
552
553 DistanceGTU(final LaneBasedGTU gtu, final Length distance)
554 {
555 this.gtu = gtu;
556 this.distance = distance;
557 }
558
559
560
561
562
563 public LaneBasedGTU getGTU()
564 {
565 return this.gtu;
566 }
567
568
569
570
571
572 public Length getDistance()
573 {
574 return this.distance;
575 }
576
577
578 @Override
579 public int compareTo(final DistanceGTU o)
580 {
581 return this.distance.compareTo(o.distance);
582 }
583 }
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598 private static class SortedNeighborsSet implements SortedSet<HeadwayGTU>
599 {
600
601
602 private final SortedSet<DistanceGTU> base;
603
604
605 private final HeadwayGtuType headwayGtuType;
606
607
608 private final LaneBasedGTU perceivingGtu;
609
610
611 private final boolean downstream;
612
613
614 private final SortedMap<String, HeadwayGTU> all = new TreeMap<>();
615
616
617
618
619
620
621
622
623 SortedNeighborsSet(final SortedSet<DistanceGTU> base, final HeadwayGtuType headwayGtuType,
624 final LaneBasedGTU perceivingGtu, final boolean downstream)
625 {
626 this.base = base;
627 this.headwayGtuType = headwayGtuType;
628 this.perceivingGtu = perceivingGtu;
629 this.downstream = downstream;
630 }
631
632
633 @Override
634 public int size()
635 {
636 return this.base.size();
637 }
638
639
640 @Override
641 public boolean isEmpty()
642 {
643 return this.base.isEmpty();
644 }
645
646
647
648
649 private void getAll()
650 {
651 Iterator<HeadwayGTU> it = iterator();
652 while (it.hasNext())
653 {
654 @SuppressWarnings("unused")
655 HeadwayGTU gtu = it.next();
656 }
657 }
658
659
660 @Override
661 public boolean contains(final Object o)
662 {
663 getAll();
664 return this.all.containsValue(o);
665 }
666
667
668 @Override
669 public Iterator<HeadwayGTU> iterator()
670 {
671 return new Iterator<HeadwayGTU>()
672 {
673 @SuppressWarnings("synthetic-access")
674 private Iterator<DistanceGTU> it = SortedNeighborsSet.this.base.iterator();
675
676 @Override
677 public boolean hasNext()
678 {
679 return this.it.hasNext();
680 }
681
682 @SuppressWarnings("synthetic-access")
683 @Override
684 public HeadwayGTU next()
685 {
686 DistanceGTU next = this.it.next();
687 if (next == null)
688 {
689 throw new ConcurrentModificationException();
690 }
691 HeadwayGTU out = SortedNeighborsSet.this.all.get(next.getGTU().getId());
692 if (out == null)
693 {
694 out = Try.assign(() -> SortedNeighborsSet.this.headwayGtuType.createHeadwayGtu(
695 SortedNeighborsSet.this.perceivingGtu, next.getGTU(), next.getDistance(),
696 SortedNeighborsSet.this.downstream), "Exception while perceiving a neighbor.");
697 SortedNeighborsSet.this.all.put(next.getGTU().getId(), out);
698 }
699 return out;
700 }
701 };
702 }
703
704
705 @Override
706 public Object[] toArray()
707 {
708 getAll();
709 return this.all.values().toArray();
710 }
711
712
713 @Override
714 public <T> T[] toArray(final T[] a)
715 {
716 getAll();
717 return this.all.values().toArray(a);
718 }
719
720
721 @Override
722 public boolean add(final HeadwayGTU e)
723 {
724 throw new UnsupportedOperationException();
725 }
726
727
728 @Override
729 public boolean remove(final Object o)
730 {
731 throw new UnsupportedOperationException();
732 }
733
734
735 @Override
736 public boolean containsAll(final Collection<?> c)
737 {
738 getAll();
739 return this.all.values().containsAll(c);
740 }
741
742
743 @Override
744 public boolean addAll(final Collection<? extends HeadwayGTU> c)
745 {
746 throw new UnsupportedOperationException();
747 }
748
749
750 @Override
751 public boolean retainAll(final Collection<?> c)
752 {
753 throw new UnsupportedOperationException();
754 }
755
756
757 @Override
758 public boolean removeAll(final Collection<?> c)
759 {
760 throw new UnsupportedOperationException();
761 }
762
763
764 @Override
765 public void clear()
766 {
767 throw new UnsupportedOperationException();
768 }
769
770
771 @Override
772 public Comparator<? super HeadwayGTU> comparator()
773 {
774 return null;
775 }
776
777
778
779
780
781
782 private DistanceGTU getGTU(final HeadwayGTU element)
783 {
784 for (DistanceGTU distanceGtu : this.base)
785 {
786 if (distanceGtu.getGTU().getId().equals(element.getId()))
787 {
788 return distanceGtu;
789 }
790 }
791 throw new IllegalArgumentException("GTU used to obtain a subset is not in the set.");
792 }
793
794
795 @Override
796 public SortedSet<HeadwayGTU> subSet(final HeadwayGTU fromElement, final HeadwayGTU toElement)
797 {
798 return new SortedNeighborsSet(this.base.subSet(getGTU(fromElement), getGTU(toElement)), this.headwayGtuType,
799 this.perceivingGtu, this.downstream);
800 }
801
802
803 @Override
804 public SortedSet<HeadwayGTU> headSet(final HeadwayGTU toElement)
805 {
806 return new SortedNeighborsSet(this.base.headSet(getGTU(toElement)), this.headwayGtuType, this.perceivingGtu,
807 this.downstream);
808 }
809
810
811 @Override
812 public SortedSet<HeadwayGTU> tailSet(final HeadwayGTU fromElement)
813 {
814 return new SortedNeighborsSet(this.base.tailSet(getGTU(fromElement)), this.headwayGtuType, this.perceivingGtu,
815 this.downstream);
816 }
817
818
819 @Override
820 public HeadwayGTU first()
821 {
822 return iterator().next();
823 }
824
825
826 @Override
827 public HeadwayGTU last()
828 {
829 getAll();
830 return this.all.get(this.all.lastKey());
831 }
832
833 }
834
835 }