1 package org.opentrafficsim.road.gtu.lane;
2
3 import java.util.ArrayList;
4 import java.util.HashMap;
5 import java.util.HashSet;
6 import java.util.Iterator;
7 import java.util.LinkedHashMap;
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.LengthUnit;
16 import org.djunits.value.vdouble.scalar.Duration;
17 import org.djunits.value.vdouble.scalar.Length;
18 import org.djunits.value.vdouble.scalar.Speed;
19 import org.djunits.value.vdouble.scalar.Time;
20 import org.opentrafficsim.core.dsol.OTSDEVSSimulatorInterface;
21 import org.opentrafficsim.core.dsol.OTSSimTimeDouble;
22 import org.opentrafficsim.core.geometry.OTSGeometryException;
23 import org.opentrafficsim.core.gtu.AbstractGTU;
24 import org.opentrafficsim.core.gtu.GTUDirectionality;
25 import org.opentrafficsim.core.gtu.GTUException;
26 import org.opentrafficsim.core.gtu.GTUType;
27 import org.opentrafficsim.core.gtu.RelativePosition;
28 import org.opentrafficsim.core.gtu.behavioralcharacteristics.BehavioralCharacteristics;
29 import org.opentrafficsim.core.gtu.behavioralcharacteristics.ParameterException;
30 import org.opentrafficsim.core.gtu.plan.operational.OperationalPlanException;
31 import org.opentrafficsim.core.network.LateralDirectionality;
32 import org.opentrafficsim.core.network.Link;
33 import org.opentrafficsim.core.network.NetworkException;
34 import org.opentrafficsim.core.network.Node;
35 import org.opentrafficsim.core.network.OTSNetwork;
36 import org.opentrafficsim.road.gtu.lane.perception.categories.DefaultSimplePerception;
37 import org.opentrafficsim.road.gtu.lane.tactical.LaneBasedTacticalPlanner;
38 import org.opentrafficsim.road.gtu.strategical.LaneBasedStrategicalPlanner;
39 import org.opentrafficsim.road.gtu.strategical.route.LaneBasedStrategicalRoutePlanner;
40 import org.opentrafficsim.road.network.lane.CrossSectionElement;
41 import org.opentrafficsim.road.network.lane.CrossSectionLink;
42 import org.opentrafficsim.road.network.lane.DirectedLanePosition;
43 import org.opentrafficsim.road.network.lane.Lane;
44
45 import nl.tudelft.simulation.dsol.SimRuntimeException;
46 import nl.tudelft.simulation.dsol.formalisms.eventscheduling.SimEvent;
47 import nl.tudelft.simulation.language.Throw;
48 import nl.tudelft.simulation.language.d3.BoundingBox;
49 import nl.tudelft.simulation.language.d3.DirectedPoint;
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76 public abstract class AbstractLaneBasedGTU extends AbstractGTU implements LaneBasedGTU
77 {
78
79 private static final long serialVersionUID = 20140822L;
80
81
82
83
84
85
86 private Map<Link, Double> fractionalLinkPositions = new LinkedHashMap<>();
87
88
89
90
91
92
93
94 private final Map<Lane, GTUDirectionality> lanesCurrentOperationalPlan = new HashMap<>();
95
96
97 private Map<Lane, List<SimEvent<OTSSimTimeDouble>>> pendingTriggers = new HashMap<>();
98
99
100 private Object lock = new Object();
101
102
103 @SuppressWarnings("checkstyle:visibilitymodifier")
104 public static Length initialLocationThresholdDifference = new Length(1.0, LengthUnit.MILLIMETER);
105
106
107
108 public static boolean CACHING = true;
109
110
111
112 public static int CACHED_POSITION = 0;
113
114
115
116 public static int NON_CACHED_POSITION = 0;
117
118
119
120
121
122
123
124
125
126 public AbstractLaneBasedGTU(final String id, final GTUType gtuType, final OTSDEVSSimulatorInterface simulator,
127 final OTSNetwork network) throws GTUException
128 {
129 super(id, gtuType, simulator, network);
130 }
131
132
133
134
135
136
137
138
139
140
141 @SuppressWarnings("checkstyle:designforextension")
142 public void init(final LaneBasedStrategicalPlanner strategicalPlanner,
143 final Set<DirectedLanePosition> initialLongitudinalPositions, final Speed initialSpeed)
144 throws NetworkException, SimRuntimeException, GTUException, OTSGeometryException
145 {
146 Throw.when(null == initialLongitudinalPositions, GTUException.class, "InitialLongitudinalPositions is null");
147 Throw.when(0 == initialLongitudinalPositions.size(), GTUException.class, "InitialLongitudinalPositions is empty set");
148
149 DirectedPoint lastPoint = null;
150 for (DirectedLanePosition pos : initialLongitudinalPositions)
151 {
152 Throw.when(lastPoint != null && pos.getLocation().distance(lastPoint) > initialLocationThresholdDifference.si,
153 GTUException.class, "initial locations for GTU have distance > " + initialLocationThresholdDifference);
154 lastPoint = pos.getLocation();
155 }
156 DirectedPoint initialLocation = lastPoint;
157
158
159 for (DirectedLanePosition directedLanePosition : initialLongitudinalPositions)
160 {
161 Lane lane = directedLanePosition.getLane();
162 enterLane(lane, directedLanePosition.getPosition(), directedLanePosition.getGtuDirection());
163 }
164
165
166
167
168
169
170
171
172 super.init(strategicalPlanner, initialLocation, initialSpeed);
173
174 Lane referenceLane = getReferencePosition().getLane();
175 fireTimedEvent(LaneBasedGTU.LANEBASED_INIT_EVENT,
176 new Object[] { getId(), initialLocation, getLength(), getWidth(), getBaseColor(), referenceLane,
177 position(referenceLane, getReference()), this.lanesCurrentOperationalPlan.get(referenceLane),
178 getGTUType() },
179 getSimulator().getSimulatorTime());
180 }
181
182
183
184
185
186
187 public final boolean isSafeToChange() throws GTUException
188 {
189 return this.fractionalLinkPositions.get(getReferencePosition().getLane().getParentLink()) > 0.0;
190 }
191
192
193 @Override
194 @SuppressWarnings("checkstyle:designforextension")
195 public void enterLane(final Lane lane, final Length position, final GTUDirectionality gtuDirection) throws GTUException
196 {
197 if (lane == null || gtuDirection == null || position == null)
198 {
199 throw new GTUException("enterLane - one of the arguments is null");
200 }
201 if (this.lanesCurrentOperationalPlan.containsKey(lane))
202 {
203 System.err.println(this + " is already registered on lane: " + lane + " at fractional position "
204 + this.fractionalPosition(lane, RelativePosition.REFERENCE_POSITION) + " intended position is " + position
205 + " length of lane is " + lane.getLength());
206 return;
207 }
208
209
210
211 if (!this.fractionalLinkPositions.containsKey(lane.getParentLink()))
212 {
213 this.fractionalLinkPositions.put(lane.getParentLink(), lane.fraction(position));
214 }
215 this.lanesCurrentOperationalPlan.put(lane, gtuDirection);
216 lane.addGTU(this, position);
217 }
218
219
220 @Override
221 @SuppressWarnings("checkstyle:designforextension")
222 public void leaveLane(final Lane lane) throws GTUException
223 {
224 leaveLane(lane, false);
225 }
226
227
228
229
230
231
232
233 @SuppressWarnings("checkstyle:designforextension")
234 public void leaveLane(final Lane lane, final boolean beingDestroyed) throws GTUException
235 {
236 Length position = position(lane, getReference());
237 this.lanesCurrentOperationalPlan.remove(lane);
238 List<SimEvent<OTSSimTimeDouble>> pending = this.pendingTriggers.get(lane);
239 if (null != pending)
240 {
241 for (SimEvent<OTSSimTimeDouble> event : pending)
242 {
243 if (event.getAbsoluteExecutionTime().get().ge(getSimulator().getSimulatorTime().get()))
244 {
245 boolean result = getSimulator().cancelEvent(event);
246 if (!result && event.getAbsoluteExecutionTime().get().ne(getSimulator().getSimulatorTime().get()))
247 {
248 System.err.println("leaveLane, trying to remove event: NOTHING REMOVED -- result=" + result
249 + ", simTime=" + getSimulator().getSimulatorTime().get() + ", eventTime="
250 + event.getAbsoluteExecutionTime().get());
251 }
252 }
253 }
254 this.pendingTriggers.remove(lane);
255 }
256
257 boolean found = false;
258 for (Lane l : this.lanesCurrentOperationalPlan.keySet())
259 {
260 if (l.getParentLink().equals(lane.getParentLink()))
261 {
262 found = true;
263 }
264 }
265 if (!found)
266 {
267 this.fractionalLinkPositions.remove(lane.getParentLink());
268 }
269 lane.removeGTU(this, !found, position);
270 if (this.lanesCurrentOperationalPlan.size() == 0 && !beingDestroyed)
271 {
272 System.err.println("leaveLane: lanes.size() = 0 for GTU " + getId());
273 }
274 }
275
276
277 @Override
278 @SuppressWarnings("checkstyle:designforextension")
279 public void changeLaneInstantaneously(final LateralDirectionality laneChangeDirection) throws GTUException
280 {
281
282
283 Map<Lane, GTUDirectionality> lanesCopy = new HashMap<>(this.lanesCurrentOperationalPlan);
284 Set<Lane> lanesToBeRemoved = new HashSet<>();
285 Map<Lane, Double> fractionalLanePositions = new HashMap<>();
286
287 for (Lane lane : lanesCopy.keySet())
288 {
289 fractionalLanePositions.put(lane, fractionalPosition(lane, getReference()));
290 lanesToBeRemoved.add(lane);
291 }
292
293
294
295
296 Map<Link, Double> newLinkPositionsLC = new HashMap<>(this.fractionalLinkPositions);
297
298 for (Lane lane : lanesCopy.keySet())
299 {
300 Set<Lane> laneSet = lane.accessibleAdjacentLanes(laneChangeDirection, getGTUType());
301 if (laneSet.size() > 0)
302 {
303 Lane adjacentLane = laneSet.iterator().next();
304 try
305 {
306 Length pos = adjacentLane.position(lane.fraction(position(lane, getReference())));
307 Length adjustedStart =
308 pos.minus(getOperationalPlan().getTraveledDistance(getSimulator().getSimulatorTime().getTime()));
309 newLinkPositionsLC.put(lane.getParentLink(), adjustedStart.si / adjacentLane.getLength().si);
310 }
311 catch (OperationalPlanException exception)
312 {
313 exception.printStackTrace();
314 }
315 enterLane(adjacentLane, adjacentLane.getLength().multiplyBy(fractionalLanePositions.get(lane)),
316 lanesCopy.get(lane));
317 lanesToBeRemoved.remove(adjacentLane);
318 }
319
320 }
321
322
323 this.fractionalLinkPositions = newLinkPositionsLC;
324
325 for (Lane lane : lanesToBeRemoved)
326 {
327 leaveLane(lane);
328 }
329
330 }
331
332
333
334
335
336
337 @SuppressWarnings("checkstyle:designforextension")
338 public void initLaneChange(final LateralDirectionality laneChangeDirection) throws GTUException
339 {
340 Map<Lane, GTUDirectionality> lanesCopy = new HashMap<>(this.lanesCurrentOperationalPlan);
341 Map<Lane, Double> fractionalLanePositions = new HashMap<>();
342 for (Lane lane : lanesCopy.keySet())
343 {
344 fractionalLanePositions.put(lane, fractionalPosition(lane, getReference()));
345 }
346 int numRegistered = 0;
347 for (Lane lane : lanesCopy.keySet())
348 {
349 Set<Lane> laneSet = lane.accessibleAdjacentLanes(laneChangeDirection, getGTUType());
350 if (laneSet.size() > 0)
351 {
352 numRegistered++;
353 Lane adjacentLane = laneSet.iterator().next();
354 enterLane(adjacentLane, adjacentLane.getLength().multiplyBy(fractionalLanePositions.get(lane)),
355 lanesCopy.get(lane));
356 }
357 }
358 Throw.when(numRegistered == 0, GTUException.class, "Gtu %s starting %s lane change, but no adjacent lane found.",
359 getId(), laneChangeDirection);
360 }
361
362
363
364
365
366
367
368 @SuppressWarnings("checkstyle:designforextension")
369 public void finalizeLaneChange(final LateralDirectionality laneChangeDirection, final Time time) throws GTUException
370 {
371 try
372 {
373 getSimulator().scheduleEventAbs(time, this, this, "executeLaneChangeFinalization",
374 new Object[] { laneChangeDirection });
375 }
376 catch (SimRuntimeException exception)
377 {
378 throw new RuntimeException("Error when finalizing lane change.", exception);
379 }
380 }
381
382
383
384
385
386 @SuppressWarnings("checkstyle:designforextension")
387 protected void executeLaneChangeFinalization(final LateralDirectionality laneChangeDirection)
388 {
389 Map<Lane, GTUDirectionality> lanesCopy = new HashMap<>(this.lanesCurrentOperationalPlan);
390 Set<Lane> lanesToBeRemoved = new HashSet<>();
391 Map<Lane, Double> fractionalLanePositions = new HashMap<>();
392 try
393 {
394 for (Lane lane : lanesCopy.keySet())
395 {
396
397 fractionalLanePositions.put(lane, fractionalPosition(lane, getReference()));
398 }
399 for (Lane lane : lanesCopy.keySet())
400 {
401 Iterator<Lane> iterator = lane.accessibleAdjacentLanes(laneChangeDirection, getGTUType()).iterator();
402 if (iterator.hasNext() && lanesCopy.keySet().contains(iterator.next()))
403 {
404 lanesToBeRemoved.add(lane);
405 }
406 }
407 for (Lane lane : lanesToBeRemoved)
408 {
409 leaveLane(lane);
410 }
411 }
412 catch (GTUException exception)
413 {
414
415 throw new RuntimeException("fractionalPosition on lane not possible", exception);
416 }
417 }
418
419
420 @Override
421 public final GTUDirectionality getDirection(final Lane lane) throws GTUException
422 {
423 Throw.when(!this.lanesCurrentOperationalPlan.containsKey(lane), GTUException.class,
424 "getDirection: Lanes %s does not contain %s", this.lanesCurrentOperationalPlan.keySet(), lane);
425 return this.lanesCurrentOperationalPlan.get(lane);
426 }
427
428
429 @Override
430 @SuppressWarnings("checkstyle:designforextension")
431 protected void move(final DirectedPoint fromLocation)
432 throws SimRuntimeException, GTUException, OperationalPlanException, NetworkException, ParameterException
433 {
434
435
436
437 if (this.lanesCurrentOperationalPlan.isEmpty())
438 {
439 destroy();
440 return;
441 }
442
443
444 Map<Link, Double> newLinkPositions = new HashMap<>();
445 for (Lane lane : this.lanesCurrentOperationalPlan.keySet())
446 {
447 newLinkPositions.put(lane.getParentLink(), lane.fraction(position(lane, getReference())));
448 }
449
450
451 super.move(fromLocation);
452
453
454 this.fractionalLinkPositions = newLinkPositions;
455
456 DirectedLanePosition dlp = getReferencePosition();
457 fireTimedEvent(
458 LaneBasedGTU.LANEBASED_MOVE_EVENT, new Object[] { getId(), fromLocation, getSpeed(), getAcceleration(),
459 getTurnIndicatorStatus(), getOdometer(), dlp.getLane(), dlp.getPosition(), dlp.getGtuDirection() },
460 getSimulator().getSimulatorTime());
461
462 if (getOperationalPlan().getAcceleration(Duration.ZERO).si < -10
463 && getOperationalPlan().getSpeed(Duration.ZERO).si > 2.5)
464 {
465 System.err.println("GTU: " + getId() + " - getOperationalPlan().getAcceleration(Duration.ZERO).si < -10)");
466 System.err.println("Lanes in current plan: " + this.lanesCurrentOperationalPlan.keySet());
467 if (getTacticalPlanner().getPerception().contains(DefaultSimplePerception.class))
468 {
469 DefaultSimplePerception p =
470 getTacticalPlanner().getPerception().getPerceptionCategory(DefaultSimplePerception.class);
471 System.err.println("HeadwayGTU: " + p.getForwardHeadwayGTU());
472 System.err.println("HeadwayObject: " + p.getForwardHeadwayObject());
473 }
474 }
475
476
477 scheduleEnterLeaveTriggers();
478 }
479
480
481 @Override
482 public final Map<Lane, Length> positions(final RelativePosition relativePosition) throws GTUException
483 {
484 return positions(relativePosition, getSimulator().getSimulatorTime().getTime());
485 }
486
487
488 @Override
489 public final Map<Lane, Length> positions(final RelativePosition relativePosition, final Time when) throws GTUException
490 {
491 Map<Lane, Length> positions = new LinkedHashMap<>();
492 for (Lane lane : this.lanesCurrentOperationalPlan.keySet())
493 {
494 positions.put(lane, position(lane, relativePosition, when));
495 }
496 return positions;
497 }
498
499
500 @Override
501 public final Length position(final Lane lane, final RelativePosition relativePosition) throws GTUException
502 {
503 return position(lane, relativePosition, getSimulator().getSimulatorTime().getTime());
504 }
505
506
507 @Override
508 @SuppressWarnings("checkstyle:designforextension")
509 public Length translatedPosition(final Lane projectionLane, final RelativePosition relativePosition, final Time when)
510 throws GTUException
511 {
512 CrossSectionLink link = projectionLane.getParentLink();
513 for (CrossSectionElement cse : link.getCrossSectionElementList())
514 {
515 if (cse instanceof Lane)
516 {
517 Lane cseLane = (Lane) cse;
518 if (null != this.lanesCurrentOperationalPlan.get(cseLane))
519 {
520 double fractionalPosition = fractionalPosition(cseLane, RelativePosition.REFERENCE_POSITION, when);
521 Length pos = new Length(projectionLane.getLength().getSI() * fractionalPosition, LengthUnit.SI);
522 if (this.lanesCurrentOperationalPlan.get(cseLane).isPlus())
523 {
524 return pos.plus(relativePosition.getDx());
525 }
526 return pos.minus(relativePosition.getDx());
527 }
528 }
529 }
530 throw new GTUException(this + " is not on any lane of Link " + link);
531 }
532
533
534 @Override
535 @SuppressWarnings("checkstyle:designforextension")
536 public Length projectedPosition(final Lane projectionLane, final RelativePosition relativePosition, final Time when)
537 throws GTUException
538 {
539 CrossSectionLink link = projectionLane.getParentLink();
540 for (CrossSectionElement cse : link.getCrossSectionElementList())
541 {
542 if (cse instanceof Lane)
543 {
544 Lane cseLane = (Lane) cse;
545 if (null != this.lanesCurrentOperationalPlan.get(cseLane))
546 {
547 double fractionalPosition = fractionalPosition(cseLane, relativePosition, when);
548 return new Length(projectionLane.getLength().getSI() * fractionalPosition, LengthUnit.SI);
549 }
550 }
551 }
552 throw new GTUException(this + " is not on any lane of Link " + link);
553 }
554
555
556 private double cachePositionsTime = Double.NaN;
557
558
559 private Map<Integer, Length> cachedPositions = new HashMap<>();
560
561
562 @Override
563 @SuppressWarnings("checkstyle:designforextension")
564 public Length position(final Lane lane, final RelativePosition relativePosition, final Time when) throws GTUException
565 {
566 if (null == lane)
567 {
568 throw new GTUException("lane is null");
569 }
570
571 int cacheIndex = 0;
572 if (CACHING)
573 {
574 cacheIndex = 17 * lane.hashCode() + relativePosition.hashCode();
575 if (when.si == this.cachePositionsTime && this.cachedPositions.containsKey(cacheIndex))
576 {
577 CACHED_POSITION++;
578 return this.cachedPositions.get(cacheIndex);
579 }
580 if (when.si != this.cachePositionsTime)
581 {
582 this.cachedPositions.clear();
583 this.cachePositionsTime = when.si;
584 }
585 }
586 NON_CACHED_POSITION++;
587
588 synchronized (this.lock)
589 {
590 if (!this.lanesCurrentOperationalPlan.containsKey(lane))
591 {
592 throw new GTUException("position() : GTU " + toString() + " is not on lane " + lane);
593 }
594 if (!this.fractionalLinkPositions.containsKey(lane.getParentLink()))
595 {
596
597 throw new GTUException(this + " does not have a fractional position on " + lane.toString());
598 }
599 double longitudinalPosition = lane.positionSI(this.fractionalLinkPositions.get(lane.getParentLink()));
600 if (getOperationalPlan() == null)
601 {
602
603 return new Length(longitudinalPosition + relativePosition.getDx().si, LengthUnit.SI);
604 }
605 double loc;
606 try
607 {
608 if (this.lanesCurrentOperationalPlan.get(lane).isPlus())
609 {
610 loc = longitudinalPosition + getOperationalPlan().getTraveledDistanceSI(when) + relativePosition.getDx().si;
611 }
612 else
613 {
614 loc = longitudinalPosition - getOperationalPlan().getTraveledDistanceSI(when) - relativePosition.getDx().si;
615 }
616 }
617 catch (Exception e)
618 {
619 System.err.println(toString());
620 System.err.println(this.lanesCurrentOperationalPlan);
621 System.err.println(this.fractionalLinkPositions);
622 throw new GTUException(e);
623 }
624 if (Double.isNaN(loc))
625 {
626 System.out.println("loc is NaN");
627 }
628 Length length = new Length(loc, LengthUnit.SI);
629 if (CACHING)
630 {
631 this.cachedPositions.put(cacheIndex, length);
632 }
633 return length;
634 }
635 }
636
637
638 @Override
639 @SuppressWarnings("checkstyle:designforextension")
640 public DirectedLanePosition getReferencePosition() throws GTUException
641 {
642 for (Lane lane : this.lanesCurrentOperationalPlan.keySet())
643 {
644 double fraction = fractionalPosition(lane, getReference());
645 if (fraction >= 0.0 && fraction <= 1.0)
646 {
647
648
649 return new DirectedLanePosition(lane, position(lane, getReference()), this.getDirection(lane));
650 }
651 }
652 throw new GTUException("The reference point of GTU " + this + " is not on any of the lanes on which it is registered");
653 }
654
655
656
657
658
659
660
661
662
663 @SuppressWarnings("checkstyle:designforextension")
664 protected void scheduleEnterLeaveTriggers() throws NetworkException, SimRuntimeException, GTUException
665 {
666
667
668
669
670
671 double moveSI = getOperationalPlan().getTotalLength().si;
672 Map<Lane, GTUDirectionality> lanesCopy = new LinkedHashMap<>(this.lanesCurrentOperationalPlan);
673 for (Lane lane : lanesCopy.keySet())
674 {
675
676 double referenceStartSI = this.fractionalLinkPositions.get(lane.getParentLink()) * lane.getLength().getSI();
677 double sign = lanesCopy.get(lane).equals(GTUDirectionality.DIR_PLUS) ? 1.0 : -1.0;
678 if (lanesCopy.get(lane).equals(GTUDirectionality.DIR_PLUS))
679 {
680 lane.scheduleSensorTriggers(this, referenceStartSI, moveSI);
681 }
682 else
683 {
684
685 lane.scheduleSensorTriggers(this, referenceStartSI - moveSI, moveSI);
686 }
687
688
689
690
691 double frontPosSI = referenceStartSI + sign * getFront().getDx().getSI();
692 double nextFrontPosSI = frontPosSI + sign * moveSI;
693
694
695 if (lanesCopy.get(lane).equals(GTUDirectionality.DIR_PLUS))
696 {
697 while (frontPosSI <= lane.getLength().si && nextFrontPosSI > lane.getLength().si)
698 {
699
700 Lane nextLane = determineNextLane(lane);
701 GTUDirectionality direction = lane.nextLanes(getGTUType()).get(nextLane);
702
703
704
705
706
707
708 if (direction.equals(GTUDirectionality.DIR_PLUS))
709 {
710 Length refPosAtLastTimestep =
711 new Length(-(lane.getLength().si - frontPosSI) - getFront().getDx().si, LengthUnit.SI);
712 enterLane(nextLane, refPosAtLastTimestep, direction);
713
714 nextLane.scheduleSensorTriggers(this, refPosAtLastTimestep.getSI(), moveSI);
715 }
716 else if (direction.equals(GTUDirectionality.DIR_MINUS))
717 {
718 Length refPosAtLastTimestep =
719 new Length(nextLane.getLength().si + (lane.getLength().si - frontPosSI) + getFront().getDx().si,
720 LengthUnit.SI);
721 enterLane(nextLane, refPosAtLastTimestep, direction);
722
723
724 nextLane.scheduleSensorTriggers(this, refPosAtLastTimestep.getSI() - moveSI, moveSI);
725 }
726 else
727 {
728 throw new NetworkException("scheduleTriggers DIR_PLUS for GTU " + toString() + ", nextLane " + nextLane
729 + ", direction not DIR_PLUS or DIR_MINUS");
730 }
731
732
733
734 frontPosSI -= lane.getLength().si;
735 nextFrontPosSI -= lane.getLength().si;
736 lane = nextLane;
737 }
738 }
739
740
741 else if (lanesCopy.get(lane).equals(GTUDirectionality.DIR_MINUS))
742 {
743 if (frontPosSI >= 0.0 && nextFrontPosSI < 0.0)
744 {
745 Lane prevLane = determinePrevLane(lane);
746 GTUDirectionality direction = lane.prevLanes(getGTUType()).get(prevLane);
747
748
749
750
751
752
753 if (direction.equals(GTUDirectionality.DIR_MINUS))
754 {
755
756 Length refPosAtLastTimestep =
757 new Length(prevLane.getLength().si + frontPosSI + getFront().getDx().si, LengthUnit.SI);
758 enterLane(prevLane, refPosAtLastTimestep, direction);
759
760 prevLane.scheduleSensorTriggers(this, refPosAtLastTimestep.getSI() - moveSI, moveSI);
761 }
762 else if (direction.equals(GTUDirectionality.DIR_PLUS))
763 {
764 Length refPosAtLastTimestep = new Length(-frontPosSI - getFront().getDx().si, LengthUnit.SI);
765 enterLane(prevLane, refPosAtLastTimestep, direction);
766
767
768 prevLane.scheduleSensorTriggers(this, refPosAtLastTimestep.getSI(), moveSI);
769 }
770 else
771 {
772 throw new NetworkException("scheduleTriggers DIR_MINUS for GTU " + toString() + ", prevLane " + prevLane
773 + ", direction not DIR_PLUS or DIR_MINUS");
774 }
775 }
776 }
777
778 else
779 {
780 throw new NetworkException(
781 "scheduleTriggers for GTU " + toString() + ", lane " + lane + ", direction not DIR_PLUS or DIR_MINUS");
782 }
783 }
784
785
786 for (Lane lane : this.lanesCurrentOperationalPlan.keySet())
787 {
788
789
790 double referenceStartSI = this.fractionalLinkPositions.get(lane.getParentLink()) * lane.getLength().getSI();
791 double sign = this.lanesCurrentOperationalPlan.get(lane).equals(GTUDirectionality.DIR_PLUS) ? 1.0 : -1.0;
792 double rearPosSI = referenceStartSI + sign * getRear().getDx().getSI();
793
794 if (this.lanesCurrentOperationalPlan.get(lane).equals(GTUDirectionality.DIR_PLUS))
795 {
796 if (rearPosSI <= lane.getLength().si && rearPosSI + moveSI > lane.getLength().si)
797 {
798 double distanceToLeave = lane.getLength().si - rearPosSI;
799 Time exitTime = getOperationalPlan().timeAtDistance(new Length(distanceToLeave, LengthUnit.SI));
800 SimEvent<OTSSimTimeDouble> event = new SimEvent<>(new OTSSimTimeDouble(exitTime), this, this, "leaveLane",
801 new Object[] { lane, new Boolean(false) });
802 getSimulator().scheduleEvent(event);
803 addTrigger(lane, event);
804
805
806
807
808 }
809 }
810 else
811 {
812 if (rearPosSI >= 0.0 && rearPosSI - moveSI < 0.0)
813 {
814 double distanceToLeave = rearPosSI;
815 Time exitTime = getOperationalPlan().timeAtDistance(new Length(distanceToLeave, LengthUnit.SI));
816 SimEvent<OTSSimTimeDouble> event = new SimEvent<>(new OTSSimTimeDouble(exitTime), this, this, "leaveLane",
817 new Object[] { lane, new Boolean(false) });
818 getSimulator().scheduleEvent(event);
819 addTrigger(lane, event);
820
821
822
823 }
824 }
825 }
826 }
827
828
829
830
831
832
833
834
835 @SuppressWarnings("checkstyle:designforextension")
836 protected Lane determineNextLane(final Lane lane) throws NetworkException, GTUException
837 {
838 Lane nextLane = null;
839 if (lane.nextLanes(getGTUType()).size() == 0)
840 {
841 throw new NetworkException(this + " - lane " + lane + " does not have a successor");
842 }
843 if (lane.nextLanes(getGTUType()).size() == 1)
844 {
845 nextLane = lane.nextLanes(getGTUType()).keySet().iterator().next();
846 }
847 else
848 {
849 if (!(getStrategicalPlanner() instanceof LaneBasedStrategicalRoutePlanner))
850 {
851 throw new GTUException(this + " reaches branch but has no route navigator ");
852 }
853 Node nextNode = ((LaneBasedStrategicalRoutePlanner) getStrategicalPlanner()).nextNode(lane.getParentLink(),
854 GTUDirectionality.DIR_PLUS, getGTUType());
855 if (null == nextNode)
856 {
857 throw new GTUException(this + " reaches branch and the route returns null as nextNodeToVisit");
858 }
859 int continuingLaneCount = 0;
860 for (Lane candidateLane : lane.nextLanes(getGTUType()).keySet())
861 {
862 if (null != this.lanesCurrentOperationalPlan.get(candidateLane))
863 {
864 continue;
865 }
866
867 if (nextNode == candidateLane.getParentLink().getEndNode()
868 || nextNode == candidateLane.getParentLink().getStartNode())
869 {
870 nextLane = candidateLane;
871 continuingLaneCount++;
872 }
873 }
874 if (continuingLaneCount == 0)
875 {
876 throw new NetworkException(this + " reached branch and the route specifies a nextNodeToVisit (" + nextNode
877 + ") that is not a next node " + "at this branch at (" + lane.getParentLink().getEndNode() + ")");
878 }
879 if (continuingLaneCount > 1)
880 {
881 throw new NetworkException(
882 this + " reached branch and the route specifies multiple lanes to continue on at this branch ("
883 + lane.getParentLink().getEndNode() + "). This is not yet supported");
884 }
885 }
886 return nextLane;
887 }
888
889
890
891
892
893
894
895
896 @SuppressWarnings("checkstyle:designforextension")
897 protected Lane determinePrevLane(final Lane lane) throws NetworkException, GTUException
898 {
899 Lane prevLane = null;
900 if (lane.prevLanes(getGTUType()).size() == 0)
901 {
902 throw new NetworkException(this + " - lane " + lane + " does not have a predecessor");
903 }
904 if (lane.prevLanes(getGTUType()).size() == 1)
905 {
906 prevLane = lane.prevLanes(getGTUType()).keySet().iterator().next();
907 }
908 else
909 {
910 if (!(getStrategicalPlanner() instanceof LaneBasedStrategicalRoutePlanner))
911 {
912 throw new GTUException(this + " reaches branch but has no route navigator");
913 }
914 Node prevNode = ((LaneBasedStrategicalRoutePlanner) getStrategicalPlanner()).nextNode(lane.getParentLink(),
915 GTUDirectionality.DIR_MINUS, getGTUType());
916 if (null == prevNode)
917 {
918 throw new GTUException(this + " reaches branch and the route returns null as nextNodeToVisit");
919 }
920 int continuingLaneCount = 0;
921 for (Lane candidateLane : lane.prevLanes(getGTUType()).keySet())
922 {
923 if (null != this.lanesCurrentOperationalPlan.get(candidateLane))
924 {
925 continue;
926 }
927
928 if (prevNode == candidateLane.getParentLink().getEndNode()
929 || prevNode == candidateLane.getParentLink().getStartNode())
930 {
931 prevLane = candidateLane;
932 continuingLaneCount++;
933 }
934 }
935 if (continuingLaneCount == 0)
936 {
937 throw new NetworkException(this + " reached branch and the route specifies a nextNodeToVisit (" + prevNode
938 + ") that is not a next node " + "at this branch at (" + lane.getParentLink().getStartNode() + ")");
939 }
940 if (continuingLaneCount > 1)
941 {
942 throw new NetworkException(
943 this + " reached branch and the route specifies multiple lanes to continue on at this branch ("
944 + lane.getParentLink().getStartNode() + "). This is not yet supported");
945 }
946 }
947 return prevLane;
948 }
949
950
951 @Override
952 public final Map<Lane, Double> fractionalPositions(final RelativePosition relativePosition) throws GTUException
953 {
954 return fractionalPositions(relativePosition, getSimulator().getSimulatorTime().getTime());
955 }
956
957
958 @Override
959 public final Map<Lane, Double> fractionalPositions(final RelativePosition relativePosition, final Time when)
960 throws GTUException
961 {
962 Map<Lane, Double> positions = new LinkedHashMap<>();
963 for (Lane lane : this.lanesCurrentOperationalPlan.keySet())
964 {
965 positions.put(lane, fractionalPosition(lane, relativePosition, when));
966 }
967 return positions;
968 }
969
970
971 @Override
972 public final double fractionalPosition(final Lane lane, final RelativePosition relativePosition, final Time when)
973 throws GTUException
974 {
975 return position(lane, relativePosition, when).getSI() / lane.getLength().getSI();
976 }
977
978
979 @Override
980 public final double fractionalPosition(final Lane lane, final RelativePosition relativePosition) throws GTUException
981 {
982 return position(lane, relativePosition).getSI() / lane.getLength().getSI();
983 }
984
985
986 @Override
987 public final LaneBasedStrategicalPlanner getStrategicalPlanner()
988 {
989 return (LaneBasedStrategicalPlanner) super.getStrategicalPlanner();
990 }
991
992
993 @Override
994 public final BehavioralCharacteristics getBehavioralCharacteristics()
995 {
996 return getStrategicalPlanner().getBehavioralCharacteristics();
997 }
998
999
1000 @Override
1001 public final LaneBasedTacticalPlanner getTacticalPlanner()
1002 {
1003 return (LaneBasedTacticalPlanner) super.getTacticalPlanner();
1004 }
1005
1006
1007 @Override
1008 public final void addTrigger(final Lane lane, final SimEvent<OTSSimTimeDouble> event)
1009 {
1010 List<SimEvent<OTSSimTimeDouble>> list = this.pendingTriggers.get(lane);
1011 if (null == list)
1012 {
1013 list = new ArrayList<>();
1014 }
1015 list.add(event);
1016 this.pendingTriggers.put(lane, list);
1017 }
1018
1019
1020 @Override
1021 @SuppressWarnings("checkstyle:designforextension")
1022 public void destroy()
1023 {
1024 DirectedLanePosition dlp = null;
1025 try
1026 {
1027 dlp = getReferencePosition();
1028 }
1029 catch (GTUException e)
1030 {
1031
1032 }
1033 DirectedPoint location = this.getOperationalPlan() == null ? new DirectedPoint(0.0, 0.0, 0.0) : getLocation();
1034
1035 synchronized (this.lock)
1036 {
1037 Set<Lane> laneSet = new HashSet<>(this.lanesCurrentOperationalPlan.keySet());
1038 for (Lane lane : laneSet)
1039 {
1040 try
1041 {
1042 leaveLane(lane, true);
1043 }
1044 catch (GTUException e)
1045 {
1046
1047 }
1048 }
1049 }
1050
1051 if (dlp != null && dlp.getLane() != null)
1052 {
1053 Lane referenceLane = dlp.getLane();
1054 fireTimedEvent(LaneBasedGTU.LANEBASED_DESTROY_EVENT,
1055 new Object[] { getId(), location, getOdometer(), referenceLane, dlp.getPosition(), dlp.getGtuDirection() },
1056 getSimulator().getSimulatorTime());
1057 }
1058 else
1059 {
1060 fireTimedEvent(LaneBasedGTU.LANEBASED_DESTROY_EVENT,
1061 new Object[] { getId(), location, getOdometer(), null, Length.ZERO, null },
1062 getSimulator().getSimulatorTime());
1063 }
1064
1065 super.destroy();
1066 }
1067
1068
1069 @Override
1070 public final Bounds getBounds()
1071 {
1072 double dx = 0.5 * getLength().doubleValue();
1073 double dy = 0.5 * getWidth().doubleValue();
1074 return new BoundingBox(new Point3d(-dx, -dy, 0.0), new Point3d(dx, dy, 0.0));
1075 }
1076
1077
1078 @Override
1079 @SuppressWarnings("checkstyle:designforextension")
1080 public String toString()
1081 {
1082 return String.format("GTU " + getId());
1083 }
1084 }