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