1 package org.opentrafficsim.road.gtu.lane.perception;
2
3 import java.util.HashSet;
4 import java.util.Set;
5
6 import org.djunits.value.vdouble.scalar.Length;
7 import org.opentrafficsim.base.parameters.ParameterException;
8 import org.opentrafficsim.core.gtu.GTUException;
9 import org.opentrafficsim.core.gtu.RelativePosition;
10 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
11 import org.opentrafficsim.road.gtu.lane.perception.categories.neighbors.HeadwayGtuType;
12 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43 public class DownstreamNeighborsIterable extends AbstractPerceptionIterable<HeadwayGTU, LaneBasedGTU, Integer>
44 {
45
46
47 private static final Length LEFT = Length.createSI(-0.000001);
48
49
50 private static final Length RIGHT = Length.createSI(0.000001);
51
52
53 private final HeadwayGtuType headwayGtuType;
54
55
56 private final Set<String> ids = new HashSet<>();
57
58
59
60
61
62 private final Length margin;
63
64
65 private final boolean ignoreIfUpstream;
66
67
68
69
70
71
72
73
74
75
76
77
78 public DownstreamNeighborsIterable(final LaneBasedGTU perceivingGtu, final LaneRecord<?> root, final Length initialPosition,
79 final Length maxDistance, final RelativePosition relativePosition, final HeadwayGtuType headwayGtuType,
80 final RelativeLane lane, final boolean ignoreIfUpstream)
81 {
82 super(perceivingGtu, root, initialPosition, true, maxDistance, relativePosition, null);
83 this.headwayGtuType = headwayGtuType;
84 this.margin = lane.getLateralDirectionality().isLeft() ? LEFT : RIGHT;
85 this.ids.add(perceivingGtu.getId());
86 this.ignoreIfUpstream = ignoreIfUpstream;
87 }
88
89
90 @Override
91 protected Entry getNext(final LaneRecord<?> record, final Length position, final Integer counter) throws GTUException
92 {
93 int n;
94 LaneBasedGTU next;
95 Length pos;
96 boolean plus = record.getDirection().isPlus();
97 if (counter == null)
98 {
99 Length searchPos = (plus ? position.plus(this.margin) : position.minus(this.margin));
100 next = record.getLane().getGtuAhead(searchPos, record.getDirection(), RelativePosition.FRONT,
101 record.getLane().getParentLink().getSimulator().getSimulatorTime());
102 if (next == null)
103 {
104 return null;
105 }
106 n = record.getLane().indexOfGtu(next);
107 pos = next.position(record.getLane(), next.getRear());
108
109 if (this.ids.contains(next.getId()))
110 {
111
112 pos = plus ? pos.plus(next.getLength()) : pos.minus(next.getLength());
113 return getNext(record, pos, n);
114 }
115 if (this.ignoreIfUpstream)
116 {
117 if (plus ? pos.si < 0.0 : pos.si > record.getLane().getLength().si)
118 {
119 pos = plus ? pos.plus(next.getLength()) : pos.minus(next.getLength());
120 return getNext(record, pos, n);
121 }
122 }
123 }
124 else
125 {
126 n = plus ? counter + 1 : counter - 1;
127 if (n < 0 || n >= record.getLane().numberOfGtus())
128 {
129 return null;
130 }
131 next = record.getLane().getGtu(n);
132 pos = next.position(record.getLane(), next.getRear());
133 }
134 return new Entry(next, n, pos);
135 }
136
137
138 @Override
139 protected Length getDistance(final LaneBasedGTU object, final LaneRecord<?> record, final Length position)
140 {
141 return record.getDistanceToPosition(position).minus(getDx());
142 }
143
144
145 @Override
146 public HeadwayGTU perceive(final LaneBasedGTU perceivingGtu, final LaneBasedGTU object, final Length distance)
147 throws GTUException, ParameterException
148 {
149 return this.headwayGtuType.createDownstreamGtu(perceivingGtu, object, distance);
150 }
151
152 }