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