1 package org.opentrafficsim.road.gtu.lane.tactical;
2
3 import java.util.ArrayList;
4 import java.util.Collection;
5 import java.util.HashMap;
6 import java.util.List;
7 import java.util.Map;
8
9 import org.djunits.unit.AccelerationUnit;
10 import org.djunits.unit.DurationUnit;
11 import org.djunits.unit.LengthUnit;
12 import org.djunits.value.StorageType;
13 import org.djunits.value.ValueException;
14 import org.djunits.value.vdouble.scalar.Acceleration;
15 import org.djunits.value.vdouble.scalar.Duration;
16 import org.djunits.value.vdouble.scalar.Length;
17 import org.djunits.value.vdouble.scalar.Speed;
18 import org.djunits.value.vdouble.scalar.Time;
19 import org.djunits.value.vdouble.vector.AccelerationVector;
20 import org.opentrafficsim.core.geometry.OTSGeometryException;
21 import org.opentrafficsim.core.geometry.OTSLine3D;
22 import org.opentrafficsim.core.gtu.GTUException;
23 import org.opentrafficsim.core.gtu.GTUType;
24 import org.opentrafficsim.core.gtu.behavioralcharacteristics.ParameterException;
25 import org.opentrafficsim.core.gtu.behavioralcharacteristics.ParameterTypes;
26 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
27 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan.Segment;
28 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
29 import org.opentrafficsim.core.network.LateralDirectionality;
30 import org.opentrafficsim.core.network.Link;
31 import org.opentrafficsim.core.network.NetworkException;
32 import org.opentrafficsim.core.network.Node;
33 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
34 import org.opentrafficsim.road.gtu.lane.perception.CategorialLanePerception;
35 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
36 import org.opentrafficsim.road.gtu.lane.perception.categories.DefaultSimplePerception;
37 import org.opentrafficsim.road.gtu.lane.perception.categories.DirectDefaultSimplePerception;
38 import org.opentrafficsim.road.gtu.lane.perception.headway.Headway;
39 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayTrafficLight;
40 import org.opentrafficsim.road.gtu.lane.tactical.following.GTUFollowingModelOld;
41 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.LaneChangeModel;
42 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.LaneMovementStep;
43 import org.opentrafficsim.road.network.lane.CrossSectionElement;
44 import org.opentrafficsim.road.network.lane.CrossSectionLink;
45 import org.opentrafficsim.road.network.lane.DirectedLanePosition;
46 import org.opentrafficsim.road.network.lane.Lane;
47 import org.opentrafficsim.road.network.lane.object.sensor.SingleSensor;
48 import org.opentrafficsim.road.network.lane.object.sensor.SinkSensor;
49
50 import nl.tudelft.simulation.language.d3.DirectedPoint;
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65 public class LaneBasedCFLCTacticalPlanner extends AbstractLaneBasedTacticalPlanner
66 {
67
68 private static final long serialVersionUID = 20151125L;
69
70
71 private static final Acceleration STAYINCURRENTLANEINCENTIVE = new Acceleration(0.1, AccelerationUnit.METER_PER_SECOND_2);
72
73
74 private static final Acceleration PREFERREDLANEINCENTIVE = new Acceleration(0.3, AccelerationUnit.METER_PER_SECOND_2);
75
76
77 private static final Acceleration NONPREFERREDLANEINCENTIVE = new Acceleration(-0.3, AccelerationUnit.METER_PER_SECOND_2);
78
79
80 public static final Length NOLANECHANGENEEDED = new Length(Double.MAX_VALUE, LengthUnit.SI);
81
82
83 public static final Length GETOFFTHISLANENOW = Length.ZERO;
84
85
86 private static final Duration TIMEHORIZON = new Duration(90, DurationUnit.SECOND);
87
88
89 private LaneChangeModel laneChangeModel;
90
91
92
93
94
95
96
97 public LaneBasedCFLCTacticalPlanner(final GTUFollowingModelOld carFollowingModel, final LaneChangeModel laneChangeModel,
98 final LaneBasedGTU gtu)
99 {
100 super(carFollowingModel, gtu, new CategorialLanePerception(gtu));
101 this.laneChangeModel = laneChangeModel;
102 getPerception().addPerceptionCategory(new DirectDefaultSimplePerception(getPerception()));
103 }
104
105
106 @Override
107 public final OperationalPlan generateOperationalPlan(final Time startTime, final DirectedPoint locationAtStartTime)
108 throws OperationalPlanException, NetworkException, GTUException, ParameterException
109 {
110 try
111 {
112
113 LaneBasedGTU laneBasedGTU = getGtu();
114 LanePerception perception = getPerception();
115
116
117 if (laneBasedGTU.getMaximumSpeed().si < OperationalPlan.DRIFTING_SPEED_SI)
118 {
119 return new OperationalPlan(getGtu(), locationAtStartTime, startTime, new Duration(1.0, DurationUnit.SECOND));
120 }
121
122
123
124 perception.perceive();
125
126 Length maximumForwardHeadway = laneBasedGTU.getBehavioralCharacteristics().getParameter(ParameterTypes.LOOKAHEAD);
127 Length maximumReverseHeadway = laneBasedGTU.getBehavioralCharacteristics().getParameter(ParameterTypes.LOOKBACKOLD);
128 Time now = getGtu().getSimulator().getSimulatorTime().getTime();
129 Speed speedLimit = perception.getPerceptionCategory(DefaultSimplePerception.class).getSpeedLimit();
130
131
132 Headway sameLaneLeader = perception.getPerceptionCategory(DefaultSimplePerception.class).getForwardHeadwayGTU();
133
134 Headway sameLaneFollower = perception.getPerceptionCategory(DefaultSimplePerception.class).getBackwardHeadway();
135 Collection<Headway> sameLaneTraffic = new ArrayList<Headway>();
136 if (sameLaneLeader.getObjectType().isGtu())
137 {
138 sameLaneTraffic.add(sameLaneLeader);
139 }
140 if (sameLaneFollower.getObjectType().isGtu())
141 {
142 sameLaneTraffic.add(sameLaneFollower);
143 }
144
145
146 LanePathInfo lanePathInfo = buildLanePathInfo(laneBasedGTU, maximumForwardHeadway);
147
148
149 NextSplitInfo nextSplitInfo = determineNextSplit(laneBasedGTU, maximumForwardHeadway);
150 boolean currentLaneFine = nextSplitInfo.getCorrectCurrentLanes().contains(lanePathInfo.getReferenceLane());
151
152
153
154
155
156 Collection<Headway> leftLaneTraffic =
157 perception.getPerceptionCategory(DefaultSimplePerception.class).getNeighboringHeadwaysLeft();
158 Collection<Headway> rightLaneTraffic =
159 perception.getPerceptionCategory(DefaultSimplePerception.class).getNeighboringHeadwaysRight();
160
161
162 final LateralDirectionality preferred = LateralDirectionality.RIGHT;
163 final Acceleration defaultLeftLaneIncentive =
164 preferred.isLeft() ? PREFERREDLANEINCENTIVE : NONPREFERREDLANEINCENTIVE;
165 final Acceleration defaultRightLaneIncentive =
166 preferred.isRight() ? PREFERREDLANEINCENTIVE : NONPREFERREDLANEINCENTIVE;
167
168 AccelerationVector defaultLaneIncentives =
169 new AccelerationVector(new double[] { defaultLeftLaneIncentive.getSI(), STAYINCURRENTLANEINCENTIVE.getSI(),
170 defaultRightLaneIncentive.getSI() }, AccelerationUnit.SI, StorageType.DENSE);
171 AccelerationVector laneIncentives = laneIncentives(laneBasedGTU, defaultLaneIncentives);
172 LaneMovementStep lcmr = this.laneChangeModel.computeLaneChangeAndAcceleration(laneBasedGTU, sameLaneTraffic,
173 rightLaneTraffic, leftLaneTraffic, speedLimit,
174 new Acceleration(laneIncentives.get(preferred.isRight() ? 2 : 0)), new Acceleration(laneIncentives.get(1)),
175 new Acceleration(laneIncentives.get(preferred.isRight() ? 0 : 2)));
176 Duration duration = lcmr.getGfmr().getValidUntil().minus(getGtu().getSimulator().getSimulatorTime().getTime());
177 if (lcmr.getLaneChangeDirection() != null)
178 {
179 laneBasedGTU.changeLaneInstantaneously(lcmr.getLaneChangeDirection());
180
181
182 lanePathInfo = buildLanePathInfo(laneBasedGTU, maximumForwardHeadway);
183 }
184
185
186 Headway object = getPerception().getPerceptionCategory(DefaultSimplePerception.class).getForwardHeadwayObject();
187 Acceleration a = lcmr.getGfmr().getAcceleration();
188 if (object instanceof HeadwayTrafficLight)
189 {
190
191 a = Acceleration.min(a, ((GTUFollowingModelOld) getCarFollowingModel()).computeAcceleration(getGtu().getSpeed(),
192 getGtu().getMaximumSpeed(), Speed.ZERO, object.getDistance(), speedLimit));
193 }
194
195
196 Length dist = lanePathInfo.getPath().getLength().minus(getGtu().getFront().getDx());
197 a = Acceleration.min(a, ((GTUFollowingModelOld) getCarFollowingModel()).computeAcceleration(getGtu().getSpeed(),
198 getGtu().getMaximumSpeed(), Speed.ZERO, dist, speedLimit));
199
200
201 OTSLine3D path = lanePathInfo.getPath();
202 if (a.si < 1E-6 && laneBasedGTU.getSpeed().si < OperationalPlan.DRIFTING_SPEED_SI)
203 {
204 try
205 {
206 return new OperationalPlan(getGtu(), path.getLocationFraction(0.0), startTime, duration);
207 }
208 catch (OTSGeometryException exception)
209 {
210
211 throw new RuntimeException(exception);
212 }
213 }
214 List<Segment> operationalPlanSegmentList = new ArrayList<>();
215
216 if (a.si == 0.0)
217 {
218 Segment segment = new OperationalPlan.SpeedSegment(duration);
219 operationalPlanSegmentList.add(segment);
220 }
221 else
222 {
223 Segment segment = new OperationalPlan.AccelerationSegment(duration, a);
224 operationalPlanSegmentList.add(segment);
225 }
226 OperationalPlan op =
227 new OperationalPlan(getGtu(), path, startTime, getGtu().getSpeed(), operationalPlanSegmentList);
228 return op;
229 }
230 catch (ValueException exception)
231 {
232 throw new GTUException(exception);
233 }
234 }
235
236
237
238
239
240
241
242
243
244
245
246
247
248 private AccelerationVector laneIncentives(final LaneBasedGTU gtu, final AccelerationVector defaultLaneIncentives)
249 throws NetworkException, ValueException, GTUException, OperationalPlanException
250 {
251 Length leftSuitability = suitability(gtu, LateralDirectionality.LEFT);
252 Length currentSuitability = suitability(gtu, null);
253 Length rightSuitability = suitability(gtu, LateralDirectionality.RIGHT);
254 if (leftSuitability == NOLANECHANGENEEDED && currentSuitability == NOLANECHANGENEEDED
255 && rightSuitability == NOLANECHANGENEEDED)
256 {
257 return checkLaneDrops(gtu, defaultLaneIncentives);
258 }
259 if ((leftSuitability == NOLANECHANGENEEDED || leftSuitability == GETOFFTHISLANENOW)
260 && currentSuitability == NOLANECHANGENEEDED
261 && (rightSuitability == NOLANECHANGENEEDED || rightSuitability == GETOFFTHISLANENOW))
262 {
263 return checkLaneDrops(gtu,
264 new AccelerationVector(new double[] { acceleration(gtu, leftSuitability),
265 defaultLaneIncentives.get(1).getSI(), acceleration(gtu, rightSuitability) }, AccelerationUnit.SI,
266 StorageType.DENSE));
267 }
268 if (currentSuitability == NOLANECHANGENEEDED)
269 {
270 return new AccelerationVector(new double[] { acceleration(gtu, leftSuitability),
271 defaultLaneIncentives.get(1).getSI(), acceleration(gtu, rightSuitability) }, AccelerationUnit.SI,
272 StorageType.DENSE);
273 }
274 return new AccelerationVector(new double[] { acceleration(gtu, leftSuitability), acceleration(gtu, currentSuitability),
275 acceleration(gtu, rightSuitability) }, AccelerationUnit.SI, StorageType.DENSE);
276 }
277
278
279
280
281
282
283
284
285
286
287
288
289
290 private AccelerationVector checkLaneDrops(final LaneBasedGTU gtu, final AccelerationVector defaultLaneIncentives)
291 throws NetworkException, ValueException, GTUException, OperationalPlanException
292 {
293
294 Length leftSuitability = Double.isNaN(defaultLaneIncentives.get(0).si) || defaultLaneIncentives.get(0).si < -10
295 ? GETOFFTHISLANENOW : laneDrop(gtu, LateralDirectionality.LEFT);
296 Length currentSuitability = laneDrop(gtu, null);
297 Length rightSuitability = Double.isNaN(defaultLaneIncentives.get(2).si) || defaultLaneIncentives.get(2).si < -10
298 ? GETOFFTHISLANENOW : laneDrop(gtu, LateralDirectionality.RIGHT);
299
300 if ((leftSuitability == NOLANECHANGENEEDED || leftSuitability == GETOFFTHISLANENOW)
301 && currentSuitability == NOLANECHANGENEEDED
302 && (rightSuitability == NOLANECHANGENEEDED || rightSuitability == GETOFFTHISLANENOW))
303 {
304 return defaultLaneIncentives;
305 }
306
307 if (currentSuitability == NOLANECHANGENEEDED)
308 {
309 return new AccelerationVector(new double[] { acceleration(gtu, leftSuitability),
310 defaultLaneIncentives.get(1).getSI(), acceleration(gtu, rightSuitability) }, AccelerationUnit.SI,
311 StorageType.DENSE);
312 }
313 if (currentSuitability.le(leftSuitability))
314 {
315 return new AccelerationVector(new double[] { PREFERREDLANEINCENTIVE.getSI(), NONPREFERREDLANEINCENTIVE.getSI(),
316 GETOFFTHISLANENOW.getSI() }, AccelerationUnit.SI, StorageType.DENSE);
317 }
318 if (currentSuitability.le(rightSuitability))
319 {
320 return new AccelerationVector(new double[] { GETOFFTHISLANENOW.getSI(), NONPREFERREDLANEINCENTIVE.getSI(),
321 PREFERREDLANEINCENTIVE.getSI() }, AccelerationUnit.SI, StorageType.DENSE);
322 }
323 return new AccelerationVector(new double[] { acceleration(gtu, leftSuitability), acceleration(gtu, currentSuitability),
324 acceleration(gtu, rightSuitability) }, AccelerationUnit.SI, StorageType.DENSE);
325 }
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340 private Length laneDrop(final LaneBasedGTU gtu, final LateralDirectionality direction)
341 throws NetworkException, GTUException, OperationalPlanException
342 {
343 DirectedLanePosition dlp = gtu.getReferencePosition();
344 Lane lane = dlp.getLane();
345 Length longitudinalPosition = dlp.getPosition();
346 if (null != direction)
347 {
348 lane = getPerception().getPerceptionCategory(DefaultSimplePerception.class).bestAccessibleAdjacentLane(lane,
349 direction, longitudinalPosition);
350 }
351 if (null == lane)
352 {
353 return GETOFFTHISLANENOW;
354 }
355 double remainingLength = lane.getLength().getSI() - longitudinalPosition.getSI();
356 double remainingTimeSI = TIMEHORIZON.getSI() - remainingLength / lane.getSpeedLimit(gtu.getGTUType()).getSI();
357 while (remainingTimeSI >= 0)
358 {
359 for (SingleSensor s : lane.getSensors())
360 {
361 if (s instanceof SinkSensor)
362 {
363 return NOLANECHANGENEEDED;
364 }
365 }
366 int branching = lane.nextLanes(gtu.getGTUType()).size();
367 if (branching == 0)
368 {
369 return new Length(remainingLength, LengthUnit.SI);
370 }
371 if (branching > 1)
372 {
373 return NOLANECHANGENEEDED;
374 }
375 lane = lane.nextLanes(gtu.getGTUType()).keySet().iterator().next();
376 remainingTimeSI -= lane.getLength().getSI() / lane.getSpeedLimit(gtu.getGTUType()).getSI();
377 remainingLength += lane.getLength().getSI();
378 }
379 return NOLANECHANGENEEDED;
380 }
381
382
383
384
385
386
387
388
389
390
391
392
393 private Length suitability(final LaneBasedGTU gtu, final LateralDirectionality direction)
394 throws NetworkException, GTUException, OperationalPlanException
395 {
396 DirectedLanePosition dlp = gtu.getReferencePosition();
397 Lane lane = dlp.getLane();
398 Length longitudinalPosition = dlp.getPosition().plus(gtu.getFront().getDx());
399 if (null != direction)
400 {
401 lane = getPerception().getPerceptionCategory(DefaultSimplePerception.class).bestAccessibleAdjacentLane(lane,
402 direction, longitudinalPosition);
403 }
404 if (null == lane)
405 {
406 return GETOFFTHISLANENOW;
407 }
408 try
409 {
410 return suitability(lane, longitudinalPosition, gtu, TIMEHORIZON);
411
412 }
413 catch (NetworkException ne)
414 {
415 System.err.println(gtu + " has a route problem in suitability: " + ne.getMessage());
416 return NOLANECHANGENEEDED;
417 }
418 }
419
420
421
422
423
424
425
426 private double acceleration(final LaneBasedGTU gtu, final Length stopDistance)
427 {
428
429
430 double v = gtu.getSpeed().getSI();
431 double a = -v * v / 2 / stopDistance.getSI();
432 return a;
433 }
434
435
436
437
438
439
440
441
442
443
444
445 private Length suitability(final Lane lane, final Length longitudinalPosition, final LaneBasedGTU gtu,
446 final Duration timeHorizon) throws NetworkException
447 {
448 double remainingDistance = lane.getLength().getSI() - longitudinalPosition.getSI();
449 double spareTime = timeHorizon.getSI() - remainingDistance / lane.getSpeedLimit(gtu.getGTUType()).getSI();
450
451 Node nextNode = lane.getParentLink().getEndNode();
452 Link lastLink = lane.getParentLink();
453 Node nextSplitNode = null;
454 Lane currentLane = lane;
455 CrossSectionLink linkBeforeBranch = lane.getParentLink();
456 while (null != nextNode)
457 {
458 if (spareTime <= 0)
459 {
460 return NOLANECHANGENEEDED;
461 }
462 int laneCount = countCompatibleLanes(linkBeforeBranch, gtu.getGTUType());
463 if (0 == laneCount)
464 {
465 throw new NetworkException("No compatible Lanes on Link " + linkBeforeBranch);
466 }
467 if (1 == laneCount)
468 {
469 return NOLANECHANGENEEDED;
470
471 }
472 int branching = nextNode.getLinks().size();
473 if (branching > 2)
474 {
475 nextSplitNode = nextNode;
476 break;
477 }
478 else if (1 == branching)
479 {
480 return NOLANECHANGENEEDED;
481 }
482 else
483 {
484 Link nextLink = gtu.getStrategicalPlanner().nextLinkDirection(nextNode, lastLink, gtu.getGTUType()).getLink();
485 if (nextLink instanceof CrossSectionLink)
486 {
487 nextNode = nextLink.getEndNode();
488
489 remainingDistance += nextLink.getLength().getSI();
490 linkBeforeBranch = (CrossSectionLink) nextLink;
491
492 if (currentLane.nextLanes(gtu.getGTUType()).size() == 0)
493 {
494
495
496 if (currentLane.accessibleAdjacentLanes(LateralDirectionality.RIGHT, gtu.getGTUType()).size() > 0)
497 {
498 for (Lane adjacentLane : currentLane.accessibleAdjacentLanes(LateralDirectionality.RIGHT,
499 gtu.getGTUType()))
500 {
501 if (adjacentLane.nextLanes(gtu.getGTUType()).size() > 0)
502 {
503 currentLane = adjacentLane;
504 break;
505 }
506
507
508 }
509 }
510 for (Lane adjacentLane : currentLane.accessibleAdjacentLanes(LateralDirectionality.LEFT,
511 gtu.getGTUType()))
512 {
513 if (adjacentLane.nextLanes(gtu.getGTUType()).size() > 0)
514 {
515 currentLane = adjacentLane;
516 break;
517 }
518
519
520 }
521 if (currentLane.nextLanes(gtu.getGTUType()).size() == 0)
522 {
523 throw new NetworkException(
524 "Lane ends and there is not a compatible adjacent lane that does " + "not end");
525 }
526 }
527
528 for (Lane nextLane : currentLane.nextLanes(gtu.getGTUType()).keySet())
529 {
530 if (nextLane.getLaneType().isCompatible(gtu.getGTUType()))
531 {
532 currentLane = currentLane.nextLanes(gtu.getGTUType()).keySet().iterator().next();
533 break;
534 }
535 }
536 spareTime -= currentLane.getLength().getSI() / currentLane.getSpeedLimit(gtu.getGTUType()).getSI();
537 }
538 else
539 {
540
541
542 return NOLANECHANGENEEDED;
543 }
544 lastLink = nextLink;
545 }
546 }
547 if (null == nextNode)
548 {
549 throw new NetworkException("Cannot find the next branch or sink node");
550 }
551
552
553 Map<Lane, Length> suitabilityOfLanesBeforeBranch = new HashMap<Lane, Length>();
554 Link linkAfterBranch =
555 gtu.getStrategicalPlanner().nextLinkDirection(nextSplitNode, lastLink, gtu.getGTUType()).getLink();
556 for (CrossSectionElement cse : linkBeforeBranch.getCrossSectionElementList())
557 {
558 if (cse instanceof Lane)
559 {
560 Lane l = (Lane) cse;
561 if (l.getLaneType().isCompatible(gtu.getGTUType()))
562 {
563 for (Lane connectingLane : l.nextLanes(gtu.getGTUType()).keySet())
564 {
565 if (connectingLane.getParentLink() == linkAfterBranch
566 && connectingLane.getLaneType().isCompatible(gtu.getGTUType()))
567 {
568 Length currentValue = suitabilityOfLanesBeforeBranch.get(l);
569
570
571
572 Length value = suitability(connectingLane, new Length(0, LengthUnit.SI), gtu,
573 new Duration(spareTime, DurationUnit.SI));
574
575 value = value.plus(new Length(remainingDistance, LengthUnit.SI));
576
577
578 suitabilityOfLanesBeforeBranch.put(l,
579 null == currentValue || value.le(currentValue) ? value : currentValue);
580 }
581 }
582 }
583 }
584 }
585 if (suitabilityOfLanesBeforeBranch.size() == 0)
586 {
587 throw new NetworkException("No lanes available on Link " + linkBeforeBranch);
588 }
589 Length currentLaneSuitability = suitabilityOfLanesBeforeBranch.get(currentLane);
590 if (null != currentLaneSuitability)
591 {
592 return currentLaneSuitability;
593 }
594
595 int totalLanes = countCompatibleLanes(currentLane.getParentLink(), gtu.getGTUType());
596 Length leftSuitability = computeSuitabilityWithLaneChanges(currentLane, remainingDistance,
597 suitabilityOfLanesBeforeBranch, totalLanes, LateralDirectionality.LEFT, gtu.getGTUType());
598 Length rightSuitability = computeSuitabilityWithLaneChanges(currentLane, remainingDistance,
599 suitabilityOfLanesBeforeBranch, totalLanes, LateralDirectionality.RIGHT, gtu.getGTUType());
600 if (leftSuitability.ge(rightSuitability))
601 {
602 return leftSuitability;
603 }
604 else if (rightSuitability.ge(leftSuitability))
605 {
606
607 return rightSuitability;
608 }
609 if (leftSuitability.le(GETOFFTHISLANENOW))
610 {
611 throw new NetworkException("Changing lanes in any direction does not get the GTU on a suitable lane");
612 }
613 return leftSuitability;
614 }
615
616
617
618
619
620
621
622
623
624
625
626
627
628 protected final Length computeSuitabilityWithLaneChanges(final Lane startLane, final double remainingDistance,
629 final Map<Lane, Length> suitabilities, final int totalLanes, final LateralDirectionality direction,
630 final GTUType gtuType)
631 {
632
633
634
635
636
637
638
639
640
641 int laneChangesUsed = 0;
642 Lane currentLane = startLane;
643 Length currentSuitability = null;
644 while (null == currentSuitability)
645 {
646 laneChangesUsed++;
647 if (currentLane.accessibleAdjacentLanes(direction, gtuType).size() == 0)
648 {
649 return GETOFFTHISLANENOW;
650 }
651 currentLane = currentLane.accessibleAdjacentLanes(direction, gtuType).iterator().next();
652 currentSuitability = suitabilities.get(currentLane);
653 }
654 double fraction = currentSuitability == NOLANECHANGENEEDED ? 0 : 0.5;
655 int notSuitableLaneCount = totalLanes - suitabilities.size();
656 return new Length(
657 remainingDistance * (notSuitableLaneCount - laneChangesUsed + 1 + fraction) / (notSuitableLaneCount + fraction),
658 LengthUnit.SI);
659 }
660
661
662
663
664
665
666
667
668 protected final int countCompatibleLanes(final CrossSectionLink link, final GTUType gtuType)
669 {
670 int result = 0;
671 for (CrossSectionElement cse : link.getCrossSectionElementList())
672 {
673 if (cse instanceof Lane)
674 {
675 Lane l = (Lane) cse;
676 if (l.getLaneType().isCompatible(gtuType))
677 {
678 result++;
679 }
680 }
681 }
682 return result;
683 }
684
685
686 @Override
687 public final String toString()
688 {
689 return "LaneBasedCFLCTacticalPlanner [laneChangeModel=" + this.laneChangeModel + "]";
690 }
691
692 }