1 package org.opentrafficsim.road.gtu.lane.tactical;
2
3 import java.util.ArrayList;
4 import java.util.Collection;
5 import java.util.LinkedHashMap;
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.ValueRuntimeException;
13 import org.djunits.value.storage.StorageType;
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.djunits.value.vdouble.vector.base.DoubleVector;
21 import org.opentrafficsim.base.parameters.ParameterException;
22 import org.opentrafficsim.base.parameters.ParameterTypeLength;
23 import org.opentrafficsim.base.parameters.ParameterTypes;
24 import org.opentrafficsim.core.geometry.DirectedPoint;
25 import org.opentrafficsim.core.geometry.OTSGeometryException;
26 import org.opentrafficsim.core.geometry.OTSLine3D;
27 import org.opentrafficsim.core.gtu.GTUDirectionality;
28 import org.opentrafficsim.core.gtu.GTUException;
29 import org.opentrafficsim.core.gtu.GTUType;
30 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
31 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan.Segment;
32 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
33 import org.opentrafficsim.core.network.LateralDirectionality;
34 import org.opentrafficsim.core.network.Link;
35 import org.opentrafficsim.core.network.NetworkException;
36 import org.opentrafficsim.core.network.Node;
37 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
38 import org.opentrafficsim.road.gtu.lane.perception.CategoricalLanePerception;
39 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
40 import org.opentrafficsim.road.gtu.lane.perception.categories.DefaultSimplePerception;
41 import org.opentrafficsim.road.gtu.lane.perception.categories.DirectDefaultSimplePerception;
42 import org.opentrafficsim.road.gtu.lane.perception.headway.Headway;
43 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayTrafficLight;
44 import org.opentrafficsim.road.gtu.lane.tactical.following.GTUFollowingModelOld;
45 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.LaneChangeModel;
46 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.LaneMovementStep;
47 import org.opentrafficsim.road.network.lane.CrossSectionElement;
48 import org.opentrafficsim.road.network.lane.CrossSectionLink;
49 import org.opentrafficsim.road.network.lane.DirectedLanePosition;
50 import org.opentrafficsim.road.network.lane.Lane;
51 import org.opentrafficsim.road.network.lane.object.sensor.SingleSensor;
52 import org.opentrafficsim.road.network.lane.object.sensor.SinkSensor;
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67 public class LaneBasedCFLCTacticalPlanner extends AbstractLaneBasedTacticalPlanner
68 {
69
70 private static final long serialVersionUID = 20151125L;
71
72
73 protected static final ParameterTypeLength LOOKBACKOLD = ParameterTypes.LOOKBACKOLD;
74
75
76 private static final Acceleration STAYINCURRENTLANEINCENTIVE = new Acceleration(0.1, AccelerationUnit.METER_PER_SECOND_2);
77
78
79 private static final Acceleration PREFERREDLANEINCENTIVE = new Acceleration(0.3, AccelerationUnit.METER_PER_SECOND_2);
80
81
82 private static final Acceleration NONPREFERREDLANEINCENTIVE = new Acceleration(-0.3, AccelerationUnit.METER_PER_SECOND_2);
83
84
85 public static final Length NOLANECHANGENEEDED = new Length(Double.MAX_VALUE, LengthUnit.SI);
86
87
88 public static final Length GETOFFTHISLANENOW = Length.ZERO;
89
90
91 private static final Duration TIMEHORIZON = new Duration(90, DurationUnit.SECOND);
92
93
94 private LaneChangeModel laneChangeModel;
95
96
97
98
99
100
101
102 public LaneBasedCFLCTacticalPlanner(final GTUFollowingModelOld carFollowingModel, final LaneChangeModel laneChangeModel,
103 final LaneBasedGTU gtu)
104 {
105 super(carFollowingModel, gtu, new CategoricalLanePerception(gtu));
106 this.laneChangeModel = laneChangeModel;
107 getPerception().addPerceptionCategory(new DirectDefaultSimplePerception(getPerception()));
108 }
109
110
111 @Override
112 public final OperationalPlan generateOperationalPlan(final Time startTime, final DirectedPoint locationAtStartTime)
113 throws OperationalPlanException, NetworkException, GTUException, ParameterException
114 {
115 try
116 {
117
118 LaneBasedGTU laneBasedGTU = getGtu();
119 LanePerception perception = getPerception();
120
121
122 if (laneBasedGTU.getMaximumSpeed().si < OperationalPlan.DRIFTING_SPEED_SI)
123 {
124 return new OperationalPlan(getGtu(), locationAtStartTime, startTime, new Duration(1.0, DurationUnit.SECOND));
125 }
126
127 Length maximumForwardHeadway = laneBasedGTU.getParameters().getParameter(LOOKAHEAD);
128 DefaultSimplePerception simplePerception = perception.getPerceptionCategory(DefaultSimplePerception.class);
129 Speed speedLimit = simplePerception.getSpeedLimit();
130
131
132 Headway sameLaneLeader = simplePerception.getForwardHeadwayGTU();
133
134 Headway sameLaneFollower = simplePerception.getBackwardHeadway();
135 Collection<Headway> sameLaneTraffic = new ArrayList<>();
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 = simplePerception.getNeighboringHeadwaysLeft();
157 Collection<Headway> rightLaneTraffic = simplePerception.getNeighboringHeadwaysRight();
158
159
160 final LateralDirectionality preferred = LateralDirectionality.RIGHT;
161 final Acceleration defaultLeftLaneIncentive =
162 preferred.isLeft() ? PREFERREDLANEINCENTIVE : NONPREFERREDLANEINCENTIVE;
163 final Acceleration defaultRightLaneIncentive =
164 preferred.isRight() ? PREFERREDLANEINCENTIVE : NONPREFERREDLANEINCENTIVE;
165
166 AccelerationVector defaultLaneIncentives =
167 DoubleVector.instantiate(new double[] {defaultLeftLaneIncentive.getSI(), STAYINCURRENTLANEINCENTIVE.getSI(),
168 defaultRightLaneIncentive.getSI()}, AccelerationUnit.SI, StorageType.DENSE);
169 AccelerationVector laneIncentives = laneIncentives(laneBasedGTU, defaultLaneIncentives);
170 LaneMovementStep lcmr = this.laneChangeModel.computeLaneChangeAndAcceleration(laneBasedGTU, sameLaneTraffic,
171 rightLaneTraffic, leftLaneTraffic, speedLimit,
172 new Acceleration(laneIncentives.get(preferred.isRight() ? 2 : 0)), new Acceleration(laneIncentives.get(1)),
173 new Acceleration(laneIncentives.get(preferred.isRight() ? 0 : 2)));
174 Duration duration = lcmr.getGfmr().getValidUntil().minus(getGtu().getSimulator().getSimulatorAbsTime());
175 if (lcmr.getLaneChangeDirection() != null)
176 {
177 laneBasedGTU.changeLaneInstantaneously(lcmr.getLaneChangeDirection());
178
179
180 lanePathInfo = buildLanePathInfo(laneBasedGTU, maximumForwardHeadway);
181 }
182
183
184 Headway object = simplePerception.getForwardHeadwayObject();
185 Acceleration a = lcmr.getGfmr().getAcceleration();
186 if (object instanceof HeadwayTrafficLight)
187 {
188
189 a = Acceleration.min(a, ((GTUFollowingModelOld) getCarFollowingModel()).computeAcceleration(getGtu().getSpeed(),
190 getGtu().getMaximumSpeed(), Speed.ZERO, object.getDistance(), speedLimit));
191 }
192
193
194 Length dist = lanePathInfo.getPath().getLength().minus(getGtu().getFront().getDx());
195 a = Acceleration.min(a, ((GTUFollowingModelOld) getCarFollowingModel()).computeAcceleration(getGtu().getSpeed(),
196 getGtu().getMaximumSpeed(), Speed.ZERO, dist, speedLimit));
197
198
199 OTSLine3D path = lanePathInfo.getPath();
200 if (a.si < 1E-6 && laneBasedGTU.getSpeed().si < OperationalPlan.DRIFTING_SPEED_SI)
201 {
202 try
203 {
204 return new OperationalPlan(getGtu(), path.getLocationFraction(0.0), startTime, duration);
205 }
206 catch (OTSGeometryException exception)
207 {
208
209 throw new RuntimeException(exception);
210 }
211 }
212 List<Segment> operationalPlanSegmentList = new ArrayList<>();
213
214 if (a.si == 0.0)
215 {
216 Segment segment = new OperationalPlan.SpeedSegment(duration);
217 operationalPlanSegmentList.add(segment);
218 }
219 else
220 {
221 Segment segment = new OperationalPlan.AccelerationSegment(duration, a);
222 operationalPlanSegmentList.add(segment);
223 }
224 OperationalPlan op =
225 new OperationalPlan(getGtu(), path, startTime, getGtu().getSpeed(), operationalPlanSegmentList);
226 return op;
227 }
228 catch (ValueRuntimeException exception)
229 {
230 throw new GTUException(exception);
231 }
232 }
233
234
235
236
237
238
239
240
241
242
243
244
245
246 private AccelerationVector laneIncentives(final LaneBasedGTU gtu, final AccelerationVector defaultLaneIncentives)
247 throws NetworkException, ValueRuntimeException, GTUException, OperationalPlanException
248 {
249 Length leftSuitability = suitability(gtu, LateralDirectionality.LEFT);
250 Length currentSuitability = suitability(gtu, null);
251 Length rightSuitability = suitability(gtu, LateralDirectionality.RIGHT);
252 if (leftSuitability == NOLANECHANGENEEDED && currentSuitability == NOLANECHANGENEEDED
253 && rightSuitability == NOLANECHANGENEEDED)
254 {
255 return checkLaneDrops(gtu, defaultLaneIncentives);
256 }
257 if ((leftSuitability == NOLANECHANGENEEDED || leftSuitability == GETOFFTHISLANENOW)
258 && currentSuitability == NOLANECHANGENEEDED
259 && (rightSuitability == NOLANECHANGENEEDED || rightSuitability == GETOFFTHISLANENOW))
260 {
261 return checkLaneDrops(gtu,
262 DoubleVector.instantiate(new double[] {acceleration(gtu, leftSuitability),
263 defaultLaneIncentives.get(1).getSI(), acceleration(gtu, rightSuitability)}, AccelerationUnit.SI,
264 StorageType.DENSE));
265 }
266 if (currentSuitability == NOLANECHANGENEEDED)
267 {
268 return DoubleVector.instantiate(new double[] {acceleration(gtu, leftSuitability),
269 defaultLaneIncentives.get(1).getSI(), acceleration(gtu, rightSuitability)}, AccelerationUnit.SI,
270 StorageType.DENSE);
271 }
272 return DoubleVector.instantiate(new double[] {acceleration(gtu, leftSuitability), acceleration(gtu, currentSuitability),
273 acceleration(gtu, rightSuitability)}, AccelerationUnit.SI, StorageType.DENSE);
274 }
275
276
277
278
279
280
281
282
283
284
285
286
287
288 private AccelerationVector checkLaneDrops(final LaneBasedGTU gtu, final AccelerationVector defaultLaneIncentives)
289 throws NetworkException, ValueRuntimeException, GTUException, OperationalPlanException
290 {
291
292 Length leftSuitability = Double.isNaN(defaultLaneIncentives.get(0).si) || defaultLaneIncentives.get(0).si < -10
293 ? GETOFFTHISLANENOW : laneDrop(gtu, LateralDirectionality.LEFT);
294 Length currentSuitability = laneDrop(gtu, null);
295 Length rightSuitability = Double.isNaN(defaultLaneIncentives.get(2).si) || defaultLaneIncentives.get(2).si < -10
296 ? GETOFFTHISLANENOW : laneDrop(gtu, LateralDirectionality.RIGHT);
297
298 if ((leftSuitability == NOLANECHANGENEEDED || leftSuitability == GETOFFTHISLANENOW)
299 && currentSuitability == NOLANECHANGENEEDED
300 && (rightSuitability == NOLANECHANGENEEDED || rightSuitability == GETOFFTHISLANENOW))
301 {
302 return defaultLaneIncentives;
303 }
304
305 if (currentSuitability == NOLANECHANGENEEDED)
306 {
307 return DoubleVector.instantiate(new double[] {acceleration(gtu, leftSuitability),
308 defaultLaneIncentives.get(1).getSI(), acceleration(gtu, rightSuitability)}, AccelerationUnit.SI,
309 StorageType.DENSE);
310 }
311 if (currentSuitability.le(leftSuitability))
312 {
313 return DoubleVector.instantiate(
314 new double[] {PREFERREDLANEINCENTIVE.getSI(), NONPREFERREDLANEINCENTIVE.getSI(), GETOFFTHISLANENOW.getSI()},
315 AccelerationUnit.SI, StorageType.DENSE);
316 }
317 if (currentSuitability.le(rightSuitability))
318 {
319 return DoubleVector.instantiate(
320 new double[] {GETOFFTHISLANENOW.getSI(), NONPREFERREDLANEINCENTIVE.getSI(), PREFERREDLANEINCENTIVE.getSI()},
321 AccelerationUnit.SI, StorageType.DENSE);
322 }
323 return DoubleVector.instantiate(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
446
447 private Length suitability(final Lane lane, final Length longitudinalPosition, final LaneBasedGTU gtu,
448 final Duration timeHorizon) throws NetworkException
449 {
450 double remainingDistance = lane.getLength().getSI() - longitudinalPosition.getSI();
451 double spareTime = timeHorizon.getSI() - remainingDistance / lane.getSpeedLimit(gtu.getGTUType()).getSI();
452
453 Node nextNode = lane.getParentLink().getEndNode();
454 Link lastLink = lane.getParentLink();
455 Node nextSplitNode = null;
456 Lane currentLane = lane;
457 CrossSectionLink linkBeforeBranch = lane.getParentLink();
458 while (null != nextNode)
459 {
460 if (spareTime <= 0)
461 {
462 return NOLANECHANGENEEDED;
463 }
464 int laneCount = countCompatibleLanes(linkBeforeBranch, gtu.getGTUType(), GTUDirectionality.DIR_PLUS);
465 if (0 == laneCount)
466 {
467 throw new NetworkException("No compatible Lanes on Link " + linkBeforeBranch);
468 }
469 if (1 == laneCount)
470 {
471 return NOLANECHANGENEEDED;
472
473 }
474 int branching = nextNode.getLinks().size();
475 if (branching > 2)
476 {
477 nextSplitNode = nextNode;
478 break;
479 }
480 else if (1 == branching)
481 {
482 return NOLANECHANGENEEDED;
483 }
484 else
485 {
486 Link nextLink = gtu.getStrategicalPlanner().nextLinkDirection(nextNode, lastLink, gtu.getGTUType()).getLink();
487 if (nextLink instanceof CrossSectionLink)
488 {
489 GTUDirectionality drivingDirection =
490 nextNode.equals(nextLink.getStartNode()) ? GTUDirectionality.DIR_PLUS : GTUDirectionality.DIR_MINUS;
491 nextNode = nextLink.getEndNode();
492
493 remainingDistance += nextLink.getLength().getSI();
494 linkBeforeBranch = (CrossSectionLink) nextLink;
495
496 if (currentLane.nextLanes(gtu.getGTUType()).size() == 0)
497 {
498
499
500 if (currentLane
501 .accessibleAdjacentLanesLegal(LateralDirectionality.RIGHT, gtu.getGTUType(), drivingDirection)
502 .size() > 0)
503 {
504 for (Lane adjacentLane : currentLane.accessibleAdjacentLanesLegal(LateralDirectionality.RIGHT,
505 gtu.getGTUType(), drivingDirection))
506 {
507 if (adjacentLane.nextLanes(gtu.getGTUType()).size() > 0)
508 {
509 currentLane = adjacentLane;
510 break;
511 }
512
513
514 }
515 }
516 for (Lane adjacentLane : currentLane.accessibleAdjacentLanesLegal(LateralDirectionality.LEFT,
517 gtu.getGTUType(), drivingDirection))
518 {
519 if (adjacentLane.nextLanes(gtu.getGTUType()).size() > 0)
520 {
521 currentLane = adjacentLane;
522 break;
523 }
524
525
526 }
527 if (currentLane.nextLanes(gtu.getGTUType()).size() == 0)
528 {
529 throw new NetworkException(
530 "Lane ends and there is not a compatible adjacent lane that does " + "not end");
531 }
532 }
533
534 for (Lane nextLane : currentLane.nextLanes(gtu.getGTUType()).keySet())
535 {
536 if (nextLane.getLaneType().getDirectionality(gtu.getGTUType()).getDirectionalities()
537 .contains(drivingDirection))
538 {
539 currentLane = currentLane.nextLanes(gtu.getGTUType()).keySet().iterator().next();
540 break;
541 }
542 }
543 spareTime -= currentLane.getLength().getSI() / currentLane.getSpeedLimit(gtu.getGTUType()).getSI();
544 }
545 else
546 {
547
548
549 return NOLANECHANGENEEDED;
550 }
551 lastLink = nextLink;
552 }
553 }
554 if (null == nextNode)
555 {
556 throw new NetworkException("Cannot find the next branch or sink node");
557 }
558
559
560 Map<Lane, Length> suitabilityOfLanesBeforeBranch = new LinkedHashMap<>();
561 Link linkAfterBranch =
562 gtu.getStrategicalPlanner().nextLinkDirection(nextSplitNode, lastLink, gtu.getGTUType()).getLink();
563 GTUDirectionality drivingDirectionOnNextLane =
564 linkAfterBranch.getStartNode().equals(nextSplitNode) ? GTUDirectionality.DIR_PLUS : GTUDirectionality.DIR_MINUS;
565 for (CrossSectionElement cse : linkBeforeBranch.getCrossSectionElementList())
566 {
567 if (cse instanceof Lane)
568 {
569 Lane l = (Lane) cse;
570 if (l.getLaneType().getDirectionality(gtu.getGTUType()).getDirectionalities()
571 .contains(GTUDirectionality.DIR_PLUS))
572 {
573 for (Lane connectingLane : l.nextLanes(gtu.getGTUType()).keySet())
574 {
575 if (connectingLane.getParentLink() == linkAfterBranch
576 && connectingLane.getLaneType().isCompatible(gtu.getGTUType(), drivingDirectionOnNextLane))
577 {
578 Length currentValue = suitabilityOfLanesBeforeBranch.get(l);
579
580
581
582 Length value = suitability(connectingLane, new Length(0, LengthUnit.SI), gtu,
583 new Duration(spareTime, DurationUnit.SI));
584
585 value = value.plus(new Length(remainingDistance, LengthUnit.SI));
586
587
588 suitabilityOfLanesBeforeBranch.put(l,
589 null == currentValue || value.le(currentValue) ? value : currentValue);
590 }
591 }
592 }
593 }
594 }
595 if (suitabilityOfLanesBeforeBranch.size() == 0)
596 {
597 throw new NetworkException("No lanes available on Link " + linkBeforeBranch);
598 }
599 Length currentLaneSuitability = suitabilityOfLanesBeforeBranch.get(currentLane);
600 if (null != currentLaneSuitability)
601 {
602 return currentLaneSuitability;
603 }
604
605 int totalLanes = countCompatibleLanes(currentLane.getParentLink(), gtu.getGTUType(), GTUDirectionality.DIR_PLUS);
606 Length leftSuitability =
607 computeSuitabilityWithLaneChanges(currentLane, remainingDistance, suitabilityOfLanesBeforeBranch, totalLanes,
608 LateralDirectionality.LEFT, gtu.getGTUType(), GTUDirectionality.DIR_PLUS);
609 Length rightSuitability =
610 computeSuitabilityWithLaneChanges(currentLane, remainingDistance, suitabilityOfLanesBeforeBranch, totalLanes,
611 LateralDirectionality.RIGHT, gtu.getGTUType(), GTUDirectionality.DIR_PLUS);
612 if (leftSuitability.ge(rightSuitability))
613 {
614 return leftSuitability;
615 }
616 else if (rightSuitability.ge(leftSuitability))
617 {
618
619 return rightSuitability;
620 }
621 if (leftSuitability.le(GETOFFTHISLANENOW))
622 {
623 throw new NetworkException("Changing lanes in any direction does not get the GTU on a suitable lane");
624 }
625 return leftSuitability;
626 }
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641 protected final Length computeSuitabilityWithLaneChanges(final Lane startLane, final double remainingDistance,
642 final Map<Lane, Length> suitabilities, final int totalLanes, final LateralDirectionality direction,
643 final GTUType gtuType, final GTUDirectionality drivingDirection)
644 {
645
646
647
648
649
650
651
652
653
654 int laneChangesUsed = 0;
655 Lane currentLane = startLane;
656 Length currentSuitability = null;
657 while (null == currentSuitability)
658 {
659 laneChangesUsed++;
660 if (currentLane.accessibleAdjacentLanesLegal(direction, gtuType, drivingDirection).size() == 0)
661 {
662 return GETOFFTHISLANENOW;
663 }
664 currentLane = currentLane.accessibleAdjacentLanesLegal(direction, gtuType, drivingDirection).iterator().next();
665 currentSuitability = suitabilities.get(currentLane);
666 }
667 double fraction = currentSuitability == NOLANECHANGENEEDED ? 0 : 0.5;
668 int notSuitableLaneCount = totalLanes - suitabilities.size();
669 return new Length(
670 remainingDistance * (notSuitableLaneCount - laneChangesUsed + 1 + fraction) / (notSuitableLaneCount + fraction),
671 LengthUnit.SI);
672 }
673
674
675
676
677
678
679
680
681
682 protected final int countCompatibleLanes(final CrossSectionLink link, final GTUType gtuType,
683 final GTUDirectionality drivingDirection)
684 {
685 int result = 0;
686 for (CrossSectionElement cse : link.getCrossSectionElementList())
687 {
688 if (cse instanceof Lane)
689 {
690 Lane l = (Lane) cse;
691 if (l.getLaneType().isCompatible(gtuType, drivingDirection))
692 {
693 result++;
694 }
695 }
696 }
697 return result;
698 }
699
700
701 @Override
702 public final String toString()
703 {
704 return "LaneBasedCFLCTacticalPlanner [laneChangeModel=" + this.laneChangeModel + "]";
705 }
706
707 }