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.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.Length;
18 import org.djunits.value.vdouble.scalar.Speed;
19 import org.djunits.value.vdouble.scalar.Time;
20 import org.djutils.event.EventInterface;
21 import org.djutils.event.EventListenerInterface;
22 import org.junit.Test;
23 import org.mockito.Mockito;
24 import org.opentrafficsim.core.dsol.OTSReplication;
25 import org.opentrafficsim.core.dsol.OTSSimulatorInterface;
26 import org.opentrafficsim.core.geometry.OTSGeometryException;
27 import org.opentrafficsim.core.geometry.OTSLine3D;
28 import org.opentrafficsim.core.geometry.OTSPoint3D;
29 import org.opentrafficsim.core.gtu.GTUDirectionality;
30 import org.opentrafficsim.core.gtu.GTUType;
31 import org.opentrafficsim.core.network.LinkType;
32 import org.opentrafficsim.core.network.NetworkException;
33 import org.opentrafficsim.core.perception.HistoryManagerDEVS;
34 import org.opentrafficsim.road.mock.MockDEVSSimulator;
35 import org.opentrafficsim.road.network.OTSRoadNetwork;
36 import org.opentrafficsim.road.network.lane.changing.LaneKeepingPolicy;
37 import org.opentrafficsim.road.network.lane.conflict.Conflict;
38 import org.opentrafficsim.road.network.lane.conflict.ConflictType;
39 import org.opentrafficsim.road.network.lane.conflict.DefaultConflictRule;
40
41
42
43
44
45
46
47
48
49 public class ConflictTest implements EventListenerInterface
50 {
51
52 private static final long serialVersionUID = 20200708L;
53
54
55 private List<EventInterface> collectedEvents = new ArrayList<>();
56
57
58
59
60
61
62 @Test
63 public void testConstructor() throws NetworkException, OTSGeometryException
64 {
65 OTSSimulatorInterface simulator = MockDEVSSimulator.createMock();
66 OTSReplication replication = Mockito.mock(OTSReplication.class);
67 HistoryManagerDEVS hmd = Mockito.mock(HistoryManagerDEVS.class);
68 Mockito.when(hmd.now()).thenReturn(Time.ZERO);
69 Mockito.when(replication.getHistoryManager(simulator)).thenReturn(hmd);
70 Mockito.when(simulator.getReplication()).thenReturn(replication);
71 Mockito.when(simulator.getSimulatorTime()).thenReturn(Time.ZERO);
72 OTSRoadNetwork network = new OTSRoadNetwork("Network for conflict test", true, simulator);
73 LinkType linkType = network.getLinkType(LinkType.DEFAULTS.ROAD);
74 LaneType laneType = network.getLaneType(LaneType.DEFAULTS.ONE_WAY_LANE);
75 OTSPoint3D pointAFrom = new OTSPoint3D(0, 0, 0);
76 OTSRoadNode nodeAFrom = new OTSRoadNode(network, "A from", pointAFrom, Direction.ZERO);
77 OTSPoint3D pointATo = new OTSPoint3D(100, 0, 0);
78 OTSRoadNode nodeATo = new OTSRoadNode(network, "A to", pointATo, Direction.ZERO);
79 CrossSectionLink linkA = new CrossSectionLink(network, "Link A", nodeAFrom, nodeATo, linkType,
80 new OTSLine3D(pointAFrom, pointATo), LaneKeepingPolicy.KEEPRIGHT);
81 Lane laneA = new Lane(linkA, "lane A", Length.ZERO, new Length(2, LengthUnit.METER), laneType,
82 new Speed(50, SpeedUnit.KM_PER_HOUR));
83 laneA.addListener(this, Lane.OBJECT_ADD_EVENT);
84
85 OTSPoint3D pointBFrom = new OTSPoint3D(30, -15, 0);
86 OTSPoint3D pointBTo = new OTSPoint3D(60, 60, 0);
87 Direction bDirection =
88 new Direction(Math.atan2(pointBTo.y - pointBFrom.y, pointBTo.x - pointBFrom.x), DirectionUnit.EAST_RADIAN);
89 OTSRoadNode nodeBFrom = new OTSRoadNode(network, "B from", pointBFrom, bDirection);
90 OTSRoadNode nodeBTo = new OTSRoadNode(network, "B to", pointBTo, bDirection);
91 CrossSectionLink linkB = new CrossSectionLink(network, "Link B", nodeBFrom, nodeBTo, linkType,
92 new OTSLine3D(pointBFrom, pointBTo), LaneKeepingPolicy.KEEPRIGHT);
93 Lane laneB = new Lane(linkB, "lane B", Length.ZERO, new Length(4, LengthUnit.METER), laneType,
94 new Speed(50, SpeedUnit.KM_PER_HOUR));
95 laneB.addListener(this, Lane.OBJECT_ADD_EVENT);
96
97 System.out.print(laneA.getContour().toPlot());
98 System.out.print(laneB.getContour().toPlot());
99 System.out.println("c0,1,0");
100 System.out.print(laneA.getCenterLine().toPlot());
101 System.out.print(laneB.getCenterLine().toPlot());
102 System.out.println("c1,0,0");
103
104
105
106 OTSPoint3D conflictStart = null;
107 double closestDistance = Double.MAX_VALUE;
108 OTSPoint3D conflictEnd = null;
109 double furthestDistance = 0.0;
110 for (OTSPoint3D intersection : intersections(laneA.getContour(), laneB.getContour()))
111 {
112 double distance = pointAFrom.distanceSI(intersection);
113 if (distance < closestDistance)
114 {
115 conflictStart = intersection;
116 closestDistance = distance;
117 }
118 if (distance > furthestDistance)
119 {
120 conflictEnd = intersection;
121 furthestDistance = distance;
122 }
123 }
124
125
126
127
128 OTSLine3D geometry1 = new OTSLine3D(conflictStart, new OTSPoint3D(conflictEnd.x, conflictStart.y, 0), conflictEnd,
129 new OTSPoint3D(conflictStart.x, conflictEnd.y, 0), conflictStart);
130 System.out.print(geometry1.toPlot());
131 OTSLine3D geometry2 = new OTSLine3D(conflictStart,
132 new OTSPoint3D(conflictStart.x + laneB.getWidth(0).si * Math.sin(bDirection.si),
133 conflictStart.y - laneB.getWidth(0).si * Math.cos(bDirection.si), 0),
134 conflictEnd, new OTSPoint3D(conflictEnd.x - laneB.getWidth(0).si * Math.sin(bDirection.si),
135 conflictEnd.y + laneB.getWidth(0).si * Math.cos(bDirection.si), 0),
136 conflictStart);
137 System.out.print(geometry2.toPlot());
138
139 System.out.println("#angle B: " + bDirection.toString(DirectionUnit.EAST_DEGREE));
140 Length conflictBStart = new Length(
141 pointBFrom.distance(new OTSPoint3D(conflictStart.x + laneB.getWidth(0).si / 2 * Math.sin(bDirection.si),
142 conflictStart.y - laneB.getWidth(0).si / 2 * Math.cos(bDirection.si), 0)).si,
143 LengthUnit.SI);
144 System.out.println("#conflict B start: " + conflictBStart);
145 Length conflictBLength = new Length(
146 laneA.getWidth(0).si / Math.sin(bDirection.si) + laneB.getWidth(0).si / Math.tan(bDirection.si), LengthUnit.SI);
147 System.out.println("#conflict B length: " + conflictBLength);
148 System.out.println("c0,0,1");
149 System.out.println(String.format("M%.3f,%.3f <%f l%f,0", pointBFrom.x, pointBFrom.y, Math.toDegrees(bDirection.si),
150 conflictBStart.si));
151 System.out.println(String.format("c0,0,0 l%f,0", conflictBLength.si));
152
153 GTUType bicycles = network.getGtuType("BICYCLE");
154 GTUType cars = network.getGtuType("CAR");
155
156 assertEquals("not events received yet", 0, this.collectedEvents.size());
157
158
159 Conflict.generateConflictPair(ConflictType.CROSSING, new DefaultConflictRule(), false, laneA,
160 new Length(conflictStart.x, LengthUnit.SI), new Length(conflictEnd.x - conflictStart.x, LengthUnit.SI),
161 GTUDirectionality.DIR_PLUS, geometry1, bicycles, laneB, conflictBStart, conflictBLength,
162 GTUDirectionality.DIR_PLUS, geometry2, cars, 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("conflict A is about bicycles", bicycles, conflictA.getGtuType());
186 assertEquals("conflict B is about cars", cars, conflictB.getGtuType());
187 assertEquals("construction of two conflicts has generated two events", 2, this.collectedEvents.size());
188
189
190 }
191
192
193
194
195
196
197
198 public Set<OTSPoint3D> intersections(final OTSLine3D a, final OTSLine3D b)
199 {
200
201 Set<OTSPoint3D> result = new LinkedHashSet<>();
202 OTSPoint3D prevA = null;
203 for (OTSPoint3D nextA : a.getPoints())
204 {
205 if (null != prevA)
206 {
207 OTSPoint3D prevB = null;
208 for (OTSPoint3D nextB : b.getPoints())
209 {
210 if (null != prevB)
211 {
212 OTSPoint3D intersection = OTSPoint3D.intersectionOfLineSegments(prevA, nextA, prevB, nextB);
213 if (null != intersection)
214 {
215 result.add(intersection);
216 }
217 }
218 prevB = nextB;
219 }
220 }
221 prevA = nextA;
222 }
223 return result;
224 }
225
226 @Override
227 public final void notify(final EventInterface event) throws RemoteException
228 {
229
230 this.collectedEvents.add(event);
231 }
232
233 }