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