1 package org.opentrafficsim.road.gtu.lane.tactical;
2
3 import java.util.ArrayList;
4 import java.util.HashSet;
5 import java.util.Iterator;
6 import java.util.List;
7 import java.util.Map;
8 import java.util.Set;
9
10 import org.djunits.value.vdouble.scalar.Length;
11 import org.opentrafficsim.core.geometry.OTSGeometryException;
12 import org.opentrafficsim.core.geometry.OTSLine3D;
13 import org.opentrafficsim.core.gtu.GTUDirectionality;
14 import org.opentrafficsim.core.gtu.GTUException;
15 import org.opentrafficsim.core.gtu.plan.tactical.TacticalPlanner;
16 import org.opentrafficsim.core.network.Link;
17 import org.opentrafficsim.core.network.LinkDirection;
18 import org.opentrafficsim.core.network.NetworkException;
19 import org.opentrafficsim.core.network.OTSNode;
20 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
21 import org.opentrafficsim.road.network.lane.CrossSectionElement;
22 import org.opentrafficsim.road.network.lane.CrossSectionLink;
23 import org.opentrafficsim.road.network.lane.Lane;
24 import org.opentrafficsim.road.network.lane.LaneDirection;
25
26 /**
27 * A lane-based tactical planner generates an operational plan for the lane-based GTU. It can ask the strategic planner for
28 * assistance on the route to take when the network splits. This abstract class contains a number of helper methods that make it
29 * easy to implement a tactical planner.
30 * <p>
31 * Copyright (c) 2013-2015 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
32 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
33 * </p>
34 * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
35 * initial version Nov 25, 2015 <br>
36 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
37 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
38 */
39 public abstract class AbstractLaneBasedTacticalPlanner implements TacticalPlanner
40 {
41 /** */
42 private static final long serialVersionUID = 20151125L;
43
44 /**
45 * Instantiated a tactical planner.
46 */
47 public AbstractLaneBasedTacticalPlanner()
48 {
49 super();
50 }
51
52 /**
53 * The reference lane is the widest lane on which the reference point of the GTU is fully registered. If the reference point
54 * is not fully registered on one of the lanes, return a lane where the reference point is not fully registered as a
55 * fallback option. This can g=for instance happen when the GTU has just been generated, or when the GTU is about to be
56 * destroyed at the end of a lane.
57 * @param gtu the GTU for which to determine the lane on which the GTU's reference point lies
58 * @return the widest lane on which the reference point lies between start and end, or any lane where the GTU is registered
59 * as a fallback option.
60 * @throws GTUException when the GTU's positions cannot be determined or when the GTU is not registered on any lane.
61 */
62 public static Lane getReferenceLane(final LaneBasedGTU gtu) throws GTUException
63 {
64 for (Lane lane : gtu.getLanes().keySet())
65 {
66 double fraction = gtu.fractionalPosition(lane, gtu.getReference());
67 if (fraction >= 0.0 && fraction <= 1.0)
68 {
69 // TODO widest lane in case we are registered on more than one lane with the reference point
70 // TODO lane that leads to our location or not if we are registered on parallel lanes?
71 return lane;
72 }
73 }
74
75 // TODO lane closest to length or 0
76 System.err.println(gtu + " does not have a reference lane with pos between 0 and length...");
77 if (gtu.getLanes().size() > 0)
78 {
79 return gtu.getLanes().keySet().iterator().next();
80 }
81
82 throw new GTUException("The reference point of GTU " + gtu + " is not on any of the lanes on which it is registered");
83 }
84
85 /**
86 * Build a list of lanes forward, with a maximum headway relative to the reference point of the GTU.
87 * @param gtu the gtu for which to calculate the lane list
88 * @param maxHeadway the maximum length for which lanes should be returned
89 * @return an instance that provides the following information for an operational plan: the lanes to follow, and the path to
90 * follow when staying on the same lane.
91 * @throws GTUException when the vehicle is not on one of the lanes on which it is registered
92 * @throws NetworkException when the strategic planner is not able to return a next node in the route
93 */
94 public static LanePathInfo buildLanePathInfo(final LaneBasedGTU gtu, final Length.Rel maxHeadway) throws GTUException,
95 NetworkException
96 {
97 Lane startLane = getReferenceLane(gtu);
98 Length.Rel startPosition = gtu.position(startLane, gtu.getReference());
99 return buildLanePathInfo(gtu, startLane, startPosition, maxHeadway);
100 }
101
102 /**
103 * Build a list of lanes forward, with a maximum headway relative to the reference point of the GTU.
104 * @param gtu the gtu for which to calculate the lane list
105 * @param startLane the lane in which the path starts
106 * @param startPosition the start position on the start lane with the Vehicle's reference point
107 * @param maxHeadway the maximum length for which lanes should be returned
108 * @return an instance that provides the following information for an operational plan: the lanes to follow, and the path to
109 * follow when staying on the same lane.
110 * @throws GTUException when the vehicle is not on one of the lanes on which it is registered
111 * @throws NetworkException when the strategic planner is not able to return a next node in the route
112 */
113 public static LanePathInfo buildLanePathInfo(final LaneBasedGTU gtu, final Lane startLane, final Length.Rel startPosition,
114 final Length.Rel maxHeadway) throws GTUException, NetworkException
115 {
116 return buildLanePathInfo(gtu, maxHeadway, startLane, startLane.fraction(startPosition), gtu.getLanes().get(startLane));
117 }
118
119 /**
120 * Build a list of lanes forward, with a maximum headway relative to the reference point of the GTU.
121 * @param gtu the gtu for which to calculate the lane list
122 * @param startLane the lane in which the path starts
123 * @param startLaneFractionalPosition the fractional position on the start lane
124 * @param startDirectionality the driving direction on the start lane
125 * @param maxHeadway the maximum length for which lanes should be returned
126 * @return an instance that provides the following information for an operational plan: the lanes to follow, and the path to
127 * follow when staying on the same lane.
128 * @throws GTUException when the vehicle is not on one of the lanes on which it is registered
129 * @throws NetworkException when the strategic planner is not able to return a next node in the route
130 * @throws GTUException when the vehicle is not on one of the lanes on which it is registered
131 * @throws NetworkException when the strategic planner is not able to return a next node in the route
132 */
133 public static LanePathInfo buildLanePathInfo(final LaneBasedGTU gtu, final Length.Rel maxHeadway, final Lane startLane,
134 final double startLaneFractionalPosition, final GTUDirectionality startDirectionality) throws GTUException,
135 NetworkException
136 {
137 List<LaneDirection> laneListForward = new ArrayList<>();
138 Lane lane = startLane;
139 GTUDirectionality lastGtuDir = startDirectionality;
140 Length.Rel position = lane.position(startLaneFractionalPosition);
141 Length.Rel startPosition = position;
142 Lane lastLane = lane;
143 laneListForward.add(new LaneDirection(lastLane, lastGtuDir));
144 Length.Rel lengthForward;
145 OTSLine3D path;
146 try
147 {
148 if (lastGtuDir.equals(GTUDirectionality.DIR_PLUS))
149 {
150 lengthForward = lane.getLength().minus(position);
151 path = lane.getCenterLine().extract(position, lane.getLength());
152 }
153 else
154 {
155 lengthForward = position;
156 path = lane.getCenterLine().extract(Length.Rel.ZERO, position).reverse();
157 }
158 }
159 catch (OTSGeometryException exception)
160 {
161 System.err.println(gtu + ": " + exception.getMessage());
162 System.err.println(lane + ", len=" + lane.getLength());
163 System.err.println(position);
164 throw new GTUException(exception);
165 }
166
167 while (lengthForward.lt(maxHeadway))
168 {
169 Map<Lane, GTUDirectionality> lanes =
170 lastGtuDir.equals(GTUDirectionality.DIR_PLUS) ? lane.nextLanes(gtu.getGTUType()) : lane.prevLanes(gtu
171 .getGTUType());
172 if (lanes.size() == 0)
173 {
174 // dead end. return with the list as is.
175 return new LanePathInfo(path, laneListForward, startPosition);
176 }
177 if (lanes.size() == 1)
178 {
179 // ask the strategical planner what the next link should be (if known), because the strategical planner knows
180 // best!
181 LinkDirection ld = null;
182 ld = gtu.getStrategicalPlanner().nextLinkDirection(lane.getParentLink(), lastGtuDir, gtu.getGTUType());
183 lane = lanes.keySet().iterator().next();
184 if (ld != null && !lane.getParentLink().equals(ld.getLink()))
185 {
186 // lane not on route anymore. return with the list as is.
187 return new LanePathInfo(path, laneListForward, startPosition);
188 }
189 }
190 else
191 {
192 // multiple next lanes; ask the strategical planner where to go
193 // note: this is not necessarily a split; it could e.g. be a bike path on a road
194 LinkDirection ld;
195 try
196 {
197 ld = gtu.getStrategicalPlanner().nextLinkDirection(lane.getParentLink(), /* gtu.getLanes().get(lane), */
198 lastGtuDir, gtu.getGTUType());
199 }
200 catch (NetworkException ne)
201 {
202 // no route found.
203 // return the data structure up to this point...
204 return new LanePathInfo(path, laneListForward, startPosition);
205 }
206 Link nextLink = ld.getLink();
207 Lane newLane = null;
208 for (Lane nextLane : lanes.keySet())
209 {
210 if (nextLane.getParentLink().equals(nextLink))
211 {
212 newLane = nextLane;
213 break;
214 }
215 }
216 if (newLane == null)
217 {
218 // we cannot reach the next node on this lane -- we have to make a lane change!
219 // return the data structure up to this point...
220 return new LanePathInfo(path, laneListForward, startPosition);
221 }
222 // otherwise: continue!
223 lane = newLane;
224 }
225
226 // determine direction for the path
227 try
228 {
229 if (lastGtuDir.equals(GTUDirectionality.DIR_PLUS))
230 {
231 if (lastLane.getParentLink().getEndNode().equals(lane.getParentLink().getStartNode()))
232 {
233 // -----> O ----->, GTU moves ---->
234 path = OTSLine3D.concatenate(Lane.MARGIN.si, path, lane.getCenterLine());
235 lastGtuDir = GTUDirectionality.DIR_PLUS;
236 }
237 else
238 {
239 // -----> O <-----, GTU moves ---->
240 path = OTSLine3D.concatenate(Lane.MARGIN.si, path, lane.getCenterLine().reverse());
241 lastGtuDir = GTUDirectionality.DIR_MINUS;
242 }
243 }
244 else
245 {
246 if (lastLane.getParentLink().getStartNode().equals(lane.getParentLink().getStartNode()))
247 {
248 // <----- O ----->, GTU moves ---->
249 path = OTSLine3D.concatenate(Lane.MARGIN.si, path, lane.getCenterLine());
250 lastGtuDir = GTUDirectionality.DIR_PLUS;
251 }
252 else
253 {
254 // <----- O <-----, GTU moves ---->
255 path = OTSLine3D.concatenate(Lane.MARGIN.si, path, lane.getCenterLine().reverse());
256 lastGtuDir = GTUDirectionality.DIR_MINUS;
257 }
258 }
259 lastLane = lane;
260 }
261 catch (OTSGeometryException exception)
262 {
263 throw new GTUException(exception);
264 }
265
266 laneListForward.add(new LaneDirection(lastLane, lastGtuDir));
267 lengthForward = lengthForward.plus(lastLane.getLength());
268
269 }
270 return new LanePathInfo(path, laneListForward, startPosition);
271 }
272
273 /**
274 * Build a list of lanes forward, with a maximum headway.
275 * @param gtu the gtu for which to calculate the lane list
276 * @param maxHeadway the maximum length for which lanes should be returned
277 * @param startLane the first lane in the list
278 * @param startLaneFractionalPosition the fractional position on the start lane
279 * @param startDirectionality the driving direction on the start lane
280 * @return a list of lanes, connected to the startLane and following the path of the StrategicalPlanner.
281 * @throws GTUException when the vehicle is not on one of the lanes on which it is registered
282 * @throws NetworkException when the strategic planner is not able to return a next node in the route
283 */
284 // TODO this method can probably disappear (lots of duplicated code)
285 protected static List<Lane> buildLaneListForwardXXX(final LaneBasedGTU gtu, final Length.Rel maxHeadway,
286 final Lane startLane, final double startLaneFractionalPosition, final GTUDirectionality startDirectionality)
287 throws GTUException, NetworkException
288 {
289 List<Lane> laneListForward = new ArrayList<>();
290 Lane lane = startLane;
291 Lane lastLane = startLane;
292 GTUDirectionality lastGtuDir = startDirectionality;
293 laneListForward.add(lane);
294 Length.Rel position = lane.position(startLaneFractionalPosition);
295 Length.Rel lengthForward = lastGtuDir.equals(GTUDirectionality.DIR_PLUS) ? lane.getLength().minus(position) : position;
296
297 while (lengthForward.lt(maxHeadway))
298 {
299 Map<Lane, GTUDirectionality> lanes =
300 lastGtuDir.equals(GTUDirectionality.DIR_PLUS) ? lane.nextLanes(gtu.getGTUType()) : lane.prevLanes(gtu
301 .getGTUType());
302 if (lanes.size() == 0)
303 {
304 // dead end. return with the list as is.
305 return laneListForward;
306 }
307 if (lanes.size() == 1)
308 {
309 lane = lanes.keySet().iterator().next();
310 }
311 else
312 {
313 // multiple next lanes; ask the strategical planner where to go
314 // note: this is not necessarily a split; it could e.g. be a bike path on a road
315 LinkDirection ld =
316 gtu.getStrategicalPlanner().nextLinkDirection(lane.getParentLink(), gtu.getLanes().get(lane),
317 gtu.getGTUType());
318 Link nextLink = ld.getLink();
319 for (Lane nextLane : lanes.keySet())
320 {
321 if (nextLane.getParentLink().equals(nextLink))
322 {
323 lane = nextLane;
324 break;
325 }
326 }
327 }
328 laneListForward.add(lane);
329 lengthForward = lengthForward.plus(lane.getLength());
330
331 // determine direction for the path
332 if (lastGtuDir.equals(GTUDirectionality.DIR_PLUS))
333 {
334 if (lastLane.getParentLink().getEndNode().equals(lane.getParentLink().getStartNode()))
335 {
336 // -----> O ----->, GTU moves ---->
337 lastGtuDir = GTUDirectionality.DIR_PLUS;
338 }
339 else
340 {
341 // -----> O <-----, GTU moves ---->
342 lastGtuDir = GTUDirectionality.DIR_MINUS;
343 }
344 }
345 else
346 {
347 if (lastLane.getParentLink().getStartNode().equals(lane.getParentLink().getStartNode()))
348 {
349 // <----- O ----->, GTU moves ---->
350 lastGtuDir = GTUDirectionality.DIR_PLUS;
351 }
352 else
353 {
354 // <----- O <-----, GTU moves ---->
355 lastGtuDir = GTUDirectionality.DIR_MINUS;
356 }
357 }
358 lastLane = lane;
359 }
360 return laneListForward;
361 }
362
363 /**
364 * Calculate the next location where the network splits, with a maximum headway relative to the reference point of the GTU.
365 * Note: a lane drop is also considered a split (!).
366 * @param gtu the gtu for which to calculate the lane list
367 * @param maxHeadway the maximum length for which lanes should be returned
368 * @return an instance that provides the following information for an operational plan: whether the network splits, the node
369 * where it splits, and the current lanes that lead to the right node after the split node.
370 * @throws GTUException when the vehicle is not on one of the lanes on which it is registered
371 * @throws NetworkException when the strategic planner is not able to return a next node in the route
372 */
373 public static NextSplitInfo determineNextSplit(final LaneBasedGTU gtu, final Length.Rel maxHeadway) throws GTUException,
374 NetworkException
375 {
376 OTSNode nextSplitNode = null;
377 Set<Lane> correctCurrentLanes = new HashSet<>();
378 Lane referenceLane = getReferenceLane(gtu);
379 Link lastLink = referenceLane.getParentLink();
380 GTUDirectionality lastGtuDir = gtu.getLanes().get(referenceLane);
381 GTUDirectionality referenceLaneDirectionality = lastGtuDir;
382 Length.Rel lengthForward;
383 Length.Rel position = gtu.position(referenceLane, gtu.getReference());
384 OTSNode lastNode;
385 if (lastGtuDir.equals(GTUDirectionality.DIR_PLUS))
386 {
387 lengthForward = referenceLane.getLength().minus(position);
388 lastNode = referenceLane.getParentLink().getEndNode();
389 }
390 else
391 {
392 lengthForward = gtu.position(referenceLane, gtu.getReference());
393 lastNode = referenceLane.getParentLink().getStartNode();
394 }
395 double referenceLaneFractionalPosition = position.si / referenceLane.getLength().si;
396
397 // see if we have a split within maxHeadway distance
398 while (lengthForward.lt(maxHeadway) && nextSplitNode == null)
399 {
400 // calculate the number of "outgoing" links
401 Set<Link> links = lastNode.getLinks(); // safe copy
402 Iterator<Link> linkIterator = links.iterator();
403 while (linkIterator.hasNext())
404 {
405 Link link = linkIterator.next();
406 if (link.equals(lastLink) || !link.getLinkType().isCompatible(gtu.getGTUType()))
407 {
408 linkIterator.remove();
409 }
410 }
411
412 // calculate the number of incoming and outgoing lanes on the link
413 boolean laneChange = false;
414 if (links.size() == 1)
415 {
416 for (CrossSectionElement cse : ((CrossSectionLink) lastLink).getCrossSectionElementList())
417 {
418 if (cse instanceof Lane && lastGtuDir.isPlus())
419 {
420 Lane lane = (Lane) cse;
421 if (lane.nextLanes(gtu.getGTUType()).size() == 0)
422 {
423 laneChange = true;
424 }
425 }
426 if (cse instanceof Lane && lastGtuDir.isMinus())
427 {
428 Lane lane = (Lane) cse;
429 if (lane.prevLanes(gtu.getGTUType()).size() == 0)
430 {
431 laneChange = true;
432 }
433 }
434 }
435 }
436
437 // see if we have a lane drop
438 if (laneChange)
439 {
440 nextSplitNode = lastNode;
441 LinkDirection ld = gtu.getStrategicalPlanner().nextLinkDirection(nextSplitNode, lastLink, gtu.getGTUType());
442 // which lane(s) we are registered on and adjacent lanes link to a lane
443 // that does not drop?
444 for (CrossSectionElement cse : referenceLane.getParentLink().getCrossSectionElementList())
445 {
446 if (cse instanceof Lane)
447 {
448 Lane l = (Lane) cse;
449 if (noLaneDrop(gtu, maxHeadway, l, referenceLaneFractionalPosition, referenceLaneDirectionality))
450 {
451 correctCurrentLanes.add(l);
452 }
453 }
454 }
455 return new NextSplitInfo(nextSplitNode, correctCurrentLanes);
456 }
457
458 // see if we have a split
459 if (links.size() > 1)
460 {
461 nextSplitNode = lastNode;
462 LinkDirection ld = gtu.getStrategicalPlanner().nextLinkDirection(nextSplitNode, lastLink, gtu.getGTUType());
463 // which lane(s) we are registered on and adjacent lanes link to a lane
464 // that is on the route at the next split?
465 for (CrossSectionElement cse : referenceLane.getParentLink().getCrossSectionElementList())
466 {
467 if (cse instanceof Lane)
468 {
469 Lane l = (Lane) cse;
470 if (connectsToPath(gtu, maxHeadway, l, referenceLaneFractionalPosition, referenceLaneDirectionality,
471 ld.getLink()))
472 {
473 correctCurrentLanes.add(l);
474 }
475 }
476 }
477 return new NextSplitInfo(nextSplitNode, correctCurrentLanes);
478 }
479
480 if (links.size() == 0)
481 {
482 return new NextSplitInfo(null, correctCurrentLanes);
483 }
484
485 // just one link
486 Link link = links.iterator().next();
487
488 // determine direction for the path
489 if (lastGtuDir.equals(GTUDirectionality.DIR_PLUS))
490 {
491 if (lastLink.getEndNode().equals(link.getStartNode()))
492 {
493 // -----> O ----->, GTU moves ---->
494 lastGtuDir = GTUDirectionality.DIR_PLUS;
495 lastNode = (OTSNode) lastLink.getEndNode();
496 }
497 else
498 {
499 // -----> O <-----, GTU moves ---->
500 lastGtuDir = GTUDirectionality.DIR_MINUS;
501 lastNode = (OTSNode) lastLink.getEndNode();
502 }
503 }
504 else
505 {
506 if (lastLink.getStartNode().equals(link.getStartNode()))
507 {
508 // <----- O ----->, GTU moves ---->
509 lastNode = (OTSNode) lastLink.getStartNode();
510 lastGtuDir = GTUDirectionality.DIR_PLUS;
511 }
512 else
513 {
514 // <----- O <-----, GTU moves ---->
515 lastNode = (OTSNode) lastLink.getStartNode();
516 lastGtuDir = GTUDirectionality.DIR_MINUS;
517 }
518 }
519 lastLink = links.iterator().next();
520 lengthForward = lengthForward.plus(lastLink.getLength());
521 }
522
523 return new NextSplitInfo(null, correctCurrentLanes);
524 }
525
526 /**
527 * Determine whether the lane is directly connected to our route, in other words: if we would (continue to) drive on the
528 * given lane, can we take the right branch at the nextSplitNode without switching lanes?
529 * @param gtu the GTU for which we have to determine the lane suitability
530 * @param maxHeadway the maximum length for use in the calculation
531 * @param startLane the first lane in the list
532 * @param startLaneFractionalPosition the fractional position on the start lane
533 * @param startDirectionality the driving direction on the start lane
534 * @param linkAfterSplit the link after the split to which we should connect
535 * @return whether the lane is connected to our path
536 * @throws GTUException when the vehicle is not on one of the lanes on which it is registered
537 * @throws NetworkException when the strategic planner is not able to return a next node in the route
538 */
539 protected static boolean connectsToPath(final LaneBasedGTU gtu, final Length.Rel maxHeadway, final Lane startLane,
540 final double startLaneFractionalPosition, final GTUDirectionality startDirectionality, final Link linkAfterSplit)
541 throws GTUException, NetworkException
542 {
543 List<LaneDirection> laneDirections =
544 buildLanePathInfo(gtu, maxHeadway, startLane, startLaneFractionalPosition, startDirectionality)
545 .getLaneDirectionList();
546 for (LaneDirection laneDirection : laneDirections)
547 {
548 if (laneDirection.getLane().getParentLink().equals(linkAfterSplit))
549 {
550 return true;
551 }
552 }
553 return false;
554 }
555
556 /**
557 * Determine whether the lane does not drop, in other words: if we would (continue to) drive on the given lane, can we
558 * continue to drive at the nextSplitNode without switching lanes?
559 * @param gtu the GTU for which we have to determine the lane suitability
560 * @param maxHeadway the maximum length for use in the calculation
561 * @param startLane the first lane in the list
562 * @param startLaneFractionalPosition the fractional position on the start lane
563 * @param startDirectionality the driving direction on the start lane
564 * @return whether the lane is connected to our path
565 * @throws GTUException when the vehicle is not on one of the lanes on which it is registered
566 * @throws NetworkException when the strategic planner is not able to return a next node in the route
567 */
568 protected static boolean noLaneDrop(final LaneBasedGTU gtu, final Length.Rel maxHeadway, final Lane startLane,
569 final double startLaneFractionalPosition, final GTUDirectionality startDirectionality) throws GTUException,
570 NetworkException
571 {
572 LanePathInfo lpi = buildLanePathInfo(gtu, maxHeadway, startLane, startLaneFractionalPosition, startDirectionality);
573 if (lpi.getPath().getLength().lt(maxHeadway))
574 {
575 return false;
576 }
577 return true;
578 }
579
580 /**
581 * Make a list of links on which to drive next, with a maximum headway relative to the reference point of the GTU.
582 * @param gtu the gtu for which to calculate the link list
583 * @param maxHeadway the maximum length for which links should be returned
584 * @return a list of links on which to drive next
585 * @throws GTUException when the vehicle is not on one of the lanes on which it is registered
586 * @throws NetworkException when the strategic planner is not able to return a next node in the route
587 */
588 protected static List<LinkDirection> buildLinkListForward(final LaneBasedGTU gtu, final Length.Rel maxHeadway)
589 throws GTUException, NetworkException
590 {
591 List<LinkDirection> linkList = new ArrayList<>();
592 Lane referenceLane = getReferenceLane(gtu);
593 Link lastLink = referenceLane.getParentLink();
594 GTUDirectionality lastGtuDir = gtu.getLanes().get(referenceLane);
595 linkList.add(new LinkDirection(lastLink, lastGtuDir));
596 Length.Rel lengthForward;
597 Length.Rel position = gtu.position(referenceLane, gtu.getReference());
598 OTSNode lastNode;
599 if (lastGtuDir.equals(GTUDirectionality.DIR_PLUS))
600 {
601 lengthForward = referenceLane.getLength().minus(position);
602 lastNode = referenceLane.getParentLink().getEndNode();
603 }
604 else
605 {
606 lengthForward = gtu.position(referenceLane, gtu.getReference());
607 lastNode = referenceLane.getParentLink().getStartNode();
608 }
609
610 // see if we have a split within maxHeadway distance
611 while (lengthForward.lt(maxHeadway))
612 {
613 // calculate the number of "outgoing" links
614 Set<Link> links = lastNode.getLinks(); // is a safe copy
615 Iterator<Link> linkIterator = links.iterator();
616 while (linkIterator.hasNext())
617 {
618 Link link = linkIterator.next();
619 if (link.equals(lastLink) || !link.getLinkType().isCompatible(gtu.getGTUType()))
620 {
621 linkIterator.remove();
622 }
623 }
624
625 if (links.size() == 0)
626 {
627 return linkList; // the path stops here...
628 }
629
630 Link link;
631 if (links.size() > 1)
632 {
633 LinkDirection ld = gtu.getStrategicalPlanner().nextLinkDirection(lastLink, lastGtuDir, gtu.getGTUType());
634 link = ld.getLink();
635 }
636 else
637 {
638 link = links.iterator().next();
639 }
640
641 // determine direction for the path
642 if (lastGtuDir.equals(GTUDirectionality.DIR_PLUS))
643 {
644 if (lastLink.getEndNode().equals(link.getStartNode()))
645 {
646 // -----> O ----->, GTU moves ---->
647 lastGtuDir = GTUDirectionality.DIR_PLUS;
648 lastNode = (OTSNode) lastLink.getEndNode();
649 }
650 else
651 {
652 // -----> O <-----, GTU moves ---->
653 lastGtuDir = GTUDirectionality.DIR_MINUS;
654 lastNode = (OTSNode) lastLink.getEndNode();
655 }
656 }
657 else
658 {
659 if (lastLink.getStartNode().equals(link.getStartNode()))
660 {
661 // <----- O ----->, GTU moves ---->
662 lastNode = (OTSNode) lastLink.getStartNode();
663 lastGtuDir = GTUDirectionality.DIR_PLUS;
664 }
665 else
666 {
667 // <----- O <-----, GTU moves ---->
668 lastNode = (OTSNode) lastLink.getStartNode();
669 lastGtuDir = GTUDirectionality.DIR_MINUS;
670 }
671 }
672 lastLink = link;
673 linkList.add(new LinkDirection(lastLink, lastGtuDir));
674 lengthForward = lengthForward.plus(lastLink.getLength());
675 }
676 return linkList;
677 }
678 }