package org.opentrafficsim.road.gtu.lane.perception; import java.io.Serializable; import java.rmi.RemoteException; import java.util.Iterator; import java.util.LinkedHashMap; import java.util.LinkedHashSet; import java.util.List; import java.util.Map; import java.util.Set; import java.util.SortedSet; import java.util.TreeMap; import java.util.TreeSet; import org.djunits.value.vdouble.scalar.Length; import org.djunits.value.vdouble.scalar.Time; import org.djutils.event.EventInterface; import org.djutils.event.EventListenerInterface; import org.djutils.exceptions.Throw; import org.djutils.exceptions.Try; import org.djutils.immutablecollections.ImmutableMap; import org.opentrafficsim.core.gtu.GTUDirectionality; import org.opentrafficsim.core.gtu.GTUException; import org.opentrafficsim.core.gtu.GTUType; import org.opentrafficsim.core.gtu.RelativePosition; import org.opentrafficsim.core.network.LateralDirectionality; import org.opentrafficsim.core.network.Link; import org.opentrafficsim.core.network.Node; import org.opentrafficsim.core.network.route.Route; import org.opentrafficsim.core.perception.Historical; import org.opentrafficsim.core.perception.HistoricalValue; import org.opentrafficsim.core.perception.HistoryManager; import org.opentrafficsim.road.gtu.lane.LaneBasedGTU; import org.opentrafficsim.road.gtu.lane.perception.RollingLaneStructureRecord.RecordLink; import org.opentrafficsim.road.gtu.lane.plan.operational.LaneBasedOperationalPlan; import org.opentrafficsim.road.network.lane.CrossSectionLink; import org.opentrafficsim.road.network.lane.DirectedLanePosition; import org.opentrafficsim.road.network.lane.Lane; import org.opentrafficsim.road.network.lane.LaneDirection; import org.opentrafficsim.road.network.lane.object.LaneBasedObject; /** * This data structure can clearly indicate the lane structure ahead of us, e.g. in the following situation: * *
 *     (---- a ----)(---- b ----)(---- c ----)(---- d ----)(---- e ----)(---- f ----)(---- g ----)  
 *                                             __________                             __________
 *                                            / _________ 1                          / _________ 2
 *                                           / /                                    / /
 *                                __________/ /             _______________________/ /
 *  1  ____________ ____________ /_ _ _ _ _ _/____________ /_ _ _ _ _ _ _ _ _ _ _ _ /      
 *  0 |_ _X_ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ \____________
 * -1 |____________|_ _ _ _ _ _ |____________|____________|  __________|____________|____________| 3
 * -2              / __________/                           \ \  
 *        ________/ /                                       \ \___________  
 *      5 _________/                                         \____________  4
 * 
* * When the GTU is looking ahead, it needs to know that when it continues to destination 3, it needs to shift one lane to the * right at some point, but not two lanes to the right in link b, and not later than at the end of link f. When it needs * to go to destination 1, it needs to shift to the left in link c. When it has to go to destination 2, it has to shift to the * left, but not earlier than at link e. At node [de], it is possible to leave the rightmost lane of link e, and go to * destination 4. The rightmost lane just splits into two lanes at the end of link d, and the GTU can either continue driving to * destination 3, turn right to destination 4. This means that the right lane of link d has two successor lanes. *

* In the data structures, lanes are numbered laterally. Suppose that the lane where vehicle X resides would be number 0. * Consistent with "left is positive" for angles, the lane right of X would have number -1, and entry 5 would have number -2. *

* In the data structure, this can be indicated as follows (N = next, P = previous, L = left, R = right, D = lane drop, . = * continued but not in this structure). The merge lane in b is considered "off limits" for the GTUs on the "main" lane -1; the * "main" lane 0 is considered off limits from the exit lanes on c, e, and f. Still, we need to maintain pointers to these * lanes, as we are interested in the GTUs potentially driving next to us, feeding into our lane, etc. * *

 *       1                0               -1               -2
 *       
 *                       ROOT 
 *                   _____|_____      ___________      ___________            
 *                  |_-_|_._|_R_|----|_L_|_._|_-_|    |_-_|_._|_-_|  a           
 *                        |                |                |
 *                   _____V_____      _____V_____      _____V_____            
 *                  |_-_|_N_|_R_|----|_L_|_N_|_R_|<---|_L_|_D_|_-_|  b           
 *                        |                |                 
 *  ___________      _____V_____      _____V_____                 
 * |_-_|_N_|_R_|<---|_L_|_N_|_R_|----|_L_|_N_|_-_|                   c
 *       |                |                |                 
 *  _____V_____      _____V_____      _____V_____                 
 * |_-_|_._|_-_|    |_-_|_N_|_R_|----|_L_|_NN|_-_|                   d          
 *                        |                ||_______________ 
 *  ___________      _____V_____      _____V_____      _____V_____            
 * |_-_|_N_|_R_|<---|_L_|_N_|_R_|----|_L_|_N_|_-_|    |_-_|_N_|_-_|  e          
 *       |                |                |                |
 *  _____V_____      _____V_____      _____V_____      _____V_____            
 * |_-_|_N_|_R_|<---|_L_|_D_|_R_|----|_L_|_N_|_-_|    |_-_|_._|_-_|  f          
 *       |                                 |                 
 *  _____V_____                       _____V_____                             
 * |_-_|_._|_-_|                     |_-_|_._|_-_|                   g
 * 
*

* Copyright (c) 2013-2022 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved.
* BSD-style license. See OpenTrafficSim License. *

* $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $, * initial version Feb 20, 2016
* @author Alexander Verbraeck */ public class RollingLaneStructure implements LaneStructure, Serializable, EventListenerInterface { /** */ private static final long serialVersionUID = 20160400L; /** The lanes from which we observe the situation. */ private final Historical root; /** Look ahead distance. */ private Length lookAhead; /** Route the structure is based on. */ private Route previousRoute; /** Whether the previous plan was deviative. */ private boolean previouslyDeviative = false; /** Lane structure records of the cross section. */ private TreeMap crossSectionRecords = new TreeMap<>(); /** First lane structure records. */ private TreeMap firstRecords = new TreeMap<>(); /** Lane structure records grouped per relative lane. */ private Map> relativeLaneMap = new LinkedHashMap<>(); /** Relative lanes storage per record, such that other records can be linked to the correct relative lane. */ private Map relativeLanes = new LinkedHashMap<>(); /** Set of lanes that can be ignored as they are beyond build bounds. */ private final Set ignoreSet = new LinkedHashSet<>(); /** Upstream edges. */ private final Set upstreamEdge = new LinkedHashSet<>(); /** Downstream edges. */ private final Set downstreamEdge = new LinkedHashSet<>(); /** Downstream distance over which the structure is made. */ private final Length down; /** Upstream distance over which the structure is made. */ private final Length up; /** Downstream distance at splits (links not on route) included in the structure. */ private final Length downSplit; /** Upstream distance at downstream merges (links not on route) included in the structure. */ private final Length upMerge; /** GTU. */ private final LaneBasedGTU containingGtu; /** the animation access. */ @SuppressWarnings("checkstyle:visibilitymodifier") public AnimationAccess animationAccess = new AnimationAccess(); /** * Constructor. * @param lookAhead Length; distance over which visual objects are included * @param down Length; downstream distance over which the structure is made * @param up Length; upstream distance over which the structure is made, should include a margin for reaction time * @param downSplit Length; downstream distance at splits (links not on route) included in the structure * @param upMerge Length; upstream distance at downstream merges (links not on route) included in the structure * @param gtu LaneBasedGTU; GTU */ public RollingLaneStructure(final Length lookAhead, final Length down, final Length up, final Length downSplit, final Length upMerge, final LaneBasedGTU gtu) { HistoryManager historyManager = gtu.getSimulator().getReplication().getHistoryManager(gtu.getSimulator()); this.root = new HistoricalValue<>(historyManager); this.lookAhead = lookAhead; this.down = down; this.up = up; this.downSplit = downSplit; this.upMerge = upMerge; this.containingGtu = gtu; try { gtu.addListener(this, LaneBasedGTU.LANE_CHANGE_EVENT); } catch (RemoteException exception) { throw new RuntimeException(exception); } } /** * Updates the underlying structure shifting the root position to the input. * @param pos DirectedLanePosition; current position of the GTU * @param route Route; current route of the GTU * @param gtuType GTUType; GTU type * @throws GTUException on a problem while updating the structure */ @Override public final void update(final DirectedLanePosition pos, final Route route, final GTUType gtuType) throws GTUException { /* * Implementation note: the LaneStructure was previously generated by AbstractLanePerception every time step. This * functionality has been moved to LaneStructure itself, in a manner that can update the LaneStructure. Start distances * of individual records are therefore made dynamic, calculated relative to a neighboring source record. For many time * steps this means that only these distances have to be updated. In other cases, the sources for start distances are * changed concerning the records that were involved in the previous time step. The LaneStructure now also maintains an * upstream and a downstream edge, i.e. set of records. These are moved forward as the GTU moves. */ // fractional position Lane lane = pos.getLane(); GTUDirectionality direction = pos.getGtuDirection(); Length position = pos.getPosition(); double fracPos = direction.isPlus() ? position.si / lane.getLength().si : 1.0 - position.si / lane.getLength().si; boolean deviative = this.containingGtu.getOperationalPlan() instanceof LaneBasedOperationalPlan && ((LaneBasedOperationalPlan) this.containingGtu.getOperationalPlan()).isDeviative(); // TODO on complex networks, e.g. with sections connectors where lane changes are not possible, the update may fail if (this.previousRoute != route || this.root.get() == null || deviative != this.previouslyDeviative) { // create new LaneStructure this.previousRoute = route; // clear this.upstreamEdge.clear(); this.downstreamEdge.clear(); this.crossSectionRecords.clear(); this.relativeLanes.clear(); this.relativeLaneMap.clear(); this.firstRecords.clear(); // build cross-section RollingLaneStructureRecord newRoot = constructRecord(lane, direction, null, RecordLink.CROSS, RelativeLane.CURRENT); this.root.set(newRoot); this.crossSectionRecords.put(RelativeLane.CURRENT, newRoot); for (LateralDirectionality latDirection : new LateralDirectionality[] {LateralDirectionality.LEFT, LateralDirectionality.RIGHT}) { RollingLaneStructureRecord current = newRoot; RelativeLane relativeLane = RelativeLane.CURRENT; Set adjacentLanes = current.getLane().accessibleAdjacentLanesPhysical(latDirection, gtuType, current .getDirection()); while (!adjacentLanes.isEmpty()) { Throw.when(adjacentLanes.size() > 1, RuntimeException.class, "Multiple adjacent lanes encountered during construction of lane map."); relativeLane = latDirection.isLeft() ? relativeLane.getLeft() : relativeLane.getRight(); Lane adjacentLane = adjacentLanes.iterator().next(); RollingLaneStructureRecord adjacentRecord = constructRecord(adjacentLane, direction, current, RecordLink.CROSS, relativeLane); this.crossSectionRecords.put(relativeLane, adjacentRecord); if (latDirection.isLeft()) { if (adjacentLane.accessibleAdjacentLanesPhysical(LateralDirectionality.RIGHT, gtuType, current .getDirection()).contains(current.getLane())) { adjacentRecord.setRight(current, gtuType); } current.setLeft(adjacentRecord, gtuType); } else { if (adjacentLane.accessibleAdjacentLanesPhysical(LateralDirectionality.LEFT, gtuType, current .getDirection()).contains(current.getLane())) { adjacentRecord.setLeft(current, gtuType); } current.setRight(adjacentRecord, gtuType); } current = adjacentRecord; adjacentLanes = current.getLane().accessibleAdjacentLanesPhysical(latDirection, gtuType, current .getDirection()); } } this.upstreamEdge.addAll(this.crossSectionRecords.values()); this.downstreamEdge.addAll(this.crossSectionRecords.values()); this.firstRecords.putAll(this.crossSectionRecords); // set the start distances so the upstream expand can work newRoot.updateStartDistance(fracPos, this); // expand upstream edge expandUpstreamEdge(gtuType, fracPos); // derive first records deriveFirstRecords(); } else { // update LaneStructure RollingLaneStructureRecord newRoot = this.root.get(); if (!lane.equals(newRoot.getLane())) { // find the root, and possible lateral shift if changed lane newRoot = null; RelativeLane lateralMove = null; double closest = Double.POSITIVE_INFINITY; for (RelativeLane relativeLane : this.relativeLaneMap.keySet()) { for (RollingLaneStructureRecord record : this.relativeLaneMap.get(relativeLane)) { if (record.getLane().equals(lane) && record.getStartDistance().si < closest && record .getStartDistance().si + record.getLength().si > 0.0) { newRoot = record; lateralMove = relativeLane; // multiple records may be present for the current lane due to a loop closest = record.getStartDistance().si; } } if (newRoot != null) { break; } } // newRoot.getPrev().contains(newRoot.getStartDistanceSource()) this.root.set(newRoot); // update start distance sources updateStartDistanceSources(); // shift if changed lane if (!lateralMove.isCurrent()) { RelativeLane delta = new RelativeLane(lateralMove.getLateralDirectionality().flip(), lateralMove .getNumLanes()); TreeMap> newRelativeLaneMap = new TreeMap<>(); for (RelativeLane relativeLane : this.relativeLaneMap.keySet()) { RelativeLane newRelativeLane = relativeLane.add(delta); newRelativeLaneMap.put(newRelativeLane, this.relativeLaneMap.get(relativeLane)); } this.relativeLaneMap = newRelativeLaneMap; Map newRelativeLanes = new LinkedHashMap<>(); for (LaneStructureRecord record : this.relativeLanes.keySet()) { newRelativeLanes.put(record, this.relativeLanes.get(record).add(delta)); } this.relativeLanes = newRelativeLanes; } this.crossSectionRecords.clear(); this.crossSectionRecords.put(RelativeLane.CURRENT, newRoot); for (LateralDirectionality latDirection : new LateralDirectionality[] {LateralDirectionality.LEFT, LateralDirectionality.RIGHT}) { RollingLaneStructureRecord record = newRoot; RollingLaneStructureRecord next = newRoot; RelativeLane delta = new RelativeLane(latDirection, 1); RelativeLane relLane = RelativeLane.CURRENT; while (next != null) { next = latDirection.isLeft() ? record.getLeft() : record.getRight(); if (next != null) { next.changeStartDistanceSource(record, RecordLink.CROSS); relLane = relLane.add(delta); this.crossSectionRecords.put(relLane, next); record = next; } } } } newRoot.updateStartDistance(fracPos, this); // update upstream edges retreatUpstreamEdge(); // derive first records deriveFirstRecords(); } this.previouslyDeviative = deviative; // update downstream edges expandDownstreamEdge(gtuType, fracPos, route); } /** * Derives the first downstream records so the extended cross-section can be returned. */ private void deriveFirstRecords() { this.firstRecords.clear(); this.firstRecords.putAll(this.crossSectionRecords); for (RelativeLane lane : this.relativeLaneMap.keySet()) { getFirstRecord(lane); // store non-null values internally } } /** * Upstream algorithm from the new source, where records along the way are assigned a new start distance source. These * records were downstream in the previous time step, but are now upstream. Hence, their start distance source should now * become their downstream record. This algorithm acts like an upstream tree, where each branch is stopped if there are no * upstream records, or the upstream records already have their downstream record as source (i.e. the record was already * upstream in the previous time step). */ private void updateStartDistanceSources() { // initial cross section Set set = new LinkedHashSet<>(); RollingLaneStructureRecord rootRecord = this.root.get(); set.add(rootRecord); rootRecord.changeStartDistanceSource(null, RecordLink.CROSS); RollingLaneStructureRecord prev = rootRecord; RollingLaneStructureRecord next = prev.getLeft(); while (next != null) { set.add(next); next.changeStartDistanceSource(prev, RecordLink.CROSS); prev = next; next = next.getLeft(); } prev = rootRecord; next = prev.getRight(); while (next != null) { set.add(next); next.changeStartDistanceSource(prev, RecordLink.CROSS); prev = next; next = next.getRight(); } // tree algorithm (branches flattened to a single set) while (!set.isEmpty()) { // lateral Set newSet = new LinkedHashSet<>(); for (RollingLaneStructureRecord record : set) { for (LateralDirectionality latDirection : new LateralDirectionality[] {LateralDirectionality.LEFT, LateralDirectionality.RIGHT}) { prev = record; next = latDirection.isLeft() ? record.getLeft() : record.getRight(); while (next != null && !set.contains(next)) { next.changeStartDistanceSource(prev, RecordLink.LATERAL_END); removeDownstream(next, latDirection.flip()); // split not taken can be thrown away newSet.add(next); prev = next; next = latDirection.isLeft() ? next.getLeft() : next.getRight(); } } } set.addAll(newSet); // longitudinal newSet.clear(); for (RollingLaneStructureRecord record : set) { for (RollingLaneStructureRecord prevRecord : record.getPrev()) { Iterator it = prevRecord.getNext().iterator(); while (it.hasNext()) { RollingLaneStructureRecord otherDown = it.next(); if (!otherDown.getLane().getParentLink().equals(record.getLane().getParentLink())) { // split not taken can be thrown away otherDown.changeStartDistanceSource(null, null); // this can throw away records that are laterally connected as they later merge... ?? removeDownstream(otherDown, LateralDirectionality.NONE); removeRecord(otherDown); it.remove(); } } LaneStructureRecord source = prevRecord.getStartDistanceSource(); if (source == null || (source != null && !source.equals(record))) { prevRecord.changeStartDistanceSource(record, RecordLink.UP); newSet.add(prevRecord); } } } // next loop set = newSet; } } /** * Removes all records downstream of the given record from underlying data structures. * @param record RollingLaneStructureRecord; record, downstream of which to remove all records * @param lat LateralDirectionality; records with an adjacent record at this side are not deleted */ private void removeDownstream(final RollingLaneStructureRecord record, final LateralDirectionality lat) { for (RollingLaneStructureRecord next : record.getNext()) { RollingLaneStructureRecord adj = lat.isLeft() ? next.getLeft() : lat.isRight() ? next.getRight() : null; if (adj == null) { next.changeStartDistanceSource(null, null); removeDownstream(next, lat); removeRecord(next); } } record.clearNextList(); } /** * Removes the record from underlying data structures. * @param record RollingLaneStructureRecord; record to remove */ private void removeRecord(final RollingLaneStructureRecord record) { RelativeLane lane = this.relativeLanes.get(record); if (lane != null) { this.relativeLaneMap.get(lane).remove(record); this.relativeLanes.remove(record); } record.changeStartDistanceSource(null, null); } /** * On a new build, this method is used to create the upstream map. * @param gtuType GTUType; GTU type * @param fractionalPosition double; fractional position on reference link * @throws GTUException on exception */ private void expandUpstreamEdge(final GTUType gtuType, final double fractionalPosition) throws GTUException { this.ignoreSet.clear(); for (LaneStructureRecord record : this.upstreamEdge) { this.ignoreSet.add(record.getLane()); } Set nextSet = new LinkedHashSet<>(); boolean expand = true; while (expand) { expand = false; // longitudinal Iterator iterator = this.upstreamEdge.iterator(); Set modifiedEdge = new LinkedHashSet<>(this.upstreamEdge); while (iterator.hasNext()) { RollingLaneStructureRecord prev = iterator.next(); ImmutableMap nexts = prev.getLane().upstreamLanes(prev.getDirection(), gtuType); if (prev.getStartDistance().si < this.up.si) { // upstream search ends on this lane prev.setCutOffStart(this.up.minus(prev.getStartDistance())); for (Lane prevLane : nexts.keySet()) { this.ignoreSet.add(prevLane); // exclude in lateral search } } else { // upstream search goes further upstream prev.clearCutOffStart(); iterator.remove(); if (!nexts.isEmpty()) { for (Lane prevLane : nexts.keySet()) { RelativeLane relativeLane = this.relativeLanes.get(prev); RollingLaneStructureRecord next = constructRecord(prevLane, nexts.get(prevLane), prev, RecordLink.UP, relativeLane); this.ignoreSet.add(prevLane); next.updateStartDistance(fractionalPosition, this); connectLaterally(next, gtuType, modifiedEdge); next.addNext(prev); prev.addPrev(next); nextSet.add(next); modifiedEdge.add(next); } } } } this.upstreamEdge.addAll(nextSet); expand |= !nextSet.isEmpty(); nextSet.clear(); // lateral Set lateralSet = expandLateral(this.upstreamEdge, RecordLink.LATERAL_END, gtuType, fractionalPosition); nextSet.addAll(lateralSet); // next iteration this.upstreamEdge.addAll(nextSet); expand |= !nextSet.isEmpty(); nextSet.clear(); } } /** * Helper method for upstream and downstream expansion. This method returns all lanes that can be laterally found from the * input set. * @param edge Set<RollingLaneStructureRecord>; input set * @param recordLink RecordLink; link to add between lateral records, depends on upstream or downstream search * @param gtuType GTUType; GTU type * @param fractionalPosition double; fractional position on reference link * @return Set<LaneStructureRecord>; output set with all laterally found lanes */ private Set expandLateral(final Set edge, final RecordLink recordLink, final GTUType gtuType, final double fractionalPosition) { Set nextSet = new LinkedHashSet<>(); Set laneSet = new LinkedHashSet<>(); // set to check that an adjacent lane is not another lane already in the set for (LaneStructureRecord record : edge) { laneSet.add(record.getLane()); } Iterator iterator = edge.iterator(); while (iterator.hasNext()) { RollingLaneStructureRecord record = iterator.next(); for (LateralDirectionality latDirection : new LateralDirectionality[] {LateralDirectionality.LEFT, LateralDirectionality.RIGHT}) { if (record.getRight() != null && latDirection.isRight() || record.getLeft() != null && latDirection.isLeft()) { // skip if there already is a record on that side continue; } RelativeLane relativeLane = this.relativeLanes.get(record); RollingLaneStructureRecord prev = record; Set adjacentLanes = prev.getLane().accessibleAdjacentLanesPhysical(latDirection, gtuType, prev .getDirection()); while (!adjacentLanes.isEmpty()) { Throw.when(adjacentLanes.size() > 1, RuntimeException.class, "Multiple adjacent lanes encountered during construction of lane map."); relativeLane = latDirection.isLeft() ? relativeLane.getLeft() : relativeLane.getRight(); Lane nextLane = adjacentLanes.iterator().next(); if (!laneSet.contains(nextLane) && !this.ignoreSet.contains(nextLane)) { RollingLaneStructureRecord next = constructRecord(nextLane, record.getDirection(), prev, recordLink, relativeLane); this.ignoreSet.add(nextLane); next.updateStartDistance(fractionalPosition, this); nextSet.add(next); laneSet.add(nextLane); if (latDirection.isLeft()) { prev.setLeft(next, gtuType); if (nextLane.accessibleAdjacentLanesPhysical(LateralDirectionality.RIGHT, gtuType, prev .getDirection()).contains(prev.getLane())) { next.setRight(prev, gtuType); } for (RollingLaneStructureRecord edgeRecord : edge) { if (!edgeRecord.equals(prev) && edgeRecord.getLane().getParentLink().equals(next.getLane() .getParentLink())) { for (Lane adjLane : edgeRecord.getLane().accessibleAdjacentLanesPhysical( LateralDirectionality.RIGHT, gtuType, edgeRecord.getDirection())) { if (adjLane.equals(next.getLane())) { edgeRecord.setRight(next, gtuType); next.setLeft(edgeRecord, gtuType); } } } } } else { prev.setRight(next, gtuType); if (nextLane.accessibleAdjacentLanesPhysical(LateralDirectionality.LEFT, gtuType, prev .getDirection()).contains(prev.getLane())) { next.setLeft(prev, gtuType); } for (RollingLaneStructureRecord edgeRecord : edge) { if (!edgeRecord.equals(prev) && edgeRecord.getLane().getParentLink().equals(next.getLane() .getParentLink())) { for (Lane adjLane : edgeRecord.getLane().accessibleAdjacentLanesPhysical( LateralDirectionality.LEFT, gtuType, edgeRecord.getDirection())) { if (adjLane.equals(next.getLane())) { edgeRecord.setLeft(next, gtuType); next.setRight(edgeRecord, gtuType); } } } } } // connect longitudinally due to merge or split Set adjs = new LinkedHashSet<>(); if (next.getLeft() != null) { adjs.add(next.getLeft()); } if (next.getRight() != null) { adjs.add(next.getRight()); } for (RollingLaneStructureRecord adj : adjs) { for (Lane lane : next.getLane().upstreamLanes(next.getDirection(), gtuType).keySet()) { for (RollingLaneStructureRecord adjPrev : adj.getPrev()) { if (lane.equals(adjPrev.getLane())) { Try.execute(() -> next.addPrev(adjPrev), "Cut-off record added as prev."); } } } for (Lane lane : next.getLane().downstreamLanes(next.getDirection(), gtuType).keySet()) { for (RollingLaneStructureRecord adjNext : adj.getNext()) { if (lane.equals(adjNext.getLane())) { Try.execute(() -> next.addNext(adjNext), "Cut-off record added as next."); } } } } prev = next; adjacentLanes = prev.getLane().accessibleAdjacentLanesPhysical(latDirection, gtuType, prev .getDirection()); } else { break; } } } } return nextSet; } /** * This method makes sure that not all history is maintained forever, and the upstream edge moves with the GTU. * @throws GTUException on exception */ private void retreatUpstreamEdge() throws GTUException { boolean moved = true; Set nexts = new LinkedHashSet<>(); while (moved) { moved = false; nexts.clear(); Iterator iterator = this.upstreamEdge.iterator(); while (iterator.hasNext()) { RollingLaneStructureRecord prev = iterator.next(); // nexts may contain 'prev' as two lanes are removed from the upstream edge that merge in to 1 downstream lane if (!nexts.contains(prev) && prev.getStartDistance().si + prev.getLane().getLength().si < this.up.si) { for (RollingLaneStructureRecord next : prev.getNext()) { next.clearPrevList(); next.setCutOffStart(this.up.minus(next.getStartDistance())); moved = true; nexts.add(next); RollingLaneStructureRecord lat = next.getLeft(); while (lat != null && lat.getPrev().isEmpty()) { nexts.add(lat); lat = lat.getLeft(); } lat = next.getRight(); while (lat != null && lat.getPrev().isEmpty()) { nexts.add(lat); lat = lat.getRight(); } } prev.clearNextList(); removeRecord(prev); iterator.remove(); } else { Length cutOff = this.up.minus(prev.getStartDistance()); if (cutOff.si > 0) { prev.setCutOffStart(cutOff); } } } this.upstreamEdge.addAll(nexts); // check adjacent lanes for (RollingLaneStructureRecord record : nexts) { RollingLaneStructureRecord prev = record; for (LateralDirectionality latDirection : new LateralDirectionality[] {LateralDirectionality.LEFT, LateralDirectionality.RIGHT}) { while (prev != null) { RollingLaneStructureRecord next = latDirection.isLeft() ? prev.getLeft() : prev.getRight(); if (next != null && !this.upstreamEdge.contains(next)) { moved |= findUpstreamEdge(next); } prev = next; } } } } } /** * Recursive method to find downstream record(s) on the upstream edge, as the edge was moved downstream and a laterally * connected lane was not yet in the upstream edge. All edge records are added to the edge set. * @param record RollingLaneStructureRecord; newly found adjacent record after moving the upstream edge downstream * @return boolean; whether a record was added to the edge, note that no record is added of the record is fully downstream * of the upstream view distance * @throws GTUException on exception */ private boolean findUpstreamEdge(final RollingLaneStructureRecord record) throws GTUException { Length cutOff = this.up.minus(record.getStartDistance()); boolean moved = false; if (cutOff.gt0()) { if (cutOff.lt(record.getLane().getLength())) { record.clearPrevList(); record.setCutOffStart(cutOff); this.upstreamEdge.add(record); moved = true; } else { if (this.relativeLanes.containsKey(record)) { // could have been removed from upstream already removeRecord(record); } for (RollingLaneStructureRecord next : record.getNext()) { moved |= findUpstreamEdge(next); } } } return moved; } /** * Main downstream search for the map. Can be used at initial build and to update. * @param gtuType GTUType; GTU type * @param fractionalPosition double; fractional position on reference link * @param route Route; route of the GTU * @throws GTUException on exception */ private void expandDownstreamEdge(final GTUType gtuType, final double fractionalPosition, final Route route) throws GTUException { this.ignoreSet.clear(); for (LaneStructureRecord record : this.downstreamEdge) { this.ignoreSet.add(record.getLane()); } Set nextSet = new LinkedHashSet<>(); Set splitSet = new LinkedHashSet<>(); boolean expand = true; while (expand) { expand = false; // longitudinal // find links to extend from so we can add lanes if -any- of the next lanes comes within the perception distance Set linksToExpandFrom = new LinkedHashSet<>(); Iterator iterator = this.downstreamEdge.iterator(); while (iterator.hasNext()) { RollingLaneStructureRecord record = iterator.next(); if (record.getStartDistance().si + record.getLane().getLength().si < this.down.si) { linksToExpandFrom.add(record.getLane().getParentLink()); } } Set modifiedEdge = new LinkedHashSet<>(this.downstreamEdge); iterator = this.downstreamEdge.iterator(); while (iterator.hasNext()) { RollingLaneStructureRecord record = iterator.next(); ImmutableMap nexts = record.getLane().downstreamLanes(record.getDirection(), gtuType); if (!linksToExpandFrom.contains(record.getLane().getParentLink())) { // downstream search ends on this lane record.setCutOffEnd(this.down.minus(record.getStartDistance())); for (Lane nextLane : nexts.keySet()) { this.ignoreSet.add(nextLane); // exclude in lateral search } } else { // downstream search goes further downstream // in case there are multiple lanes on the same link after a lane split, we need to choose one LaneDirection nextLaneDirection = new LaneDirection(record.getLane(), record.getDirection()) .getNextLaneDirection(this.containingGtu); record.clearCutOffEnd(); iterator.remove(); // can remove from edge, no algorithm needs it anymore in the downstream edge for (Lane nextLane : nexts.keySet()) { if (nextLaneDirection != null && nextLane.getParentLink().equals(nextLaneDirection.getLane() .getParentLink()) && !nextLane.equals(nextLaneDirection.getLane())) { // skip this lane as its a not chosen lane on the next link after a lane split continue; } RelativeLane relativeLane = this.relativeLanes.get(record); GTUDirectionality dir = nexts.get(nextLane); RollingLaneStructureRecord next = constructRecord(nextLane, dir, record, RecordLink.DOWN, relativeLane); this.ignoreSet.add(nextLane); next.updateStartDistance(fractionalPosition, this); record.addNext(next); next.addPrev(record); connectLaterally(next, gtuType, modifiedEdge); // check route int from = route == null ? 0 : route.indexOf(next.getFromNode()); int to = route == null ? 1 : route.indexOf(next.getToNode()); if (to < 0 || to - from != 1) { // not on our route, add some distance and stop splitSet.add(next); } else { // regular edge nextSet.add(next); } modifiedEdge.add(next); // expand upstream over any possible other lane that merges in to this Set set = new LinkedHashSet<>(); set.add(next); expandUpstreamMerge(set, gtuType, fractionalPosition, route); } } } this.downstreamEdge.addAll(nextSet); expand |= !nextSet.isEmpty(); nextSet.clear(); // split expandDownstreamSplit(splitSet, gtuType, fractionalPosition, route); splitSet.clear(); // lateral Set lateralSet = expandLateral(this.downstreamEdge, RecordLink.LATERAL_END, gtuType, fractionalPosition); nextSet.addAll(lateralSet); expandUpstreamMerge(lateralSet, gtuType, fractionalPosition, route); // next iteration this.downstreamEdge.addAll(nextSet); expand |= !nextSet.isEmpty(); nextSet.clear(); } } /** * Expand the map to include a limited section downstream of a split, regarding links not on the route. * @param set Set<RollingLaneStructureRecord>; set of lanes that have been laterally found * @param gtuType GTUType; GTU type * @param fractionalPosition double; fractional position on reference link * @param route Route; route * @throws GTUException on exception */ private void expandDownstreamSplit(final Set set, final GTUType gtuType, final double fractionalPosition, final Route route) throws GTUException { Map prevs = new LinkedHashMap<>(); Map nexts = new LinkedHashMap<>(); for (RollingLaneStructureRecord record : set) { prevs.put(record, record.getStartDistance().plus(this.downSplit)); } while (!prevs.isEmpty()) { for (RollingLaneStructureRecord prev : prevs.keySet()) { ImmutableMap nextLanes = prev.getLane().downstreamLanes(prev.getDirection(), gtuType); RelativeLane relativeLane = this.relativeLanes.get(prev); for (Lane nextLane : nextLanes.keySet()) { GTUDirectionality dir = nextLanes.get(nextLane); Node fromNode = dir.isPlus() ? nextLane.getParentLink().getStartNode() : nextLane.getParentLink() .getEndNode(); Node toNode = dir.isPlus() ? nextLane.getParentLink().getEndNode() : nextLane.getParentLink() .getStartNode(); int from = route.indexOf(fromNode); int to = route.indexOf(toNode); if (from == -1 || to == -1 || to - from != 1) { RollingLaneStructureRecord next = constructRecord(nextLane, dir, prev, RecordLink.DOWN, relativeLane); next.updateStartDistance(fractionalPosition, this); next.addPrev(prev); prev.addNext(next); connectLaterally(next, gtuType, nexts.keySet()); Length downHere = prevs.get(prev); if (next.getStartDistance().si > downHere.si) { next.setCutOffEnd(downHere.minus(next.getStartDistance())); } else { nexts.put(next, downHere); } } } } prevs = nexts; nexts = new LinkedHashMap<>(); } } /** * Expand the map to include a limited section upstream of a merge that is downstream, regarding links not on the route. * @param set Set<RollingLaneStructureRecord>; set of lanes that have been laterally found * @param gtuType GTUType; GTU type * @param fractionalPosition double; fractional position on reference link * @param route Route; route of the GTU * @throws GTUException on exception */ private void expandUpstreamMerge(final Set set, final GTUType gtuType, final double fractionalPosition, final Route route) throws GTUException { Map prevs = new LinkedHashMap<>(); Map nexts = new LinkedHashMap<>(); for (RollingLaneStructureRecord record : set) { prevs.put(record, record.getStartDistance().plus(this.upMerge)); // upMerge is negative } while (!prevs.isEmpty()) { for (RollingLaneStructureRecord prev : prevs.keySet()) { ImmutableMap nextLanes = prev.getLane().upstreamLanes(prev.getDirection(), gtuType); boolean anyAdded = false; for (Lane nextLane : nextLanes.keySet()) { GTUDirectionality dir = nextLanes.get(nextLane); Node fromNode = dir.isPlus() ? nextLane.getParentLink().getStartNode() : nextLane.getParentLink() .getEndNode(); Node toNode = dir.isPlus() ? nextLane.getParentLink().getEndNode() : nextLane.getParentLink() .getStartNode(); int from = route == null ? 0 : route.indexOf(fromNode); int to = route == null ? 1 : route.indexOf(toNode); // TODO we now assume everything is on the route, but merges could be ok without route // so, without a route we should be able to recognize which upstream 'nextLane' is on the other link if (from == -1 || to == -1 || to - from != 1) { anyAdded = true; RelativeLane relativeLane = this.relativeLanes.get(prev); RollingLaneStructureRecord next = constructRecord(nextLane, nextLanes.get(nextLane), prev, RecordLink.UP, relativeLane); next.updateStartDistance(fractionalPosition, this); next.addNext(prev); prev.addPrev(next); connectLaterally(next, gtuType, nexts.keySet()); Length upHere = prevs.get(prev); if (next.getStartDistance().si < upHere.si) { next.setCutOffStart(upHere.minus(next.getStartDistance())); this.upstreamEdge.add(next); } else { nexts.put(next, upHere); } } } if (!anyAdded && !set.contains(prev)) { this.upstreamEdge.add(prev); } } prevs = nexts; nexts = new LinkedHashMap<>(); } } /** * Helper method of various other methods that laterally couples lanes that have been longitudinally found. * @param record RollingLaneStructureRecord; longitudinally found lane * @param gtuType GTUType; GTU type * @param nextSet Set<RollingLaneStructureRecord>; set of records on current build edge */ private void connectLaterally(final RollingLaneStructureRecord record, final GTUType gtuType, final Set nextSet) { for (RollingLaneStructureRecord other : nextSet) { for (LateralDirectionality latDirection : new LateralDirectionality[] {LateralDirectionality.LEFT, LateralDirectionality.RIGHT}) { if ((latDirection.isLeft() ? other.getLeft() : other.getRight()) == null) { for (Lane otherLane : other.getLane().accessibleAdjacentLanesPhysical(latDirection, gtuType, other .getDirection())) { if (otherLane.equals(record.getLane())) { if (latDirection.isLeft()) { other.setLeft(record, gtuType); record.setRight(other, gtuType); } else { other.setRight(record, gtuType); record.setLeft(other, gtuType); } } } } } } } /** * Creates a lane structure record and adds it to relevant maps. * @param lane Lane; lane * @param direction GTUDirectionality; direction * @param startDistanceSource RollingLaneStructureRecord; source of the start distance * @param recordLink RecordLink; record link * @param relativeLane RelativeLane; relative lane * @return created lane structure record */ private RollingLaneStructureRecord constructRecord(final Lane lane, final GTUDirectionality direction, final RollingLaneStructureRecord startDistanceSource, final RecordLink recordLink, final RelativeLane relativeLane) { RollingLaneStructureRecord record = new RollingLaneStructureRecord(lane, direction, startDistanceSource, recordLink); if (!this.relativeLaneMap.containsKey(relativeLane)) { this.relativeLaneMap.put(relativeLane, new LinkedHashSet<>()); } this.relativeLaneMap.get(relativeLane).add(record); this.relativeLanes.put(record, relativeLane); return record; } /** {@inheritDoc} */ @Override public final LaneStructureRecord getRootRecord() { return this.root.get(); } /** * @param time Time; time to obtain the root at * @return rootRecord */ public final LaneStructureRecord getRootRecord(final Time time) { return this.root.get(time); } /** {@inheritDoc} */ @Override public final SortedSet getExtendedCrossSection() { return this.firstRecords.navigableKeySet(); } /** * Returns the first record on the given lane. This is often a record in the current cross section, but it may be one * downstream for a lane that starts further downstream. * @param lane RelativeLane; lane * @return first record on the given lane, or {@code null} if no such record */ @Override public final RollingLaneStructureRecord getFirstRecord(final RelativeLane lane) { if (this.firstRecords.containsKey(lane)) { return this.firstRecords.get(lane); } // not in current cross section, get first via downstream RelativeLane rel = RelativeLane.CURRENT; int dMin = Integer.MAX_VALUE; for (RelativeLane relLane : this.crossSectionRecords.keySet()) { if (relLane.getLateralDirectionality().equals(lane.getLateralDirectionality())) { int d = lane.getNumLanes() - relLane.getNumLanes(); if (d < dMin) { rel = relLane; d = dMin; } } } RollingLaneStructureRecord record = this.crossSectionRecords.get(rel); // move downstream until a lateral move is made to the right relative lane while (rel.getNumLanes() < lane.getNumLanes()) { RollingLaneStructureRecord adj = lane.getLateralDirectionality().isLeft() ? record.getLeft() : record.getRight(); if (adj != null) { rel = lane.getLateralDirectionality().isLeft() ? rel.getLeft() : rel.getRight(); record = adj; } else if (!record.getNext().isEmpty()) { LaneDirection laneDir = new LaneDirection(record.getLane(), record.getDirection()).getNextLaneDirection( this.containingGtu); if (laneDir == null) { record = null; break; } RollingLaneStructureRecord chosenNext = null; for (RollingLaneStructureRecord next : record.getNext()) { if (next.getLane().equals(laneDir.getLane())) { chosenNext = next; break; } } // Throw.when(chosenNext == null, RuntimeException.class, // "Unexpected exception while deriving first record not on the cross-section."); record = chosenNext; if (record == null) { // TODO: Temporary fix for Aimsun demo break; } } else { // reached a dead-end record = null; break; } } if (record != null) { // now move upstream until we are at x = 0 while (record.getPrev().size() == 1 && record.getStartDistance().gt0()) { record = record.getPrev().get(0); } this.firstRecords.put(lane, record); } return record; } /** * Retrieve objects of a specific type. Returns objects over a maximum length of the look ahead distance downstream from the * relative position, or as far as the lane map goes. * @param clazz Class<T>; class of objects to find * @param gtu LaneBasedGTU; gtu * @param pos RelativePosition.TYPE; relative position to start search from * @param type of objects to find * @return Sorted set of objects of requested type per lane * @throws GTUException if lane is not in current set */ @Override public final Map>> getDownstreamObjects( final Class clazz, final LaneBasedGTU gtu, final RelativePosition.TYPE pos) throws GTUException { Map>> out = new LinkedHashMap<>(); for (RelativeLane relativeLane : this.relativeLaneMap.keySet()) { out.put(relativeLane, getDownstreamObjects(relativeLane, clazz, gtu, pos)); } return out; } /** * Retrieve objects on a lane of a specific type. Returns objects over a maximum length of the look ahead distance * downstream from the relative position, or as far as the lane map goes. * @param lane RelativeLane; lane * @param clazz Class<T>; class of objects to find * @param gtu LaneBasedGTU; gtu * @param pos RelativePosition.TYPE; relative position to start search from * @param type of objects to find * @return Sorted set of objects of requested type * @throws GTUException if lane is not in current set */ @Override @SuppressWarnings("unchecked") public final SortedSet> getDownstreamObjects(final RelativeLane lane, final Class clazz, final LaneBasedGTU gtu, final RelativePosition.TYPE pos) throws GTUException { LaneStructureRecord record = getFirstRecord(lane); SortedSet> set = new TreeSet<>(); if (record != null) { double ds = gtu.getRelativePositions().get(pos).getDx().si - gtu.getReference().getDx().si; if (record.isDownstreamBranch()) { // the list is ordered, but only for DIR_PLUS, need to do our own ordering Length minimumPosition; Length maximumPosition; if (record.getDirection().isPlus()) { minimumPosition = Length.instantiateSI(ds - record.getStartDistance().si); maximumPosition = Length.instantiateSI(record.getLane().getLength().si); } else { minimumPosition = Length.ZERO; maximumPosition = Length.instantiateSI(record.getLane().getLength().si + record.getStartDistance().si - ds); } for (LaneBasedObject object : record.getLane().getLaneBasedObjects(minimumPosition, maximumPosition)) { if (clazz.isAssignableFrom(object.getClass()) && ((record.getDirection().isPlus() && object .getDirection().isForwardOrBoth()) || (record.getDirection().isMinus() && object.getDirection() .isBackwardOrBoth()))) { // unchecked, but the above isAssignableFrom assures correctness double distance = record.getDistanceToPosition(object.getLongitudinalPosition()).si - ds; if (distance <= this.lookAhead.si) { set.add(new Entry<>(Length.instantiateSI(distance), (T) object)); } } } } getDownstreamObjectsRecursive(set, record, clazz, ds); } return set; } /** * Retrieve objects on a lane of a specific type. Returns objects over a maximum length of the look ahead distance * downstream from the relative position, or as far as the lane map goes. Objects on links not on the route are ignored. * @param lane RelativeLane; lane * @param clazz Class<T>; class of objects to find * @param gtu LaneBasedGTU; gtu * @param pos RelativePosition.TYPE; relative position to start search from * @param type of objects to find * @param route Route; the route * @return Sorted set of objects of requested type * @throws GTUException if lane is not in current set */ @Override public final SortedSet> getDownstreamObjectsOnRoute(final RelativeLane lane, final Class clazz, final LaneBasedGTU gtu, final RelativePosition.TYPE pos, final Route route) throws GTUException { SortedSet> set = getDownstreamObjects(lane, clazz, gtu, pos); if (route != null) { Iterator> iterator = set.iterator(); while (iterator.hasNext()) { Entry entry = iterator.next(); CrossSectionLink link = entry.getLaneBasedObject().getLane().getParentLink(); if (!route.contains(link.getStartNode()) || !route.contains(link.getEndNode()) || Math.abs(route.indexOf(link .getStartNode()) - route.indexOf(link.getEndNode())) != 1) { iterator.remove(); } } } return set; } /** * Recursive search for lane based objects downstream. * @param set SortedSet<Entry<T>>; set to store entries into * @param record LaneStructureRecord; current record * @param clazz Class<T>; class of objects to find * @param ds double; distance from reference to chosen relative position * @param type of objects to find */ @SuppressWarnings("unchecked") private void getDownstreamObjectsRecursive(final SortedSet> set, final LaneStructureRecord record, final Class clazz, final double ds) { if (record.getNext().isEmpty() || record.getNext().get(0).getStartDistance().gt(this.lookAhead)) { return; } for (LaneStructureRecord next : record.getNext()) { if (next.isDownstreamBranch()) { List list = next.getLane().getLaneBasedObjects(); int iStart, di; if (record.getDirection().isPlus()) { iStart = 0; di = 1; } else { iStart = list.size() - 1; di = -1; } for (int i = iStart; i >= 0 & i < list.size(); i += di) { LaneBasedObject object = list.get(i); if (clazz.isAssignableFrom(object.getClass()) && ((record.getDirection().isPlus() && object .getDirection().isForwardOrBoth()) || (record.getDirection().isMinus() && object.getDirection() .isBackwardOrBoth()))) { // unchecked, but the above isAssignableFrom assures correctness double distance = next.getDistanceToPosition(object.getLongitudinalPosition()).si - ds; if (distance <= this.lookAhead.si) { set.add(new Entry<>(Length.instantiateSI(distance), (T) object)); } else { return; } } } } getDownstreamObjectsRecursive(set, next, clazz, ds); } } /** * Retrieve objects of a specific type. Returns objects over a maximum length of the look ahead distance downstream from the * relative position, or as far as the lane map goes. Objects on links not on the route are ignored. * @param clazz Class<T>; class of objects to find * @param gtu LaneBasedGTU; gtu * @param pos RelativePosition.TYPE; relative position to start search from * @param type of objects to find * @param route Route; the route * @return Sorted set of objects of requested type per lane * @throws GTUException if lane is not in current set */ @Override public final Map>> getDownstreamObjectsOnRoute( final Class clazz, final LaneBasedGTU gtu, final RelativePosition.TYPE pos, final Route route) throws GTUException { Map>> out = new LinkedHashMap<>(); for (RelativeLane relativeLane : this.relativeLaneMap.keySet()) { out.put(relativeLane, getDownstreamObjectsOnRoute(relativeLane, clazz, gtu, pos, route)); } return out; } /** * Retrieve objects on a lane of a specific type. Returns upstream objects from the relative position for as far as the lane * map goes. Distances to upstream objects are given as positive values. * @param lane RelativeLane; lane * @param clazz Class<T>; class of objects to find * @param gtu LaneBasedGTU; gtu * @param pos RelativePosition.TYPE; relative position to start search from * @param type of objects to find * @return Sorted set of objects of requested type * @throws GTUException if lane is not in current set */ @Override @SuppressWarnings("unchecked") public final SortedSet> getUpstreamObjects(final RelativeLane lane, final Class clazz, final LaneBasedGTU gtu, final RelativePosition.TYPE pos) throws GTUException { SortedSet> set = new TreeSet<>(); LaneStructureRecord record = this.getFirstRecord(lane); if (record.getStartDistance().gt0()) { return set; // this lane is only downstream } Length ds = gtu.getReference().getDx().minus(gtu.getRelativePositions().get(pos).getDx()); // the list is ordered, but only for DIR_PLUS, need to do our own ordering Length minimumPosition; Length maximumPosition; if (record.getDirection().isPlus()) { minimumPosition = Length.ZERO; maximumPosition = record.getStartDistance().neg().minus(ds); } else { minimumPosition = record.getLane().getLength().plus(record.getStartDistance()).plus(ds); maximumPosition = record.getLane().getLength(); } Length distance; for (LaneBasedObject object : record.getLane().getLaneBasedObjects(minimumPosition, maximumPosition)) { if (clazz.isAssignableFrom(object.getClass()) && ((record.getDirection().isPlus() && object.getDirection() .isForwardOrBoth()) || (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth()))) { distance = record.getDistanceToPosition(object.getLongitudinalPosition()).neg().minus(ds); // unchecked, but the above isAssignableFrom assures correctness set.add(new Entry<>(distance, (T) object)); } } getUpstreamObjectsRecursive(set, record, clazz, ds); return set; } /** * Recursive search for lane based objects upstream. * @param set SortedSet<Entry<T>>; set to store entries into * @param record LaneStructureRecord; current record * @param clazz Class<T>; class of objects to find * @param ds Length; distance from reference to chosen relative position * @param type of objects to find */ @SuppressWarnings("unchecked") private void getUpstreamObjectsRecursive(final SortedSet> set, final LaneStructureRecord record, final Class clazz, final Length ds) { for (LaneStructureRecord prev : record.getPrev()) { Length distance; for (LaneBasedObject object : prev.getLane().getLaneBasedObjects()) { if (clazz.isAssignableFrom(object.getClass()) && ((record.getDirection().isPlus() && object.getDirection() .isForwardOrBoth()) || (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth()))) { distance = prev.getDistanceToPosition(object.getLongitudinalPosition()).neg().minus(ds); // unchecked, but the above isAssignableFrom assures correctness set.add(new Entry<>(distance, (T) object)); } } getUpstreamObjectsRecursive(set, prev, clazz, ds); } } /** * Print the lane structure as a number of lines in a String. * @param ls RollingLaneStructure; the lane structure to print * @param gtu LaneBasedGTU; the GTTU for which the lane structure is printed * @return a String with information about the RollingLaneStructire */ public static String print(final RollingLaneStructure ls, final LaneBasedGTU gtu) { StringBuffer s = new StringBuffer(); s.append(gtu.getSimulator().getSimulatorTime() + " " + gtu.getId() + " LANESTRUCTURE: "); for (LaneStructureRecord lsr : ls.relativeLanes.keySet()) { s.append(lsr.toString() + " "); } int totSize = 0; for (Set set : ls.relativeLaneMap.values()) { totSize += set.size(); } s.append("\n relativeLanes.size()=" + ls.relativeLanes.size() + " relativeLaneMap.totalSize()=" + totSize); return s.toString(); } /** {@inheritDoc} */ @Override public final String toString() { return "LaneStructure [rootLSR=" + this.root + "]"; } /** * AnimationAccess provides access to a number of private fields in the structure, which should only be used read-only!
*
* Copyright (c) 2003-2022 Delft University of Technology, Jaffalaan 5, 2628 BX Delft, the Netherlands. All rights reserved. * See for project information www.simulation.tudelft.nl. * The source code and binary code of this software is proprietary information of Delft University of Technology. * @author Alexander Verbraeck */ public class AnimationAccess { /** * @return the lane structure records of the cross section */ @SuppressWarnings("synthetic-access") public TreeMap getCrossSectionRecords() { return RollingLaneStructure.this.crossSectionRecords; } /** * @return the upstream edge */ @SuppressWarnings("synthetic-access") public Set getUpstreamEdge() { return RollingLaneStructure.this.upstreamEdge; } /** * @return the downstream edge */ @SuppressWarnings("synthetic-access") public Set getDownstreamEdge() { return RollingLaneStructure.this.downstreamEdge; } } /** {@inheritDoc} */ @Override public void notify(final EventInterface event) throws RemoteException { // triggers an update of the lane structure at the end of the final plan during the lane change, which is deviative this.previouslyDeviative = false; } }