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