RollingLaneStructure.java

  1. package org.opentrafficsim.road.gtu.lane.perception;

  3. import java.awt.BasicStroke;
  4. import java.awt.Color;
  5. import java.awt.Graphics2D;
  6. import java.awt.geom.Path2D;
  7. import java.awt.image.ImageObserver;
  8. import java.io.Serializable;
  9. import java.lang.reflect.Field;
  10. import java.rmi.RemoteException;
  11. import java.util.HashMap;
  12. import java.util.HashSet;
  13. import java.util.Iterator;
  14. import java.util.LinkedHashMap;
  15. import java.util.LinkedHashSet;
  16. import java.util.List;
  17. import java.util.Map;
  18. import java.util.Set;
  19. import java.util.SortedSet;
  20. import java.util.TreeMap;
  21. import java.util.TreeSet;

  23. import javax.media.j3d.BoundingBox;
  24. import javax.media.j3d.Bounds;
  25. import javax.naming.NamingException;
  26. import javax.vecmath.Point3d;

  28. import org.djunits.value.vdouble.scalar.Length;
  29. import org.djunits.value.vdouble.scalar.Time;
  30. import org.opentrafficsim.core.geometry.OTSGeometryException;
  31. import org.opentrafficsim.core.geometry.OTSLine3D;
  32. import org.opentrafficsim.core.geometry.OTSPoint3D;
  33. import org.opentrafficsim.core.gtu.GTU;
  34. import org.opentrafficsim.core.gtu.GTUDirectionality;
  35. import org.opentrafficsim.core.gtu.GTUException;
  36. import org.opentrafficsim.core.gtu.GTUType;
  37. import org.opentrafficsim.core.gtu.RelativePosition;
  38. import org.opentrafficsim.core.gtu.Try;
  39. import org.opentrafficsim.core.network.LateralDirectionality;
  40. import org.opentrafficsim.core.network.Node;
  41. import org.opentrafficsim.core.network.route.Route;
  42. import org.opentrafficsim.core.perception.Historical;
  43. import org.opentrafficsim.core.perception.HistoricalValue;
  44. import org.opentrafficsim.core.perception.HistoryManager;
  45. import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
  46. import org.opentrafficsim.road.gtu.lane.perception.RollingLaneStructureRecord.RecordLink;
  47. import org.opentrafficsim.road.network.lane.CrossSectionLink;
  48. import org.opentrafficsim.road.network.lane.DirectedLanePosition;
  49. import org.opentrafficsim.road.network.lane.Lane;
  50. import org.opentrafficsim.road.network.lane.LaneDirection;
  51. import org.opentrafficsim.road.network.lane.object.LaneBasedObject;

  53. import nl.tudelft.simulation.dsol.animation.Locatable;
  54. import nl.tudelft.simulation.dsol.animation.D2.Renderable2D;
  55. import nl.tudelft.simulation.language.Throw;
  56. import nl.tudelft.simulation.language.d3.DirectedPoint;

  58. /**
  59.  * This data structure can clearly indicate the lane structure ahead of us, e.g. in the following situation:
  60.  * 
  61.  * <pre>
  62.  *     (---- a ----)(---- b ----)(---- c ----)(---- d ----)(---- e ----)(---- f ----)(---- g ----)  
  63.  *                                             __________                             __________
  64.  *                                            / _________ 1                          / _________ 2
  65.  *                                           / /                                    / /
  66.  *                                __________/ /             _______________________/ /
  67.  *  1  ____________ ____________ /_ _ _ _ _ _/____________ /_ _ _ _ _ _ _ _ _ _ _ _ /      
  68.  *  0 |_ _X_ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ |_ _ _ _ _ _ \____________
  69.  * -1 |____________|_ _ _ _ _ _ |____________|____________|  __________|____________|____________| 3
  70.  * -2              / __________/                           \ \  
  71.  *        ________/ /                                       \ \___________  
  72.  *      5 _________/                                         \____________  4
  73.  * </pre>
  74.  * 
  75.  * 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
  76.  * right at some point, but <b>not</b> two lanes to the right in link b, and not later than at the end of link f. When it needs
  77.  * 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
  78.  * 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
  79.  * destination 4. The rightmost lane just splits into two lanes at the end of link d, and the GTU can either continue driving to
  80.  * destination 3, turn right to destination 4. This means that the right lane of link d has <b>two</b> successor lanes.
  81.  * <p>
  82.  * In the data structures, lanes are numbered laterally. Suppose that the lane where vehicle X resides would be number 0.
  83.  * Consistent with "left is positive" for angles, the lane right of X would have number -1, and entry 5 would have number -2.
  84.  * <p>
  85.  * In the data structure, this can be indicated as follows (N = next, P = previous, L = left, R = right, D = lane drop, . =
  86.  * continued but not in this structure). The merge lane in b is considered "off limits" for the GTUs on the "main" lane -1; the
  87.  * "main" lane 0 is considered off limits from the exit lanes on c, e, and f. Still, we need to maintain pointers to these
  88.  * lanes, as we are interested in the GTUs potentially driving next to us, feeding into our lane, etc.
  89.  * 
  90.  * <pre>
  91.  *       1                0               -1               -2
  92.  *       
  93.  *                       ROOT 
  94.  *                   _____|_____      ___________      ___________            
  95.  *                  |_-_|_._|_R_|----|_L_|_._|_-_|    |_-_|_._|_-_|  a           
  96.  *                        |                |                |
  97.  *                   _____V_____      _____V_____      _____V_____            
  98.  *                  |_-_|_N_|_R_|----|_L_|_N_|_R_|&lt;---|_L_|_D_|_-_|  b           
  99.  *                        |                |                 
  100.  *  ___________      _____V_____      _____V_____                 
  101.  * |_-_|_N_|_R_|&lt;---|_L_|_N_|_R_|----|_L_|_N_|_-_|                   c
  102.  *       |                |                |                 
  103.  *  _____V_____      _____V_____      _____V_____                 
  104.  * |_-_|_._|_-_|    |_-_|_N_|_R_|----|_L_|_NN|_-_|                   d          
  105.  *                        |                ||_______________ 
  106.  *  ___________      _____V_____      _____V_____      _____V_____            
  107.  * |_-_|_N_|_R_|&lt;---|_L_|_N_|_R_|----|_L_|_N_|_-_|    |_-_|_N_|_-_|  e          
  108.  *       |                |                |                |
  109.  *  _____V_____      _____V_____      _____V_____      _____V_____            
  110.  * |_-_|_N_|_R_|&lt;---|_L_|_D_|_R_|----|_L_|_N_|_-_|    |_-_|_._|_-_|  f          
  111.  *       |                                 |                 
  112.  *  _____V_____                       _____V_____                             
  113.  * |_-_|_._|_-_|                     |_-_|_._|_-_|                   g
  114.  * </pre>
  115.  * <p>
  116.  * Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
  117.  * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
  118.  * </p>
  119.  * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
  120.  * initial version Feb 20, 2016 <br>
  121.  * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
  122.  */
  123. public class RollingLaneStructure implements LaneStructure, Serializable
  124. {

  126.     /** */
  127.     private static final long serialVersionUID = 20160400L;

  129.     /** The lanes from which we observe the situation. */
  130.     private final Historical<RollingLaneStructureRecord> root;

  132.     /** Look ahead distance. */
  133.     private Length lookAhead;

  135.     /** Route the structure is based on. */
  136.     private Route previousRoute;

  138.     /** Lane structure records of the cross section. */
  139.     private TreeMap<RelativeLane, RollingLaneStructureRecord> crossSectionRecords = new TreeMap<>();

  141.     /** First lane structure records. */
  142.     private TreeMap<RelativeLane, RollingLaneStructureRecord> firstRecords = new TreeMap<>();

  144.     /** Lane structure records grouped per relative lane. */
  145.     private Map<RelativeLane, Set<RollingLaneStructureRecord>> relativeLaneMap = new HashMap<>();

  147.     /** Relative lanes storage per record, such that other records can be linked to the correct relative lane. */
  148.     private Map<LaneStructureRecord, RelativeLane> relativeLanes = new HashMap<>();

  150.     /** Set of lanes that can be ignored as they are beyond build bounds. */
  151.     private final Set<Lane> ignoreSet = new HashSet<>();

  153.     /** Upstream edges. */
  154.     private final Set<RollingLaneStructureRecord> upstreamEdge = new LinkedHashSet<>();

  156.     /** Downstream edges. */
  157.     private final Set<RollingLaneStructureRecord> downstreamEdge = new LinkedHashSet<>();

  159.     /** Downstream distance over which the structure is made. */
  160.     private final Length down;

  162.     /** Upstream distance over which the structure is made. */
  163.     private final Length up;

  165.     /** Downstream distance at splits (links not on route) included in the structure. */
  166.     private final Length downSplit;

  168.     /** Upstream distance at downstream merges (links not on route) included in the structure. */
  169.     private final Length upMerge;

  171.     /** GTU. */
  172.     private final LaneBasedGTU containingGtu;

  174.     /**
  175.      * Constructor.
  176.      * @param lookAhead Length; distance over which visual objects are included
  177.      * @param down Length; downstream distance over which the structure is made
  178.      * @param up Length; upstream distance over which the structure is made, should include a margin for reaction time
  179.      * @param downSplit Length; downstream distance at splits (links not on route) included in the structure
  180.      * @param upMerge Length; upstream distance at downstream merges (links not on route) included in the structure
  181.      * @param gtu LaneBasedGTU; GTU
  182.      */
  183.     public RollingLaneStructure(final Length lookAhead, final Length down, final Length up, final Length downSplit,
  184.             final Length upMerge, final LaneBasedGTU gtu)
  185.     {
  186.         this.root = new HistoricalValue<>(HistoryManager.get(gtu.getSimulator()));
  187.         this.lookAhead = lookAhead;
  188.         this.down = down;
  189.         this.up = up;
  190.         this.downSplit = downSplit;
  191.         this.upMerge = upMerge;
  192.         this.containingGtu = gtu;
  193.         // if (gtu.getId().equals("922"))
  194.         // {
  195.         // visualize(gtu);
  196.         // }
  197.     }

  199.     /**
  200.      * Updates the underlying structure shifting the root position to the input.
  201.      * @param pos DirectedLanePosition; current position of the GTU
  202.      * @param route Route; current route of the GTU
  203.      * @param gtuType GTUType; GTU type
  204.      * @throws GTUException on a problem while updating the structure
  205.      */
  206.     public final void update(final DirectedLanePosition pos, final Route route, final GTUType gtuType) throws GTUException
  207.     {

  209.         /*
  210.          * Implementation note: the LaneStructure was previously generated by AbstractLanePerception every time step. This
  211.          * functionality has been moved to LaneStructure itself, in a manner that can update the LaneStructure. Start distances
  212.          * of individual records are therefore made dynamic, calculated relative to a neighboring source record. For many time
  213.          * steps this means that only these distances have to be updated. In other cases, the sources for start distances are
  214.          * changed concerning the records that were involved in the previous time step. The LaneStructure now also maintains an
  215.          * upstream and a downstream edge, i.e. set of records. These are moved forward as the GTU moves.
  216.          */

  218.         // fractional position
  219.         Lane lane = pos.getLane();
  220.         GTUDirectionality direction = pos.getGtuDirection();
  221.         Length position = pos.getPosition();
  222.         double fracPos = direction.isPlus() ? position.si / lane.getLength().si : 1.0 - position.si / lane.getLength().si;

  224.         // TODO on complex networks, e.g. with sections connectors where lane changes are not possible, the update may fail
  225.         if (this.previousRoute != route || this.root.get() == null)
  226.         {
  227.             // create new LaneStructure
  228.             this.previousRoute = route;

  230.             // clear
  231.             this.upstreamEdge.clear();
  232.             this.downstreamEdge.clear();
  233.             this.crossSectionRecords.clear();
  234.             this.relativeLanes.clear();

  236.             // build cross-section
  237.             RollingLaneStructureRecord newRoot = constructRecord(lane, direction, null, RecordLink.CROSS, RelativeLane.CURRENT);
  238.             this.root.set(newRoot);
  239.             this.crossSectionRecords.put(RelativeLane.CURRENT, newRoot);
  240.             for (LateralDirectionality latDirection : new LateralDirectionality[] { LateralDirectionality.LEFT,
  241.                     LateralDirectionality.RIGHT })
  242.             {
  243.                 RollingLaneStructureRecord current = newRoot;
  244.                 RelativeLane relativeLane = RelativeLane.CURRENT;
  245.                 Set<Lane> adjacentLanes =
  246.                         current.getLane().accessibleAdjacentLanesPhysical(latDirection, gtuType, current.getDirection());
  247.                 while (!adjacentLanes.isEmpty())
  248.                 {
  249.                     Throw.when(adjacentLanes.size() > 1, RuntimeException.class,
  250.                             "Multiple adjacent lanes encountered during construction of lane map.");
  251.                     relativeLane = latDirection.isLeft() ? relativeLane.getLeft() : relativeLane.getRight();
  252.                     Lane adjacentLane = adjacentLanes.iterator().next();
  253.                     RollingLaneStructureRecord adjacentRecord =
  254.                             constructRecord(adjacentLane, direction, current, RecordLink.CROSS, relativeLane);
  255.                     this.crossSectionRecords.put(relativeLane, adjacentRecord);
  256.                     if (latDirection.isLeft())
  257.                     {
  258.                         if (adjacentLane
  259.                                 .accessibleAdjacentLanesPhysical(LateralDirectionality.RIGHT, gtuType, current.getDirection())
  260.                                 .contains(current.getLane()))
  261.                         {
  262.                             adjacentRecord.setRight(current, gtuType);
  263.                         }
  264.                         current.setLeft(adjacentRecord, gtuType);
  265.                     }
  266.                     else
  267.                     {
  268.                         if (adjacentLane
  269.                                 .accessibleAdjacentLanesPhysical(LateralDirectionality.LEFT, gtuType, current.getDirection())
  270.                                 .contains(current.getLane()))
  271.                         {
  272.                             adjacentRecord.setLeft(current, gtuType);
  273.                         }
  274.                         current.setRight(adjacentRecord, gtuType);
  275.                     }
  276.                     current = adjacentRecord;
  277.                     adjacentLanes =
  278.                             current.getLane().accessibleAdjacentLanesPhysical(latDirection, gtuType, current.getDirection());
  279.                 }
  280.             }
  281.             this.upstreamEdge.addAll(this.crossSectionRecords.values());
  282.             this.downstreamEdge.addAll(this.crossSectionRecords.values());
  283.             this.firstRecords.putAll(this.crossSectionRecords);

  285.             // set the start distances so the upstream expand can work
  286.             newRoot.updateStartDistance(fracPos, this);

  288.             // expand upstream edge
  289.             expandUpstreamEdge(gtuType, fracPos);

  291.             // derive first records
  292.             deriveFirstRecords();
  293.         }
  294.         else
  295.         {

  297.             // update LaneStructure
  298.             RollingLaneStructureRecord newRoot = this.root.get();
  299.             if (!lane.equals(newRoot.getLane()))
  300.             {

  302.                 // find the root, and possible lateral shift if changed lane
  303.                 newRoot = null;
  304.                 RelativeLane lateralMove = null;
  305.                 double closest = Double.POSITIVE_INFINITY;
  306.                 for (RelativeLane relativeLane : this.relativeLaneMap.keySet())
  307.                 {
  308.                     for (RollingLaneStructureRecord record : this.relativeLaneMap.get(relativeLane))
  309.                     {
  310.                         if (record.getLane().equals(lane) && record.getStartDistance().si < closest
  311.                                 && record.getStartDistance().si + record.getLength().si > 0.0)
  312.                         {
  313.                             newRoot = record;
  314.                             lateralMove = relativeLane;
  315.                             // multiple records may be present for the current lane due to a loop
  316.                             closest = record.getStartDistance().si;
  317.                         }
  318.                     }
  319.                 }
  320.                 // newRoot.getPrev().contains(newRoot.getStartDistanceSource())
  321.                 this.root.set(newRoot);

  323.                 // update start distance sources
  324.                 updateStartDistanceSources();

  326.                 // shift if changed lane
  327.                 if (!lateralMove.isCurrent())
  328.                 {
  329.                     RelativeLane delta =
  330.                             new RelativeLane(lateralMove.getLateralDirectionality().flip(), lateralMove.getNumLanes());

  332.                     TreeMap<RelativeLane, Set<RollingLaneStructureRecord>> newRelativeLaneMap = new TreeMap<>();
  333.                     for (RelativeLane relativeLane : this.relativeLaneMap.keySet())
  334.                     {
  335.                         RelativeLane newRelativeLane = relativeLane.add(delta);
  336.                         newRelativeLaneMap.put(newRelativeLane, this.relativeLaneMap.get(relativeLane));
  337.                     }
  338.                     this.relativeLaneMap = newRelativeLaneMap;

  340.                     Map<LaneStructureRecord, RelativeLane> newRelativeLanes = new HashMap<>();
  341.                     for (LaneStructureRecord record : this.relativeLanes.keySet())
  342.                     {
  343.                         newRelativeLanes.put(record, this.relativeLanes.get(record).add(delta));
  344.                     }
  345.                     this.relativeLanes = newRelativeLanes;
  346.                 }

  348.                 this.crossSectionRecords.clear();
  349.                 this.crossSectionRecords.put(RelativeLane.CURRENT, newRoot);
  350.                 for (LateralDirectionality latDirection : new LateralDirectionality[] { LateralDirectionality.LEFT,
  351.                         LateralDirectionality.RIGHT })
  352.                 {
  353.                     RollingLaneStructureRecord record = newRoot;
  354.                     RollingLaneStructureRecord next = newRoot;
  355.                     RelativeLane delta = new RelativeLane(latDirection, 1);
  356.                     RelativeLane relLane = RelativeLane.CURRENT;
  357.                     while (next != null)
  358.                     {
  359.                         next = latDirection.isLeft() ? record.getLeft() : record.getRight();
  360.                         if (next != null)
  361.                         {
  362.                             next.changeStartDistanceSource(record, RecordLink.CROSS);
  363.                             relLane = relLane.add(delta);
  364.                             this.crossSectionRecords.put(relLane, next);
  365.                             record = next;
  366.                         }
  367.                     }
  368.                 }

  370.             }
  371.             newRoot.updateStartDistance(fracPos, this);

  373.             // update upstream edges
  374.             retreatUpstreamEdge();

  376.             // derive first records
  377.             deriveFirstRecords();

  379.         }

  381.         // update downstream edges
  382.         expandDownstreamEdge(gtuType, fracPos, route);

  384.     }

  386.     /**
  387.      * Derives the first downstream records so the extended cross-section can be returned.
  388.      */
  389.     private void deriveFirstRecords()
  390.     {
  391.         this.firstRecords.clear();
  392.         this.firstRecords.putAll(this.crossSectionRecords);
  393.         for (RelativeLane lane : this.relativeLaneMap.keySet())
  394.         {
  395.             getFirstRecord(lane); // store non-null values internally
  396.         }
  397.     }

  399.     /**
  400.      * Upstream algorithm from the new source, where records along the way are assigned a new start distance source. These
  401.      * records were downstream in the previous time step, but are now upstream. Hence, their start distance source should now
  402.      * become their downstream record. This algorithm acts like an upstream tree, where each branch is stopped if there are no
  403.      * upstream records, or the upstream records already have their downstream record as source (i.e. the record was already
  404.      * upstream in the previous time step).
  405.      */
  406.     private void updateStartDistanceSources()
  407.     {

  409.         // initial cross section
  410.         Set<RollingLaneStructureRecord> set = new HashSet<>();
  411.         RollingLaneStructureRecord rootRecord = this.root.get();
  412.         set.add(rootRecord);
  413.         rootRecord.changeStartDistanceSource(null, RecordLink.CROSS);
  414.         RollingLaneStructureRecord prev = rootRecord;
  415.         RollingLaneStructureRecord next = prev.getLeft();
  416.         while (next != null)
  417.         {
  418.             set.add(next);
  419.             next.changeStartDistanceSource(prev, RecordLink.CROSS);
  420.             prev = next;
  421.             next = next.getLeft();
  422.         }
  423.         prev = rootRecord;
  424.         next = prev.getRight();
  425.         while (next != null)
  426.         {
  427.             set.add(next);
  428.             next.changeStartDistanceSource(prev, RecordLink.CROSS);
  429.             prev = next;
  430.             next = next.getRight();
  431.         }
  432.         // tree algorithm (branches flattened to a single set)
  433.         while (!set.isEmpty())
  434.         {
  435.             // lateral
  436.             Set<RollingLaneStructureRecord> newSet = new HashSet<>();
  437.             for (RollingLaneStructureRecord record : set)
  438.             {
  439.                 for (LateralDirectionality latDirection : new LateralDirectionality[] { LateralDirectionality.LEFT,
  440.                         LateralDirectionality.RIGHT })
  441.                 {
  442.                     prev = record;
  443.                     next = latDirection.isLeft() ? record.getLeft() : record.getRight();
  444.                     while (next != null && !set.contains(next))
  445.                     {
  446.                         next.changeStartDistanceSource(prev, RecordLink.LATERAL_END);
  447.                         removeDownstream(next, latDirection.flip()); // split not taken can be thrown away
  448.                         newSet.add(next);
  449.                         prev = next;
  450.                         next = latDirection.isLeft() ? next.getLeft() : next.getRight();
  451.                     }
  452.                 }
  453.             }
  454.             set.addAll(newSet);

  456.             // longitudinal
  457.             newSet.clear();
  458.             for (RollingLaneStructureRecord record : set)
  459.             {
  460.                 for (RollingLaneStructureRecord prevRecord : record.getPrev())
  461.                 {
  462.                     Iterator<RollingLaneStructureRecord> it = prevRecord.getNext().iterator();
  463.                     while (it.hasNext())
  464.                     {
  465.                         RollingLaneStructureRecord otherDown = it.next();
  466.                         if (!otherDown.getLane().getParentLink().equals(record.getLane().getParentLink()))
  467.                         {
  468.                             // split not taken can be thrown away
  469.                             otherDown.changeStartDistanceSource(null, null);
  470.                             // this can throw away records that are laterally connected as they later merge... ??
  471.                             removeDownstream(otherDown, LateralDirectionality.NONE);
  472.                             removeRecord(otherDown);
  473.                             it.remove();
  474.                         }
  475.                     }
  476.                     LaneStructureRecord source = prevRecord.getStartDistanceSource();
  477.                     if (source == null || (source != null && !source.equals(record)))
  478.                     {
  479.                         prevRecord.changeStartDistanceSource(record, RecordLink.UP);
  480.                         newSet.add(prevRecord);
  481.                     }
  482.                 }
  483.             }

  485.             // next loop
  486.             set = newSet;
  487.         }
  488.     }

  490.     /**
  491.      * Removes all records downstream of the given record from underlying data structures.
  492.      * @param record LaneStructureRecord; record, downstream of which to remove all records
  493.      * @param lat LateralDirectionality; records with an adjacent record at this side are not deleted
  494.      */
  495.     private void removeDownstream(final RollingLaneStructureRecord record, final LateralDirectionality lat)
  496.     {
  497.         for (RollingLaneStructureRecord next : record.getNext())
  498.         {
  499.             RollingLaneStructureRecord adj = lat.isLeft() ? next.getLeft() : lat.isRight() ? next.getRight() : null;
  500.             if (adj == null)
  501.             {
  502.                 next.changeStartDistanceSource(null, null);
  503.                 removeDownstream(next, lat);
  504.                 removeRecord(next);
  505.             }
  506.         }
  507.         record.clearNextList();
  508.     }

  510.     /**
  511.      * Removes the record from underlying data structures.
  512.      * @param record LaneStructureRecord; record to remove
  513.      */
  514.     private void removeRecord(final RollingLaneStructureRecord record)
  515.     {
  516.         RelativeLane lane = this.relativeLanes.get(record);
  517.         if (lane != null)
  518.         {
  519.             this.relativeLanes.remove(record);
  520.         }
  521.         record.changeStartDistanceSource(null, null);

  523.     }

  525.     /**
  526.      * On a new build, this method is used to create the upstream map.
  527.      * @param gtuType GTUType; GTU type
  528.      * @param fractionalPosition double; fractional position on reference link
  529.      * @throws GTUException on exception
  530.      */
  531.     private void expandUpstreamEdge(final GTUType gtuType, final double fractionalPosition) throws GTUException
  532.     {
  533.         this.ignoreSet.clear();
  534.         for (LaneStructureRecord record : this.upstreamEdge)
  535.         {
  536.             this.ignoreSet.add(record.getLane());
  537.         }
  538.         Set<RollingLaneStructureRecord> nextSet = new LinkedHashSet<>();
  539.         boolean expand = true;
  540.         while (expand)
  541.         {
  542.             expand = false;

  544.             // longitudinal
  545.             Iterator<RollingLaneStructureRecord> iterator = this.upstreamEdge.iterator();
  546.             Set<RollingLaneStructureRecord> modifiedEdge = new LinkedHashSet<>(this.upstreamEdge);
  547.             while (iterator.hasNext())
  548.             {
  549.                 RollingLaneStructureRecord prev = iterator.next();
  550.                 Map<Lane, GTUDirectionality> nexts = prev.getLane().upstreamLanes(prev.getDirection(), gtuType);
  551.                 if (prev.getStartDistance().si < this.up.si)
  552.                 {
  553.                     // upstream search ends on this lane
  554.                     prev.setCutOffStart(this.up.minus(prev.getStartDistance()));
  555.                     for (Lane prevLane : nexts.keySet())
  556.                     {
  557.                         this.ignoreSet.add(prevLane); // exclude in lateral search
  558.                     }
  559.                 }
  560.                 else
  561.                 {
  562.                     // upstream search goes further upstream
  563.                     prev.clearCutOffStart();
  564.                     iterator.remove();
  565.                     if (!nexts.isEmpty())
  566.                     {
  567.                         for (Lane prevLane : nexts.keySet())
  568.                         {
  569.                             RelativeLane relativeLane = this.relativeLanes.get(prev);
  570.                             RollingLaneStructureRecord next =
  571.                                     constructRecord(prevLane, nexts.get(prevLane), prev, RecordLink.UP, relativeLane);
  572.                             this.ignoreSet.add(prevLane);
  573.                             next.updateStartDistance(fractionalPosition, this);
  574.                             connectLaterally(next, gtuType, modifiedEdge);
  575.                             next.addNext(prev);
  576.                             prev.addPrev(next);
  577.                             nextSet.add(next);
  578.                             modifiedEdge.add(next);
  579.                         }
  580.                     }
  581.                 }
  582.             }
  583.             this.upstreamEdge.addAll(nextSet);
  584.             expand |= !nextSet.isEmpty();
  585.             nextSet.clear();

  587.             // lateral
  588.             Set<RollingLaneStructureRecord> lateralSet =
  589.                     expandLateral(this.upstreamEdge, RecordLink.LATERAL_END, gtuType, fractionalPosition);
  590.             nextSet.addAll(lateralSet);

  592.             // next iteration
  593.             this.upstreamEdge.addAll(nextSet);
  594.             expand |= !nextSet.isEmpty();
  595.             nextSet.clear();
  596.         }
  597.     }

  599.     /**
  600.      * Helper method for upstream and downstream expansion. This method returns all lanes that can be laterally found from the
  601.      * input set.
  602.      * @param edge Set&lt;LaneStructureRecord&gt;; input set
  603.      * @param recordLink RecordLink; link to add between lateral records, depends on upstream or downstream search
  604.      * @param gtuType GTUType; GTU type
  605.      * @param fractionalPosition double; fractional position on reference link
  606.      * @return Set&lt;LaneStructureRecord&gt;; output set with all laterally found lanes
  607.      */
  608.     private Set<RollingLaneStructureRecord> expandLateral(final Set<RollingLaneStructureRecord> edge,
  609.             final RecordLink recordLink, final GTUType gtuType, final double fractionalPosition)
  610.     {
  611.         Set<RollingLaneStructureRecord> nextSet = new LinkedHashSet<>();
  612.         Set<Lane> laneSet = new HashSet<>(); // set to check that an adjacent lane is not another lane already in the set
  613.         for (LaneStructureRecord record : edge)
  614.         {
  615.             laneSet.add(record.getLane());
  616.         }
  617.         Iterator<RollingLaneStructureRecord> iterator = edge.iterator();
  618.         while (iterator.hasNext())
  619.         {
  620.             RollingLaneStructureRecord record = iterator.next();
  621.             for (LateralDirectionality latDirection : new LateralDirectionality[] { LateralDirectionality.LEFT,
  622.                     LateralDirectionality.RIGHT })
  623.             {
  624.                 RelativeLane relativeLane = this.relativeLanes.get(record);
  625.                 RollingLaneStructureRecord prev = record;
  626.                 Set<Lane> adjacentLanes =
  627.                         prev.getLane().accessibleAdjacentLanesPhysical(latDirection, gtuType, prev.getDirection());
  628.                 while (!adjacentLanes.isEmpty())
  629.                 {
  630.                     Throw.when(adjacentLanes.size() > 1, RuntimeException.class,
  631.                             "Multiple adjacent lanes encountered during construction of lane map.");
  632.                     relativeLane = latDirection.isLeft() ? relativeLane.getLeft() : relativeLane.getRight();
  633.                     Lane nextLane = adjacentLanes.iterator().next();
  634.                     if (!laneSet.contains(nextLane) && !this.ignoreSet.contains(nextLane))
  635.                     {
  636.                         RollingLaneStructureRecord next =
  637.                                 constructRecord(nextLane, record.getDirection(), prev, recordLink, relativeLane);
  638.                         this.ignoreSet.add(nextLane);
  639.                         next.updateStartDistance(fractionalPosition, this);
  640.                         nextSet.add(next);
  641.                         laneSet.add(nextLane);
  642.                         if (latDirection.isLeft())
  643.                         {
  644.                             prev.setLeft(next, gtuType);
  645.                             if (nextLane
  646.                                     .accessibleAdjacentLanesPhysical(LateralDirectionality.RIGHT, gtuType, prev.getDirection())
  647.                                     .contains(prev.getLane()))
  648.                             {
  649.                                 next.setRight(prev, gtuType);
  650.                             }
  651.                             for (RollingLaneStructureRecord edgeRecord : edge)
  652.                             {
  653.                                 if (!edgeRecord.equals(prev)
  654.                                         && edgeRecord.getLane().getParentLink().equals(next.getLane().getParentLink()))
  655.                                 {
  656.                                     for (Lane adjLane : edgeRecord.getLane().accessibleAdjacentLanesPhysical(
  657.                                             LateralDirectionality.RIGHT, gtuType, edgeRecord.getDirection()))
  658.                                     {
  659.                                         if (adjLane.equals(next.getLane()))
  660.                                         {
  661.                                             edgeRecord.setRight(next, gtuType);
  662.                                             next.setLeft(edgeRecord, gtuType);
  663.                                         }
  664.                                     }
  665.                                 }
  666.                             }
  667.                         }
  668.                         else
  669.                         {
  670.                             prev.setRight(next, gtuType);
  671.                             if (nextLane
  672.                                     .accessibleAdjacentLanesPhysical(LateralDirectionality.LEFT, gtuType, prev.getDirection())
  673.                                     .contains(prev.getLane()))
  674.                             {
  675.                                 next.setLeft(prev, gtuType);
  676.                             }
  677.                             for (RollingLaneStructureRecord edgeRecord : edge)
  678.                             {
  679.                                 if (!edgeRecord.equals(prev)
  680.                                         && edgeRecord.getLane().getParentLink().equals(next.getLane().getParentLink()))
  681.                                 {
  682.                                     for (Lane adjLane : edgeRecord.getLane().accessibleAdjacentLanesPhysical(
  683.                                             LateralDirectionality.LEFT, gtuType, edgeRecord.getDirection()))
  684.                                     {
  685.                                         if (adjLane.equals(next.getLane()))
  686.                                         {
  687.                                             edgeRecord.setLeft(next, gtuType);
  688.                                             next.setRight(edgeRecord, gtuType);
  689.                                         }
  690.                                     }
  691.                                 }
  692.                             }
  693.                         }
  694.                         prev = next;
  695.                         adjacentLanes =
  696.                                 prev.getLane().accessibleAdjacentLanesPhysical(latDirection, gtuType, prev.getDirection());
  697.                     }
  698.                     else
  699.                     {
  700.                         break;
  701.                     }
  702.                 }
  703.             }
  704.         }
  705.         return nextSet;
  706.     }

  708.     /**
  709.      * This method makes sure that not all history is maintained forever, and the upstream edge moves with the GTU.
  710.      * @throws GTUException on exception
  711.      */
  712.     private void retreatUpstreamEdge() throws GTUException
  713.     {
  714.         boolean moved = true;
  715.         Set<RollingLaneStructureRecord> nexts = new LinkedHashSet<>();
  716.         while (moved)
  717.         {
  718.             moved = false;
  719.             nexts.clear();
  720.             Iterator<RollingLaneStructureRecord> iterator = this.upstreamEdge.iterator();
  721.             while (iterator.hasNext())
  722.             {
  723.                 RollingLaneStructureRecord prev = iterator.next();
  724.                 // nexts may contain 'prev' as two lanes are removed from the upstream edge that merge in to 1 downstream lane
  725.                 if (!nexts.contains(prev) && prev.getStartDistance().si + prev.getLane().getLength().si < this.up.si)
  726.                 {
  727.                     for (RollingLaneStructureRecord next : prev.getNext())
  728.                     {
  729.                         next.clearPrevList();
  730.                         next.setCutOffStart(this.up.minus(next.getStartDistance()));
  731.                         moved = true;
  732.                         nexts.add(next);
  733.                         RollingLaneStructureRecord lat = next.getLeft();
  734.                         while (lat != null && lat.getPrev().isEmpty())
  735.                         {
  736.                             nexts.add(lat);
  737.                             lat = lat.getLeft();
  738.                         }
  739.                         lat = next.getRight();
  740.                         while (lat != null && lat.getPrev().isEmpty())
  741.                         {
  742.                             nexts.add(lat);
  743.                             lat = lat.getRight();
  744.                         }
  745.                     }
  746.                     prev.clearNextList();
  747.                     removeRecord(prev);
  748.                     iterator.remove();
  749.                 }
  750.                 else
  751.                 {
  752.                     Length cutOff = this.up.minus(prev.getStartDistance());
  753.                     if (cutOff.si > 0)
  754.                     {
  755.                         prev.setCutOffStart(cutOff);
  756.                     }
  757.                 }
  758.             }
  759.             this.upstreamEdge.addAll(nexts);

  761.             // check adjacent lanes
  762.             for (RollingLaneStructureRecord record : nexts)
  763.             {
  764.                 RollingLaneStructureRecord prev = record;
  765.                 for (LateralDirectionality latDirection : new LateralDirectionality[] { LateralDirectionality.LEFT,
  766.                         LateralDirectionality.RIGHT })
  767.                 {
  768.                     while (prev != null)
  769.                     {
  770.                         RollingLaneStructureRecord next = latDirection.isLeft() ? prev.getLeft() : prev.getRight();
  771.                         if (next != null && !this.upstreamEdge.contains(next))
  772.                         {
  773.                             moved |= findUpstreamEdge(next);
  774.                         }
  775.                         prev = next;
  776.                     }
  777.                 }
  778.             }
  779.         }
  780.     }

  782.     /**
  783.      * Recursive method to find downstream record(s) on the upstream edge, as the edge was moved downstream and a laterally
  784.      * connected lane was not yet in the upstream edge. All edge records are added to the edge set.
  785.      * @param record LaneStructureRecord; newly found adjacent record after moving the upstream edge downstream
  786.      * @return boolean; whether a record was added to the edge, note that no record is added of the record is fully downstream
  787.      *         of the upstream view distance
  788.      * @throws GTUException on exception
  789.      */
  790.     private boolean findUpstreamEdge(final RollingLaneStructureRecord record) throws GTUException
  791.     {
  792.         Length cutOff = this.up.minus(record.getStartDistance());
  793.         boolean moved = false;
  794.         if (cutOff.gt0())
  795.         {
  796.             if (cutOff.lt(record.getLane().getLength()))
  797.             {
  798.                 record.clearPrevList();
  799.                 record.setCutOffStart(cutOff);
  800.                 this.upstreamEdge.add(record);
  801.                 moved = true;
  802.             }
  803.             else
  804.             {
  805.                 if (this.relativeLanes.containsKey(record))
  806.                 {
  807.                     // could have been removed from upstream already
  808.                     removeRecord(record);
  809.                 }
  810.                 for (RollingLaneStructureRecord next : record.getNext())
  811.                 {
  812.                     moved |= findUpstreamEdge(next);
  813.                 }
  814.             }
  815.         }
  816.         return moved;
  817.     }

  819.     /**
  820.      * Main downstream search for the map. Can be used at initial build and to update.
  821.      * @param gtuType GTUType; GTU type
  822.      * @param fractionalPosition double; fractional position on reference link
  823.      * @param route Route; route of the GTU
  824.      * @throws GTUException on exception
  825.      */
  826.     private void expandDownstreamEdge(final GTUType gtuType, final double fractionalPosition, final Route route)
  827.             throws GTUException
  828.     {
  829.         this.ignoreSet.clear();
  830.         for (LaneStructureRecord record : this.downstreamEdge)
  831.         {
  832.             this.ignoreSet.add(record.getLane());
  833.         }
  834.         Set<RollingLaneStructureRecord> nextSet = new LinkedHashSet<>();
  835.         Set<RollingLaneStructureRecord> splitSet = new LinkedHashSet<>();
  836.         boolean expand = true;
  837.         while (expand)
  838.         {
  839.             expand = false;

  841.             // longitudinal
  842.             Iterator<RollingLaneStructureRecord> iterator = this.downstreamEdge.iterator();
  843.             Set<RollingLaneStructureRecord> modifiedEdge = new LinkedHashSet<>(this.downstreamEdge);
  844.             while (iterator.hasNext())
  845.             {
  846.                 RollingLaneStructureRecord record = iterator.next();
  847.                 Map<Lane, GTUDirectionality> nexts = record.getLane().downstreamLanes(record.getDirection(), gtuType);
  848.                 if (record.getStartDistance().si + record.getLane().getLength().si > this.down.si)
  849.                 {
  850.                     // downstream search ends on this lane
  851.                     record.setCutOffEnd(this.down.minus(record.getStartDistance()));
  852.                     for (Lane nextLane : nexts.keySet())
  853.                     {
  854.                         this.ignoreSet.add(nextLane); // exclude in lateral search
  855.                     }
  856.                 }
  857.                 else
  858.                 {
  859.                     // downstream search goes further downstream

  861.                     // in case there are multiple lanes on the same link after a lane split, we need to choose one
  862.                     LaneDirection nextLaneDirection =
  863.                             new LaneDirection(record.getLane(), record.getDirection()).getNextLaneDirection(this.containingGtu);

  865.                     record.clearCutOffEnd();
  866.                     iterator.remove(); // can remove from edge, no algorithm needs it anymore in the downstream edge
  867.                     for (Lane nextLane : nexts.keySet())
  868.                     {
  869.                         if (nextLaneDirection != null
  870.                                 && nextLane.getParentLink().equals(nextLaneDirection.getLane().getParentLink())
  871.                                 && !nextLane.equals(nextLaneDirection.getLane()))
  872.                         {
  873.                             // skip this lane as its a not chosen lane on the next link after a lane split
  874.                             continue;
  875.                         }
  876.                         RelativeLane relativeLane = this.relativeLanes.get(record);
  877.                         GTUDirectionality dir = nexts.get(nextLane);
  878.                         RollingLaneStructureRecord next = constructRecord(nextLane, dir, record, RecordLink.DOWN, relativeLane);
  879.                         this.ignoreSet.add(nextLane);
  880.                         next.updateStartDistance(fractionalPosition, this);
  881.                         record.addNext(next);
  882.                         next.addPrev(record);
  883.                         connectLaterally(next, gtuType, modifiedEdge);
  884.                         // check route
  885.                         int from = route == null ? 0 : route.indexOf(next.getFromNode());
  886.                         int to = route == null ? 1 : route.indexOf(next.getToNode());
  887.                         if (to < 0 || to - from != 1)
  888.                         {
  889.                             // not on our route, add some distance and stop
  890.                             splitSet.add(next);
  891.                         }
  892.                         else
  893.                         {
  894.                             // regular edge
  895.                             nextSet.add(next);
  896.                         }
  897.                         modifiedEdge.add(next);
  898.                         // expand upstream over any possible other lane that merges in to this
  899.                         Set<RollingLaneStructureRecord> set = new HashSet<>();
  900.                         set.add(next);
  901.                         expandUpstreamMerge(set, gtuType, fractionalPosition, route);
  902.                     }
  903.                 }
  904.             }
  905.             this.downstreamEdge.addAll(nextSet);
  906.             expand |= !nextSet.isEmpty();
  907.             nextSet.clear();

  909.             // split
  910.             expandDownstreamSplit(splitSet, gtuType, fractionalPosition, route);
  911.             splitSet.clear();

  913.             // lateral
  914.             Set<RollingLaneStructureRecord> lateralSet =
  915.                     expandLateral(this.downstreamEdge, RecordLink.LATERAL_END, gtuType, fractionalPosition);
  916.             nextSet.addAll(lateralSet);
  917.             expandUpstreamMerge(lateralSet, gtuType, fractionalPosition, route);

  919.             // next iteration
  920.             this.downstreamEdge.addAll(nextSet);
  921.             expand |= !nextSet.isEmpty();
  922.             nextSet.clear();
  923.         }
  924.     }

  926.     /**
  927.      * Expand the map to include a limited section downstream of a split, regarding links not on the route.
  928.      * @param set Set&lt;LaneStructureRecord&gt;; set of lanes that have been laterally found
  929.      * @param gtuType GTUType; GTU type
  930.      * @param fractionalPosition double; fractional position on reference link
  931.      * @param route Route; route
  932.      * @throws GTUException on exception
  933.      */
  934.     private void expandDownstreamSplit(final Set<RollingLaneStructureRecord> set, final GTUType gtuType,
  935.             final double fractionalPosition, final Route route) throws GTUException
  936.     {
  937.         Map<RollingLaneStructureRecord, Length> prevs = new LinkedHashMap<>();
  938.         Map<RollingLaneStructureRecord, Length> nexts = new LinkedHashMap<>();
  939.         for (RollingLaneStructureRecord record : set)
  940.         {
  941.             prevs.put(record, record.getStartDistance().plus(this.downSplit));
  942.         }
  943.         while (!prevs.isEmpty())
  944.         {
  945.             for (RollingLaneStructureRecord prev : prevs.keySet())
  946.             {
  947.                 Map<Lane, GTUDirectionality> nextLanes = prev.getLane().downstreamLanes(prev.getDirection(), gtuType);
  948.                 RelativeLane relativeLane = this.relativeLanes.get(prev);
  949.                 for (Lane nextLane : nextLanes.keySet())
  950.                 {
  951.                     GTUDirectionality dir = nextLanes.get(nextLane);
  952.                     Node fromNode =
  953.                             dir.isPlus() ? nextLane.getParentLink().getStartNode() : nextLane.getParentLink().getEndNode();
  954.                     Node toNode =
  955.                             dir.isPlus() ? nextLane.getParentLink().getEndNode() : nextLane.getParentLink().getStartNode();
  956.                     int from = route.indexOf(fromNode);
  957.                     int to = route.indexOf(toNode);
  958.                     if (from == -1 || to == -1 || to - from != 1)
  959.                     {
  960.                         RollingLaneStructureRecord next = constructRecord(nextLane, dir, prev, RecordLink.DOWN, relativeLane);
  961.                         next.updateStartDistance(fractionalPosition, this);
  962.                         next.addPrev(prev);
  963.                         prev.addNext(next);
  964.                         connectLaterally(next, gtuType, nexts.keySet());
  965.                         Length downHere = prevs.get(prev);
  966.                         if (next.getStartDistance().si > downHere.si)
  967.                         {
  968.                             next.setCutOffEnd(downHere.minus(next.getStartDistance()));
  969.                         }
  970.                         else
  971.                         {
  972.                             nexts.put(next, downHere);
  973.                         }
  974.                     }
  975.                 }
  976.             }
  977.             prevs = nexts;
  978.             nexts = new LinkedHashMap<>();
  979.         }
  980.     }

  982.     /**
  983.      * Expand the map to include a limited section upstream of a merge that is downstream, regarding links not on the route.
  984.      * @param set Set&lt;LaneStructureRecord&gt;; set of lanes that have been laterally found
  985.      * @param gtuType GTUType; GTU type
  986.      * @param fractionalPosition double; fractional position on reference link
  987.      * @param route Route; route of the GTU
  988.      * @throws GTUException on exception
  989.      */
  990.     private void expandUpstreamMerge(final Set<RollingLaneStructureRecord> set, final GTUType gtuType,
  991.             final double fractionalPosition, final Route route) throws GTUException
  992.     {
  993.         Map<RollingLaneStructureRecord, Length> prevs = new LinkedHashMap<>();
  994.         Map<RollingLaneStructureRecord, Length> nexts = new LinkedHashMap<>();
  995.         for (RollingLaneStructureRecord record : set)
  996.         {
  997.             prevs.put(record, record.getStartDistance().plus(this.upMerge)); // upMerge is negative
  998.         }
  999.         while (!prevs.isEmpty())
  1000.         {
  1001.             for (RollingLaneStructureRecord prev : prevs.keySet())
  1002.             {
  1003.                 Map<Lane, GTUDirectionality> nextLanes = prev.getLane().upstreamLanes(prev.getDirection(), gtuType);
  1004.                 boolean anyAdded = false;
  1005.                 for (Lane nextLane : nextLanes.keySet())
  1006.                 {
  1007.                     GTUDirectionality dir = nextLanes.get(nextLane);
  1008.                     Node fromNode =
  1009.                             dir.isPlus() ? nextLane.getParentLink().getStartNode() : nextLane.getParentLink().getEndNode();
  1010.                     Node toNode =
  1011.                             dir.isPlus() ? nextLane.getParentLink().getEndNode() : nextLane.getParentLink().getStartNode();
  1012.                     int from = route == null ? 0 : route.indexOf(fromNode);
  1013.                     int to = route == null ? 1 : route.indexOf(toNode);
  1014.                     // TODO we now assume everything is on the route, but merges could be ok without route
  1015.                     // so, without a route we should be able to recognize which upstream 'nextLane' is on the other link
  1016.                     if (from == -1 || to == -1 || to - from != 1)
  1017.                     {
  1018.                         anyAdded = true;
  1019.                         RelativeLane relativeLane = this.relativeLanes.get(prev);
  1020.                         RollingLaneStructureRecord next =
  1021.                                 constructRecord(nextLane, nextLanes.get(nextLane), prev, RecordLink.UP, relativeLane);
  1022.                         next.updateStartDistance(fractionalPosition, this);
  1023.                         next.addNext(prev);
  1024.                         prev.addPrev(next);
  1025.                         connectLaterally(next, gtuType, nexts.keySet());
  1026.                         Length upHere = prevs.get(prev);
  1027.                         if (next.getStartDistance().si < upHere.si)
  1028.                         {
  1029.                             next.setCutOffStart(upHere.minus(next.getStartDistance()));
  1030.                             this.upstreamEdge.add(next);
  1031.                         }
  1032.                         else
  1033.                         {
  1034.                             nexts.put(next, upHere);
  1035.                         }
  1036.                     }
  1037.                 }
  1038.                 if (!anyAdded && !set.contains(prev))
  1039.                 {
  1040.                     this.upstreamEdge.add(prev);
  1041.                 }
  1042.             }
  1043.             prevs = nexts;
  1044.             nexts = new LinkedHashMap<>();
  1045.         }
  1046.     }

  1048.     /**
  1049.      * Helper method of various other methods that laterally couples lanes that have been longitudinally found.
  1050.      * @param record LaneStructureRecord; longitudinally found lane
  1051.      * @param gtuType GTUType; GTU type
  1052.      * @param nextSet Set&lt;RollingLaneStructureRecord&gt;; set of records on current build edge
  1053.      */
  1054.     private void connectLaterally(final RollingLaneStructureRecord record, final GTUType gtuType,
  1055.             final Set<RollingLaneStructureRecord> nextSet)
  1056.     {
  1057.         for (RollingLaneStructureRecord other : nextSet)
  1058.         {
  1059.             for (LateralDirectionality latDirection : new LateralDirectionality[] { LateralDirectionality.LEFT,
  1060.                     LateralDirectionality.RIGHT })
  1061.             {
  1062.                 if ((latDirection.isLeft() ? other.getLeft() : other.getRight()) == null)
  1063.                 {
  1064.                     for (Lane otherLane : other.getLane().accessibleAdjacentLanesPhysical(latDirection, gtuType,
  1065.                             other.getDirection()))
  1066.                     {
  1067.                         if (otherLane.equals(record.getLane()))
  1068.                         {
  1069.                             if (latDirection.isLeft())
  1070.                             {
  1071.                                 other.setLeft(record, gtuType);
  1072.                                 record.setRight(other, gtuType);
  1073.                             }
  1074.                             else
  1075.                             {
  1076.                                 other.setRight(record, gtuType);
  1077.                                 record.setLeft(other, gtuType);
  1078.                             }
  1079.                         }
  1080.                     }
  1081.                 }
  1082.             }
  1083.         }
  1084.     }

  1086.     /**
  1087.      * Creates a lane structure record and adds it to relevant maps.
  1088.      * @param lane lane
  1089.      * @param direction direction
  1090.      * @param startDistanceSource source of the start distance
  1091.      * @param recordLink record link
  1092.      * @param relativeLane relative lane
  1093.      * @return created lane structure record
  1094.      */
  1095.     private RollingLaneStructureRecord constructRecord(final Lane lane, final GTUDirectionality direction,
  1096.             final RollingLaneStructureRecord startDistanceSource, final RecordLink recordLink, final RelativeLane relativeLane)
  1097.     {
  1098.         RollingLaneStructureRecord record = new RollingLaneStructureRecord(lane, direction, startDistanceSource, recordLink);
  1099.         if (!this.relativeLaneMap.containsKey(relativeLane))
  1100.         {
  1101.             this.relativeLaneMap.put(relativeLane, new LinkedHashSet<>());
  1102.         }
  1103.         this.relativeLaneMap.get(relativeLane).add(record);
  1104.         this.relativeLanes.put(record, relativeLane);
  1105.         return record;
  1106.     }

  1108.     /** {@inheritDoc} */
  1109.     @Override
  1110.     public final LaneStructureRecord getRootRecord()
  1111.     {
  1112.         return this.root.get();
  1113.     }

  1115.     /**
  1116.      * @param time Time; time to obtain the root at
  1117.      * @return rootRecord
  1118.      */
  1119.     public final LaneStructureRecord getRootRecord(final Time time)
  1120.     {
  1121.         return this.root.get(time);
  1122.     }

  1124.     /** {@inheritDoc} */
  1125.     @Override
  1126.     public final SortedSet<RelativeLane> getExtendedCrossSection()
  1127.     {
  1128.         return this.firstRecords.navigableKeySet();
  1129.     }

  1131.     /**
  1132.      * Returns the first record on the given lane. This is often a record in the current cross section, but it may be one
  1133.      * downstream for a lane that starts further downstream.
  1134.      * @param lane RelativeLane; lane
  1135.      * @return first record on the given lane, or {@code null} if no such record
  1136.      */
  1137.     public final RollingLaneStructureRecord getFirstRecord(final RelativeLane lane)
  1138.     {
  1139.         if (this.firstRecords.containsKey(lane))
  1140.         {
  1141.             return this.firstRecords.get(lane);
  1142.         }
  1143.         // not in current cross section, get first via downstream
  1144.         RelativeLane rel = RelativeLane.CURRENT;
  1145.         int dMin = Integer.MAX_VALUE;
  1146.         for (RelativeLane relLane : this.crossSectionRecords.keySet())
  1147.         {
  1148.             if (relLane.getLateralDirectionality().equals(lane.getLateralDirectionality()))
  1149.             {
  1150.                 int d = lane.getNumLanes() - relLane.getNumLanes();
  1151.                 if (d < dMin)
  1152.                 {
  1153.                     rel = relLane;
  1154.                     d = dMin;
  1155.                 }
  1156.             }
  1157.         }
  1158.         RollingLaneStructureRecord record = this.crossSectionRecords.get(rel);
  1159.         // move downstream until a lateral move is made to the right relative lane
  1160.         while (rel.getNumLanes() < lane.getNumLanes())
  1161.         {
  1162.             RollingLaneStructureRecord adj = lane.getLateralDirectionality().isLeft() ? record.getLeft() : record.getRight();
  1163.             if (adj != null)
  1164.             {
  1165.                 rel = lane.getLateralDirectionality().isLeft() ? rel.getLeft() : rel.getRight();
  1166.                 record = adj;
  1167.             }
  1168.             else if (!record.getNext().isEmpty())
  1169.             {
  1170.                 LaneDirection laneDir =
  1171.                         new LaneDirection(record.getLane(), record.getDirection()).getNextLaneDirection(this.containingGtu);
  1172.                 if (laneDir == null)
  1173.                 {
  1174.                     record = null;
  1175.                     break;
  1176.                 }
  1177.                 RollingLaneStructureRecord chosenNext = null;
  1178.                 for (RollingLaneStructureRecord next : record.getNext())
  1179.                 {
  1180.                     if (next.getLane().equals(laneDir.getLane()))
  1181.                     {
  1182.                         chosenNext = next;
  1183.                         break;
  1184.                     }
  1185.                 }
  1186.                 Throw.when(chosenNext == null, RuntimeException.class,
  1187.                         "Unexpected exception while deriving first record not on the cross-section.");
  1188.                 record = chosenNext;
  1189.             }
  1190.             else
  1191.             {
  1192.                 // reached a dead-end
  1193.                 record = null;
  1194.                 break;
  1195.             }
  1196.         }
  1197.         if (record != null)
  1198.         {
  1199.             // now move upstream until we are at x = 0
  1200.             while (record.getPrev().size() == 1 && record.getStartDistance().gt0())
  1201.             {
  1202.                 record = record.getPrev().get(0);
  1203.             }
  1204.             this.firstRecords.put(lane, record);
  1205.         }
  1206.         return record;
  1207.     }

  1209.     /**
  1210.      * Retrieve objects of a specific type. Returns objects over a maximum length of the look ahead distance downstream from the
  1211.      * relative position, or as far as the lane map goes.
  1212.      * @param clazz class of objects to find
  1213.      * @param gtu gtu
  1214.      * @param pos relative position to start search from
  1215.      * @param <T> type of objects to find
  1216.      * @return Sorted set of objects of requested type per lane
  1217.      * @throws GTUException if lane is not in current set
  1218.      */
  1219.     public final <T extends LaneBasedObject> Map<RelativeLane, SortedSet<Entry<T>>> getDownstreamObjects(final Class<T> clazz,
  1220.             final LaneBasedGTU gtu, final RelativePosition.TYPE pos) throws GTUException
  1221.     {
  1222.         Map<RelativeLane, SortedSet<Entry<T>>> out = new HashMap<>();
  1223.         for (RelativeLane relativeLane : this.relativeLaneMap.keySet())
  1224.         {
  1225.             out.put(relativeLane, getDownstreamObjects(relativeLane, clazz, gtu, pos));
  1226.         }
  1227.         return out;
  1228.     }

  1230.     /**
  1231.      * Retrieve objects on a lane of a specific type. Returns objects over a maximum length of the look ahead distance
  1232.      * downstream from the relative position, or as far as the lane map goes.
  1233.      * @param lane lane
  1234.      * @param clazz class of objects to find
  1235.      * @param gtu gtu
  1236.      * @param pos relative position to start search from
  1237.      * @param <T> type of objects to find
  1238.      * @return Sorted set of objects of requested type
  1239.      * @throws GTUException if lane is not in current set
  1240.      */
  1241.     @SuppressWarnings("unchecked")
  1242.     public final <T extends LaneBasedObject> SortedSet<Entry<T>> getDownstreamObjects(final RelativeLane lane,
  1243.             final Class<T> clazz, final LaneBasedGTU gtu, final RelativePosition.TYPE pos) throws GTUException
  1244.     {
  1245.         LaneStructureRecord record = getFirstRecord(lane);
  1246.         SortedSet<Entry<T>> set = new TreeSet<>();
  1247.         if (record != null)
  1248.         {
  1249.             double ds = gtu.getRelativePositions().get(pos).getDx().si - gtu.getReference().getDx().si;
  1250.             if (record.isDownstreamBranch())
  1251.             {
  1252.                 // the list is ordered, but only for DIR_PLUS, need to do our own ordering
  1253.                 Length minimumPosition;
  1254.                 Length maximumPosition;
  1255.                 if (record.getDirection().isPlus())
  1256.                 {
  1257.                     minimumPosition = Length.createSI(ds - record.getStartDistance().si);
  1258.                     maximumPosition = Length.createSI(record.getLane().getLength().si);
  1259.                 }
  1260.                 else
  1261.                 {
  1262.                     minimumPosition = Length.ZERO;
  1263.                     maximumPosition = Length.createSI(record.getLane().getLength().si + record.getStartDistance().si - ds);
  1264.                 }

  1266.                 for (LaneBasedObject object : record.getLane().getLaneBasedObjects(minimumPosition, maximumPosition))
  1267.                 {
  1268.                     if (clazz.isAssignableFrom(object.getClass())
  1269.                             && ((record.getDirection().isPlus() && object.getDirection().isForwardOrBoth())
  1270.                                     || (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth())))
  1271.                     {
  1272.                         // unchecked, but the above isAssignableFrom assures correctness
  1273.                         double distance = record.getDistanceToPosition(object.getLongitudinalPosition()).si - ds;
  1274.                         if (distance <= this.lookAhead.si)
  1275.                         {
  1276.                             set.add(new Entry<>(Length.createSI(distance), (T) object));
  1277.                         }
  1278.                     }
  1279.                 }
  1280.             }
  1281.             getDownstreamObjectsRecursive(set, record, clazz, ds);
  1282.         }
  1283.         return set;
  1284.     }

  1286.     /**
  1287.      * Retrieve objects on a lane of a specific type. Returns objects over a maximum length of the look ahead distance
  1288.      * downstream from the relative position, or as far as the lane map goes. Objects on links not on the route are ignored.
  1289.      * @param lane lane
  1290.      * @param clazz class of objects to find
  1291.      * @param gtu gtu
  1292.      * @param pos relative position to start search from
  1293.      * @param <T> type of objects to find
  1294.      * @param route the route
  1295.      * @return Sorted set of objects of requested type
  1296.      * @throws GTUException if lane is not in current set
  1297.      */
  1298.     public final <T extends LaneBasedObject> SortedSet<Entry<T>> getDownstreamObjectsOnRoute(final RelativeLane lane,
  1299.             final Class<T> clazz, final LaneBasedGTU gtu, final RelativePosition.TYPE pos, final Route route)
  1300.             throws GTUException
  1301.     {
  1302.         SortedSet<Entry<T>> set = getDownstreamObjects(lane, clazz, gtu, pos);
  1303.         if (route != null)
  1304.         {
  1305.             Iterator<Entry<T>> iterator = set.iterator();
  1306.             while (iterator.hasNext())
  1307.             {
  1308.                 Entry<T> entry = iterator.next();
  1309.                 CrossSectionLink link = entry.getLaneBasedObject().getLane().getParentLink();
  1310.                 if (!route.contains(link.getStartNode()) || !route.contains(link.getEndNode())
  1311.                         || Math.abs(route.indexOf(link.getStartNode()) - route.indexOf(link.getEndNode())) != 1)
  1312.                 {
  1313.                     iterator.remove();
  1314.                 }
  1315.             }
  1316.         }
  1317.         return set;
  1318.     }

  1320.     /**
  1321.      * Recursive search for lane based objects downstream.
  1322.      * @param set set to store entries into
  1323.      * @param record current record
  1324.      * @param clazz class of objects to find
  1325.      * @param ds distance from reference to chosen relative position
  1326.      * @param <T> type of objects to find
  1327.      */
  1328.     @SuppressWarnings("unchecked")
  1329.     private <T extends LaneBasedObject> void getDownstreamObjectsRecursive(final SortedSet<Entry<T>> set,
  1330.             final LaneStructureRecord record, final Class<T> clazz, final double ds)
  1331.     {
  1332.         if (record.getNext().isEmpty() || record.getNext().get(0).getStartDistance().gt(this.lookAhead))
  1333.         {
  1334.             return;
  1335.         }
  1336.         for (LaneStructureRecord next : record.getNext())
  1337.         {
  1338.             if (next.isDownstreamBranch())
  1339.             {
  1340.                 List<LaneBasedObject> list = next.getLane().getLaneBasedObjects();
  1341.                 int iStart, di;
  1342.                 if (record.getDirection().isPlus())
  1343.                 {
  1344.                     iStart = 0;
  1345.                     di = 1;
  1346.                 }
  1347.                 else
  1348.                 {
  1349.                     iStart = list.size() - 1;
  1350.                     di = -1;
  1351.                 }
  1352.                 for (int i = iStart; i >= 0 & i < list.size(); i += di)
  1353.                 {
  1354.                     LaneBasedObject object = list.get(i);
  1355.                     if (clazz.isAssignableFrom(object.getClass())
  1356.                             && ((record.getDirection().isPlus() && object.getDirection().isForwardOrBoth())
  1357.                                     || (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth())))
  1358.                     {
  1359.                         // unchecked, but the above isAssignableFrom assures correctness
  1360.                         double distance = next.getDistanceToPosition(object.getLongitudinalPosition()).si - ds;
  1361.                         if (distance <= this.lookAhead.si)
  1362.                         {
  1363.                             set.add(new Entry<>(Length.createSI(distance), (T) object));
  1364.                         }
  1365.                         else
  1366.                         {
  1367.                             return;
  1368.                         }
  1369.                     }
  1370.                 }
  1371.             }
  1372.             getDownstreamObjectsRecursive(set, next, clazz, ds);
  1373.         }
  1374.     }

  1376.     /**
  1377.      * Retrieve objects of a specific type. Returns objects over a maximum length of the look ahead distance downstream from the
  1378.      * relative position, or as far as the lane map goes. Objects on links not on the route are ignored.
  1379.      * @param clazz class of objects to find
  1380.      * @param gtu gtu
  1381.      * @param pos relative position to start search from
  1382.      * @param <T> type of objects to find
  1383.      * @param route the route
  1384.      * @return Sorted set of objects of requested type per lane
  1385.      * @throws GTUException if lane is not in current set
  1386.      */
  1387.     public final <T extends LaneBasedObject> Map<RelativeLane, SortedSet<Entry<T>>> getDownstreamObjectsOnRoute(
  1388.             final Class<T> clazz, final LaneBasedGTU gtu, final RelativePosition.TYPE pos, final Route route)
  1389.             throws GTUException
  1390.     {
  1391.         Map<RelativeLane, SortedSet<Entry<T>>> out = new HashMap<>();
  1392.         for (RelativeLane relativeLane : this.relativeLaneMap.keySet())
  1393.         {
  1394.             out.put(relativeLane, getDownstreamObjectsOnRoute(relativeLane, clazz, gtu, pos, route));
  1395.         }
  1396.         return out;
  1397.     }

  1399.     /**
  1400.      * Retrieve objects on a lane of a specific type. Returns upstream objects from the relative position for as far as the lane
  1401.      * map goes. Distances to upstream objects are given as positive values.
  1402.      * @param lane lane
  1403.      * @param clazz class of objects to find
  1404.      * @param gtu gtu
  1405.      * @param pos relative position to start search from
  1406.      * @param <T> type of objects to find
  1407.      * @return Sorted set of objects of requested type
  1408.      * @throws GTUException if lane is not in current set
  1409.      */
  1410.     @SuppressWarnings("unchecked")
  1411.     public final <T extends LaneBasedObject> SortedSet<Entry<T>> getUpstreamObjects(final RelativeLane lane,
  1412.             final Class<T> clazz, final LaneBasedGTU gtu, final RelativePosition.TYPE pos) throws GTUException
  1413.     {
  1414.         SortedSet<Entry<T>> set = new TreeSet<>();
  1415.         LaneStructureRecord record = this.getFirstRecord(lane);
  1416.         if (record.getStartDistance().gt0())
  1417.         {
  1418.             return set; // this lane is only downstream
  1419.         }
  1420.         Length ds = gtu.getReference().getDx().minus(gtu.getRelativePositions().get(pos).getDx());
  1421.         // the list is ordered, but only for DIR_PLUS, need to do our own ordering
  1422.         Length minimumPosition;
  1423.         Length maximumPosition;
  1424.         if (record.getDirection().isPlus())
  1425.         {
  1426.             minimumPosition = Length.ZERO;
  1427.             maximumPosition = record.getStartDistance().neg().minus(ds);
  1428.         }
  1429.         else
  1430.         {
  1431.             minimumPosition = record.getLane().getLength().plus(record.getStartDistance()).plus(ds);
  1432.             maximumPosition = record.getLane().getLength();
  1433.         }
  1434.         Length distance;
  1435.         for (LaneBasedObject object : record.getLane().getLaneBasedObjects(minimumPosition, maximumPosition))
  1436.         {
  1437.             if (clazz.isAssignableFrom(object.getClass())
  1438.                     && ((record.getDirection().isPlus() && object.getDirection().isForwardOrBoth())
  1439.                             || (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth())))
  1440.             {
  1441.                 distance = record.getDistanceToPosition(object.getLongitudinalPosition()).neg().minus(ds);
  1442.                 // unchecked, but the above isAssignableFrom assures correctness
  1443.                 set.add(new Entry<>(distance, (T) object));
  1444.             }
  1445.         }
  1446.         getUpstreamObjectsRecursive(set, record, clazz, ds);
  1447.         return set;
  1448.     }

  1450.     /**
  1451.      * Recursive search for lane based objects upstream.
  1452.      * @param set set to store entries into
  1453.      * @param record current record
  1454.      * @param clazz class of objects to find
  1455.      * @param ds distance from reference to chosen relative position
  1456.      * @param <T> type of objects to find
  1457.      */
  1458.     @SuppressWarnings("unchecked")
  1459.     private <T extends LaneBasedObject> void getUpstreamObjectsRecursive(final SortedSet<Entry<T>> set,
  1460.             final LaneStructureRecord record, final Class<T> clazz, final Length ds)
  1461.     {
  1462.         for (LaneStructureRecord prev : record.getPrev())
  1463.         {
  1464.             Length distance;
  1465.             for (LaneBasedObject object : prev.getLane().getLaneBasedObjects())
  1466.             {
  1467.                 if (clazz.isAssignableFrom(object.getClass())
  1468.                         && ((record.getDirection().isPlus() && object.getDirection().isForwardOrBoth())
  1469.                                 || (record.getDirection().isMinus() && object.getDirection().isBackwardOrBoth())))
  1470.                 {
  1471.                     distance = prev.getDistanceToPosition(object.getLongitudinalPosition()).neg().minus(ds);
  1472.                     // unchecked, but the above isAssignableFrom assures correctness
  1473.                     set.add(new Entry<>(distance, (T) object));
  1474.                 }
  1475.             }
  1476.             getUpstreamObjectsRecursive(set, prev, clazz, ds);
  1477.         }
  1478.     }

  1480.     /** {@inheritDoc} */
  1481.     @Override
  1482.     public final String toString()
  1483.     {
  1484.         return "LaneStructure [rootLSR=" + this.root + "]";
  1485.     }

  1487.     /**
  1488.      * Enables visualization of this lane structure. This is purely for debugging purposes.
  1489.      * @param gtu GTU to animate the LaneStructure off
  1490.      */
  1491.     public final void visualize(final GTU gtu)
  1492.     {
  1493.         /**
  1494.          * Locatable.
  1495.          */
  1496.         class LaneStructureLocatable implements Locatable
  1497.         {
  1498.             /** {@inheritDoc} */
  1499.             @Override
  1500.             public DirectedPoint getLocation() throws RemoteException
  1501.             {
  1502.                 LaneStructureRecord rt = getRootRecord();
  1503.                 if (rt == null)
  1504.                 {
  1505.                     return gtu.getLocation();
  1506.                 }
  1507.                 Length position = rt.getDirection().isPlus() ? rt.getStartDistance().neg()
  1508.                         : rt.getLane().getLength().plus(rt.getStartDistance());
  1509.                 position = position.lt0() ? Length.ZERO : position;
  1510.                 try
  1511.                 {
  1512.                     return rt.getLane().getCenterLine().getLocation(position);
  1513.                 }
  1514.                 catch (OTSGeometryException exception)
  1515.                 {
  1516.                     throw new RuntimeException("Unable to return location.", exception);
  1517.                 }
  1518.             }

  1520.             /** {@inheritDoc} */
  1521.             @Override
  1522.             public Bounds getBounds() throws RemoteException
  1523.             {
  1524.                 Point3d p1 = new Point3d(-1000000, -1000000, 0.0);
  1525.                 Point3d p2 = new Point3d(1000000, 1000000, 0.0);
  1526.                 return new BoundingBox(p1, p2);
  1527.             }
  1528.         }
  1529.         /**
  1530.          * Animation of lane structure.
  1531.          */
  1532.         class LaneStructureAnimation extends Renderable2D<LaneStructureLocatable>
  1533.         {

  1535.             /** Destroyed. */
  1536.             private boolean isDestroyed = false;

  1538.             /**
  1539.              * @param source LaneStructureLocatable; dummy locatable
  1540.              * @throws NamingException on naming exception
  1541.              * @throws RemoteException on remote exception
  1542.              */
  1543.             LaneStructureAnimation(final LaneStructureLocatable source) throws NamingException, RemoteException
  1544.             {
  1545.                 super(source, gtu.getSimulator());
  1546.                 this.setFlip(false);
  1547.                 this.setRotate(false);
  1548.             }

  1550.             /** {@inheritDoc} */
  1551.             @Override
  1552.             public void paint(final Graphics2D graphics, final ImageObserver observer) throws RemoteException
  1553.             {
  1554.                 if (!this.isDestroyed)
  1555.                 {
  1556.                     if (gtu.isDestroyed())
  1557.                     {
  1558.                         this.isDestroyed = true;
  1559.                         Try.execute(() -> destroy(), "Exception during deletion of LaneStructureAnimation.");
  1560.                         return;
  1561.                     }
  1562.                     else
  1563.                     {
  1564.                         LaneStructureRecord rt = getRootRecord();
  1565.                         if (rt != null)
  1566.                         {
  1567.                             paintRecord(rt, graphics);
  1568.                         }
  1569.                     }
  1570.                 }
  1571.             }

  1573.             /**
  1574.              * @param lsr LaneStructureRecord; record
  1575.              * @param graphics Graphics2D; graphics
  1576.              */
  1577.             @SuppressWarnings({ "unchecked", "synthetic-access" })
  1578.             private void paintRecord(final LaneStructureRecord lsr, final Graphics2D graphics)
  1579.             {
  1580.                 // line
  1581.                 DirectedPoint loc = Try.assign(() -> getSource().getLocation(), "Unable to return location.");
  1582.                 graphics.setStroke(
  1583.                         new BasicStroke(RollingLaneStructure.this.crossSectionRecords.containsValue(lsr) ? 1.0f : 0.5f));
  1584.                 graphics.setColor(RollingLaneStructure.this.crossSectionRecords.containsValue(lsr) ? Color.PINK
  1585.                         : RollingLaneStructure.this.upstreamEdge.contains(lsr) ? Color.MAGENTA
  1586.                                 : RollingLaneStructure.this.downstreamEdge.contains(lsr) ? Color.GREEN : Color.CYAN);
  1587.                 OTSLine3D line = Try.assign(() -> lsr.getLane().getCenterLine().extractFractional(0.1, 0.9),
  1588.                         "Exception while painting LaneStructures");
  1589.                 Path2D.Double path = new Path2D.Double();
  1590.                 boolean start = true;
  1591.                 for (OTSPoint3D point : line.getPoints())
  1592.                 {
  1593.                     if (start)
  1594.                     {
  1595.                         path.moveTo(point.x - loc.x, -(point.y - loc.y));
  1596.                         start = false;
  1597.                     }
  1598.                     else
  1599.                     {
  1600.                         path.lineTo(point.x - loc.x, -(point.y - loc.y));
  1601.                     }
  1602.                 }
  1603.                 graphics.draw(path);
  1604.                 // connection
  1605.                 Field sourceField = Try.assign(() -> RollingLaneStructureRecord.class.getDeclaredField("source"),
  1606.                         "Exception while painting LaneStructure");
  1607.                 sourceField.setAccessible(true);
  1608.                 LaneStructureRecord src =
  1609.                         Try.assign(() -> (LaneStructureRecord) sourceField.get(lsr), "Exception while painting LaneStructure");
  1610.                 if (src != null)
  1611.                 {
  1612.                     Field sourceLinkField = Try.assign(() -> RollingLaneStructureRecord.class.getDeclaredField("sourceLink"),
  1613.                             "Exception while painting LaneStructure");
  1614.                     sourceLinkField.setAccessible(true);
  1615.                     RecordLink link =
  1616.                             (RecordLink) Try.assign(() -> sourceLinkField.get(lsr), "Exception while painting LaneStructure");
  1617.                     float f1 = link.equals(RecordLink.DOWN) ? 0.9f : link.equals(RecordLink.UP) ? 0.1f : 0.5f;
  1618.                     float f2 = link.equals(RecordLink.DOWN) ? 0.0f : link.equals(RecordLink.UP) ? 1.0f : 0.5f;
  1619.                     f1 = src.getDirection().isPlus() ? f1 : 1.0f - f1;
  1620.                     f2 = lsr.getDirection().isPlus() ? f2 : 1.0f - f2;
  1621.                     float f3 = f1;
  1622.                     float f4 = f2;
  1623.                     DirectedPoint p1 = Try.assign(() -> src.getLane().getCenterLine().getLocationFraction(f3),
  1624.                             "Exception while painting LaneStructure");
  1625.                     DirectedPoint p2 = Try.assign(() -> line.getLocationFraction(f4), "Exception while painting LaneStructure");
  1626.                     path = new Path2D.Double();
  1627.                     path.moveTo(p1.x - loc.x, -(p1.y - loc.y));
  1628.                     path.lineTo(p2.x - loc.x, -(p2.y - loc.y));
  1629.                     graphics.setStroke(new BasicStroke(0.15f, BasicStroke.CAP_SQUARE, BasicStroke.JOIN_MITER, 10f,
  1630.                             new float[] { .3f, 1.2f }, 0f));
  1631.                     graphics.setColor(Color.DARK_GRAY);
  1632.                     graphics.draw(path);
  1633.                 }
  1634.                 // left/right
  1635.                 paintLateralConnection(lsr, lsr.getLeft(), Color.RED, graphics, loc);
  1636.                 paintLateralConnection(lsr, lsr.getRight(), Color.BLUE, graphics, loc);
  1637.                 // recursion to depending records
  1638.                 Field dependentField = Try.assign(() -> RollingLaneStructureRecord.class.getDeclaredField("dependentRecords"),
  1639.                         "Exception while painting LaneStructure");
  1640.                 dependentField.setAccessible(true);
  1641.                 Set<LaneStructureRecord> dependables = (Set<LaneStructureRecord>) Try.assign(() -> dependentField.get(lsr),
  1642.                         "Exception while painting LaneStructure");
  1643.                 if (dependables != null)
  1644.                 {
  1645.                     for (LaneStructureRecord dependable : new LinkedHashSet<>(dependables)) // concurrency
  1646.                     {
  1647.                         paintRecord(dependable, graphics);
  1648.                     }
  1649.                 }
  1650.             }

  1652.             /**
  1653.              * Paint the connection to a lateral record.
  1654.              * @param main LaneStructureRecord; main record
  1655.              * @param adj LaneStructureRecord; adjacent record, can be {@code null}
  1656.              * @param color Color; color
  1657.              * @param graphics Graphics2D; graphics
  1658.              * @param loc DirectedPoint; location
  1659.              */
  1660.             private void paintLateralConnection(final LaneStructureRecord main, final LaneStructureRecord adj,
  1661.                     final Color color, final Graphics2D graphics, final DirectedPoint loc)
  1662.             {
  1663.                 if (adj == null)
  1664.                 {
  1665.                     return;
  1666.                 }
  1667.                 float f1 = main.getDirection().isPlus() ? 0.45f : 0.55f;
  1668.                 float f2 = adj.getDirection().isPlus() ? 0.55f : 0.45f;
  1669.                 DirectedPoint p1 = Try.assign(() -> main.getLane().getCenterLine().getLocationFraction(f1),
  1670.                         "Exception while painting LaneStructure");
  1671.                 DirectedPoint p2 = Try.assign(() -> adj.getLane().getCenterLine().getLocationFraction(f2),
  1672.                         "Exception while painting LaneStructure");
  1673.                 Path2D.Double path = new Path2D.Double();
  1674.                 path.moveTo(p1.x - loc.x, -(p1.y - loc.y));
  1675.                 path.lineTo(p2.x - loc.x, -(p2.y - loc.y));
  1676.                 graphics.setStroke(new BasicStroke(0.05f, BasicStroke.CAP_SQUARE, BasicStroke.JOIN_MITER, 10f,
  1677.                         new float[] { .15f, 0.6f }, 0f));
  1678.                 graphics.setColor(color);
  1679.                 graphics.draw(path);
  1680.             }
  1681.         }
  1682.         Try.execute(() -> new LaneStructureAnimation(new LaneStructureLocatable()), "Could not create animation.");
  1683.     }

  1685. }
Created with JaCoCo 0.8.2.201808211720