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