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