1 package org.opentrafficsim.road.gtu.lane;
2
3 import java.util.ArrayList;
4 import java.util.Collections;
5 import java.util.Comparator;
6 import java.util.LinkedHashMap;
7 import java.util.LinkedHashSet;
8 import java.util.List;
9 import java.util.Map;
10 import java.util.Map.Entry;
11 import java.util.Set;
12 import java.util.SortedMap;
13
14 import org.djunits.unit.DirectionUnit;
15 import org.djunits.unit.DurationUnit;
16 import org.djunits.unit.LengthUnit;
17 import org.djunits.unit.PositionUnit;
18 import org.djunits.value.vdouble.scalar.Acceleration;
19 import org.djunits.value.vdouble.scalar.Duration;
20 import org.djunits.value.vdouble.scalar.Length;
21 import org.djunits.value.vdouble.scalar.Speed;
22 import org.djunits.value.vdouble.scalar.Time;
23 import org.djutils.draw.point.Point3d;
24 import org.djutils.exceptions.Throw;
25 import org.djutils.exceptions.Try;
26 import org.djutils.immutablecollections.ImmutableMap;
27 import org.djutils.logger.CategoryLogger;
28 import org.djutils.multikeymap.MultiKeyMap;
29 import org.opentrafficsim.base.parameters.ParameterException;
30 import org.opentrafficsim.core.dsol.OTSSimulatorInterface;
31 import org.opentrafficsim.core.geometry.Bounds;
32 import org.opentrafficsim.core.geometry.DirectedPoint;
33 import org.opentrafficsim.core.geometry.OTSGeometryException;
34 import org.opentrafficsim.core.geometry.OTSLine3D;
35 import org.opentrafficsim.core.geometry.OTSLine3D.FractionalFallback;
36 import org.opentrafficsim.core.geometry.OTSPoint3D;
37 import org.opentrafficsim.core.gtu.AbstractGTU;
38 import org.opentrafficsim.core.gtu.GTU;
39 import org.opentrafficsim.core.gtu.GTUDirectionality;
40 import org.opentrafficsim.core.gtu.GTUException;
41 import org.opentrafficsim.core.gtu.GTUType;
42 import org.opentrafficsim.core.gtu.RelativePosition;
43 import org.opentrafficsim.core.gtu.TurnIndicatorStatus;
44 import org.opentrafficsim.core.gtu.perception.EgoPerception;
45 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlan;
46 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanBuilder;
47 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
48 import org.opentrafficsim.core.network.LateralDirectionality;
49 import org.opentrafficsim.core.network.NetworkException;
50 import org.opentrafficsim.core.perception.Historical;
51 import org.opentrafficsim.core.perception.HistoricalValue;
52 import org.opentrafficsim.core.perception.HistoryManager;
53 import org.opentrafficsim.core.perception.collections.HistoricalArrayList;
54 import org.opentrafficsim.core.perception.collections.HistoricalList;
55 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
56 import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable;
57 import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
58 import org.opentrafficsim.road.gtu.lane.perception.categories.InfrastructurePerception;
59 import org.opentrafficsim.road.gtu.lane.perception.categories.neighbors.NeighborsPerception;
60 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
61 import org.opentrafficsim.road.gtu.lane.plan.operational.LaneBasedOperationalPlan;
62 import org.opentrafficsim.road.gtu.strategical.LaneBasedStrategicalPlanner;
63 import org.opentrafficsim.road.network.OTSRoadNetwork;
64 import org.opentrafficsim.road.network.RoadNetwork;
65 import org.opentrafficsim.road.network.lane.CrossSectionLink;
66 import org.opentrafficsim.road.network.lane.DirectedLanePosition;
67 import org.opentrafficsim.road.network.lane.Lane;
68 import org.opentrafficsim.road.network.lane.LaneDirection;
69 import org.opentrafficsim.road.network.lane.object.sensor.SingleSensor;
70 import org.opentrafficsim.road.network.speed.SpeedLimitInfo;
71 import org.opentrafficsim.road.network.speed.SpeedLimitTypes;
72
73 import nl.tudelft.simulation.dsol.SimRuntimeException;
74 import nl.tudelft.simulation.dsol.formalisms.eventscheduling.SimEventInterface;
75 import nl.tudelft.simulation.dsol.simtime.SimTimeDoubleUnit;
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102 public abstract class AbstractLaneBasedGTU2 extends AbstractGTU implements LaneBasedGTU
103 {
104
105 private static final long serialVersionUID = 20140822L;
106
107
108 private final HistoricalList<CrossSection> crossSections;
109
110
111 private int referenceLaneIndex = 0;
112
113
114 private double referencePositionTime = Double.NaN;
115
116
117 private DirectedLanePosition cachedReferencePosition = null;
118
119
120 private SimEventInterface<SimTimeDoubleUnit> pendingLeaveTrigger;
121
122
123 private SimEventInterface<SimTimeDoubleUnit> pendingEnterTrigger;
124
125
126 private SimEventInterface<SimTimeDoubleUnit> finalizeLaneChangeEvent;
127
128
129 private Set<SimEventInterface<SimTimeDoubleUnit>> sensorEvents = new LinkedHashSet<>();
130
131
132 private Speed cachedDesiredSpeed;
133
134
135 private Time desiredSpeedTime;
136
137
138 private Acceleration cachedCarFollowingAcceleration;
139
140
141 private Time carFollowingAccelerationTime;
142
143
144 private Object lock = new Object();
145
146
147 @SuppressWarnings("checkstyle:visibilitymodifier")
148 public static Length initialLocationThresholdDifference = new Length(1.0, LengthUnit.MILLIMETER);
149
150
151 public static Length eventMargin = Length.instantiateSI(50.0);
152
153
154 private final Historical<TurnIndicatorStatus> turnIndicatorStatus;
155
156
157
158 public static boolean CACHING = true;
159
160
161
162 public static int CACHED_POSITION = 0;
163
164
165
166 public static int NON_CACHED_POSITION = 0;
167
168
169 private VehicleModel vehicleModel = VehicleModel.MINMAX;
170
171
172 private boolean instantaneousLaneChange = false;
173
174
175
176
177
178
179
180
181 public AbstractLaneBasedGTU2(final String id, final GTUType gtuType, final OTSRoadNetwork network) throws GTUException
182 {
183 super(id, gtuType, network.getSimulator(), network);
184 OTSSimulatorInterface simulator = network.getSimulator();
185 HistoryManager historyManager = simulator.getReplication().getHistoryManager(simulator);
186 this.crossSections = new HistoricalArrayList<>(historyManager);
187 this.turnIndicatorStatus = new HistoricalValue<>(historyManager, TurnIndicatorStatus.NOTPRESENT);
188 }
189
190
191
192
193
194
195
196
197
198
199
200 @SuppressWarnings("checkstyle:designforextension")
201 public void init(final LaneBasedStrategicalPlanner strategicalPlanner,
202 final Set<DirectedLanePosition> initialLongitudinalPositions, final Speed initialSpeed)
203 throws NetworkException, SimRuntimeException, GTUException, OTSGeometryException
204 {
205 Throw.when(null == initialLongitudinalPositions, GTUException.class, "InitialLongitudinalPositions is null");
206 Throw.when(0 == initialLongitudinalPositions.size(), GTUException.class, "InitialLongitudinalPositions is empty set");
207
208 DirectedPoint lastPoint = null;
209 for (DirectedLanePosition pos : initialLongitudinalPositions)
210 {
211
212
213 lastPoint = pos.getLocation();
214 }
215 DirectedPoint initialLocation = lastPoint;
216
217
218 Time now = getSimulator().getSimulatorTime();
219 try
220 {
221 if (initialSpeed.si < OperationalPlan.DRIFTING_SPEED_SI)
222 {
223 this.operationalPlan
224 .set(new OperationalPlan(this, initialLocation, now, new Duration(1E-6, DurationUnit.SECOND)));
225 }
226 else
227 {
228 OTSPoint3D p2 = new OTSPoint3D(initialLocation.x + 1E-6 * Math.cos(initialLocation.getRotZ()),
229 initialLocation.y + 1E-6 * Math.sin(initialLocation.getRotZ()), initialLocation.z);
230 OTSLine3D path = new OTSLine3D(new OTSPoint3D(initialLocation), p2);
231 this.operationalPlan.set(OperationalPlanBuilder.buildConstantSpeedPlan(this, path, now, initialSpeed));
232 }
233 }
234 catch (OperationalPlanException e)
235 {
236 throw new RuntimeException("Initial operational plan could not be created.", e);
237 }
238
239
240 List<DirectedLanePosition> inits = new ArrayList<>();
241 inits.addAll(initialLongitudinalPositions);
242 Collections.sort(inits, new Comparator<DirectedLanePosition>()
243 {
244 @Override
245 public int compare(final DirectedLanePosition/DirectedLanePosition.html#DirectedLanePosition">DirectedLanePosition o1, final DirectedLanePosition o2)
246 {
247 Length length1 =
248 o1.getGtuDirection().isPlus() ? o1.getPosition() : o1.getLane().getLength().minus(o1.getPosition());
249 Length length2 =
250 o2.getGtuDirection().isPlus() ? o2.getPosition() : o2.getLane().getLength().minus(o2.getPosition());
251 return length1.compareTo(length2);
252 }
253 });
254 for (DirectedLanePosition directedLanePosition : inits)
255 {
256 List<Lane> lanes = new ArrayList<>();
257 lanes.add(directedLanePosition.getLane());
258 this.crossSections.add(new CrossSection(lanes, directedLanePosition.getGtuDirection()));
259 }
260
261
262 DirectedLanePosition referencePosition = getReferencePosition();
263 fireTimedEvent(LaneBasedGTU.LANEBASED_INIT_EVENT,
264 new Object[] { getId(), new OTSPoint3D(initialLocation).doubleVector(PositionUnit.METER),
265 OTSPoint3D.direction(initialLocation, DirectionUnit.EAST_RADIAN), getLength(), getWidth(),
266 referencePosition.getLane().getParentLink().getId(), referencePosition.getLane().getId(),
267 referencePosition.getPosition(), referencePosition.getGtuDirection().name(), getGTUType().getId() },
268 getSimulator().getSimulatorTime());
269
270
271 for (DirectedLanePosition directedLanePosition : initialLongitudinalPositions)
272 {
273 Lane lane = directedLanePosition.getLane();
274 lane.addGTU(this, directedLanePosition.getPosition());
275 }
276
277
278 super.init(strategicalPlanner, initialLocation, initialSpeed);
279
280 this.referencePositionTime = Double.NaN;
281
282 }
283
284
285
286
287 @Override
288 public synchronized void setParent(final GTU gtu) throws GTUException
289 {
290 leaveAllLanes();
291 super.setParent(gtu);
292 }
293
294
295
296
297 private void leaveAllLanes()
298 {
299 for (CrossSection crossSection : this.crossSections)
300 {
301 boolean removeFromParentLink = true;
302 for (Lane lane : crossSection.getLanes())
303 {
304
305 Length pos = Try.assign(() -> position(lane, getReference()), "Unexpected exception.");
306 lane.removeGTU(this, removeFromParentLink, pos);
307 removeFromParentLink = false;
308 }
309 }
310 this.crossSections.clear();
311 }
312
313
314
315
316
317
318
319
320
321 public void reinit(final Set<DirectedLanePosition> initialLongitudinalPositions)
322 throws NetworkException, SimRuntimeException, GTUException, OTSGeometryException
323 {
324 init(getStrategicalPlanner(), initialLongitudinalPositions, Speed.ZERO);
325 }
326
327
328 @Override
329 public synchronized void changeLaneInstantaneously(final LateralDirectionality laneChangeDirection) throws GTUException
330 {
331
332
333 DirectedLanePosition from = getReferencePosition();
334
335
336 GTUDirectionality direction = getDirection(from.getLane());
337 Set<Lane> adjLanes = from.getLane().accessibleAdjacentLanesPhysical(laneChangeDirection, getGTUType(), direction);
338 Lane adjLane = adjLanes.iterator().next();
339 Length position = adjLane.position(from.getLane().fraction(from.getPosition()));
340 leaveAllLanes();
341 enterLaneRecursive(new LaneDirection(adjLane, direction), position, 0);
342
343
344 this.referencePositionTime = Double.NaN;
345 this.cachedPositions.clear();
346
347
348 this.fireTimedEvent(
349 LaneBasedGTU.LANE_CHANGE_EVENT, new Object[] { getId(), laneChangeDirection.name(),
350 from.getLane().getParentLink().getId(), from.getLane().getId(), from.getPosition() },
351 getSimulator().getSimulatorTime());
352
353 }
354
355
356
357
358
359
360
361
362 private void enterLaneRecursive(final LaneDirection lane, final Length position, final int dir) throws GTUException
363 {
364 List<Lane> lanes = new ArrayList<>();
365 lanes.add(lane.getLane());
366 int index = dir > 0 ? this.crossSections.size() : 0;
367 this.crossSections.add(index, new CrossSection(lanes, lane.getDirection()));
368 lane.getLane().addGTU(this, position);
369
370
371 if (dir < 1)
372 {
373 Length rear = lane.getDirection().isPlus() ? position.plus(getRear().getDx()) : position.minus(getRear().getDx());
374 Length before = null;
375 if (lane.getDirection().isPlus() && rear.si < 0.0)
376 {
377 before = rear.neg();
378 }
379 else if (lane.getDirection().isMinus() && rear.si > lane.getLength().si)
380 {
381 before = rear.minus(lane.getLength());
382 }
383 if (before != null)
384 {
385 GTUDirectionality upDir = lane.getDirection();
386 ImmutableMap<Lane, GTUDirectionality> upstream = lane.getLane().upstreamLanes(upDir, getGTUType());
387 if (!upstream.isEmpty())
388 {
389 Lane upLane = null;
390 for (Lane nextUp : upstream.keySet())
391 {
392 for (CrossSection crossSection : this.crossSections)
393 {
394 if (crossSection.getLanes().contains(nextUp))
395 {
396
397
398 upLane = nextUp;
399 break;
400 }
401 }
402 }
403 if (upLane == null)
404 {
405
406
407 upLane = upstream.keySet().iterator().next();
408 }
409 upDir = upstream.get(upLane);
410 LaneDirectionk/lane/LaneDirection.html#LaneDirection">LaneDirection next = new LaneDirection(upLane, upDir);
411 Length nextPos = upDir.isPlus() ? next.getLength().minus(before).minus(getRear().getDx())
412 : before.plus(getRear().getDx());
413 enterLaneRecursive(next, nextPos, -1);
414 }
415 }
416 }
417
418
419 if (dir > -1)
420 {
421 Length front =
422 lane.getDirection().isPlus() ? position.plus(getFront().getDx()) : position.minus(getFront().getDx());
423 Length passed = null;
424 if (lane.getDirection().isPlus() && front.si > lane.getLength().si)
425 {
426 passed = front.minus(lane.getLength());
427 }
428 else if (lane.getDirection().isMinus() && front.si < 0.0)
429 {
430 passed = front.neg();
431 }
432 if (passed != null)
433 {
434 LaneDirection next = lane.getNextLaneDirection(this);
435 Length nextPos = next.getDirection().isPlus() ? passed.minus(getFront().getDx())
436 : next.getLength().minus(passed).plus(getFront().getDx());
437 enterLaneRecursive(next, nextPos, 1);
438 }
439 }
440 }
441
442
443
444
445
446
447 @Override
448 @SuppressWarnings("checkstyle:designforextension")
449 public synchronized void initLaneChange(final LateralDirectionality laneChangeDirection) throws GTUException
450 {
451 List<CrossSection> newLanes = new ArrayList<>();
452 int index = laneChangeDirection.isLeft() ? 0 : 1;
453 int numRegistered = 0;
454 DirectedPoint point = getLocation();
455 Map<Lane, Double> addToLanes = new LinkedHashMap<>();
456 for (CrossSection crossSection : this.crossSections)
457 {
458 List<Lane> resultingLanes = new ArrayList<>();
459 Lane lane = crossSection.getLanes().get(0);
460 resultingLanes.add(lane);
461 Set<Lane> laneSet = lane.accessibleAdjacentLanesLegal(laneChangeDirection, getGTUType(), getDirection(lane));
462 if (laneSet.size() > 0)
463 {
464 numRegistered++;
465 Lane adjacentLane = laneSet.iterator().next();
466 double f = adjacentLane.getCenterLine().projectFractional(null, null, point.x, point.y, FractionalFallback.NaN);
467 if (Double.isNaN(f))
468 {
469
470
471
472 Length pos = position(lane, getReference());
473 addToLanes.put(adjacentLane, pos.si < lane.getLength().si / 2 ? 0.0 : 1.0);
474 }
475 else
476 {
477 f = crossSection.getDirection().isPlus() ? f : 1.0 - f;
478 addToLanes.put(adjacentLane, adjacentLane.getLength().times(f).si / adjacentLane.getLength().si);
479 }
480 resultingLanes.add(index, adjacentLane);
481 }
482 newLanes.add(new CrossSection(resultingLanes, crossSection.getDirection()));
483 }
484 Throw.when(numRegistered == 0, GTUException.class, "Gtu %s starting %s lane change, but no adjacent lane found.",
485 getId(), laneChangeDirection);
486 this.crossSections.clear();
487 this.crossSections.addAll(newLanes);
488 for (Entry<Lane, Double> entry : addToLanes.entrySet())
489 {
490 entry.getKey().addGTU(this, entry.getValue());
491 }
492 this.referenceLaneIndex = 1 - index;
493 }
494
495
496
497
498
499
500 @SuppressWarnings("checkstyle:designforextension")
501 protected synchronized void finalizeLaneChange(final LateralDirectionality laneChangeDirection) throws GTUException
502 {
503 if (getId().equals("140"))
504 {
505 System.err.println("140 finalizing lane change");
506 }
507 List<CrossSection> newLanes = new ArrayList<>();
508 Lane fromLane = null;
509 Length fromPosition = null;
510 GTUDirectionality fromDirection = null;
511 for (CrossSection crossSection : this.crossSections)
512 {
513 Lane lane = crossSection.getLanes().get(this.referenceLaneIndex);
514 if (getId().equals("140"))
515 {
516 System.err.println(" 140 finalizing lane change lane " + lane);
517 }
518 if (lane != null)
519 {
520 Length pos = position(lane, RelativePosition.REFERENCE_POSITION);
521 if (0.0 <= pos.si && pos.si <= lane.getLength().si)
522 {
523 fromLane = lane;
524 fromPosition = pos;
525 fromDirection = getDirection(lane);
526 }
527 lane.removeGTU(this, false, pos);
528 }
529 List<Lane> remainingLane = new ArrayList<>();
530 remainingLane.add(crossSection.getLanes().get(1 - this.referenceLaneIndex));
531 newLanes.add(new CrossSection(remainingLane, crossSection.getDirection()));
532 }
533 this.crossSections.clear();
534 this.crossSections.addAll(newLanes);
535 this.referenceLaneIndex = 0;
536
537 Throw.when(fromLane == null, RuntimeException.class, "No from lane for lane change event.");
538 DirectedLanePosition from;
539 try
540 {
541 from = new DirectedLanePosition(fromLane, fromPosition, fromDirection);
542 }
543 catch (GTUException exception)
544 {
545 throw new RuntimeException(exception);
546 }
547
548
549
550 this.fireTimedEvent(
551 LaneBasedGTU.LANE_CHANGE_EVENT, new Object[] { getId(), laneChangeDirection.name(),
552 from.getLane().getParentLink().getId(), from.getLane().getId(), from.getPosition() },
553 getSimulator().getSimulatorTime());
554
555 this.finalizeLaneChangeEvent = null;
556 }
557
558
559 @Override
560 public void setFinalizeLaneChangeEvent(final SimEventInterface<SimTimeDoubleUnit> event)
561 {
562 if (getId().equals("140"))
563 {
564 System.err.println("setFinalizeLaneChangeEvent for 140");
565 }
566 this.finalizeLaneChangeEvent = event;
567 }
568
569
570 @Override
571 public final synchronized GTUDirectionality getDirection(final Lane lane) throws GTUException
572 {
573 for (CrossSection crossSection : this.crossSections)
574 {
575 if (crossSection.getLanes().contains(lane))
576 {
577 return crossSection.getDirection();
578 }
579 }
580 throw new GTUException("getDirection: GTU does not contain " + lane);
581 }
582
583
584 @Override
585 @SuppressWarnings("checkstyle:designforextension")
586 protected synchronized boolean move(final DirectedPoint fromLocation)
587 throws SimRuntimeException, GTUException, OperationalPlanException, NetworkException, ParameterException
588 {
589 if (getId().equals("140"))
590 {
591
592 }
593 if (this.isDestroyed())
594 {
595 return false;
596 }
597 try
598 {
599 if (this.crossSections.isEmpty())
600 {
601 destroy();
602 return false;
603 }
604
605
606 cancelAllEvents();
607
608
609
610 boolean error = super.move(fromLocation);
611 if (error)
612 {
613 return error;
614 }
615
616 DirectedLanePosition dlp = getReferencePosition();
617
618 scheduleEnterEvent();
619 scheduleLeaveEvent();
620
621
622 for (CrossSection crossSection : this.crossSections)
623 {
624 for (Lane lane : crossSection.getLanes())
625 {
626 scheduleTriggers(lane, crossSection.getDirection());
627 }
628 }
629
630 fireTimedEvent(LaneBasedGTU.LANEBASED_MOVE_EVENT,
631 new Object[] { getId(), new OTSPoint3D(fromLocation).doubleVector(PositionUnit.METER),
632 OTSPoint3D.direction(fromLocation, DirectionUnit.EAST_RADIAN), getSpeed(), getAcceleration(),
633 getTurnIndicatorStatus().name(), getOdometer(), dlp.getLane().getParentLink().getId(),
634 dlp.getLane().getId(), dlp.getPosition(), dlp.getGtuDirection().name() },
635 getSimulator().getSimulatorTime());
636
637 return false;
638
639 }
640 catch (Exception ex)
641 {
642 try
643 {
644 getErrorHandler().handle(this, ex);
645 }
646 catch (Exception exception)
647 {
648 throw new GTUException(exception);
649 }
650 return true;
651 }
652
653 }
654
655
656
657
658 private void cancelAllEvents()
659 {
660 if (this.pendingEnterTrigger != null)
661 {
662 getSimulator().cancelEvent(this.pendingEnterTrigger);
663 }
664 if (this.pendingLeaveTrigger != null)
665 {
666 getSimulator().cancelEvent(this.pendingLeaveTrigger);
667 }
668 if (this.finalizeLaneChangeEvent != null)
669 {
670 getSimulator().cancelEvent(this.finalizeLaneChangeEvent);
671 }
672 for (SimEventInterface<SimTimeDoubleUnit> event : this.sensorEvents)
673 {
674 if (event.getAbsoluteExecutionTime().gt(getSimulator().getSimTime()))
675 {
676 getSimulator().cancelEvent(event);
677 }
678 }
679 this.sensorEvents.clear();
680 }
681
682
683
684
685
686
687
688
689 protected void scheduleEnterEvent() throws GTUException, OperationalPlanException, SimRuntimeException
690 {
691 CrossSection lastCrossSection = this.crossSections.get(this.crossSections.size() - 1);
692
693 Length remain = remainingEventDistance();
694 Lane lane = lastCrossSection.getLanes().get(this.referenceLaneIndex);
695 Length position = position(lane, getFront());
696 boolean possiblyNearNextSection =
697 lastCrossSection.getDirection().isPlus() ? lane.getLength().minus(position).lt(remain) : position.lt(remain);
698 if (possiblyNearNextSection)
699 {
700 CrossSectionLink link = lastCrossSection.getLanes().get(0).getParentLink();
701 OTSLine3D enterLine = lastCrossSection.getDirection().isPlus() ? link.getEndLine() : link.getStartLine();
702 Time enterTime = timeAtLine(enterLine, getFront());
703 if (enterTime != null)
704 {
705 if (enterTime.lt(getSimulator().getSimulatorTime()))
706 {
707 System.err.println(
708 "Time travel? enterTime=" + enterTime + "; simulator time=" + getSimulator().getSimulatorTime());
709 enterTime = getSimulator().getSimulatorTime();
710 }
711 this.pendingEnterTrigger = getSimulator().scheduleEventAbs(enterTime, this, this, "enterCrossSection", null);
712 }
713 }
714 }
715
716
717
718
719
720
721
722 protected synchronized void enterCrossSection() throws GTUException, OperationalPlanException, SimRuntimeException
723 {
724 CrossSection lastCrossSection = this.crossSections.get(this.crossSections.size() - 1);
725 LaneDirection laneDirection =
726 new LaneDirection(lastCrossSection.getLanes().get(this.referenceLaneIndex), lastCrossSection.getDirection());
727 LaneDirection nextLaneDirection = laneDirection.getNextLaneDirection(this);
728 if (nextLaneDirection == null)
729 {
730 forceLaneChangeFinalization();
731 return;
732 }
733 double insertFraction = nextLaneDirection.getDirection().isPlus() ? 0.0 : 1.0;
734 List<Lane> nextLanes = new ArrayList<>();
735 for (int i = 0; i < lastCrossSection.getLanes().size(); i++)
736 {
737 if (i == this.referenceLaneIndex)
738 {
739 nextLanes.add(nextLaneDirection.getLane());
740 }
741 else
742 {
743 Lane lane = lastCrossSection.getLanes().get(i);
744 ImmutableMap<Lane, GTUDirectionality> lanes = lane.downstreamLanes(laneDirection.getDirection(), getGTUType());
745 if (lanes.size() == 1)
746 {
747 Lane nextLane = lanes.keySet().iterator().next();
748 nextLanes.add(nextLane);
749 }
750 else
751 {
752 boolean added = false;
753 for (Lane nextLane : lanes.keySet())
754 {
755 if (nextLane.getParentLink().equals(nextLaneDirection.getLane().getParentLink())
756 && nextLane.accessibleAdjacentLanesPhysical(
757 this.referenceLaneIndex == 0 ? LateralDirectionality.LEFT : LateralDirectionality.RIGHT,
758 getGTUType(), nextLaneDirection.getDirection()).contains(nextLaneDirection.getLane()))
759 {
760 nextLanes.add(nextLane);
761 added = true;
762 break;
763 }
764 }
765 if (!added)
766 {
767 forceLaneChangeFinalization();
768 return;
769 }
770 }
771 }
772 }
773 this.crossSections.add(new CrossSection(nextLanes, nextLaneDirection.getDirection()));
774 for (Lane lane : nextLanes)
775 {
776 lane.addGTU(this, insertFraction);
777 }
778 this.pendingEnterTrigger = null;
779 scheduleEnterEvent();
780 for (Lane lane : nextLanes)
781 {
782 scheduleTriggers(lane, nextLaneDirection.getDirection());
783 }
784 }
785
786
787
788
789
790
791
792
793
794 private void forceLaneChangeFinalization() throws GTUException, OperationalPlanException, SimRuntimeException
795 {
796 if (getId().equals("140"))
797 {
798 System.err.println("forceLineChangeFinalization for 140");
799 }
800 if (this.finalizeLaneChangeEvent != null)
801 {
802
803 SimEventInterface<SimTimeDoubleUnit> tmp = this.finalizeLaneChangeEvent;
804 finalizeLaneChange(this.referenceLaneIndex == 0 ? LateralDirectionality.RIGHT : LateralDirectionality.LEFT);
805 getSimulator().cancelEvent(tmp);
806 enterCrossSection();
807 if (getId().equals("140"))
808 {
809 System.err.println(" forceLineChangeFinalization -> enterCrossSection() for 140");
810 }
811 }
812
813 }
814
815
816
817
818
819
820
821
822 protected void scheduleLeaveEvent() throws GTUException, OperationalPlanException, SimRuntimeException
823 {
824 if (this.crossSections.isEmpty())
825 {
826 CategoryLogger.always().error("GTU {} has empty crossSections", this);
827 return;
828 }
829 CrossSection firstCrossSection = this.crossSections.get(0);
830
831 boolean possiblyNearNextSection =
832 !getReferencePosition().getLane().equals(firstCrossSection.getLanes().get(this.referenceLaneIndex));
833 if (!possiblyNearNextSection)
834 {
835 Length remain = remainingEventDistance();
836 Lane lane = firstCrossSection.getLanes().get(this.referenceLaneIndex);
837 Length position = position(lane, getRear());
838 possiblyNearNextSection = firstCrossSection.getDirection().isPlus() ? lane.getLength().minus(position).lt(remain)
839 : position.lt(remain);
840 }
841 if (possiblyNearNextSection)
842 {
843 CrossSectionLink link = firstCrossSection.getLanes().get(0).getParentLink();
844 OTSLine3D leaveLine = firstCrossSection.getDirection().isPlus() ? link.getEndLine() : link.getStartLine();
845 Time leaveTime = timeAtLine(leaveLine, getRear());
846 if (leaveTime == null)
847 {
848
849 Lane lane = this.crossSections.get(0).getLanes().get(this.referenceLaneIndex);
850 Length pos = position(lane, getRear());
851 if (pos.gt(lane.getLength()))
852 {
853 pos = position(lane, getRear());
854 this.pendingLeaveTrigger = getSimulator().scheduleEventNow(this, this, "leaveCrossSection", null);
855 if (getId().equals("140"))
856 {
857 System.err.println("140 scheduled to leave lane " + lane);
858 }
859
860 getSimulator().getLogger().always().info("Forcing leave for GTU {} on lane {}", getId(), lane.getFullId());
861 }
862 }
863 if (leaveTime != null)
864 {
865 if (leaveTime.lt(getSimulator().getSimulatorTime()))
866 {
867 System.err.println(
868 "Time travel? leaveTime=" + leaveTime + "; simulator time=" + getSimulator().getSimulatorTime());
869 leaveTime = getSimulator().getSimulatorTime();
870 }
871 this.pendingLeaveTrigger = getSimulator().scheduleEventAbs(leaveTime, this, this, "leaveCrossSection", null);
872 }
873 }
874 }
875
876
877
878
879
880
881
882
883 protected synchronized void leaveCrossSection() throws GTUException, OperationalPlanException, SimRuntimeException
884 {
885
886 List<Lane> lanes = this.crossSections.get(0).getLanes();
887 for (int i = 0; i < lanes.size(); i++)
888 {
889 Lane lane = lanes.get(i);
890 if (getId().equals("140"))
891 {
892 System.err.println(" 140 left lane " + lane);
893 }
894 if (lane != null)
895 {
896 lane.removeGTU(this, i == lanes.size() - 1, position(lane, getReference()));
897 }
898 }
899 this.crossSections.remove(0);
900 this.pendingLeaveTrigger = null;
901 scheduleLeaveEvent();
902 }
903
904
905
906
907
908
909
910
911
912 protected void scheduleTriggers(final Lane lane, final GTUDirectionality direction)
913 throws GTUException, OperationalPlanException, SimRuntimeException
914 {
915 double min;
916 double max;
917 Length remain = remainingEventDistance();
918 if (direction.isPlus())
919 {
920 min = position(lane, getRear()).si;
921 max = min + remain.si + getLength().si;
922 }
923 else
924 {
925 max = position(lane, getRear()).si;
926 min = max - remain.si - getLength().si;
927 }
928 SortedMap<Double, List<SingleSensor>> sensors = lane.getSensorMap(getGTUType(), direction).subMap(min, max);
929 for (List<SingleSensor> list : sensors.values())
930 {
931 for (SingleSensor sensor : list)
932 {
933 RelativePosition pos = this.getRelativePositions().get(sensor.getPositionType());
934 Time time = timeAtLine(sensor.getGeometry(), pos);
935 if (time != null)
936 {
937 this.sensorEvents
938 .add(getSimulator().scheduleEventAbs(time, this, sensor, "trigger", new Object[] { this }));
939 }
940 }
941 }
942 }
943
944
945
946
947
948
949 private Length remainingEventDistance() throws OperationalPlanException
950 {
951 if (getOperationalPlan() instanceof LaneBasedOperationalPlan)
952 {
953 LaneBasedOperationalPlanrg/opentrafficsim/road/gtu/lane/plan/operational/LaneBasedOperationalPlan.html#LaneBasedOperationalPlan">LaneBasedOperationalPlan plan = (LaneBasedOperationalPlan) getOperationalPlan();
954 return plan.getTotalLength().minus(plan.getTraveledDistance(getSimulator().getSimulatorTime())).plus(eventMargin);
955 }
956 return getOperationalPlan().getTotalLength().plus(eventMargin);
957 }
958
959
960
961
962
963
964
965
966
967
968 private Time timeAtLine(final OTSLine3D line, final RelativePosition relativePosition) throws GTUException
969 {
970 Throw.when(line.size() != 2, IllegalArgumentException.class, "Line to cross with path should have 2 points.");
971 OTSLine3D path = getOperationalPlan().getPath();
972 OTSPoint3D[] points;
973 double adjust;
974 if (relativePosition.getDx().gt0())
975 {
976
977 points = new OTSPoint3D[path.size() + 1];
978 System.arraycopy(path.getPoints(), 0, points, 0, path.size());
979 points[path.size()] = new OTSPoint3D(path.getLocationExtendedSI(path.getLengthSI() + relativePosition.getDx().si));
980 adjust = -relativePosition.getDx().si;
981 }
982 else if (relativePosition.getDx().lt0())
983 {
984 points = new OTSPoint3D[path.size() + 1];
985 System.arraycopy(path.getPoints(), 0, points, 1, path.size());
986 points[0] = new OTSPoint3D(path.getLocationExtendedSI(relativePosition.getDx().si));
987 adjust = 0.0;
988 }
989 else
990 {
991 points = path.getPoints();
992 adjust = 0.0;
993 }
994
995
996 double cumul = 0.0;
997 for (int i = 0; i < points.length - 1; i++)
998 {
999 OTSPoint3D intersect;
1000 try
1001 {
1002 intersect = OTSPoint3D.intersectionOfLineSegments(points[i], points[i + 1], line.get(0), line.get(1));
1003 }
1004 catch (OTSGeometryException exception)
1005 {
1006
1007 throw new RuntimeException("Unexpected exception while obtaining points from line to cross.", exception);
1008 }
1009 if (intersect != null)
1010 {
1011 cumul += points[i].distanceSI(intersect);
1012 cumul += adjust;
1013
1014 if (cumul < 0.0)
1015 {
1016 return getSimulator().getSimulatorTime();
1017 }
1018 if (cumul <= getOperationalPlan().getTotalLength().si)
1019 {
1020 return getOperationalPlan().timeAtDistance(Length.instantiateSI(cumul));
1021 }
1022
1023 return null;
1024 }
1025 else if (i < points.length - 2)
1026 {
1027 cumul += points[i].distanceSI(points[i + 1]);
1028 }
1029 }
1030
1031 return null;
1032 }
1033
1034
1035 @Override
1036 public final Map<Lane, Length> positions(final RelativePosition relativePosition) throws GTUException
1037 {
1038 return positions(relativePosition, getSimulator().getSimulatorTime());
1039 }
1040
1041
1042 @Override
1043 public final Map<Lane, Length> positions(final RelativePosition relativePosition, final Time when) throws GTUException
1044 {
1045 Map<Lane, Length> positions = new LinkedHashMap<>();
1046 for (CrossSection crossSection : this.crossSections.get(when))
1047 {
1048 for (Lane lane : crossSection.getLanes())
1049 {
1050 positions.put(lane, position(lane, relativePosition, when));
1051 }
1052 }
1053 return positions;
1054 }
1055
1056
1057 @Override
1058 public final Length position(final Lane lane, final RelativePosition relativePosition) throws GTUException
1059 {
1060 return position(lane, relativePosition, getSimulator().getSimulatorTime());
1061 }
1062
1063
1064 private double cachePositionsTime = Double.NaN;
1065
1066
1067 private OperationalPlan cacheOperationalPlan = null;
1068
1069
1070 private MultiKeyMap<Length> cachedPositions = new MultiKeyMap<>(Lane.class, RelativePosition.class);
1071
1072
1073 @Override
1074 @SuppressWarnings("checkstyle:designforextension")
1075 public Length position(final Lane lane, final RelativePosition relativePosition, final Time when) throws GTUException
1076 {
1077 synchronized (this)
1078 {
1079 OperationalPlan plan = getOperationalPlan(when);
1080 if (CACHING)
1081 {
1082 if (when.si == this.cachePositionsTime && plan == this.cacheOperationalPlan)
1083 {
1084 Length l = this.cachedPositions.get(lane, relativePosition);
1085 if (l != null && (!Double.isNaN(l.si)))
1086 {
1087 CACHED_POSITION++;
1088
1089
1090
1091
1092
1093
1094
1095
1096
1097
1098
1099
1100 return l;
1101 }
1102 }
1103 if (when.si != this.cachePositionsTime || plan != this.cacheOperationalPlan)
1104 {
1105 this.cachePositionsTime = Double.NaN;
1106 this.cacheOperationalPlan = null;
1107 this.cachedPositions.clear();
1108 }
1109 }
1110 NON_CACHED_POSITION++;
1111
1112 synchronized (this.lock)
1113 {
1114 List<CrossSection> whenCrossSections = this.crossSections.get(when);
1115 double loc = Double.NaN;
1116
1117 try
1118 {
1119 int crossSectionIndex = -1;
1120 int lateralIndex = -1;
1121 for (int i = 0; i < whenCrossSections.size(); i++)
1122 {
1123 if (whenCrossSections.get(i).getLanes().contains(lane))
1124 {
1125 crossSectionIndex = i;
1126 lateralIndex = whenCrossSections.get(i).getLanes().indexOf(lane);
1127 break;
1128 }
1129 }
1130 Throw.when(lateralIndex == -1, GTUException.class, "GTU %s is not on lane %s.", this, lane);
1131
1132 DirectedPoint p = plan.getLocation(when, relativePosition);
1133 double f = lane.getCenterLine().projectFractional(null, null, p.x, p.y, FractionalFallback.NaN);
1134 if (!Double.isNaN(f))
1135 {
1136 loc = f * lane.getLength().si;
1137 }
1138 else
1139 {
1140
1141
1142 double distance = 0.0;
1143 for (int i = crossSectionIndex - 1; i >= 0; i--)
1144 {
1145 Lane tryLane = whenCrossSections.get(i).getLanes().get(lateralIndex);
1146 f = tryLane.getCenterLine().projectFractional(null, null, p.x, p.y, FractionalFallback.NaN);
1147 if (!Double.isNaN(f))
1148 {
1149 f = whenCrossSections.get(i).getDirection() == GTUDirectionality.DIR_PLUS ? 1 - f : f;
1150 loc = distance - f * tryLane.getLength().si;
1151 break;
1152 }
1153 distance -= tryLane.getLength().si;
1154 }
1155
1156 if (Double.isNaN(loc))
1157 {
1158 distance = lane.getLength().si;
1159 for (int i = crossSectionIndex + 1; i < whenCrossSections.size(); i++)
1160 {
1161 Lane tryLane = whenCrossSections.get(i).getLanes().get(lateralIndex);
1162 f = tryLane.getCenterLine().projectFractional(null, null, p.x, p.y, FractionalFallback.NaN);
1163 if (!Double.isNaN(f))
1164 {
1165 f = whenCrossSections.get(i).getDirection() == GTUDirectionality.DIR_PLUS ? f : 1 - f;
1166 loc = distance + f * tryLane.getLength().si;
1167 break;
1168 }
1169 distance += tryLane.getLength().si;
1170 }
1171 }
1172
1173 }
1174
1175 if (Double.isNaN(loc))
1176 {
1177
1178
1179
1180 f = lane.getCenterLine().projectFractional(null, null, p.x, p.y, FractionalFallback.ENDPOINT);
1181 if (Double.isNaN(f))
1182 {
1183 CategoryLogger.always().error("GTU {} at location {} cannot project itself onto {}; p is {}", this,
1184 getLocation(), lane.getCenterLine(), p);
1185 plan.getLocation(when, relativePosition);
1186 }
1187 loc = lane.getLength().si * f;
1188
1189
1190
1191
1192
1193
1194
1195
1196
1197
1198
1199
1200
1201
1202
1203 }
1204 }
1205 catch (Exception e)
1206 {
1207
1208 throw new GTUException(e);
1209 }
1210
1211 Length length = Length.instantiateSI(loc);
1212 if (CACHING)
1213 {
1214 this.cachedPositions.put(length, lane, relativePosition);
1215 this.cachePositionsTime = when.si;
1216 this.cacheOperationalPlan = plan;
1217 }
1218 return length;
1219 }
1220 }
1221 }
1222
1223
1224 @Override
1225 @SuppressWarnings("checkstyle:designforextension")
1226 public DirectedLanePosition getReferencePosition() throws GTUException
1227 {
1228 synchronized (this)
1229 {
1230 if (this.referencePositionTime == getSimulator().getSimulatorTime().si)
1231 {
1232 return this.cachedReferencePosition;
1233 }
1234 Lane refLane = null;
1235 for (CrossSection crossSection : this.crossSections)
1236 {
1237 Lane lane = crossSection.getLanes().get(this.referenceLaneIndex);
1238 double fraction = fractionalPosition(lane, getReference());
1239 if (fraction >= 0.0 && fraction <= 1.0)
1240 {
1241 refLane = lane;
1242 break;
1243 }
1244 }
1245 if (refLane != null)
1246 {
1247 this.cachedReferencePosition =
1248 new DirectedLanePosition(refLane, position(refLane, getReference()), this.getDirection(refLane));
1249 this.referencePositionTime = getSimulator().getSimulatorTime().si;
1250 return this.cachedReferencePosition;
1251 }
1252 CategoryLogger.always().error("The reference point of GTU {} is not on any of the lanes on which it is registered",
1253 this);
1254 for (CrossSection crossSection : this.crossSections)
1255 {
1256 Lane lane = crossSection.getLanes().get(this.referenceLaneIndex);
1257 double fraction = fractionalPosition(lane, getReference());
1258 CategoryLogger.always().error("\tGTU is on lane \"{}\" at fraction {}", lane, fraction);
1259 }
1260 throw new GTUException(
1261 "The reference point of GTU " + this + " is not on any of the lanes on which it is registered");
1262 }
1263 }
1264
1265
1266 @Override
1267 public final Map<Lane, Double> fractionalPositions(final RelativePosition relativePosition) throws GTUException
1268 {
1269 return fractionalPositions(relativePosition, getSimulator().getSimulatorTime());
1270 }
1271
1272
1273 @Override
1274 public final Map<Lane, Double> fractionalPositions(final RelativePosition relativePosition, final Time when)
1275 throws GTUException
1276 {
1277 Map<Lane, Double> positions = new LinkedHashMap<>();
1278 for (CrossSection crossSection : this.crossSections)
1279 {
1280 for (Lane lane : crossSection.getLanes())
1281 {
1282 positions.put(lane, fractionalPosition(lane, relativePosition, when));
1283 }
1284 }
1285 return positions;
1286 }
1287
1288
1289 @Override
1290 public final double fractionalPosition(final Lane lane, final RelativePosition relativePosition, final Time when)
1291 throws GTUException
1292 {
1293 return position(lane, relativePosition, when).getSI() / lane.getLength().getSI();
1294 }
1295
1296
1297 @Override
1298 public final double fractionalPosition(final Lane lane, final RelativePosition relativePosition) throws GTUException
1299 {
1300 return position(lane, relativePosition).getSI() / lane.getLength().getSI();
1301 }
1302
1303
1304 @Override
1305 public final void addTrigger(final Lane lane, final SimEventInterface<SimTimeDoubleUnit> event)
1306 {
1307 throw new UnsupportedOperationException("Method addTrigger is not supported.");
1308 }
1309
1310
1311
1312
1313
1314 public void setVehicleModel(final VehicleModel vehicleModel)
1315 {
1316 this.vehicleModel = vehicleModel;
1317 }
1318
1319
1320 @Override
1321 public VehicleModel getVehicleModel()
1322 {
1323 return this.vehicleModel;
1324 }
1325
1326
1327 @Override
1328 @SuppressWarnings("checkstyle:designforextension")
1329 public void destroy()
1330 {
1331 DirectedLanePosition dlp = null;
1332 try
1333 {
1334 dlp = getReferencePosition();
1335 }
1336 catch (GTUException e)
1337 {
1338
1339 }
1340 DirectedPoint location = this.getOperationalPlan() == null ? new DirectedPoint(0.0, 0.0, 0.0) : getLocation();
1341 synchronized (this.lock)
1342 {
1343 for (CrossSection crossSection : this.crossSections)
1344 {
1345 boolean removeFromParentLink = true;
1346 for (Lane lane : crossSection.getLanes())
1347 {
1348 Length position;
1349 try
1350 {
1351 position = position(lane, getReference());
1352 }
1353 catch (GTUException exception)
1354 {
1355
1356
1357 throw new RuntimeException(exception);
1358 }
1359 lane.removeGTU(this, removeFromParentLink, position);
1360 removeFromParentLink = false;
1361 }
1362 }
1363 }
1364 if (dlp != null && dlp.getLane() != null)
1365 {
1366 Lane referenceLane = dlp.getLane();
1367 fireTimedEvent(LaneBasedGTU.LANEBASED_DESTROY_EVENT,
1368 new Object[] { getId(), new OTSPoint3D(location).doubleVector(PositionUnit.METER),
1369 OTSPoint3D.direction(location, DirectionUnit.EAST_RADIAN), getOdometer(),
1370 referenceLane.getParentLink().getId(), referenceLane.getId(), dlp.getPosition(),
1371 dlp.getGtuDirection().name() },
1372 getSimulator().getSimulatorTime());
1373 }
1374 else
1375 {
1376 fireTimedEvent(LaneBasedGTU.LANEBASED_DESTROY_EVENT,
1377 new Object[] { getId(), location, getOdometer(), null, Length.ZERO, null },
1378 getSimulator().getSimulatorTime());
1379 }
1380 cancelAllEvents();
1381
1382 super.destroy();
1383 }
1384
1385
1386 @Override
1387 public final Bounds getBounds()
1388 {
1389 double dx = 0.5 * getLength().doubleValue();
1390 double dy = 0.5 * getWidth().doubleValue();
1391 return new Bounds(new Point3d(-dx, -dy, 0.0), new Point3d(dx, dy, 0.0));
1392 }
1393
1394
1395 @Override
1396 public final LaneBasedStrategicalPlanner getStrategicalPlanner()
1397 {
1398 return (LaneBasedStrategicalPlanner) super.getStrategicalPlanner();
1399 }
1400
1401
1402 @Override
1403 public final LaneBasedStrategicalPlanner getStrategicalPlanner(final Time time)
1404 {
1405 return (LaneBasedStrategicalPlanner) super.getStrategicalPlanner(time);
1406 }
1407
1408
1409 @Override
1410 public RoadNetwork getNetwork()
1411 {
1412 return (RoadNetwork) super.getPerceivableContext();
1413 }
1414
1415
1416 @Override
1417 public Speed getDesiredSpeed()
1418 {
1419 synchronized (this)
1420 {
1421 Time simTime = getSimulator().getSimulatorTime();
1422 if (this.desiredSpeedTime == null || this.desiredSpeedTime.si < simTime.si)
1423 {
1424 InfrastructurePerception infra =
1425 getTacticalPlanner().getPerception().getPerceptionCategoryOrNull(InfrastructurePerception.class);
1426 SpeedLimitInfo speedInfo;
1427 if (infra == null)
1428 {
1429 speedInfo = new SpeedLimitInfo();
1430 speedInfo.addSpeedInfo(SpeedLimitTypes.MAX_VEHICLE_SPEED, getMaximumSpeed());
1431 }
1432 else
1433 {
1434
1435 speedInfo = infra.getSpeedLimitProspect(RelativeLane.CURRENT).getSpeedLimitInfo(Length.ZERO);
1436 }
1437 this.cachedDesiredSpeed =
1438 Try.assign(() -> getTacticalPlanner().getCarFollowingModel().desiredSpeed(getParameters(), speedInfo),
1439 "Parameter exception while obtaining the desired speed.");
1440 this.desiredSpeedTime = simTime;
1441 }
1442 return this.cachedDesiredSpeed;
1443 }
1444 }
1445
1446
1447 @Override
1448 public Acceleration getCarFollowingAcceleration()
1449 {
1450 synchronized (this)
1451 {
1452 Time simTime = getSimulator().getSimulatorTime();
1453 if (this.carFollowingAccelerationTime == null || this.carFollowingAccelerationTime.si < simTime.si)
1454 {
1455 LanePerception perception = getTacticalPlanner().getPerception();
1456
1457 EgoPerception<?, ?> ego = perception.getPerceptionCategoryOrNull(EgoPerception.class);
1458 Throw.whenNull(ego, "EgoPerception is required to determine the speed.");
1459 Speed speed = ego.getSpeed();
1460
1461 InfrastructurePerception infra = perception.getPerceptionCategoryOrNull(InfrastructurePerception.class);
1462 Throw.whenNull(infra, "InfrastructurePerception is required to determine the desired speed.");
1463 SpeedLimitInfo speedInfo = infra.getSpeedLimitProspect(RelativeLane.CURRENT).getSpeedLimitInfo(Length.ZERO);
1464
1465 NeighborsPerception neighbors = perception.getPerceptionCategoryOrNull(NeighborsPerception.class);
1466 Throw.whenNull(neighbors, "NeighborsPerception is required to determine the car-following acceleration.");
1467 PerceptionCollectable<HeadwayGTU, LaneBasedGTU> leaders = neighbors.getLeaders(RelativeLane.CURRENT);
1468
1469 this.cachedCarFollowingAcceleration =
1470 Try.assign(() -> getTacticalPlanner().getCarFollowingModel().followingAcceleration(getParameters(),
1471 speed, speedInfo, leaders), "Parameter exception while obtaining the desired speed.");
1472 this.carFollowingAccelerationTime = simTime;
1473 }
1474 return this.cachedCarFollowingAcceleration;
1475 }
1476 }
1477
1478
1479 @Override
1480 public final TurnIndicatorStatus getTurnIndicatorStatus()
1481 {
1482 return this.turnIndicatorStatus.get();
1483 }
1484
1485
1486 @Override
1487 public final TurnIndicatorStatus getTurnIndicatorStatus(final Time time)
1488 {
1489 return this.turnIndicatorStatus.get(time);
1490 }
1491
1492
1493 @Override
1494 public final void setTurnIndicatorStatus(final TurnIndicatorStatus turnIndicatorStatus)
1495 {
1496 this.turnIndicatorStatus.set(turnIndicatorStatus);
1497 }
1498
1499
1500 @Override
1501 public Length getLateralPosition(final Lane lane) throws GTUException
1502 {
1503 OperationalPlan plan = getOperationalPlan();
1504 if (plan instanceof LaneBasedOperationalPlanntrafficsim/road/gtu/lane/plan/operational/LaneBasedOperationalPlan.html#LaneBasedOperationalPlan">LaneBasedOperationalPlan && !((LaneBasedOperationalPlan) plan).isDeviative())
1505 {
1506 return Length.ZERO;
1507 }
1508 DirectedLanePosition ref = getReferencePosition();
1509 int latIndex = -1;
1510 int longIndex = -1;
1511 for (int i = 0; i < this.crossSections.size(); i++)
1512 {
1513 List<Lane> lanes = this.crossSections.get(i).getLanes();
1514 if (lanes.contains(lane))
1515 {
1516 latIndex = lanes.indexOf(lane);
1517 }
1518 if (lanes.contains(ref.getLane()))
1519 {
1520 longIndex = i;
1521 }
1522 }
1523 Throw.when(latIndex == -1 || longIndex == -1, GTUException.class, "GTU %s is not on %s", getId(), lane);
1524 Lane refCrossSectionLane = this.crossSections.get(longIndex).getLanes().get(latIndex);
1525 DirectedPoint loc = getLocation();
1526 double f = refCrossSectionLane.getCenterLine().projectOrthogonal(loc.x, loc.y);
1527 DirectedPoint p = Try.assign(() -> refCrossSectionLane.getCenterLine().getLocationFraction(f), GTUException.class,
1528 "GTU %s is not orthogonal to the reference lane.", getId());
1529 double d = p.distance(loc);
1530 d = ref.getGtuDirection().isPlus() ? d : -d;
1531 if (this.crossSections.get(0).getLanes().size() > 1)
1532 {
1533 return Length.instantiateSI(latIndex == 0 ? -d : d);
1534 }
1535 double x2 = p.x + Math.cos(p.getRotZ());
1536 double y2 = p.y + Math.sin(p.getRotZ());
1537 double det = (loc.x - p.x) * (y2 - p.y) - (loc.y - p.y) * (x2 - p.x);
1538 return Length.instantiateSI(det < 0.0 ? -d : d);
1539 }
1540
1541
1542 @Override
1543 public void setInstantaneousLaneChange(final boolean instantaneous)
1544 {
1545 this.instantaneousLaneChange = instantaneous;
1546 }
1547
1548
1549 @Override
1550 public boolean isInstantaneousLaneChange()
1551 {
1552 return this.instantaneousLaneChange;
1553 }
1554
1555
1556 @Override
1557 @SuppressWarnings("checkstyle:designforextension")
1558 public String toString()
1559 {
1560 return String.format("GTU " + getId());
1561 }
1562
1563
1564 private static class CrossSection
1565 {
1566
1567
1568 private final List<Lane> lanes;
1569
1570
1571 private final GTUDirectionality direction;
1572
1573
1574
1575
1576
1577 protected CrossSection(final List<Lane> lanes, final GTUDirectionality direction)
1578 {
1579 this.lanes = lanes;
1580 this.direction = direction;
1581 }
1582
1583
1584
1585
1586 protected List<Lane> getLanes()
1587 {
1588 return this.lanes;
1589 }
1590
1591
1592
1593
1594 protected GTUDirectionality getDirection()
1595 {
1596 return this.direction;
1597 }
1598
1599 }
1600
1601 }