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