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 List<CrossSection> newLanes = new ArrayList<>();
506 Lane fromLane = null;
507 Length fromPosition = null;
508 GTUDirectionality fromDirection = null;
509 for (CrossSection crossSection : this.crossSections)
510 {
511 Lane lane = crossSection.getLanes().get(this.referenceLaneIndex);
512 if (lane != null)
513 {
514 Length pos = position(lane, RelativePosition.REFERENCE_POSITION);
515 if (0.0 <= pos.si && pos.si <= lane.getLength().si)
516 {
517 fromLane = lane;
518 fromPosition = pos;
519 fromDirection = getDirection(lane);
520 }
521 lane.removeGTU(this, false, pos);
522 }
523 List<Lane> remainingLane = new ArrayList<>();
524 remainingLane.add(crossSection.getLanes().get(1 - this.referenceLaneIndex));
525 newLanes.add(new CrossSection(remainingLane, crossSection.getDirection()));
526 }
527 this.crossSections.clear();
528 this.crossSections.addAll(newLanes);
529 this.referenceLaneIndex = 0;
530
531 Throw.when(fromLane == null, RuntimeException.class, "No from lane for lane change event.");
532 DirectedLanePosition from;
533 try
534 {
535 from = new DirectedLanePosition(fromLane, fromPosition, fromDirection);
536 }
537 catch (GTUException exception)
538 {
539 throw new RuntimeException(exception);
540 }
541
542
543
544 this.fireTimedEvent(
545 LaneBasedGTU.LANE_CHANGE_EVENT, new Object[] { getId(), laneChangeDirection.name(),
546 from.getLane().getParentLink().getId(), from.getLane().getId(), from.getPosition() },
547 getSimulator().getSimulatorTime());
548
549 this.finalizeLaneChangeEvent = null;
550 }
551
552
553 @Override
554 public void setFinalizeLaneChangeEvent(final SimEventInterface<SimTimeDoubleUnit> event)
555 {
556 this.finalizeLaneChangeEvent = event;
557 }
558
559
560 @Override
561 public final synchronized GTUDirectionality getDirection(final Lane lane) throws GTUException
562 {
563 for (CrossSection crossSection : this.crossSections)
564 {
565 if (crossSection.getLanes().contains(lane))
566 {
567 return crossSection.getDirection();
568 }
569 }
570 throw new GTUException("getDirection: GTU does not contain " + lane);
571 }
572
573
574 @Override
575 @SuppressWarnings("checkstyle:designforextension")
576 protected synchronized boolean move(final DirectedPoint fromLocation)
577 throws SimRuntimeException, GTUException, OperationalPlanException, NetworkException, ParameterException
578 {
579 if (this.isDestroyed())
580 {
581 return false;
582 }
583 try
584 {
585 if (this.crossSections.isEmpty())
586 {
587 destroy();
588 return false;
589 }
590
591
592 cancelAllEvents();
593
594
595
596 boolean error = super.move(fromLocation);
597 if (error)
598 {
599 return error;
600 }
601
602 DirectedLanePosition dlp = getReferencePosition();
603
604 scheduleEnterEvent();
605 scheduleLeaveEvent();
606
607
608 for (CrossSection crossSection : this.crossSections)
609 {
610 for (Lane lane : crossSection.getLanes())
611 {
612 scheduleTriggers(lane, crossSection.getDirection());
613 }
614 }
615
616 fireTimedEvent(LaneBasedGTU.LANEBASED_MOVE_EVENT,
617 new Object[] { getId(), new OTSPoint3D(fromLocation).doubleVector(PositionUnit.METER),
618 OTSPoint3D.direction(fromLocation, DirectionUnit.EAST_RADIAN), getSpeed(), getAcceleration(),
619 getTurnIndicatorStatus().name(), getOdometer(), dlp.getLane().getParentLink().getId(),
620 dlp.getLane().getId(), dlp.getPosition(), dlp.getGtuDirection().name() },
621 getSimulator().getSimulatorTime());
622
623 return false;
624
625 }
626 catch (Exception ex)
627 {
628 try
629 {
630 getErrorHandler().handle(this, ex);
631 }
632 catch (Exception exception)
633 {
634 throw new GTUException(exception);
635 }
636 return true;
637 }
638
639 }
640
641
642
643
644 private void cancelAllEvents()
645 {
646 if (this.pendingEnterTrigger != null)
647 {
648 getSimulator().cancelEvent(this.pendingEnterTrigger);
649 }
650 if (this.pendingLeaveTrigger != null)
651 {
652 getSimulator().cancelEvent(this.pendingLeaveTrigger);
653 }
654 if (this.finalizeLaneChangeEvent != null)
655 {
656 getSimulator().cancelEvent(this.finalizeLaneChangeEvent);
657 }
658 for (SimEventInterface<SimTimeDoubleUnit> event : this.sensorEvents)
659 {
660 if (event.getAbsoluteExecutionTime().gt(getSimulator().getSimTime()))
661 {
662 getSimulator().cancelEvent(event);
663 }
664 }
665 this.sensorEvents.clear();
666 }
667
668
669
670
671
672
673
674
675 protected void scheduleEnterEvent() throws GTUException, OperationalPlanException, SimRuntimeException
676 {
677 CrossSection lastCrossSection = this.crossSections.get(this.crossSections.size() - 1);
678
679 Length remain = remainingEventDistance();
680 Lane lane = lastCrossSection.getLanes().get(this.referenceLaneIndex);
681 Length position = position(lane, getFront());
682 boolean possiblyNearNextSection =
683 lastCrossSection.getDirection().isPlus() ? lane.getLength().minus(position).lt(remain) : position.lt(remain);
684 if (possiblyNearNextSection)
685 {
686 CrossSectionLink link = lastCrossSection.getLanes().get(0).getParentLink();
687 OTSLine3D enterLine = lastCrossSection.getDirection().isPlus() ? link.getEndLine() : link.getStartLine();
688 Time enterTime = timeAtLine(enterLine, getFront());
689 if (enterTime != null)
690 {
691 if (enterTime.lt(getSimulator().getSimulatorTime()))
692 {
693 System.err.println(
694 "Time travel? enterTime=" + enterTime + "; simulator time=" + getSimulator().getSimulatorTime());
695 enterTime = getSimulator().getSimulatorTime();
696 }
697 this.pendingEnterTrigger = getSimulator().scheduleEventAbs(enterTime, this, this, "enterCrossSection", null);
698 }
699 }
700 }
701
702
703
704
705
706
707
708 protected synchronized void enterCrossSection() throws GTUException, OperationalPlanException, SimRuntimeException
709 {
710 CrossSection lastCrossSection = this.crossSections.get(this.crossSections.size() - 1);
711 LaneDirection laneDirection =
712 new LaneDirection(lastCrossSection.getLanes().get(this.referenceLaneIndex), lastCrossSection.getDirection());
713 LaneDirection nextLaneDirection = laneDirection.getNextLaneDirection(this);
714 if (nextLaneDirection == null)
715 {
716 forceLaneChangeFinalization();
717 return;
718 }
719 double insertFraction = nextLaneDirection.getDirection().isPlus() ? 0.0 : 1.0;
720 List<Lane> nextLanes = new ArrayList<>();
721 for (int i = 0; i < lastCrossSection.getLanes().size(); i++)
722 {
723 if (i == this.referenceLaneIndex)
724 {
725 nextLanes.add(nextLaneDirection.getLane());
726 }
727 else
728 {
729 Lane lane = lastCrossSection.getLanes().get(i);
730 ImmutableMap<Lane, GTUDirectionality> lanes = lane.downstreamLanes(laneDirection.getDirection(), getGTUType());
731 if (lanes.size() == 1)
732 {
733 Lane nextLane = lanes.keySet().iterator().next();
734 nextLanes.add(nextLane);
735 }
736 else
737 {
738 boolean added = false;
739 for (Lane nextLane : lanes.keySet())
740 {
741 if (nextLane.getParentLink().equals(nextLaneDirection.getLane().getParentLink())
742 && nextLane.accessibleAdjacentLanesPhysical(
743 this.referenceLaneIndex == 0 ? LateralDirectionality.LEFT : LateralDirectionality.RIGHT,
744 getGTUType(), nextLaneDirection.getDirection()).contains(nextLaneDirection.getLane()))
745 {
746 nextLanes.add(nextLane);
747 added = true;
748 break;
749 }
750 }
751 if (!added)
752 {
753 forceLaneChangeFinalization();
754 return;
755 }
756 }
757 }
758 }
759 this.crossSections.add(new CrossSection(nextLanes, nextLaneDirection.getDirection()));
760 for (Lane lane : nextLanes)
761 {
762 lane.addGTU(this, insertFraction);
763 }
764 this.pendingEnterTrigger = null;
765 scheduleEnterEvent();
766 for (Lane lane : nextLanes)
767 {
768 scheduleTriggers(lane, nextLaneDirection.getDirection());
769 }
770 }
771
772
773
774
775
776
777
778
779
780 private void forceLaneChangeFinalization() throws GTUException, OperationalPlanException, SimRuntimeException
781 {
782 if (this.finalizeLaneChangeEvent != null)
783 {
784
785 SimEventInterface<SimTimeDoubleUnit> tmp = this.finalizeLaneChangeEvent;
786 finalizeLaneChange(this.referenceLaneIndex == 0 ? LateralDirectionality.RIGHT : LateralDirectionality.LEFT);
787 getSimulator().cancelEvent(tmp);
788 enterCrossSection();
789 }
790
791 }
792
793
794
795
796
797
798
799
800 protected void scheduleLeaveEvent() throws GTUException, OperationalPlanException, SimRuntimeException
801 {
802 if (this.crossSections.isEmpty())
803 {
804 CategoryLogger.always().error("GTU {} has empty crossSections", this);
805 return;
806 }
807 CrossSection firstCrossSection = this.crossSections.get(0);
808
809 boolean possiblyNearNextSection =
810 !getReferencePosition().getLane().equals(firstCrossSection.getLanes().get(this.referenceLaneIndex));
811 if (!possiblyNearNextSection)
812 {
813 Length remain = remainingEventDistance();
814 Lane lane = firstCrossSection.getLanes().get(this.referenceLaneIndex);
815 Length position = position(lane, getRear());
816 possiblyNearNextSection = firstCrossSection.getDirection().isPlus() ? lane.getLength().minus(position).lt(remain)
817 : position.lt(remain);
818 }
819 if (possiblyNearNextSection)
820 {
821 CrossSectionLink link = firstCrossSection.getLanes().get(0).getParentLink();
822 OTSLine3D leaveLine = firstCrossSection.getDirection().isPlus() ? link.getEndLine() : link.getStartLine();
823 Time leaveTime = timeAtLine(leaveLine, getRear());
824 if (leaveTime == null)
825 {
826
827 Lane lane = this.crossSections.get(0).getLanes().get(this.referenceLaneIndex);
828 Length pos = position(lane, getRear());
829 if (pos.gt(lane.getLength()))
830 {
831 pos = position(lane, getRear());
832 this.pendingLeaveTrigger = getSimulator().scheduleEventNow(this, this, "leaveCrossSection", null);
833 getSimulator().getLogger().always().info("Forcing leave for GTU {} on lane {}", getId(), lane.getFullId());
834 }
835 }
836 if (leaveTime != null)
837 {
838 if (leaveTime.lt(getSimulator().getSimulatorTime()))
839 {
840 System.err.println(
841 "Time travel? leaveTime=" + leaveTime + "; simulator time=" + getSimulator().getSimulatorTime());
842 leaveTime = getSimulator().getSimulatorTime();
843 }
844 this.pendingLeaveTrigger = getSimulator().scheduleEventAbs(leaveTime, this, this, "leaveCrossSection", null);
845 }
846 }
847 }
848
849
850
851
852
853
854
855
856 protected synchronized void leaveCrossSection() throws GTUException, OperationalPlanException, SimRuntimeException
857 {
858 List<Lane> lanes = this.crossSections.get(0).getLanes();
859 for (int i = 0; i < lanes.size(); i++)
860 {
861 Lane lane = lanes.get(i);
862 if (lane != null)
863 {
864 lane.removeGTU(this, i == lanes.size() - 1, position(lane, getReference()));
865 }
866 }
867 this.crossSections.remove(0);
868 this.pendingLeaveTrigger = null;
869 scheduleLeaveEvent();
870 }
871
872
873
874
875
876
877
878
879
880 protected void scheduleTriggers(final Lane lane, final GTUDirectionality direction)
881 throws GTUException, OperationalPlanException, SimRuntimeException
882 {
883 double min;
884 double max;
885 Length remain = remainingEventDistance();
886 if (direction.isPlus())
887 {
888 min = position(lane, getRear()).si;
889 max = min + remain.si + getLength().si;
890 }
891 else
892 {
893 max = position(lane, getRear()).si;
894 min = max - remain.si - getLength().si;
895 }
896 SortedMap<Double, List<SingleSensor>> sensors = lane.getSensorMap(getGTUType(), direction).subMap(min, max);
897 for (List<SingleSensor> list : sensors.values())
898 {
899 for (SingleSensor sensor : list)
900 {
901 RelativePosition pos = this.getRelativePositions().get(sensor.getPositionType());
902 Time time = timeAtLine(sensor.getGeometry(), pos);
903 if (time != null)
904 {
905 this.sensorEvents
906 .add(getSimulator().scheduleEventAbs(time, this, sensor, "trigger", new Object[] { this }));
907 }
908 }
909 }
910 }
911
912
913
914
915
916
917 private Length remainingEventDistance() throws OperationalPlanException
918 {
919 if (getOperationalPlan() instanceof LaneBasedOperationalPlan)
920 {
921 LaneBasedOperationalPlanrg/opentrafficsim/road/gtu/lane/plan/operational/LaneBasedOperationalPlan.html#LaneBasedOperationalPlan">LaneBasedOperationalPlan plan = (LaneBasedOperationalPlan) getOperationalPlan();
922 return plan.getTotalLength().minus(plan.getTraveledDistance(getSimulator().getSimulatorTime())).plus(eventMargin);
923 }
924 return getOperationalPlan().getTotalLength().plus(eventMargin);
925 }
926
927
928
929
930
931
932
933
934
935
936 private Time timeAtLine(final OTSLine3D line, final RelativePosition relativePosition) throws GTUException
937 {
938 Throw.when(line.size() != 2, IllegalArgumentException.class, "Line to cross with path should have 2 points.");
939 OTSLine3D path = getOperationalPlan().getPath();
940 OTSPoint3D[] points;
941 double adjust;
942 if (relativePosition.getDx().gt0())
943 {
944
945 points = new OTSPoint3D[path.size() + 1];
946 System.arraycopy(path.getPoints(), 0, points, 0, path.size());
947 points[path.size()] = new OTSPoint3D(path.getLocationExtendedSI(path.getLengthSI() + relativePosition.getDx().si));
948 adjust = -relativePosition.getDx().si;
949 }
950 else if (relativePosition.getDx().lt0())
951 {
952 points = new OTSPoint3D[path.size() + 1];
953 System.arraycopy(path.getPoints(), 0, points, 1, path.size());
954 points[0] = new OTSPoint3D(path.getLocationExtendedSI(relativePosition.getDx().si));
955 adjust = 0.0;
956 }
957 else
958 {
959 points = path.getPoints();
960 adjust = 0.0;
961 }
962
963
964 double cumul = 0.0;
965 for (int i = 0; i < points.length - 1; i++)
966 {
967 OTSPoint3D intersect;
968 try
969 {
970 intersect = OTSPoint3D.intersectionOfLineSegments(points[i], points[i + 1], line.get(0), line.get(1));
971 }
972 catch (OTSGeometryException exception)
973 {
974
975 throw new RuntimeException("Unexpected exception while obtaining points from line to cross.", exception);
976 }
977 if (intersect != null)
978 {
979 cumul += points[i].distanceSI(intersect);
980 cumul += adjust;
981
982 if (cumul < 0.0)
983 {
984 return getSimulator().getSimulatorTime();
985 }
986 if (cumul <= getOperationalPlan().getTotalLength().si)
987 {
988 return getOperationalPlan().timeAtDistance(Length.instantiateSI(cumul));
989 }
990
991 return null;
992 }
993 else if (i < points.length - 2)
994 {
995 cumul += points[i].distanceSI(points[i + 1]);
996 }
997 }
998
999 return null;
1000 }
1001
1002
1003 @Override
1004 public final Map<Lane, Length> positions(final RelativePosition relativePosition) throws GTUException
1005 {
1006 return positions(relativePosition, getSimulator().getSimulatorTime());
1007 }
1008
1009
1010 @Override
1011 public final Map<Lane, Length> positions(final RelativePosition relativePosition, final Time when) throws GTUException
1012 {
1013 Map<Lane, Length> positions = new LinkedHashMap<>();
1014 for (CrossSection crossSection : this.crossSections.get(when))
1015 {
1016 for (Lane lane : crossSection.getLanes())
1017 {
1018 positions.put(lane, position(lane, relativePosition, when));
1019 }
1020 }
1021 return positions;
1022 }
1023
1024
1025 @Override
1026 public final Length position(final Lane lane, final RelativePosition relativePosition) throws GTUException
1027 {
1028 return position(lane, relativePosition, getSimulator().getSimulatorTime());
1029 }
1030
1031
1032 private double cachePositionsTime = Double.NaN;
1033
1034
1035 private OperationalPlan cacheOperationalPlan = null;
1036
1037
1038 private MultiKeyMap<Length> cachedPositions = new MultiKeyMap<>(Lane.class, RelativePosition.class);
1039
1040
1041 @Override
1042 @SuppressWarnings("checkstyle:designforextension")
1043 public Length position(final Lane lane, final RelativePosition relativePosition, final Time when) throws GTUException
1044 {
1045 synchronized (this)
1046 {
1047 OperationalPlan plan = getOperationalPlan(when);
1048 if (CACHING)
1049 {
1050 if (when.si == this.cachePositionsTime && plan == this.cacheOperationalPlan)
1051 {
1052 Length l = this.cachedPositions.get(lane, relativePosition);
1053 if (l != null)
1054 {
1055 CACHED_POSITION++;
1056
1057
1058
1059
1060
1061
1062
1063
1064
1065
1066
1067
1068 return l;
1069 }
1070 }
1071 if (when.si != this.cachePositionsTime || plan != this.cacheOperationalPlan)
1072 {
1073 this.cachePositionsTime = Double.NaN;
1074 this.cacheOperationalPlan = null;
1075 this.cachedPositions.clear();
1076 }
1077 }
1078 NON_CACHED_POSITION++;
1079
1080 synchronized (this.lock)
1081 {
1082 List<CrossSection> whenCrossSections = this.crossSections.get(when);
1083 double loc = Double.NaN;
1084
1085 try
1086 {
1087 int crossSectionIndex = -1;
1088 int lateralIndex = -1;
1089 for (int i = 0; i < whenCrossSections.size(); i++)
1090 {
1091 if (whenCrossSections.get(i).getLanes().contains(lane))
1092 {
1093 crossSectionIndex = i;
1094 lateralIndex = whenCrossSections.get(i).getLanes().indexOf(lane);
1095 break;
1096 }
1097 }
1098 Throw.when(lateralIndex == -1, GTUException.class, "GTU %s is not on lane %s.", this, lane);
1099
1100 DirectedPoint p = plan.getLocation(when, relativePosition);
1101 double f = lane.getCenterLine().projectFractional(null, null, p.x, p.y, FractionalFallback.NaN);
1102 if (!Double.isNaN(f))
1103 {
1104 loc = f * lane.getLength().si;
1105 }
1106 else
1107 {
1108
1109
1110 double distance = 0.0;
1111 for (int i = crossSectionIndex - 1; i >= 0; i--)
1112 {
1113 Lane tryLane = whenCrossSections.get(i).getLanes().get(lateralIndex);
1114 f = tryLane.getCenterLine().projectFractional(null, null, p.x, p.y, FractionalFallback.NaN);
1115 if (!Double.isNaN(f))
1116 {
1117 f = whenCrossSections.get(i).getDirection() == GTUDirectionality.DIR_PLUS ? 1 - f : f;
1118 loc = distance - f * tryLane.getLength().si;
1119 break;
1120 }
1121 distance -= tryLane.getLength().si;
1122 }
1123
1124 if (Double.isNaN(loc))
1125 {
1126 distance = lane.getLength().si;
1127 for (int i = crossSectionIndex + 1; i < whenCrossSections.size(); i++)
1128 {
1129 Lane tryLane = whenCrossSections.get(i).getLanes().get(lateralIndex);
1130 f = tryLane.getCenterLine().projectFractional(null, null, p.x, p.y, FractionalFallback.NaN);
1131 if (!Double.isNaN(f))
1132 {
1133 f = whenCrossSections.get(i).getDirection() == GTUDirectionality.DIR_PLUS ? f : 1 - f;
1134 loc = distance + f * tryLane.getLength().si;
1135 break;
1136 }
1137 distance += tryLane.getLength().si;
1138 }
1139 }
1140
1141 }
1142
1143 if (Double.isNaN(loc))
1144 {
1145
1146
1147
1148 f = lane.getCenterLine().projectFractional(null, null, p.x, p.y, FractionalFallback.ENDPOINT);
1149 loc = lane.getLength().si * f;
1150
1151
1152
1153
1154
1155
1156
1157
1158
1159
1160
1161
1162
1163
1164
1165 }
1166 }
1167 catch (Exception e)
1168 {
1169
1170 throw new GTUException(e);
1171 }
1172
1173 Length length = Length.instantiateSI(loc);
1174 if (CACHING)
1175 {
1176 this.cachedPositions.put(length, lane, relativePosition);
1177 this.cachePositionsTime = when.si;
1178 this.cacheOperationalPlan = plan;
1179 }
1180 return length;
1181 }
1182 }
1183 }
1184
1185
1186 @Override
1187 @SuppressWarnings("checkstyle:designforextension")
1188 public DirectedLanePosition getReferencePosition() throws GTUException
1189 {
1190 synchronized (this)
1191 {
1192 if (this.referencePositionTime == getSimulator().getSimulatorTime().si)
1193 {
1194 return this.cachedReferencePosition;
1195 }
1196 Lane refLane = null;
1197 for (CrossSection crossSection : this.crossSections)
1198 {
1199 Lane lane = crossSection.getLanes().get(this.referenceLaneIndex);
1200 double fraction = fractionalPosition(lane, getReference());
1201 if (fraction >= 0.0 && fraction <= 1.0)
1202 {
1203 refLane = lane;
1204 break;
1205 }
1206 }
1207 if (refLane != null)
1208 {
1209 this.cachedReferencePosition =
1210 new DirectedLanePosition(refLane, position(refLane, getReference()), this.getDirection(refLane));
1211 this.referencePositionTime = getSimulator().getSimulatorTime().si;
1212 return this.cachedReferencePosition;
1213 }
1214 throw new GTUException(
1215 "The reference point of GTU " + this + " is not on any of the lanes on which it is registered");
1216 }
1217 }
1218
1219
1220 @Override
1221 public final Map<Lane, Double> fractionalPositions(final RelativePosition relativePosition) throws GTUException
1222 {
1223 return fractionalPositions(relativePosition, getSimulator().getSimulatorTime());
1224 }
1225
1226
1227 @Override
1228 public final Map<Lane, Double> fractionalPositions(final RelativePosition relativePosition, final Time when)
1229 throws GTUException
1230 {
1231 Map<Lane, Double> positions = new LinkedHashMap<>();
1232 for (CrossSection crossSection : this.crossSections)
1233 {
1234 for (Lane lane : crossSection.getLanes())
1235 {
1236 positions.put(lane, fractionalPosition(lane, relativePosition, when));
1237 }
1238 }
1239 return positions;
1240 }
1241
1242
1243 @Override
1244 public final double fractionalPosition(final Lane lane, final RelativePosition relativePosition, final Time when)
1245 throws GTUException
1246 {
1247 return position(lane, relativePosition, when).getSI() / lane.getLength().getSI();
1248 }
1249
1250
1251 @Override
1252 public final double fractionalPosition(final Lane lane, final RelativePosition relativePosition) throws GTUException
1253 {
1254 return position(lane, relativePosition).getSI() / lane.getLength().getSI();
1255 }
1256
1257
1258 @Override
1259 public final void addTrigger(final Lane lane, final SimEventInterface<SimTimeDoubleUnit> event)
1260 {
1261 throw new UnsupportedOperationException("Method addTrigger is not supported.");
1262 }
1263
1264
1265
1266
1267
1268 public void setVehicleModel(final VehicleModel vehicleModel)
1269 {
1270 this.vehicleModel = vehicleModel;
1271 }
1272
1273
1274 @Override
1275 public VehicleModel getVehicleModel()
1276 {
1277 return this.vehicleModel;
1278 }
1279
1280
1281 @Override
1282 @SuppressWarnings("checkstyle:designforextension")
1283 public void destroy()
1284 {
1285 DirectedLanePosition dlp = null;
1286 try
1287 {
1288 dlp = getReferencePosition();
1289 }
1290 catch (GTUException e)
1291 {
1292
1293 }
1294 DirectedPoint location = this.getOperationalPlan() == null ? new DirectedPoint(0.0, 0.0, 0.0) : getLocation();
1295 synchronized (this.lock)
1296 {
1297 for (CrossSection crossSection : this.crossSections)
1298 {
1299 boolean removeFromParentLink = true;
1300 for (Lane lane : crossSection.getLanes())
1301 {
1302 Length position;
1303 try
1304 {
1305 position = position(lane, getReference());
1306 }
1307 catch (GTUException exception)
1308 {
1309
1310
1311 throw new RuntimeException(exception);
1312 }
1313 lane.removeGTU(this, removeFromParentLink, position);
1314 removeFromParentLink = false;
1315 }
1316 }
1317 }
1318 if (dlp != null && dlp.getLane() != null)
1319 {
1320 Lane referenceLane = dlp.getLane();
1321 fireTimedEvent(LaneBasedGTU.LANEBASED_DESTROY_EVENT,
1322 new Object[] { getId(), new OTSPoint3D(location).doubleVector(PositionUnit.METER),
1323 OTSPoint3D.direction(location, DirectionUnit.EAST_RADIAN), getOdometer(),
1324 referenceLane.getParentLink().getId(), referenceLane.getId(), dlp.getPosition(),
1325 dlp.getGtuDirection().name() },
1326 getSimulator().getSimulatorTime());
1327 }
1328 else
1329 {
1330 fireTimedEvent(LaneBasedGTU.LANEBASED_DESTROY_EVENT,
1331 new Object[] { getId(), location, getOdometer(), null, Length.ZERO, null },
1332 getSimulator().getSimulatorTime());
1333 }
1334 cancelAllEvents();
1335
1336 super.destroy();
1337 }
1338
1339
1340 @Override
1341 public final Bounds getBounds()
1342 {
1343 double dx = 0.5 * getLength().doubleValue();
1344 double dy = 0.5 * getWidth().doubleValue();
1345 return new BoundingBox(new Point3d(-dx, -dy, 0.0), new Point3d(dx, dy, 0.0));
1346 }
1347
1348
1349 @Override
1350 public final LaneBasedStrategicalPlanner getStrategicalPlanner()
1351 {
1352 return (LaneBasedStrategicalPlanner) super.getStrategicalPlanner();
1353 }
1354
1355
1356 @Override
1357 public final LaneBasedStrategicalPlanner getStrategicalPlanner(final Time time)
1358 {
1359 return (LaneBasedStrategicalPlanner) super.getStrategicalPlanner(time);
1360 }
1361
1362
1363 @Override
1364 public RoadNetwork getNetwork()
1365 {
1366 return (RoadNetwork) super.getPerceivableContext();
1367 }
1368
1369
1370 @Override
1371 public Speed getDesiredSpeed()
1372 {
1373 synchronized (this)
1374 {
1375 Time simTime = getSimulator().getSimulatorTime();
1376 if (this.desiredSpeedTime == null || this.desiredSpeedTime.si < simTime.si)
1377 {
1378 InfrastructurePerception infra =
1379 getTacticalPlanner().getPerception().getPerceptionCategoryOrNull(InfrastructurePerception.class);
1380 SpeedLimitInfo speedInfo;
1381 if (infra == null)
1382 {
1383 speedInfo = new SpeedLimitInfo();
1384 speedInfo.addSpeedInfo(SpeedLimitTypes.MAX_VEHICLE_SPEED, getMaximumSpeed());
1385 }
1386 else
1387 {
1388
1389 speedInfo = infra.getSpeedLimitProspect(RelativeLane.CURRENT).getSpeedLimitInfo(Length.ZERO);
1390 }
1391 this.cachedDesiredSpeed =
1392 Try.assign(() -> getTacticalPlanner().getCarFollowingModel().desiredSpeed(getParameters(), speedInfo),
1393 "Parameter exception while obtaining the desired speed.");
1394 this.desiredSpeedTime = simTime;
1395 }
1396 return this.cachedDesiredSpeed;
1397 }
1398 }
1399
1400
1401 @Override
1402 public Acceleration getCarFollowingAcceleration()
1403 {
1404 synchronized (this)
1405 {
1406 Time simTime = getSimulator().getSimulatorTime();
1407 if (this.carFollowingAccelerationTime == null || this.carFollowingAccelerationTime.si < simTime.si)
1408 {
1409 LanePerception perception = getTacticalPlanner().getPerception();
1410
1411 EgoPerception<?, ?> ego = perception.getPerceptionCategoryOrNull(EgoPerception.class);
1412 Throw.whenNull(ego, "EgoPerception is required to determine the speed.");
1413 Speed speed = ego.getSpeed();
1414
1415 InfrastructurePerception infra = perception.getPerceptionCategoryOrNull(InfrastructurePerception.class);
1416 Throw.whenNull(infra, "InfrastructurePerception is required to determine the desired speed.");
1417 SpeedLimitInfo speedInfo = infra.getSpeedLimitProspect(RelativeLane.CURRENT).getSpeedLimitInfo(Length.ZERO);
1418
1419 NeighborsPerception neighbors = perception.getPerceptionCategoryOrNull(NeighborsPerception.class);
1420 Throw.whenNull(neighbors, "NeighborsPerception is required to determine the car-following acceleration.");
1421 PerceptionCollectable<HeadwayGTU, LaneBasedGTU> leaders = neighbors.getLeaders(RelativeLane.CURRENT);
1422
1423 this.cachedCarFollowingAcceleration =
1424 Try.assign(() -> getTacticalPlanner().getCarFollowingModel().followingAcceleration(getParameters(),
1425 speed, speedInfo, leaders), "Parameter exception while obtaining the desired speed.");
1426 this.carFollowingAccelerationTime = simTime;
1427 }
1428 return this.cachedCarFollowingAcceleration;
1429 }
1430 }
1431
1432
1433 @Override
1434 public final TurnIndicatorStatus getTurnIndicatorStatus()
1435 {
1436 return this.turnIndicatorStatus.get();
1437 }
1438
1439
1440 @Override
1441 public final TurnIndicatorStatus getTurnIndicatorStatus(final Time time)
1442 {
1443 return this.turnIndicatorStatus.get(time);
1444 }
1445
1446
1447 @Override
1448 public final void setTurnIndicatorStatus(final TurnIndicatorStatus turnIndicatorStatus)
1449 {
1450 this.turnIndicatorStatus.set(turnIndicatorStatus);
1451 }
1452
1453
1454 @Override
1455 public Length getLateralPosition(final Lane lane) throws GTUException
1456 {
1457 OperationalPlan plan = getOperationalPlan();
1458 if (plan instanceof LaneBasedOperationalPlanntrafficsim/road/gtu/lane/plan/operational/LaneBasedOperationalPlan.html#LaneBasedOperationalPlan">LaneBasedOperationalPlan && !((LaneBasedOperationalPlan) plan).isDeviative())
1459 {
1460 return Length.ZERO;
1461 }
1462 DirectedLanePosition ref = getReferencePosition();
1463 int latIndex = -1;
1464 int longIndex = -1;
1465 for (int i = 0; i < this.crossSections.size(); i++)
1466 {
1467 List<Lane> lanes = this.crossSections.get(i).getLanes();
1468 if (lanes.contains(lane))
1469 {
1470 latIndex = lanes.indexOf(lane);
1471 }
1472 if (lanes.contains(ref.getLane()))
1473 {
1474 longIndex = i;
1475 }
1476 }
1477 Throw.when(latIndex == -1 || longIndex == -1, GTUException.class, "GTU %s is not on %s", getId(), lane);
1478 Lane refCrossSectionLane = this.crossSections.get(longIndex).getLanes().get(latIndex);
1479 DirectedPoint loc = getLocation();
1480 double f = refCrossSectionLane.getCenterLine().projectOrthogonal(loc.x, loc.y);
1481 DirectedPoint p = Try.assign(() -> refCrossSectionLane.getCenterLine().getLocationFraction(f), GTUException.class,
1482 "GTU %s is not orthogonal to the reference lane.", getId());
1483 double d = p.distance(loc);
1484 d = ref.getGtuDirection().isPlus() ? d : -d;
1485 if (this.crossSections.get(0).getLanes().size() > 1)
1486 {
1487 return Length.instantiateSI(latIndex == 0 ? -d : d);
1488 }
1489 double x2 = p.x + Math.cos(p.getRotZ());
1490 double y2 = p.y + Math.sin(p.getRotZ());
1491 double det = (loc.x - p.x) * (y2 - p.y) - (loc.y - p.y) * (x2 - p.x);
1492 return Length.instantiateSI(det < 0.0 ? -d : d);
1493 }
1494
1495
1496 @Override
1497 public void setInstantaneousLaneChange(final boolean instantaneous)
1498 {
1499 this.instantaneousLaneChange = instantaneous;
1500 }
1501
1502
1503 @Override
1504 public boolean isInstantaneousLaneChange()
1505 {
1506 return this.instantaneousLaneChange;
1507 }
1508
1509
1510 @Override
1511 @SuppressWarnings("checkstyle:designforextension")
1512 public String toString()
1513 {
1514 return String.format("GTU " + getId());
1515 }
1516
1517
1518 private static class CrossSection
1519 {
1520
1521
1522 private final List<Lane> lanes;
1523
1524
1525 private final GTUDirectionality direction;
1526
1527
1528
1529
1530
1531 protected CrossSection(final List<Lane> lanes, final GTUDirectionality direction)
1532 {
1533 this.lanes = lanes;
1534 this.direction = direction;
1535 }
1536
1537
1538
1539
1540 protected List<Lane> getLanes()
1541 {
1542 return this.lanes;
1543 }
1544
1545
1546
1547
1548 protected GTUDirectionality getDirection()
1549 {
1550 return this.direction;
1551 }
1552
1553 }
1554
1555 }