1 package org.opentrafficsim.road.network.lane;
2
3 import static org.junit.jupiter.api.Assertions.assertEquals;
4 import static org.junit.jupiter.api.Assertions.assertFalse;
5 import static org.junit.jupiter.api.Assertions.assertTrue;
6
7 import java.util.ArrayList;
8 import java.util.LinkedHashSet;
9 import java.util.List;
10 import java.util.Map;
11 import java.util.Set;
12
13 import org.djunits.unit.DirectionUnit;
14 import org.djunits.unit.LengthUnit;
15 import org.djunits.unit.SpeedUnit;
16 import org.djunits.value.vdouble.scalar.Direction;
17 import org.djunits.value.vdouble.scalar.Duration;
18 import org.djunits.value.vdouble.scalar.Length;
19 import org.djunits.value.vdouble.scalar.Speed;
20 import org.djutils.draw.Export;
21 import org.djutils.draw.line.Polygon2d;
22 import org.djutils.draw.point.Point2d;
23 import org.djutils.event.Event;
24 import org.djutils.event.EventListener;
25 import org.junit.jupiter.api.Test;
26 import org.mockito.Mockito;
27 import org.opentrafficsim.base.geometry.OtsLine2d;
28 import org.opentrafficsim.core.definitions.DefaultsNl;
29 import org.opentrafficsim.core.dsol.OtsReplication;
30 import org.opentrafficsim.core.dsol.OtsSimulatorInterface;
31 import org.opentrafficsim.core.network.LinkType;
32 import org.opentrafficsim.core.network.NetworkException;
33 import org.opentrafficsim.core.network.Node;
34 import org.opentrafficsim.core.perception.HistoryManagerDevs;
35 import org.opentrafficsim.road.definitions.DefaultsRoadNl;
36 import org.opentrafficsim.road.mock.MockDevsSimulator;
37 import org.opentrafficsim.road.network.CrossSectionLink;
38 import org.opentrafficsim.road.network.Lane;
39 import org.opentrafficsim.road.network.LaneGeometryUtil;
40 import org.opentrafficsim.road.network.LaneKeepingPolicy;
41 import org.opentrafficsim.road.network.LaneType;
42 import org.opentrafficsim.road.network.RoadNetwork;
43 import org.opentrafficsim.road.network.conflict.Conflict;
44 import org.opentrafficsim.road.network.conflict.ConflictType;
45 import org.opentrafficsim.road.network.conflict.DefaultConflictRule;
46 import org.opentrafficsim.road.network.speed.LaneSpeedLimits;
47
48
49
50
51
52
53
54
55
56 public final class ConflictTest implements EventListener
57 {
58
59 private static final boolean VERBOSE = false;
60
61
62 private List<Event> collectedEvents = new ArrayList<>();
63
64
65 private ConflictTest()
66 {
67
68 }
69
70
71
72
73
74 @Test
75 public void testConstructor() throws NetworkException
76 {
77 OtsSimulatorInterface simulator = MockDevsSimulator.createMock();
78 OtsReplication replication = Mockito.mock(OtsReplication.class);
79 HistoryManagerDevs hmd = Mockito.mock(HistoryManagerDevs.class);
80 Mockito.when(hmd.now()).thenReturn(Duration.ZERO);
81 Mockito.when(replication.getHistoryManager(simulator)).thenReturn(hmd);
82 Mockito.when(simulator.getReplication()).thenReturn(replication);
83 Mockito.when(simulator.getSimulatorTime()).thenReturn(Duration.ZERO);
84 RoadNetwork network = new RoadNetwork("Network for conflict test", simulator);
85 LinkType linkType = DefaultsNl.ROAD;
86 LaneType laneType = DefaultsRoadNl.ONE_WAY_LANE;
87 Point2d pointAFrom = new Point2d(0, 0);
88 Node nodeAFrom = new Node(network, "A from", pointAFrom, Direction.ZERO);
89 Point2d pointATo = new Point2d(100, 0);
90 Node nodeATo = new Node(network, "A to", pointATo, Direction.ZERO);
91 CrossSectionLink linkA = new CrossSectionLink(network, "Link A", nodeAFrom, nodeATo, linkType,
92 new OtsLine2d(pointAFrom, pointATo), null, LaneKeepingPolicy.KEEPRIGHT);
93 Lane laneA = LaneGeometryUtil.createStraightLane(linkA, "lane A", Length.ZERO, new Length(2, LengthUnit.METER),
94 laneType, new LaneSpeedLimits(Map.of(DefaultsNl.VEHICLE, new Speed(50, SpeedUnit.KM_PER_HOUR))));
95 laneA.addListener(this, Lane.OBJECT_ADD_EVENT);
96
97 Point2d pointBFrom = new Point2d(30, -15);
98 Point2d pointBTo = new Point2d(60, 60);
99 Direction bDirection =
100 new Direction(Math.atan2(pointBTo.y - pointBFrom.y, pointBTo.x - pointBFrom.x), DirectionUnit.EAST_RADIAN);
101 Node nodeBFrom = new Node(network, "B from", pointBFrom, bDirection);
102 Node nodeBTo = new Node(network, "B to", pointBTo, bDirection);
103 CrossSectionLink linkB = new CrossSectionLink(network, "Link B", nodeBFrom, nodeBTo, linkType,
104 new OtsLine2d(pointBFrom, pointBTo), null, LaneKeepingPolicy.KEEPRIGHT);
105 Lane laneB = LaneGeometryUtil.createStraightLane(linkB, "lane B", Length.ZERO, new Length(4, LengthUnit.METER),
106 laneType, new LaneSpeedLimits(Map.of(DefaultsNl.VEHICLE, new Speed(50, SpeedUnit.KM_PER_HOUR))));
107 laneB.addListener(this, Lane.OBJECT_ADD_EVENT);
108
109 if (VERBOSE)
110 {
111 System.out.print(Export.toPlot(laneA.getAbsoluteContour()));
112 System.out.print(Export.toPlot(laneB.getAbsoluteContour()));
113 System.out.println("c0,1,0");
114 System.out.print(Export.toPlot(laneA.getCenterLine()));
115 System.out.print(Export.toPlot(laneB.getCenterLine()));
116 System.out.println("c1,0,0");
117 }
118
119
120
121 Point2d conflictStart = null;
122 double closestDistance = Double.MAX_VALUE;
123 Point2d conflictEnd = null;
124 double furthestDistance = 0.0;
125 for (Point2d intersection : intersections(laneA.getAbsoluteContour(), laneB.getAbsoluteContour()))
126 {
127 double distance = pointAFrom.distance(intersection);
128 if (distance < closestDistance)
129 {
130 conflictStart = intersection;
131 closestDistance = distance;
132 }
133 if (distance > furthestDistance)
134 {
135 conflictEnd = intersection;
136 furthestDistance = distance;
137 }
138 }
139
140
141
142
143 Polygon2d geometry1 =
144 new Polygon2d(conflictStart, conflictEnd, new Point2d(conflictStart.x, conflictEnd.y), conflictStart);
145
146 Polygon2d geometry2 = new Polygon2d(conflictStart,
147 new Point2d(conflictStart.x + laneB.getWidth(0).si * Math.sin(bDirection.si),
148 conflictStart.y - laneB.getWidth(0).si * Math.cos(bDirection.si)),
149 conflictEnd, new Point2d(conflictEnd.x - laneB.getWidth(0).si * Math.sin(bDirection.si),
150 conflictEnd.y + laneB.getWidth(0).si * Math.cos(bDirection.si)),
151 conflictStart);
152
153 Length conflictBStart =
154 new Length(pointBFrom.distance(new Point2d(conflictStart.x + laneB.getWidth(0).si / 2 * Math.sin(bDirection.si),
155 conflictStart.y - laneB.getWidth(0).si / 2 * Math.cos(bDirection.si))), LengthUnit.SI);
156
157 Length conflictBLength = new Length(
158 laneA.getWidth(0).si / Math.sin(bDirection.si) + laneB.getWidth(0).si / Math.tan(bDirection.si), LengthUnit.SI);
159
160 if (VERBOSE)
161 {
162 System.out.print(Export.toPlot(geometry1));
163 System.out.print(Export.toPlot(geometry2));
164 System.out.println("#angle B: " + bDirection.toString(DirectionUnit.EAST_DEGREE));
165 System.out.println("#conflict B start: " + conflictBStart);
166 System.out.println("#conflict B length: " + conflictBLength);
167 System.out.println("c0,0,1");
168 System.out.println(String.format("M%.3f,%.3f <%f l%f,0", pointBFrom.x, pointBFrom.y, Math.toDegrees(bDirection.si),
169 conflictBStart.si));
170 System.out.println(String.format("c0,0,0 l%f,0", conflictBLength.si));
171 }
172
173 assertEquals(0, this.collectedEvents.size(), "not events received yet");
174
175
176 Conflict.generateConflictPair(ConflictType.CROSSING, new DefaultConflictRule(), false, laneA,
177 new Length(conflictStart.x, LengthUnit.SI), new Length(conflictEnd.x - conflictStart.x, LengthUnit.SI),
178 geometry1, laneB, conflictBStart, conflictBLength, geometry2, simulator);
179
180
181 assertEquals(1, laneA.getLaneBasedObjects().size(), "one conflict on lane A");
182 assertEquals(1, laneB.getLaneBasedObjects().size(), "one conflict on lane B");
183
184 Conflict conflictA = (Conflict) laneA.getLaneBasedObjects().get(0);
185 Conflict conflictB = (Conflict) laneB.getLaneBasedObjects().get(0);
186 if (VERBOSE)
187 {
188 System.out.println("Conflict A: " + conflictA);
189 System.out.println("Conflict B: " + conflictB);
190 }
191
192 assertEquals(conflictA, conflictB.getOtherConflict(), "the conflicts are each others counter part");
193 assertEquals(conflictB, conflictA.getOtherConflict(), "the conflicts are each others counter part");
194 assertEquals(new Length(conflictStart.x, LengthUnit.SI), conflictA.getLongitudinalPosition(), "longitudinal position");
195 assertEquals(conflictBStart, conflictB.getLongitudinalPosition(), "longitudinal position");
196 assertEquals(new Length(conflictEnd.x - conflictStart.x, LengthUnit.SI), conflictA.getLength(), "length");
197 assertEquals(conflictBLength, conflictB.getLength(), "length");
198 assertEquals(geometry1, conflictA.getAbsoluteContour(), "contour");
199 assertEquals(geometry2, conflictB.getAbsoluteContour(), "contour");
200 assertTrue(conflictA.getConflictRule() instanceof DefaultConflictRule, "conflict rule");
201 assertTrue(conflictB.getConflictRule() instanceof DefaultConflictRule, "conflict rule");
202 assertFalse(conflictA.isPermitted(), "conflict A is not permitted");
203 assertFalse(conflictB.isPermitted(), "conflict B is not permitted");
204 assertEquals(2, this.collectedEvents.size(), "construction of two conflicts has generated two events");
205
206
207 }
208
209
210
211
212
213
214
215 public Set<Point2d> intersections(final Polygon2d a, final Polygon2d b)
216 {
217
218 Set<Point2d> result = new LinkedHashSet<>();
219 Point2d prevA = null;
220 for (Point2d nextA : a.getPointList())
221 {
222 if (null != prevA)
223 {
224 Point2d prevB = null;
225 for (Point2d nextB : b.getPointList())
226 {
227 if (null != prevB)
228 {
229 Point2d intersection = Point2d.intersectionOfLineSegments(prevA, nextA, prevB, nextB);
230 if (null != intersection)
231 {
232 result.add(intersection);
233 }
234 }
235 prevB = nextB;
236 }
237 }
238 prevA = nextA;
239 }
240 return result;
241 }
242
243 @Override
244 public void notify(final Event event)
245 {
246
247 this.collectedEvents.add(event);
248 }
249
250 }