1 package org.opentrafficsim.road.gtu.lane.perception.categories.neighbors;
2
3 import java.util.SortedSet;
4
5 import org.djunits.value.vdouble.scalar.Length;
6 import org.djutils.exceptions.Throw;
7 import org.opentrafficsim.base.parameters.ParameterException;
8 import org.opentrafficsim.base.parameters.ParameterTypeLength;
9 import org.opentrafficsim.base.parameters.ParameterTypes;
10 import org.opentrafficsim.core.gtu.GTUException;
11 import org.opentrafficsim.core.gtu.RelativePosition;
12 import org.opentrafficsim.core.network.LateralDirectionality;
13 import org.opentrafficsim.core.network.NetworkException;
14 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
15 import org.opentrafficsim.road.gtu.lane.perception.DownstreamNeighborsIterable;
16 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
17 import org.opentrafficsim.road.gtu.lane.perception.LaneStructureRecord;
18 import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable;
19 import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
20 import org.opentrafficsim.road.gtu.lane.perception.UpstreamNeighborsIterable;
21 import org.opentrafficsim.road.gtu.lane.perception.categories.LaneBasedAbstractPerceptionCategory;
22 import org.opentrafficsim.road.gtu.lane.perception.categories.neighbors.NeighborsUtil.DistanceGTU;
23 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
24
25
26
27
28
29
30
31
32
33
34
35
36 public class DirectNeighborsPerception extends LaneBasedAbstractPerceptionCategory implements NeighborsPerception
37 {
38
39
40 private static final long serialVersionUID = 20160811L;
41
42
43 protected static final ParameterTypeLength LOOKAHEAD = ParameterTypes.LOOKAHEAD;
44
45
46 protected static final ParameterTypeLength LOOKBACK = ParameterTypes.LOOKBACK;
47
48
49 private final HeadwayGtuType headwayGtuType;
50
51
52
53
54
55 public DirectNeighborsPerception(final LanePerception perception, final HeadwayGtuType headwayGtuType)
56 {
57 super(perception);
58 this.headwayGtuType = headwayGtuType;
59 }
60
61
62 @Override
63 public final void updateAll() throws GTUException, NetworkException, ParameterException
64 {
65
66 }
67
68
69 @Override
70 public final SortedSet<HeadwayGTU> getFirstLeaders(final LateralDirectionality lat)
71 throws ParameterException, NullPointerException, IllegalArgumentException
72 {
73 checkLateralDirectionality(lat);
74 return computeIfAbsent("firstLeaders", () -> computeFirstLeaders(lat), lat);
75 }
76
77
78
79
80
81
82 private SortedSet<HeadwayGTU> computeFirstLeaders(final LateralDirectionality lat)
83 {
84 try
85 {
86 return NeighborsUtil.perceive(
87 NeighborsUtil.getFirstDownstreamGTUs(
88 getPerception().getLaneStructure().getFirstRecord(new RelativeLane(lat, 1)), getGtu().getFront(),
89 getGtu().getFront(), RelativePosition.REAR, getTimestamp()),
90 this.headwayGtuType, getGtu(), true);
91 }
92 catch (ParameterException | GTUException | IllegalArgumentException exception)
93 {
94 throw new RuntimeException("Unexpected exception while computing first leaders.", exception);
95 }
96 }
97
98
99 @Override
100 public final SortedSet<HeadwayGTU> getFirstFollowers(final LateralDirectionality lat)
101 throws ParameterException, NullPointerException, IllegalArgumentException
102 {
103 checkLateralDirectionality(lat);
104 return computeIfAbsent("firstFollowers", () -> computeFirstFollowers(lat), lat);
105 }
106
107
108
109
110
111
112 private SortedSet<HeadwayGTU> computeFirstFollowers(final LateralDirectionality lat)
113 {
114 try
115 {
116 return NeighborsUtil.perceive(
117 NeighborsUtil.getFirstUpstreamGTUs(
118 getPerception().getLaneStructure().getFirstRecord(new RelativeLane(lat, 1)), getGtu().getRear(),
119 getGtu().getRear(), RelativePosition.FRONT, getTimestamp()),
120 this.headwayGtuType, getGtu(), false);
121 }
122 catch (ParameterException | GTUException | IllegalArgumentException exception)
123 {
124 throw new RuntimeException("Unexpected exception while computing first followers.", exception);
125 }
126 }
127
128
129 @Override
130 public final boolean isGtuAlongside(final LateralDirectionality lat)
131 throws ParameterException, NullPointerException, IllegalArgumentException
132 {
133 checkLateralDirectionality(lat);
134 return computeIfAbsent("gtuAlongside", () -> computeGtuAlongside(lat), lat);
135 }
136
137
138
139
140
141
142 public boolean computeGtuAlongside(final LateralDirectionality lat)
143 {
144 try
145 {
146
147 SortedSet<DistanceGTU> headwaySet = NeighborsUtil.getFirstDownstreamGTUs(
148 getPerception().getLaneStructure().getFirstRecord(new RelativeLane(lat, 1)), getGtu().getRear(),
149 getGtu().getFront(), RelativePosition.FRONT, getTimestamp());
150 if (!headwaySet.isEmpty() && headwaySet.first().getDistance().le0())
151 {
152 return true;
153 }
154
155 headwaySet = NeighborsUtil.getFirstUpstreamGTUs(
156 getPerception().getLaneStructure().getFirstRecord(new RelativeLane(lat, 1)), getGtu().getFront(),
157 getGtu().getRear(), RelativePosition.REAR, getTimestamp());
158 if (!headwaySet.isEmpty() && headwaySet.first().getDistance().le0())
159 {
160 return true;
161 }
162 }
163 catch (ParameterException | GTUException | IllegalArgumentException exception)
164 {
165 throw new RuntimeException("Unexpected exception while computing gtu alongside.", exception);
166 }
167
168 return false;
169 }
170
171
172 @Override
173 public final PerceptionCollectable<HeadwayGTU, LaneBasedGTU> getLeaders(final RelativeLane lane)
174 {
175 Throw.whenNull(lane, "Lane may not be null.");
176 return computeIfAbsent("leaders", () -> computeLeaders(lane), lane);
177 }
178
179
180
181
182
183
184 private PerceptionCollectable<HeadwayGTU, LaneBasedGTU> computeLeaders(final RelativeLane lane)
185 {
186 try
187 {
188 if (!getPerception().getLaneStructure().getExtendedCrossSection().contains(lane))
189 {
190 return null;
191 }
192 LaneStructureRecord record = getPerception().getLaneStructure().getFirstRecord(lane);
193 Length pos = record.getStartDistance().neg();
194 pos = record.getDirection().isPlus() ? pos.plus(getGtu().getFront().getDx())
195 : pos.minus(getGtu().getFront().getDx());
196 boolean ignoreIfUpstream = true;
197 return new DownstreamNeighborsIterable(getGtu(), record, Length.max(Length.ZERO, pos),
198 getGtu().getParameters().getParameter(LOOKAHEAD), getGtu().getFront(), this.headwayGtuType, lane,
199 ignoreIfUpstream);
200 }
201 catch (ParameterException | GTUException exception)
202 {
203 throw new RuntimeException("Unexpected exception while computing gtu alongside.", exception);
204 }
205 }
206
207
208 @Override
209 public final PerceptionCollectable<HeadwayGTU, LaneBasedGTU> getFollowers(final RelativeLane lane)
210 {
211 Throw.whenNull(lane, "Lane may not be null.");
212 return computeIfAbsent("followers", () -> computeFollowers(lane), lane);
213 }
214
215
216
217
218
219
220 private PerceptionCollectable<HeadwayGTU, LaneBasedGTU> computeFollowers(final RelativeLane lane)
221 {
222 try
223 {
224 if (!getPerception().getLaneStructure().getExtendedCrossSection().contains(lane))
225 {
226 return null;
227 }
228 Throw.whenNull(lane, "Lane may not be null.");
229 LaneStructureRecord record = getPerception().getLaneStructure().getFirstRecord(lane);
230 Length pos;
231 pos = record.getStartDistance().neg();
232 pos = record.getDirection().isPlus() ? pos.plus(getGtu().getFront().getDx())
233 : pos.minus(getGtu().getFront().getDx());
234 return new UpstreamNeighborsIterable(getGtu(), record, Length.max(Length.ZERO, pos),
235 getGtu().getParameters().getParameter(LOOKBACK), getGtu().getRear(), this.headwayGtuType, lane);
236 }
237 catch (ParameterException | GTUException exception)
238 {
239 throw new RuntimeException("Unexpected exception while computing gtu alongside.", exception);
240 }
241 }
242
243
244
245
246
247
248
249
250 private void checkLateralDirectionality(final LateralDirectionality lat)
251 throws ParameterException, NullPointerException, IllegalArgumentException
252 {
253
254 Throw.whenNull(lat, "Lateral directionality may not be null.");
255 Throw.when(lat.equals(LateralDirectionality.NONE), IllegalArgumentException.class,
256 "Lateral directionality may not be NONE.");
257 Throw.when(
258 (lat.equals(LateralDirectionality.LEFT)
259 && !getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.LEFT))
260 || (lat.equals(LateralDirectionality.RIGHT)
261 && !getPerception().getLaneStructure().getExtendedCrossSection().contains(RelativeLane.RIGHT)),
262 IllegalArgumentException.class, "Lateral directionality may only point to an existing adjacent lane.");
263 }
264
265
266 @Override
267 public final String toString()
268 {
269 return "DirectNeighborsPesrception";
270 }
271
272 }