1 package org.opentrafficsim.road.gtu.lane.perception.categories;
2
3 import java.util.ArrayList;
4 import java.util.List;
5 import java.util.SortedSet;
6
7 import org.djunits.value.vdouble.scalar.Length;
8 import org.djunits.value.vdouble.scalar.Speed;
9 import org.djutils.exceptions.Throw;
10 import org.djutils.exceptions.Try;
11 import org.opentrafficsim.base.parameters.ParameterException;
12 import org.opentrafficsim.base.parameters.ParameterTypeLength;
13 import org.opentrafficsim.base.parameters.ParameterTypes;
14 import org.opentrafficsim.core.geometry.OTSGeometryException;
15 import org.opentrafficsim.core.geometry.OTSLine3D;
16 import org.opentrafficsim.core.gtu.GTUException;
17 import org.opentrafficsim.core.gtu.RelativePosition;
18 import org.opentrafficsim.core.network.LateralDirectionality;
19 import org.opentrafficsim.core.network.LongitudinalDirectionality;
20 import org.opentrafficsim.core.network.NetworkException;
21 import org.opentrafficsim.core.network.route.Route;
22 import org.opentrafficsim.road.gtu.lane.LaneBasedGTU;
23 import org.opentrafficsim.road.gtu.lane.perception.LaneBasedObjectIterable;
24 import org.opentrafficsim.road.gtu.lane.perception.LanePerception;
25 import org.opentrafficsim.road.gtu.lane.perception.LaneStructure.Entry;
26 import org.opentrafficsim.road.gtu.lane.perception.LaneStructureRecord;
27 import org.opentrafficsim.road.gtu.lane.perception.PerceptionCollectable;
28 import org.opentrafficsim.road.gtu.lane.perception.RelativeLane;
29 import org.opentrafficsim.road.gtu.lane.perception.categories.neighbors.HeadwayGtuType;
30 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayConflict;
31 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayGTU;
32 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayStopLine;
33 import org.opentrafficsim.road.gtu.lane.perception.headway.HeadwayTrafficLight;
34 import org.opentrafficsim.road.network.lane.CrossSectionLink;
35 import org.opentrafficsim.road.network.lane.Lane;
36 import org.opentrafficsim.road.network.lane.conflict.Conflict;
37 import org.opentrafficsim.road.network.lane.conflict.Conflict.ConflictEnd;
38 import org.opentrafficsim.road.network.lane.conflict.ConflictPriority;
39 import org.opentrafficsim.road.network.lane.conflict.ConflictRule;
40 import org.opentrafficsim.road.network.lane.conflict.ConflictType;
41 import org.opentrafficsim.road.network.lane.object.LaneBasedObject;
42 import org.opentrafficsim.road.network.lane.object.trafficlight.TrafficLight;
43
44
45
46
47
48
49
50
51
52
53
54
55 public class DirectIntersectionPerception extends LaneBasedAbstractPerceptionCategory implements IntersectionPerception
56 {
57
58
59 private static final long serialVersionUID = 20160811L;
60
61
62 protected static final ParameterTypeLength LOOKAHEAD = ParameterTypes.LOOKAHEAD;
63
64
65 private static final Length MARGIN = Length.createSI(0.001);
66
67
68 private final HeadwayGtuType headwayGtuType;
69
70
71
72
73
74 public DirectIntersectionPerception(final LanePerception perception, final HeadwayGtuType headwayGtuType)
75 {
76 super(perception);
77 this.headwayGtuType = headwayGtuType;
78 }
79
80
81
82
83
84
85 private PerceptionCollectable<HeadwayTrafficLight, TrafficLight> computeTrafficLights(final RelativeLane lane)
86 {
87 try
88 {
89 Route route = getPerception().getGtu().getStrategicalPlanner().getRoute();
90 LaneStructureRecord record = getPerception().getLaneStructure().getFirstRecord(lane);
91 Length pos = record.getStartDistance().neg();
92 pos = record.getDirection().isPlus() ? pos.plus(getGtu().getFront().getDx())
93 : pos.minus(getGtu().getFront().getDx());
94 return new LaneBasedObjectIterable<HeadwayTrafficLight, TrafficLight>(getGtu(), TrafficLight.class, record,
95 Length.max(Length.ZERO, pos), getGtu().getParameters().getParameter(LOOKAHEAD), getGtu().getFront(), route)
96 {
97
98 @Override
99 public HeadwayTrafficLight perceive(final LaneBasedGTU perceivingGtu, final TrafficLight trafficLight,
100 final Length distance)
101 {
102 try
103 {
104 return new HeadwayTrafficLight(trafficLight, distance);
105 }
106 catch (GTUException exception)
107 {
108 throw new RuntimeException(exception);
109 }
110 }
111 };
112 }
113 catch (GTUException | ParameterException exception)
114 {
115 throw new RuntimeException("Unexpected exception while computing traffic lights.", exception);
116 }
117
118 }
119
120
121
122
123
124
125 private PerceptionCollectable<HeadwayConflict, Conflict> computeConflicts(final RelativeLane lane)
126 {
127 try
128 {
129 Route route = getPerception().getGtu().getStrategicalPlanner().getRoute();
130 LaneStructureRecord record = getPerception().getLaneStructure().getFirstRecord(lane);
131 Length pos = record.getStartDistance().neg().plus(getGtu().getRear().getDx());
132 while (pos.lt0() && !record.getPrev().isEmpty())
133 {
134 pos = pos.plus(record.getLength());
135 record = record.getPrev().get(0);
136 }
137
138 List<LaneBasedObject> laneObjs;
139 if (record.isDownstreamBranch())
140 {
141 if (record.getDirection().isPlus())
142 {
143 laneObjs = record.getLane().getLaneBasedObjects(Length.max(Length.ZERO, pos), record.getLane().getLength());
144 }
145 else
146 {
147 laneObjs = record.getLane().getLaneBasedObjects(Length.ZERO, pos);
148 }
149 }
150 else
151 {
152 laneObjs = new ArrayList<>();
153 }
154
155 for (LaneBasedObject object : laneObjs)
156 {
157 if (object instanceof ConflictEnd)
158 {
159 Conflict c = ((ConflictEnd) object).getConflict();
160 Length cPos = record.getDirection().isPlus() ? c.getLongitudinalPosition().minus(MARGIN)
161 : c.getLongitudinalPosition().plus(MARGIN);
162 pos = record.getDirection().isPlus() ? Length.min(pos, cPos) : Length.max(pos, cPos);
163 }
164 }
165 return new LaneBasedObjectIterable<HeadwayConflict, Conflict>(getGtu(), Conflict.class, record,
166 Length.max(MARGIN.neg(), pos), getGtu().getParameters().getParameter(LOOKAHEAD), getGtu().getFront(), route)
167 {
168
169 @SuppressWarnings("synthetic-access")
170 @Override
171 public HeadwayConflict perceive(final LaneBasedGTU perceivingGtu, final Conflict conflict,
172 final Length distance)
173 {
174 Conflict otherConflict = conflict.getOtherConflict();
175 ConflictType conflictType = conflict.getConflictType();
176 ConflictPriority conflictPriority = conflict.conflictPriority();
177 Class<? extends ConflictRule> conflictRuleType = conflict.getConflictRule().getClass();
178 String id = conflict.getId();
179 Length length = conflict.getLength();
180 Length conflictingLength = otherConflict.getLength();
181 CrossSectionLink conflictingLink = otherConflict.getLane().getParentLink();
182
183
184 Length lookAhead =
185 Try.assign(() -> getGtu().getParameters().getParameter(LOOKAHEAD), "Parameter not present.");
186 Length conflictingVisibility = lookAhead;
187 Speed conflictingSpeedLimit;
188 try
189 {
190 conflictingSpeedLimit = otherConflict.getLane().getHighestSpeedLimit();
191 }
192 catch (NetworkException exception)
193 {
194 throw new RuntimeException("GTU type not available on conflicting lane.", exception);
195 }
196
197 LongitudinalDirectionality otherDir = otherConflict.getDirection();
198 Throw.when(otherDir.isBoth(), UnsupportedOperationException.class,
199 "Conflicts on lanes with direction BOTH are not supported.");
200
201
202
203 HeadwayConflict headwayConflict;
204 try
205 {
206 PerceptionCollectable<HeadwayGTU, LaneBasedGTU> upstreamConflictingGTUs = otherConflict.getUpstreamGtus(
207 getGtu(), DirectIntersectionPerception.this.headwayGtuType, conflictingVisibility);
208 PerceptionCollectable<HeadwayGTU, LaneBasedGTU> downstreamConflictingGTUs =
209 otherConflict.getDownstreamGtus(getGtu(), DirectIntersectionPerception.this.headwayGtuType,
210 conflictingVisibility);
211
212 HeadwayStopLine stopLine = new HeadwayStopLine("stopLineId", Length.ZERO);
213 HeadwayStopLine conflictingStopLine = new HeadwayStopLine("conflictingStopLineId", Length.ZERO);
214
215 Lane thisLane = conflict.getLane();
216 Lane otherLane = otherConflict.getLane();
217 Length pos1a = conflict.getLongitudinalPosition();
218 Length pos2a = otherConflict.getLongitudinalPosition();
219 Length pos1b = Length.min(pos1a.plus(conflict.getLength()), thisLane.getLength());
220 Length pos2b = Length.min(pos2a.plus(otherConflict.getLength()), otherLane.getLength());
221 OTSLine3D line1 = thisLane.getCenterLine();
222 OTSLine3D line2 = otherLane.getCenterLine();
223 double dStart = line1.getLocation(pos1a).distance(line2.getLocation(pos2a));
224 double dEnd = line1.getLocation(pos1b).distance(line2.getLocation(pos2b));
225 Length startWidth =
226 Length.createSI(dStart + .5 * thisLane.getWidth(pos1a).si + .5 * otherLane.getWidth(pos2a).si);
227 Length endWidth =
228 Length.createSI(dEnd + .5 * thisLane.getWidth(pos1b).si + .5 * otherLane.getWidth(pos2b).si);
229
230 headwayConflict = new HeadwayConflict(conflictType, conflictPriority, conflictRuleType, id, distance,
231 length, conflictingLength, upstreamConflictingGTUs, downstreamConflictingGTUs,
232 conflictingVisibility, conflictingSpeedLimit, conflictingLink,
233 HeadwayConflict.Width.linear(startWidth, endWidth), stopLine, conflictingStopLine);
234 }
235 catch (GTUException | OTSGeometryException exception)
236 {
237 throw new RuntimeException("Could not create headway objects.", exception);
238 }
239
240
241
242
243
244 return headwayConflict;
245 }
246 };
247 }
248 catch (GTUException | ParameterException exception)
249 {
250 throw new RuntimeException("Unexpected exception while computing conflicts.", exception);
251 }
252 }
253
254
255
256
257
258
259 private boolean computeConflictAlongside(final LateralDirectionality lat)
260 {
261 try
262 {
263 RelativeLane lane = new RelativeLane(lat, 1);
264 if (getPerception().getLaneStructure().getExtendedCrossSection().contains(lane))
265 {
266 SortedSet<Entry<Conflict>> conflictEntries = getPerception().getLaneStructure().getUpstreamObjects(lane,
267 Conflict.class, getGtu(), RelativePosition.FRONT);
268 if (!conflictEntries.isEmpty())
269 {
270 Entry<Conflict> entry = conflictEntries.first();
271 return entry.getDistance().si < entry.getLaneBasedObject().getLength().si + getGtu().getLength().si;
272 }
273 }
274 return false;
275 }
276 catch (GTUException | ParameterException exception)
277 {
278 throw new RuntimeException("Unexpected exception while computing conflict alongside.", exception);
279 }
280 }
281
282
283 @Override
284 public final Iterable<HeadwayTrafficLight> getTrafficLights(final RelativeLane lane)
285 {
286 return computeIfAbsent("trafficLights", () -> computeTrafficLights(lane), lane);
287 }
288
289
290 @Override
291 public final PerceptionCollectable<HeadwayConflict, Conflict> getConflicts(final RelativeLane lane)
292 {
293 return computeIfAbsent("conflicts", () -> computeConflicts(lane), lane);
294 }
295
296
297 @Override
298 public final boolean isAlongsideConflictLeft()
299 {
300 return computeIfAbsent("alongside", () -> computeConflictAlongside(LateralDirectionality.LEFT),
301 LateralDirectionality.LEFT);
302 }
303
304
305 @Override
306 public final boolean isAlongsideConflictRight()
307 {
308 return computeIfAbsent("alongside", () -> computeConflictAlongside(LateralDirectionality.RIGHT),
309 LateralDirectionality.RIGHT);
310 }
311
312
313 @Override
314 public void updateAll() throws GTUException, NetworkException, ParameterException
315 {
316
317 }
318
319
320 @Override
321 public final String toString()
322 {
323 return "DirectIntersectionPerception";
324 }
325
326 }