1 package org.opentrafficsim.road.gtu.lane.perception;
2
3 import java.util.HashMap;
4 import java.util.HashSet;
5 import java.util.Iterator;
6 import java.util.LinkedList;
7 import java.util.Map;
8 import java.util.Queue;
9 import java.util.Set;
10 import java.util.SortedMap;
11 import java.util.TreeMap;
12
13 import org.djunits.value.vdouble.scalar.Length;
14 import org.djutils.exceptions.Try;
15 import org.opentrafficsim.core.gtu.GTUException;
16 import org.opentrafficsim.core.gtu.RelativePosition;
17 import org.opentrafficsim.core.network.Link;
18 import org.opentrafficsim.core.network.route.Route;
19 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
20 import org.opentrafficsim.road.gtu.lane.perception.headway.Headway;
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36 public abstract class AbstractPerceptionIterable<H extends Headway, U, C> extends AbstractPerceptionReiterable<H, U>
37 {
38
39
40 private final LaneRecord<?> root;
41
42
43 private final Length initialPosition;
44
45
46 private final boolean downstream;
47
48
49 private final double maxDistance;
50
51
52 private final RelativePosition relativePosition;
53
54
55 private final Route route;
56
57
58
59
60
61
62
63
64
65
66
67 public AbstractPerceptionIterable(final LaneBasedGTU perceivingGtu, final LaneRecord<?> root, final Length initialPosition,
68 final boolean downstream, final Length maxDistance, final RelativePosition relativePosition, final Route route)
69 {
70 super(perceivingGtu);
71 this.root = root;
72 this.initialPosition = initialPosition;
73 this.downstream = downstream;
74 this.maxDistance = maxDistance.si;
75 this.relativePosition = relativePosition;
76 this.route = route;
77 }
78
79
80 @Override
81 public Iterator<PrimaryIteratorEntry> primaryIterator()
82 {
83 return new PrimaryIterator();
84 }
85
86
87
88
89
90
91
92
93
94
95 protected abstract Entry getNext(LaneRecord<?> record, Length position, C counter) throws GTUException;
96
97
98
99
100
101
102
103
104
105 protected abstract Length getDistance(U object, LaneRecord<?> record, Length position);
106
107
108
109
110
111
112 protected Length getDx()
113 {
114 return this.relativePosition.getDx();
115 }
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130 private class PrimaryIterator implements Iterator<PrimaryIteratorEntry>
131 {
132
133
134 private SortedMap<PrimaryIteratorEntry, LaneRecord<?>> map;
135
136
137 private Map<LaneRecord<?>, Length> positions = new HashMap<>();
138
139
140 private Set<U> returnedItems = new HashSet<>();
141
142
143 private Map<LaneRecord<?>, Queue<PrimaryIteratorEntry>> queues = new HashMap<>();
144
145
146 private Map<LaneRecord<?>, C> counters = new HashMap<>();
147
148
149 private LaneRecord<?> postponedRecord = null;
150
151
152 private Length postponedPosition = null;
153
154
155 PrimaryIterator()
156 {
157
158 }
159
160
161 @Override
162 public boolean hasNext()
163 {
164
165 startProcess();
166
167
168 return !this.map.isEmpty();
169 }
170
171
172 @Override
173 public PrimaryIteratorEntry next()
174 {
175
176 startProcess();
177
178
179 PrimaryIteratorEntry nextEntry = this.map.firstKey();
180 U next = nextEntry.getObject();
181 LaneRecord<?> record = this.map.get(nextEntry);
182 this.map.remove(nextEntry);
183
184
185 Queue<PrimaryIteratorEntry> queue = this.queues.get(next);
186 if (queue != null)
187 {
188 PrimaryIteratorEntry nextNext = queue.poll();
189 this.map.put(nextNext, record);
190 if (queue.isEmpty())
191 {
192 this.queues.remove(record);
193 }
194 preventDuplicateEntries(nextEntry.getObject());
195 return nextNext;
196 }
197
198
199 this.postponedRecord = record;
200 this.postponedPosition = this.positions.get(record);
201 preventDuplicateEntries(nextEntry.getObject());
202 return nextEntry;
203 }
204
205
206
207
208
209 private void preventDuplicateEntries(final U object)
210 {
211 this.returnedItems.add(object);
212 Iterator<PrimaryIteratorEntry> it = this.map.keySet().iterator();
213 while (it.hasNext())
214 {
215 PrimaryIteratorEntry entry = it.next();
216 if (entry.getObject().equals(object))
217 {
218 it.remove();
219 }
220 }
221 }
222
223
224
225
226 @SuppressWarnings("synthetic-access")
227 private void startProcess()
228 {
229 if (this.postponedRecord != null)
230 {
231
232 prepareNext(this.postponedRecord, this.postponedPosition);
233 this.postponedRecord = null;
234 this.postponedPosition = null;
235 }
236 else if (this.map == null)
237 {
238
239 this.map = new TreeMap<>();
240 prepareNext(AbstractPerceptionIterable.this.root, AbstractPerceptionIterable.this.initialPosition);
241 }
242 }
243
244
245
246
247
248
249 @SuppressWarnings("synthetic-access")
250 private void prepareNext(final LaneRecord<?> record, final Length position)
251 {
252 Entry next = Try.assign(() -> AbstractPerceptionIterable.this.getNext(record, position, this.counters.get(record)),
253 "Exception while deriving next object.");
254 if (next == null)
255 {
256 this.counters.remove(record);
257 double distance = AbstractPerceptionIterable.this.downstream
258 ? record.getStartDistance().si + record.getLength().si : -record.getStartDistance().si;
259
260 if (distance < AbstractPerceptionIterable.this.maxDistance)
261 {
262 if (AbstractPerceptionIterable.this.downstream)
263 {
264 for (LaneRecord<?> nextRecord : record.getNext())
265 {
266 if (isOnRoute(nextRecord))
267 {
268 prepareNext(nextRecord, Length.createSI(-1e-9));
269 }
270 }
271 }
272 else
273 {
274 for (LaneRecord<?> nextRecord : record.getPrev())
275 {
276 if (isOnRoute(nextRecord))
277 {
278 prepareNext(nextRecord, nextRecord.getLength());
279 }
280 }
281 }
282 }
283 }
284 else
285 {
286 this.counters.put(record, next.counter);
287 if (next.isSet())
288 {
289 Iterator<U> it = next.set.iterator();
290 U nextNext = it.next();
291 if (!this.returnedItems.contains(nextNext))
292 {
293 Length distance = getDistance(nextNext, record, next.position);
294 if (distance == null
295 || distance.si <= AbstractPerceptionIterable.this.maxDistance)
296 {
297
298 this.map.put(new PrimaryIteratorEntry(nextNext, distance), record);
299 this.positions.put(record, next.position);
300 if (next.set.size() > 1)
301 {
302
303 Queue<PrimaryIteratorEntry> queue = new LinkedList<>();
304 while (it.hasNext())
305 {
306 nextNext = it.next();
307 queue.add(new PrimaryIteratorEntry(nextNext, getDistance(nextNext, record, next.position)));
308 }
309 this.queues.put(record, queue);
310 }
311 }
312 }
313 }
314 else if (!this.returnedItems.contains(next.object))
315 {
316 Length distance = getDistance(next.object, record, next.position);
317 if (distance == null
318 || distance.si <= AbstractPerceptionIterable.this.maxDistance)
319 {
320
321 this.map.put(new PrimaryIteratorEntry(next.object, distance), record);
322 this.positions.put(record, next.position);
323 }
324 }
325 }
326 }
327
328 }
329
330
331
332
333
334
335 final boolean isOnRoute(final LaneRecord<?> record)
336 {
337 if (this.route == null)
338 {
339 return true;
340 }
341 Link link = record.getLane().getParentLink();
342 int from;
343 int to;
344 if (record.getDirection().isPlus())
345 {
346 from = this.route.indexOf(link.getStartNode());
347 to = this.route.indexOf(link.getEndNode());
348 }
349 else
350 {
351 from = this.route.indexOf(link.getEndNode());
352 to = this.route.indexOf(link.getStartNode());
353 }
354 return from != -1 && to != -1 && to - from == 1;
355 }
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370 protected class Entry
371 {
372
373
374 private final Set<U> set;
375
376
377 private final U object;
378
379
380 private final C counter;
381
382
383 private final Length position;
384
385
386
387
388
389
390
391 public Entry(final U object, final C counter, final Length position)
392 {
393 this.set = null;
394 this.object = object;
395 this.counter = counter;
396 this.position = position;
397 }
398
399
400
401
402
403
404
405 public Entry(final Set<U> set, final C counter, final Length position)
406 {
407 this.set = set;
408 this.object = null;
409 this.counter = counter;
410 this.position = position;
411 }
412
413
414
415
416
417 final boolean isSet()
418 {
419 return this.set != null;
420 }
421
422
423
424
425
426 public U getObject()
427 {
428 return this.object;
429 }
430
431
432
433
434
435 public Set<U> getSet()
436 {
437 return this.set;
438 }
439
440 }
441
442 }