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