1 package org.opentrafficsim.road.gtu.lane.perception;
2
3 import java.io.Serializable;
4 import java.util.HashMap;
5 import java.util.HashSet;
6 import java.util.Iterator;
7 import java.util.Map;
8 import java.util.Set;
9 import java.util.SortedSet;
10 import java.util.TreeMap;
11 import java.util.TreeSet;
12
13 import org.djunits.value.vdouble.scalar.Length;
14 import org.opentrafficsim.core.gtu.GTU;
15 import org.opentrafficsim.core.gtu.GTUException;
16 import org.opentrafficsim.core.gtu.RelativePosition;
17 import org.opentrafficsim.core.network.route.Route;
18 import org.opentrafficsim.road.network.lane.CrossSectionLink;
19 import org.opentrafficsim.road.network.lane.Lane;
20 import org.opentrafficsim.road.network.lane.object.LaneBasedObject;
21
22 import nl.tudelft.simulation.language.Throw;
23
24 /**
25 * This data structure can clearly indicate the lane structure ahead of us, e.g. in the following situation:
26 *
27 * <pre>
28 * (---- a ----)(---- b ----)(---- c ----)(---- d ----)(---- e ----)(---- f ----)(---- g ----)
29 * __________ __________
30 * / _________ 1 / _________ 2
31 * / / / /
32 * __________/ / _______________________/ /
33 * 1 ____________ ____________ /_ _ _ _ _ _/____________ /_ _ _ _ _ _ _ _ _ _ _ _ /
34 * 0 |_ _X_ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ \____________
35 * -1 |____________|_ _ _ _ _ _ |____________|____________| __________|____________|____________| 3
36 * -2 / __________/ \ \
37 * ________/ / \ \___________
38 * 5 _________/ \____________ 4
39 * </pre>
40 *
41 * When the GTU is looking ahead, it needs to know that when it continues to destination 3, it needs to shift one lane to the
42 * right at some point, but <b>not</b> two lanes to the right in link b, and not later than at the end of link f. When it needs
43 * to go to destination 1, it needs to shift to the left in link c. When it has to go to destination 2, it has to shift to the
44 * left, but not earlier than at link e. At node [de], it is possible to leave the rightmost lane of link e, and go to
45 * destination 4. The rightmost lane just splits into two lanes at the end of link d, and the GTU can either continue driving to
46 * destination 3, turn right to destination 4. This means that the right lane of link d has <b>two</b> successor lanes.
47 * <p>
48 * In the data structures, lanes are numbered laterally. Suppose that the lane where vehicle X resides would be number 0.
49 * Consistent with "left is positive" for angles, the lane right of X would have number -1, and entry 5 would have number -2.
50 * <p>
51 * In the data structure, this can be indicated as follows (N = next, P = previous, L = left, R = right, D = lane drop, . =
52 * continued but not in this structure). The merge lane in b is considered "off limits" for the GTUs on the "main" lane -1; the
53 * "main" lane 0 is considered off limits from the exit lanes on c, e, and f. Still, we need to maintain pointers to these
54 * lanes, as we are interested in the GTUs potentially driving next to us, feeding into our lane, etc.
55 *
56 * <pre>
57 * 1 0 -1 -2
58 *
59 * ROOT
60 * _____|_____ ___________ ___________
61 * |_-_|_._|_R_|----|_L_|_._|_-_| |_-_|_._|_-_| a
62 * | | |
63 * _____V_____ _____V_____ _____V_____
64 * |_-_|_N_|_R_|----|_L_|_N_|_R_|<---|_L_|_D_|_-_| b
65 * | |
66 * ___________ _____V_____ _____V_____
67 * |_-_|_N_|_R_|<---|_L_|_N_|_R_|----|_L_|_N_|_-_| c
68 * | | |
69 * _____V_____ _____V_____ _____V_____
70 * |_-_|_._|_-_| |_-_|_N_|_R_|----|_L_|_NN|_-_| d
71 * | ||_______________
72 * ___________ _____V_____ _____V_____ _____V_____
73 * |_-_|_N_|_R_|<---|_L_|_N_|_R_|----|_L_|_N_|_-_| |_-_|_N_|_-_| e
74 * | | | |
75 * _____V_____ _____V_____ _____V_____ _____V_____
76 * |_-_|_N_|_R_|<---|_L_|_D_|_R_|----|_L_|_N_|_-_| |_-_|_._|_-_| f
77 * | |
78 * _____V_____ _____V_____
79 * |_-_|_._|_-_| |_-_|_._|_-_| g
80 * </pre>
81 * <p>
82 * Copyright (c) 2013-2016 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
83 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
84 * </p>
85 * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
86 * initial version Feb 20, 2016 <br>
87 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
88 */
89 public class LaneStructure implements Serializable
90 {
91 /** */
92 private static final long serialVersionUID = 20160400L;
93
94 /** The lanes from which we observe the situation. */
95 private final LaneStructureRecord rootLSR;
96
97 /** Look ahead distance. */
98 private Length lookAhead;
99
100 /** Lane structure records of the cross section. */
101 private TreeMap<RelativeLane, LaneStructureRecord> crossSectionRecords = new TreeMap<>();
102
103 /** Lane structure records grouped per relative lane. */
104 private final Map<RelativeLane, Set<LaneStructureRecord>> relativeLaneMap = new HashMap<>();
105
106 /**
107 * @param rootLSR the root record.
108 * @param lookAhead look ahead distance
109 */
110 public LaneStructure(final LaneStructureRecord rootLSR, final Length lookAhead)
111 {
112 this.rootLSR = rootLSR;
113 this.lookAhead = lookAhead;
114 }
115
116 /**
117 * @return rootLSR
118 */
119 public final LaneStructureRecord getRootLSR()
120 {
121 return this.rootLSR;
122 }
123
124 /**
125 * Returns the cross section.
126 * @return cross section
127 */
128 public final SortedSet<RelativeLane> getCrossSection()
129 {
130 return this.crossSectionRecords.navigableKeySet();
131 }
132
133 /**
134 * @param lane lane to check
135 * @return record at given lane
136 * @throws GTUException if the lane is not in the cross section
137 */
138 public final LaneStructureRecord getLaneLSR(final RelativeLane lane) throws GTUException
139 {
140 Throw.when(!this.crossSectionRecords.containsKey(lane), GTUException.class,
141 "The requested lane %s is not in the most recent cross section.", lane);
142 return this.crossSectionRecords.get(lane);
143 }
144
145 /**
146 * Removes all mappings to relative lanes that are not in the most recent cross section.
147 * @param map map to clear mappings from
148 */
149 public final void removeInvalidMappings(final Map<RelativeLane, ?> map)
150 {
151 Iterator<RelativeLane> iterator = map.keySet().iterator();
152 while (iterator.hasNext())
153 {
154 RelativeLane lane = iterator.next();
155 if (!this.crossSectionRecords.containsKey(lane))
156 {
157 iterator.remove();
158 }
159 }
160 }
161
162 /**
163 * Adds a lane structure record in a mapping from relative lanes. The record is also added to the current cross section if
164 * the start distance is negative, and the start distance plus length is positive. If the relative lane is already in the
165 * current cross section, it is <b>not</b> overwritten.
166 * @param lsr lane structure record
167 * @param relativeLane relative lane
168 */
169 public final void addLaneStructureRecord(final LaneStructureRecord lsr, final RelativeLane relativeLane)
170 {
171 if (!this.relativeLaneMap.containsKey(relativeLane))
172 {
173 this.relativeLaneMap.put(relativeLane, new HashSet<>());
174 }
175 this.relativeLaneMap.get(relativeLane).add(lsr);
176 if (lsr.getStartDistance().le(Length.ZERO) && lsr.getStartDistance().plus(lsr.getLane().getLength()).ge(Length.ZERO)
177 && !this.crossSectionRecords.containsKey(relativeLane))
178 {
179 this.crossSectionRecords.put(relativeLane, lsr);
180 }
181 }
182
183 /**
184 * Retrieve objects on a lane of a specific type. Returns objects over a maximum length of the look ahead distance
185 * downstream from the relative position, or as far as the lane map goes.
186 * @param lane lane
187 * @param clazz class of objects to find
188 * @param gtu gtu
189 * @param pos relative position to start search from
190 * @param <T> type of objects to find
191 * @return Sorted set of objects of requested type
192 * @throws GTUException if lane is not in current set
193 */
194 @SuppressWarnings("unchecked")
195 public final <T extends LaneBasedObject> SortedSet<Entry<T>> getDownstreamObjects(final RelativeLane lane,
196 final Class<T> clazz, final GTU gtu, final RelativePosition.TYPE pos) throws GTUException
197 {
198 LaneStructureRecord record = this.getLaneLSR(lane);
199 Length ds = gtu.getRelativePositions().get(pos).getDx().minus(gtu.getReference().getDx());
200 // the list is ordered, but only for DIR_PLUS, need to do our own ordering
201 Length minimumPosition;
202 Length maximumPosition;
203 if (record.getDirection().isPlus())
204 {
205 minimumPosition = ds.minus(record.getStartDistance());
206 maximumPosition = record.getLane().getLength();
207 }
208 else
209 {
210 minimumPosition = Length.ZERO;
211 maximumPosition = record.getLane().getLength().plus(record.getStartDistance()).minus(ds);
212 }
213 SortedSet<Entry<T>> set = new TreeSet<>();
214 Length distance;
215 for (LaneBasedObject object : record.getLane().getLaneBasedObjects(minimumPosition, maximumPosition))
216 {
217 distance = record.getDistanceToPosition(object.getLongitudinalPosition()).minus(ds);
218 if (clazz.isAssignableFrom(object.getClass()) && distance.le(this.lookAhead)
219 && ((record.getDirection().isPlus() && object.getDirection().isForwardOrBoth())
220 || (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth())))
221 {
222 // unchecked, but the above isAssignableFrom assures correctness
223 set.add(new Entry<>(distance, (T) object));
224 }
225 }
226 getDownstreamObjectsRecursive(set, record, clazz, ds);
227 return set;
228 }
229
230 /**
231 * Retrieve objects on a lane of a specific type. Returns objects over a maximum length of the look ahead distance
232 * downstream from the relative position, or as far as the lane map goes. Objects on links not on the route are ignored.
233 * @param lane lane
234 * @param clazz class of objects to find
235 * @param gtu gtu
236 * @param pos relative position to start search from
237 * @param <T> type of objects to find
238 * @param route the route
239 * @return Sorted set of objects of requested type
240 * @throws GTUException if lane is not in current set
241 */
242 public final <T extends LaneBasedObject> SortedSet<Entry<T>> getDownstreamObjectsOnRoute(final RelativeLane lane,
243 final Class<T> clazz, final GTU gtu, final RelativePosition.TYPE pos, final Route route) throws GTUException
244 {
245 SortedSet<Entry<T>> set = getDownstreamObjects(lane, clazz, gtu, pos);
246 if (route != null)
247 {
248 Iterator<Entry<T>> iterator = set.iterator();
249 while (iterator.hasNext())
250 {
251 Entry<T> entry = iterator.next();
252 CrossSectionLink link = entry.getLaneBasedObject().getLane().getParentLink();
253 if (!route.contains(link.getStartNode()) || !route.contains(link.getEndNode())
254 || Math.abs(route.indexOf(link.getStartNode()) - route.indexOf(link.getEndNode())) != 1)
255 {
256 iterator.remove();
257 }
258 }
259 }
260 return set;
261 }
262
263 /**
264 * Recursive search for lane based objects downstream.
265 * @param set set to store entries into
266 * @param record current record
267 * @param clazz class of objects to find
268 * @param ds distance from reference to chosen relative position
269 * @param <T> type of objects to find
270 */
271 @SuppressWarnings("unchecked")
272 private <T extends LaneBasedObject> void getDownstreamObjectsRecursive(final SortedSet<Entry<T>> set,
273 final LaneStructureRecord record, final Class<T> clazz, final Length ds)
274 {
275 if (record.getNext().isEmpty() || record.getNext().get(0).getStartDistance().gt(this.lookAhead))
276 {
277 return;
278 }
279 for (LaneStructureRecord next : record.getNext())
280 {
281 Length distance;
282 for (LaneBasedObject object : next.getLane().getLaneBasedObjects())
283 {
284 distance = next.getDistanceToPosition(object.getLongitudinalPosition()).minus(ds);
285 if (clazz.isAssignableFrom(object.getClass()) && distance.le(this.lookAhead)
286 && ((record.getDirection().isPlus() && object.getDirection().isForwardOrBoth())
287 || (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth())))
288 {
289 // unchecked, but the above isAssignableFrom assures correctness
290 set.add(new Entry<>(distance, (T) object));
291 }
292 }
293 getDownstreamObjectsRecursive(set, next, clazz, ds);
294 }
295 }
296
297 /**
298 * Retrieve objects on a lane of a specific type. Returns upstream objects from the relative position for as far as the lane
299 * map goes. Distances to upstream objects are given as positive values.
300 * @param lane lane
301 * @param clazz class of objects to find
302 * @param gtu gtu
303 * @param pos relative position to start search from
304 * @param <T> type of objects to find
305 * @return Sorted set of objects of requested type
306 * @throws GTUException if lane is not in current set
307 */
308 @SuppressWarnings("unchecked")
309 public final <T extends LaneBasedObject> SortedSet<Entry<T>> getUpstreamObjects(final RelativeLane lane,
310 final Class<T> clazz, final GTU gtu, final RelativePosition.TYPE pos) throws GTUException
311 {
312 LaneStructureRecord record = this.getLaneLSR(lane);
313 Length ds = gtu.getReference().getDx().minus(gtu.getRelativePositions().get(pos).getDx());
314 // the list is ordered, but only for DIR_PLUS, need to do our own ordering
315 Length minimumPosition;
316 Length maximumPosition;
317 if (record.getDirection().isPlus())
318 {
319 minimumPosition = Length.ZERO;
320 maximumPosition = record.getStartDistance().neg().minus(ds);
321 }
322 else
323 {
324 minimumPosition = record.getLane().getLength().plus(record.getStartDistance()).plus(ds);
325 maximumPosition = record.getLane().getLength();
326 }
327 SortedSet<Entry<T>> set = new TreeSet<>();
328 Length distance;
329 for (LaneBasedObject object : record.getLane().getLaneBasedObjects(minimumPosition, maximumPosition))
330 {
331 if (clazz.isAssignableFrom(object.getClass())
332 && ((record.getDirection().isPlus() && object.getDirection().isForwardOrBoth())
333 || (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth())))
334 {
335 distance = record.getDistanceToPosition(object.getLongitudinalPosition()).neg().minus(ds);
336 // unchecked, but the above isAssignableFrom assures correctness
337 set.add(new Entry<>(distance, (T) object));
338 }
339 }
340 getUpstreamObjectsRecursive(set, record, clazz, ds);
341 return set;
342 }
343
344 /**
345 * Recursive search for lane based objects upstream.
346 * @param set set to store entries into
347 * @param record current record
348 * @param clazz class of objects to find
349 * @param ds distance from reference to chosen relative position
350 * @param <T> type of objects to find
351 */
352 @SuppressWarnings("unchecked")
353 private <T extends LaneBasedObject> void getUpstreamObjectsRecursive(final SortedSet<Entry<T>> set,
354 final LaneStructureRecord record, final Class<T> clazz, final Length ds)
355 {
356 for (LaneStructureRecord prev : record.getPrev())
357 {
358 Length distance;
359 for (LaneBasedObject object : prev.getLane().getLaneBasedObjects())
360 {
361 if (clazz.isAssignableFrom(object.getClass())
362 && ((record.getDirection().isPlus() && object.getDirection().isForwardOrBoth())
363 || (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth())))
364 {
365 distance = prev.getDistanceToPosition(object.getLongitudinalPosition()).neg().minus(ds);
366 // unchecked, but the above isAssignableFrom assures correctness
367 set.add(new Entry<>(distance, (T) object));
368 }
369 }
370 getUpstreamObjectsRecursive(set, prev, clazz, ds);
371 }
372 }
373
374 /** {@inheritDoc} */
375 @Override
376 public final String toString()
377 {
378 return "LaneStructure [rootLSR=" + this.rootLSR + "]";
379 }
380
381 /**
382 * <p>
383 * Copyright (c) 2013-2016 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
384 * <br>
385 * BSD-style license. See <a href="http://opentrafficsim.org/docs/current/license.html">OpenTrafficSim License</a>.
386 * <p>
387 * @version $Revision$, $LastChangedDate$, by $Author$, initial version Sep 15, 2016 <br>
388 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
389 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
390 * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
391 * @param <T> class of lane based object contained
392 */
393 public static class Entry<T extends LaneBasedObject> implements Comparable<Entry<T>>
394 {
395
396 /** Distance to lane based object. */
397 private final Length distance;
398
399 /** Lane based object. */
400 private final T laneBasedObject;
401
402 /**
403 * @param distance distance to lane based object
404 * @param laneBasedObject lane based object
405 */
406 public Entry(final Length distance, final T laneBasedObject)
407 {
408 this.distance = distance;
409 this.laneBasedObject = laneBasedObject;
410 }
411
412 /**
413 * @return distance.
414 */
415 public final Length getDistance()
416 {
417 return this.distance;
418 }
419
420 /**
421 * @return laneBasedObject.
422 */
423 public final T getLaneBasedObject()
424 {
425 return this.laneBasedObject;
426 }
427
428 /** {@inheritDoc} */
429 @Override
430 public final int hashCode()
431 {
432 final int prime = 31;
433 int result = 1;
434 result = prime * result + ((this.distance == null) ? 0 : this.distance.hashCode());
435 result = prime * result + ((this.laneBasedObject == null) ? 0 : this.laneBasedObject.hashCode());
436 return result;
437 }
438
439 /** {@inheritDoc} */
440 @Override
441 public final boolean equals(final Object obj)
442 {
443 if (this == obj)
444 {
445 return true;
446 }
447 if (obj == null)
448 {
449 return false;
450 }
451 if (getClass() != obj.getClass())
452 {
453 return false;
454 }
455 Entry<?> other = (Entry<?>) obj;
456 if (this.distance == null)
457 {
458 if (other.distance != null)
459 {
460 return false;
461 }
462 }
463 else if (!this.distance.equals(other.distance))
464 {
465 return false;
466 }
467 if (this.laneBasedObject == null)
468 {
469 if (other.laneBasedObject != null)
470 {
471 return false;
472 }
473 }
474 // laneBasedObject does not implement equals...
475 else if (!this.laneBasedObject.equals(other.laneBasedObject))
476 {
477 return false;
478 }
479 return true;
480 }
481
482 /** {@inheritDoc} */
483 @Override
484 public final int compareTo(final Entry<T> arg)
485 {
486 int d = this.distance.compareTo(arg.distance);
487 if (d != 0 || this.laneBasedObject.equals(arg.laneBasedObject))
488 {
489 return d; // different distance (-1 or 1), or same distance but also equal lane based object (0)
490 }
491 return 1; // same distance, unequal lane based object (1)
492 }
493
494 /** {@inheritDoc} */
495 @Override
496 public final String toString()
497 {
498 return "LaneStructure.Entry [distance=" + this.distance + ", laneBasedObject=" + this.laneBasedObject + "]";
499 }
500
501 }
502
503 }