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
12 import org.djunits.value.vdouble.scalar.Length;
13 import org.djunits.value.vdouble.scalar.Time;
14 import org.opentrafficsim.core.gtu.GTUException;
15 import org.opentrafficsim.road.network.lane.object.LaneBasedObject;
16
17 import nl.tudelft.simulation.language.Throw;
18
19 /**
20 * This data structure can clearly indicate the lane structure ahead of us, e.g. in the following situation:
21 *
22 * <pre>
23 * (---- a ----)(---- b ----)(---- c ----)(---- d ----)(---- e ----)(---- f ----)(---- g ----)
24 * __________ __________
25 * / _________ 1 / _________ 2
26 * / / / /
27 * __________/ / _______________________/ /
28 * 1 ____________ ____________ /_ _ _ _ _ _/____________ /_ _ _ _ _ _ _ _ _ _ _ _ /
29 * 0 |_ _X_ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ \____________
30 * -1 |____________|_ _ _ _ _ _ |____________|____________| __________|____________|____________| 3
31 * -2 / __________/ \ \
32 * ________/ / \ \___________
33 * 5 _________/ \____________ 4
34 * </pre>
35 *
36 * 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
37 * 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
38 * 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
39 * 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
40 * destination 4. The rightmost lane just splits into two lanes at the end of link d, and the GTU can either continue driving to
41 * destination 3, turn right to destination 4. This means that the right lane of link d has <b>two</b> successor lanes.
42 * <p>
43 * In the data structures, lanes are numbered laterally. Suppose that the lane where vehicle X resides would be number 0.
44 * Consistent with "left is positive" for angles, the lane right of X would have number -1, and entry 5 would have number -2.
45 * <p>
46 * In the data structure, this can be indicated as follows (N = next, P = previous, L = left, R = right, D = lane drop, . =
47 * continued but not in this structure). The merge lane in b is considered "off limits" for the GTUs on the "main" lane -1; the
48 * "main" lane 0 is considered off limits from the exit lanes on c, e, and f. Still, we need to maintain pointers to these
49 * lanes, as we are interested in the GTUs potentially driving next to us, feeding into our lane, etc.
50 *
51 * <pre>
52 * 1 0 -1 -2
53 *
54 * ROOT
55 * _____|_____ ___________ ___________
56 * |_-_|_._|_R_|----|_L_|_._|_-_| |_-_|_._|_-_| a
57 * | | |
58 * _____V_____ _____V_____ _____V_____
59 * |_-_|_N_|_R_|----|_L_|_N_|_R_|<---|_L_|_D_|_-_| b
60 * | |
61 * ___________ _____V_____ _____V_____
62 * |_-_|_N_|_R_|<---|_L_|_N_|_R_|----|_L_|_N_|_-_| c
63 * | | |
64 * _____V_____ _____V_____ _____V_____
65 * |_-_|_._|_-_| |_-_|_N_|_R_|----|_L_|_NN|_-_| d
66 * | ||_______________
67 * ___________ _____V_____ _____V_____ _____V_____
68 * |_-_|_N_|_R_|<---|_L_|_N_|_R_|----|_L_|_N_|_-_| |_-_|_N_|_-_| e
69 * | | | |
70 * _____V_____ _____V_____ _____V_____ _____V_____
71 * |_-_|_N_|_R_|<---|_L_|_D_|_R_|----|_L_|_N_|_-_| |_-_|_._|_-_| f
72 * | |
73 * _____V_____ _____V_____
74 * |_-_|_._|_-_| |_-_|_._|_-_| g
75 *
76 *
77 * </pre>
78 * <p>
79 * Copyright (c) 2013-2016 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
80 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
81 * </p>
82 * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
83 * initial version Feb 20, 2016 <br>
84 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
85 */
86 public class LaneStructure implements EnvironmentState, Serializable
87 {
88 /** */
89 private static final long serialVersionUID = 20160400L;
90
91 /** The lanes from which we observe the situation. */
92 private LaneStructureRecord rootLSR;
93
94 /** Look ahead distance. */
95 private Length lookAhead;
96
97 /** Lane structure records of the cross section. */
98 private TreeMap<RelativeLane, LaneStructureRecord> crossSectionRecords = new TreeMap<>();
99
100 // /** Time of last cross section update. */
101 // private Time crossSectionUpdateTime;
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 //updateCrossSection(now);
131 return this.crossSectionRecords.navigableKeySet();
132 }
133
134 // /**
135 // * @param now current time to check if the cross section needs to be updated
136 // */
137 // private void updateCrossSection(final Time now)
138 // {
139 // if (this.crossSectionRecords == null || now.gt(this.crossSectionUpdateTime))
140 // {
141 // this.crossSectionRecords = new TreeMap<>();
142 // // current lane
143 // this.crossSectionRecords.put(RelativeLane.CURRENT, getRootLSR());
144 // // left
145 // LaneStructureRecord lane = getRootLSR();
146 // int left = 1;
147 // while (lane.getLeft() != null)
148 // {
149 // RelativeLane relLane = new RelativeLane(LateralDirectionality.LEFT, left);
150 // this.crossSectionRecords.put(relLane, lane.getLeft());
151 // left++;
152 // lane = lane.getLeft();
153 // }
154 // addFirstMergeToCrossSection(lane, LateralDirectionality.LEFT, left);
155 // // right
156 // lane = getRootLSR();
157 // int right = 1;
158 // while (lane.getRight() != null)
159 // {
160 // RelativeLane relLane = new RelativeLane(LateralDirectionality.RIGHT, right);
161 // this.crossSectionRecords.put(relLane, lane.getRight());
162 // right++;
163 // lane = lane.getRight();
164 // }
165 // addFirstMergeToCrossSection(lane, LateralDirectionality.RIGHT, right);
166 // }
167 // }
168 //
169 // /**
170 // * Adds a single lane of the other link to the current cross section at a merge.
171 // * @param farMost record on far-most left or right side of current link
172 // * @param dir direction to search in, left or right
173 // * @param n number of lanes in left or right direction that the next lane will be
174 // */
175 // private void
176 // addFirstMergeToCrossSection(final LaneStructureRecord farMost, final LateralDirectionality dir, final int n)
177 // {
178 // Length cumulLengthDown = farMost.getLane().getLength();
179 // LaneStructureRecord next = getNextOnSide(farMost, dir);
180 // LaneStructureRecord mergeRecord = null; // first downstream record past merge
181 // while (next != null)
182 // {
183 // if (next.isLinkMerge())
184 // {
185 // mergeRecord = next;
186 // next = null;
187 // }
188 // else
189 // {
190 // cumulLengthDown = cumulLengthDown.plus(next.getLane().getLength());
191 // next = getNextOnSide(next, dir);
192 // }
193 // }
194 // if (mergeRecord != null)
195 // {
196 // LaneStructureRecord adjacentRecord =
197 // dir.equals(LateralDirectionality.LEFT) ? mergeRecord.getLeft() : mergeRecord.getRight();
198 // if (adjacentRecord == null)
199 // {
200 // // merge is on other side, add nothing
201 // return;
202 // }
203 // adjacentRecord = getPrevOnSide(adjacentRecord, dir);
204 // Length cumulLengthUp = Length.ZERO;
205 // while (adjacentRecord != null)
206 // {
207 // cumulLengthUp = cumulLengthUp.plus(adjacentRecord.getLane().getLength());
208 // if (cumulLengthUp.ge(cumulLengthDown))
209 // {
210 // RelativeLane relLane = new RelativeLane(dir, n);
211 // this.crossSectionRecords.put(relLane, adjacentRecord);
212 // return;
213 // }
214 // adjacentRecord = getPrevOnSide(adjacentRecord, dir);
215 // }
216 // }
217 // }
218 //
219 // /**
220 // * Returns the correct next record for searching the next merge.
221 // * @param lane current lane record
222 // * @param dir direction of search
223 // * @return correct next record for searching the next merge
224 // */
225 // private LaneStructureRecord getNextOnSide(final LaneStructureRecord lane, final LateralDirectionality dir)
226 // {
227 // if (lane.getNext().size() == 1)
228 // {
229 // return lane.getNext().get(0);
230 // }
231 // for (LaneStructureRecord next : lane.getNext())
232 // {
233 // if ((dir.equals(LateralDirectionality.LEFT) && next.getLeft() == null)
234 // || (dir.equals(LateralDirectionality.RIGHT) && next.getRight() == null))
235 // {
236 // return next;
237 // }
238 // }
239 // return null;
240 // }
241 //
242 // /**
243 // * Returns the correct previous record for searching upstream from the next merge.
244 // * @param lane current lane record
245 // * @param dir direction of search
246 // * @return correct previous record for searching upstream from the next merge
247 // */
248 // private LaneStructureRecord getPrevOnSide(final LaneStructureRecord lane, final LateralDirectionality dir)
249 // {
250 // if (lane.getPrev().size() == 1)
251 // {
252 // return lane.getPrev().get(0);
253 // }
254 // for (LaneStructureRecord prev : lane.getPrev())
255 // {
256 // // note: looking left from current link, requires looking right from left adjacent link at merge
257 // if ((dir.equals(LateralDirectionality.LEFT) && prev.getRight() == null)
258 // || (dir.equals(LateralDirectionality.RIGHT) && prev.getLeft() == null))
259 // {
260 // return prev;
261 // }
262 // }
263 // return null;
264 // }
265
266 /**
267 * @param lane lane to check
268 * @param now current time to check if the cross section needs to be updated
269 * @return record at given lane
270 * @throws GTUException if the lane is not in the cross section
271 */
272 public final LaneStructureRecord getLaneLSR(final RelativeLane lane, final Time now) throws GTUException
273 {
274 //updateCrossSection(now);
275 Throw.when(!this.crossSectionRecords.containsKey(lane), GTUException.class,
276 "The requested lane %s is not in the most recent cross section.", lane);
277 return this.crossSectionRecords.get(lane);
278 }
279
280 /**
281 * Removes all mappings to relative lanes that are not in the most recent cross section.
282 * @param map map to clear mappings from
283 */
284 public final void removeInvalidMappings(final Map<RelativeLane, ?> map)
285 {
286 Iterator<RelativeLane> iterator = map.keySet().iterator();
287 while (iterator.hasNext())
288 {
289 RelativeLane lane = iterator.next();
290 if (!this.crossSectionRecords.containsKey(lane))
291 {
292 iterator.remove();
293 }
294 }
295 }
296
297 /**
298 * Adds a lane structure record in a mapping from relative lanes.
299 * @param lsr lane structure record
300 * @param relativeLane relative lane
301 */
302 public final void addLaneStructureRecord(final LaneStructureRecord lsr, final RelativeLane relativeLane)
303 {
304 if (!this.relativeLaneMap.containsKey(relativeLane))
305 {
306 this.relativeLaneMap.put(relativeLane, new HashSet<>());
307 }
308 this.relativeLaneMap.get(relativeLane).add(lsr);
309 if (lsr.getStartDistance().le(Length.ZERO) && lsr.getStartDistance().plus(lsr.getLane().getLength()).ge(Length.ZERO))
310 {
311 this.crossSectionRecords.put(relativeLane, lsr);
312 }
313 }
314
315 /**
316 * {@inheritDoc}
317 * Returns objects over a maximum length of the look ahead distance downstream, or as far as the lane map goes. Upstream,
318 * only the map limits included objects.
319 */
320 @Override
321 public final <T extends LaneBasedObject> TreeMap<Length, Set<T>> getSortedObjects(final ViewingDirection viewingDirection,
322 final RelativeLane relativeLane, final Class<T> clazz)
323 {
324 // TODO
325 return new TreeMap<>();
326 }
327
328 /** {@inheritDoc} */
329 @Override
330 public final String toString()
331 {
332 return "LaneStructure [rootLSR=" + this.rootLSR + "]";
333 }
334 }