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