1 package org.opentrafficsim.road.gtu.lane.perception;
2
3 import java.util.HashSet;
4 import java.util.Set;
5
6 import org.djunits.value.vdouble.scalar.Length;
7 import org.opentrafficsim.base.parameters.ParameterException;
8 import org.opentrafficsim.core.gtu.GTU;
9 import org.opentrafficsim.core.gtu.GTUException;
10 import org.opentrafficsim.core.gtu.RelativePosition;
11 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
12 import org.opentrafficsim.road.gtu.lane.perception.categories.HeadwayGtuType;
13 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
14
15 /**
16 * Iterable to find downstream GTU's.<br>
17 * <br>
18 * The behavior of this search is slightly altered using {@code boolean ignoreIfUpstream}. This is to deal with the following
19 * situations in case a GTU with it's rear upstream of the considered lane is found:<br>
20 * <br>
21 * Following downstream GTUs ({@code ignoreIfUpstream = true})
22 * <ol>
23 * <li>From the same direction (or not a merge): the GTU can be ignored as it is also found on the upstream lane.</li>
24 * <li>From the other direction of a merge: the GTU can be ignored as it is followed through considering the merge conflict.
25 * Note that we cannot follow the GTU in a regular fashion. If the rear of the GTU is upstream of the conflict, the subject GTU
26 * can move up to the conflict without hitting the GTU from the other direction. Considering the GTU through the conflict deals
27 * with this, and the GTU can be ignored for regular following of downstream GTUs.</li>
28 * </ol>
29 * <br>
30 * GTUs downstream of a conflict ({@code ignoreIfUpstream = false})
31 * <ol>
32 * <li>From the same direction: the GTU is considered both through the conflict and as a regular downstream GTU.</li>
33 * <li>From the other direction of a merge: the GTU needs to be considered.</li>
34 * </ol>
35 * <p>
36 * Copyright (c) 2013-2018 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
37 * BSD-style license. See <a href="http://opentrafficsim.org/node/13">OpenTrafficSim License</a>.
38 * <p>
39 * @version $Revision$, $LastChangedDate$, by $Author$, initial version 19 feb. 2018 <br>
40 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
41 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
42 * @author <a href="http://www.transport.citg.tudelft.nl">Wouter Schakel</a>
43 */
44 public class DownstreamNeighborsIterable extends AbstractPerceptionIterable<HeadwayGTU, LaneBasedGTU, Integer>
45 {
46
47 /** Margin in case of a left lane. */
48 private static final Length LEFT = Length.createSI(-0.000001);
49
50 /** Margin in case of a right lane. */
51 private static final Length RIGHT = Length.createSI(0.000001);
52
53 /** Headway GTU type that should be used. */
54 private final HeadwayGtuType headwayGtuType;
55
56 /** Added GTU's so far. */
57 private final Set<String> ids = new HashSet<>();
58
59 /**
60 * Margin used for neighbor search in some cases to prevent possible deadlock. This does not affect calculated distances to
61 * neighbors, but only whether they are considered a leader or follower.
62 */
63 private final Length margin;
64
65 /** Ignore downstream GTU's if their rear is upstream of the lane start. Note that equal GTU id's are always ignored. */
66 private final boolean ignoreIfUpstream;
67
68 /**
69 * Constructor.
70 * @param perceivingGtu LaneBasedGTU; perceiving GTU
71 * @param root LaneRecord; root record
72 * @param initialPosition Length; position on the root record
73 * @param maxDistance Length; maximum distance to search
74 * @param relativePosition RelativePosition; position to which distance are calculated by subclasses
75 * @param headwayGtuType HeadwayGtuType; type of HeadwayGTU to return
76 * @param gtu GTU; the GTU, may be {@code null}
77 * @param lane RelativeLane; relative lane (used for a left/right distinction to prevent dead-locks)
78 * @param ignoreIfUpstream boolean; whether to ignore GTU that are partially upstream of a record
79 */
80 public DownstreamNeighborsIterable(final LaneBasedGTU perceivingGtu, final LaneRecord<?> root, final Length initialPosition,
81 final Length maxDistance, final RelativePosition relativePosition, final HeadwayGtuType headwayGtuType,
82 final GTU gtu, final RelativeLane lane, final boolean ignoreIfUpstream)
83 {
84 super(perceivingGtu, root, initialPosition, true, maxDistance, relativePosition, null);
85 this.headwayGtuType = headwayGtuType;
86 this.margin = lane.getLateralDirectionality().isLeft() ? LEFT : RIGHT;
87 if (gtu != null)
88 {
89 this.ids.add(gtu.getId());
90 }
91 this.ignoreIfUpstream = ignoreIfUpstream;
92 }
93
94 /** {@inheritDoc} */
95 @Override
96 protected Entry getNext(final LaneRecord<?> record, final Length position, final Integer counter) throws GTUException
97 {
98 int n;
99 LaneBasedGTU next;
100 Length pos;
101 boolean plus = record.getDirection().isPlus();
102 if (counter == null)
103 {
104 Length searchPos = (plus ? position.plus(this.margin) : position.minus(this.margin));
105 next = record.getLane().getGtuAhead(searchPos, record.getDirection(), RelativePosition.FRONT,
106 record.getLane().getParentLink().getSimulator().getSimulatorTime());
107 if (next == null)
108 {
109 return null;
110 }
111 n = record.getLane().indexOfGtu(next);
112 pos = next.position(record.getLane(), next.getRear());
113
114 if (this.ids.contains(next.getId()))
115 {
116 // rear still on previous lane; it is found there, get next gtu
117 pos = plus ? pos.plus(next.getLength()) : pos.minus(next.getLength());
118 return getNext(record, pos, n);
119 }
120 if (this.ignoreIfUpstream)
121 {
122 if (plus ? pos.si < 0.0 : pos.si > record.getLane().getLength().si)
123 {
124 pos = plus ? pos.plus(next.getLength()) : pos.minus(next.getLength());
125 return getNext(record, pos, n);
126 }
127 }
128 }
129 else
130 {
131 n = plus ? counter + 1 : counter - 1;
132 if (n < 0 || n >= record.getLane().numberOfGtus())
133 {
134 return null;
135 }
136 next = record.getLane().getGtu(n);
137 pos = next.position(record.getLane(), next.getRear());
138 }
139 return new Entry(next, n, pos);
140 }
141
142 /** {@inheritDoc} */
143 @Override
144 protected Length getDistance(final LaneBasedGTU object, final LaneRecord<?> record, final Length position)
145 {
146 return record.getDistanceToPosition(position).minus(getDx());
147 }
148
149 /** {@inheritDoc} */
150 @Override
151 public HeadwayGTU perceive(final LaneBasedGTU perceivingGtu, final LaneBasedGTU object, final Length distance)
152 throws GTUException, ParameterException
153 {
154 return this.headwayGtuType.createHeadwayGtu(perceivingGtu, object, distance, true);
155 }
156
157 }