1 package org.opentrafficsim.road.gtu.lane.perception;
2
3 import org.djunits.value.vdouble.scalar.Length;
4 import org.opentrafficsim.base.parameters.ParameterException;
5 import org.opentrafficsim.core.gtu.GTUException;
6 import org.opentrafficsim.core.gtu.RelativePosition;
7 import org.opentrafficsim.road.gtu.lane.Break;
8 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
9 import org.opentrafficsim.road.gtu.lane.perception.categories.neighbors.HeadwayGtuType;
10 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
11
12
13
14
15
16
17
18
19
20
21
22
23 public class UpstreamNeighborsIterable extends AbstractPerceptionIterable<HeadwayGTU, LaneBasedGTU, Integer>
24 {
25
26
27 private static final Length LEFT = Length.createSI(0.000001);
28
29
30 private static final Length RIGHT = Length.createSI(-0.000001);
31
32
33 private final HeadwayGtuType headwayGtuType;
34
35
36
37
38
39 private final Length margin;
40
41
42
43
44
45
46
47
48
49
50
51 public UpstreamNeighborsIterable(final LaneBasedGTU perceivingGtu, final LaneRecord<?> root, final Length initialPosition,
52 final Length maxDistance, final RelativePosition relativePosition, final HeadwayGtuType headwayGtuType,
53 final RelativeLane lane)
54 {
55 super(perceivingGtu, root, initialPosition, false, maxDistance, relativePosition, null);
56 this.headwayGtuType = headwayGtuType;
57 this.margin = lane.getLateralDirectionality().isLeft() ? LEFT : RIGHT;
58 }
59
60
61 @Override
62 protected Entry getNext(final LaneRecord<?> record, final Length position, final Integer counter) throws GTUException
63 {
64 int n;
65 LaneBasedGTU next;
66 Length pos;
67 boolean plus = record.getDirection().isPlus();
68 if (counter == null)
69 {
70 if (plus ? position.ge(record.getLane().getLength()) : position.eq0())
71 {
72 next = record.getLane().getLastGtu(record.getDirection());
73 }
74 else
75 {
76 Length searchPos = (plus ? position.plus(this.margin) : position.minus(this.margin));
77 next = record.getLane().getGtuBehind(searchPos, record.getDirection(), RelativePosition.FRONT,
78 record.getLane().getParentLink().getSimulator().getSimulatorTime());
79 }
80 if (next == null)
81 {
82 return null;
83 }
84 n = record.getLane().indexOfGtu(next);
85 pos = next.position(record.getLane(), next.getFront());
86
87 if (this.getGtu() != null && next.getId().equals(this.getGtu().getId()))
88 {
89
90 pos = plus ? pos.minus(next.getLength()) : pos.plus(next.getLength());
91 return getNext(record, pos, n);
92 }
93 }
94 else
95 {
96 n = plus ? counter - 1 : counter + 1;
97 if (n < 0 || n >= record.getLane().numberOfGtus())
98 {
99 return null;
100 }
101 next = record.getLane().getGtu(n);
102 pos = next.position(record.getLane(), next.getFront());
103 }
104 return new Entry(next, n, pos);
105 }
106
107
108 @Override
109 protected Length getDistance(final LaneBasedGTU object, final LaneRecord<?> record, final Length position)
110 {
111 return record.getDistanceToPosition(position).neg().plus(getDx());
112 }
113
114
115 @Override
116 public HeadwayGTU perceive(final LaneBasedGTU perceivingGtu, final LaneBasedGTU object, final Length distance)
117 throws GTUException, ParameterException
118 {
119 return this.headwayGtuType.createUpstreamGtu(perceivingGtu, object, distance);
120 }
121
122 }