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.assertNotNull;
6 import static org.junit.Assert.assertTrue;
7 import static org.opentrafficsim.core.gtu.GTUType.CAR;
8
9 import java.awt.geom.Point2D;
10 import java.util.LinkedHashMap;
11 import java.util.Map;
12
13 import javax.media.j3d.BoundingBox;
14 import javax.media.j3d.Bounds;
15 import javax.vecmath.Point3d;
16
17 import org.djunits.unit.UNITS;
18 import org.djunits.value.vdouble.scalar.Length;
19 import org.djunits.value.vdouble.scalar.Speed;
20 import org.junit.Test;
21 import org.opentrafficsim.core.compatibility.GTUCompatibility;
22 import org.opentrafficsim.core.geometry.OTSLine3D;
23 import org.opentrafficsim.core.geometry.OTSPoint3D;
24 import org.opentrafficsim.core.gtu.GTUType;
25 import org.opentrafficsim.core.network.LinkType;
26 import org.opentrafficsim.core.network.LongitudinalDirectionality;
27 import org.opentrafficsim.core.network.Network;
28 import org.opentrafficsim.core.network.Node;
29 import org.opentrafficsim.core.network.OTSNetwork;
30 import org.opentrafficsim.core.network.OTSNode;
31 import org.opentrafficsim.road.mock.MockDEVSSimulator;
32 import org.opentrafficsim.road.network.lane.changing.LaneKeepingPolicy;
33 import org.opentrafficsim.road.network.lane.changing.OvertakingConditions;
34
35 import com.vividsolutions.jts.geom.Coordinate;
36 import com.vividsolutions.jts.geom.Geometry;
37 import com.vividsolutions.jts.geom.GeometryFactory;
38
39 import nl.tudelft.simulation.dsol.simulators.DEVSSimulatorInterface;
40 import nl.tudelft.simulation.language.d3.DirectedPoint;
41
42
43
44
45
46
47
48
49
50
51
52 public class LaneTest implements UNITS
53 {
54
55
56
57
58 @Test
59 public void laneConstructorTest() throws Exception
60 {
61
62 Network network = new OTSNetwork("lane test network");
63 OTSNode nodeFrom = new OTSNode(network, "A", new OTSPoint3D(0, 0, 0));
64 OTSNode nodeTo = new OTSNode(network, "B", new OTSPoint3D(1000, 0, 0));
65
66 OTSPoint3D[] coordinates = new OTSPoint3D[2];
67 coordinates[0] = new OTSPoint3D(nodeFrom.getPoint().x, nodeFrom.getPoint().y, 0);
68 coordinates[1] = new OTSPoint3D(nodeTo.getPoint().x, nodeTo.getPoint().y, 0);
69 DEVSSimulatorInterface.TimeDoubleUnit simulator = MockDEVSSimulator.createMock();
70 CrossSectionLink link = new CrossSectionLink(network, "A to B", nodeFrom, nodeTo, LinkType.ROAD,
71 new OTSLine3D(coordinates), simulator, LaneKeepingPolicy.KEEP_RIGHT);
72 Length startLateralPos = new Length(2, METER);
73 Length endLateralPos = new Length(5, METER);
74 Length startWidth = new Length(3, METER);
75 Length endWidth = new Length(4, METER);
76 GTUType gtuTypeCar = CAR;
77
78 GTUCompatibility<LaneType> gtuCompatibility = new GTUCompatibility<>((LaneType) null);
79 gtuCompatibility.addAllowedGTUType(GTUType.VEHICLE, LongitudinalDirectionality.DIR_PLUS);
80 LaneType laneType = new LaneType("One way", LaneType.FREEWAY, gtuCompatibility);
81 Map<GTUType, Speed> speedMap = new LinkedHashMap<>();
82 speedMap.put(GTUType.VEHICLE, new Speed(100, KM_PER_HOUR));
83
84
85 Lane lane = new Lane(link, "lane", startLateralPos, endLateralPos, startWidth, endWidth, laneType, speedMap,
86 new OvertakingConditions.LeftAndRight());
87
88 assertEquals("PrevLanes should be empty", 0, lane.prevLanes(gtuTypeCar).size());
89 assertEquals("NextLanes should be empty", 0, lane.nextLanes(gtuTypeCar).size());
90 double approximateLengthOfContour =
91 2 * nodeFrom.getPoint().distanceSI(nodeTo.getPoint()) + startWidth.getSI() + endWidth.getSI();
92 assertEquals("Length of contour is approximately " + approximateLengthOfContour, approximateLengthOfContour,
93 lane.getContour().getLengthSI(), 0.1);
94 assertEquals("Directionality should be " + LongitudinalDirectionality.DIR_PLUS, LongitudinalDirectionality.DIR_PLUS,
95 lane.getLaneType().getDirectionality(GTUType.VEHICLE));
96 assertEquals("SpeedLimit should be " + (new Speed(100, KM_PER_HOUR)), new Speed(100, KM_PER_HOUR),
97 lane.getSpeedLimit(GTUType.VEHICLE));
98 assertEquals("There should be no GTUs on the lane", 0, lane.getGtuList().size());
99 assertEquals("LaneType should be " + laneType, laneType, lane.getLaneType());
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132 coordinates = new OTSPoint3D[3];
133 coordinates[0] = new OTSPoint3D(nodeFrom.getPoint().x, nodeFrom.getPoint().y, 0);
134 coordinates[1] = new OTSPoint3D(200, 100);
135 coordinates[2] = new OTSPoint3D(nodeTo.getPoint().x, nodeTo.getPoint().y, 0);
136 link = new CrossSectionLink(network, "A to B with Kink", nodeFrom, nodeTo, LinkType.ROAD, new OTSLine3D(coordinates),
137 simulator, LaneKeepingPolicy.KEEP_RIGHT);
138
139 lane = new Lane(link, "lane.1", startLateralPos, endLateralPos, startWidth, endWidth, laneType, speedMap,
140 new OvertakingConditions.LeftAndRight());
141
142 assertEquals("PrevLanes should be empty", 0, lane.prevLanes(gtuTypeCar).size());
143 assertEquals("NextLanes should be empty", 0, lane.nextLanes(gtuTypeCar).size());
144 approximateLengthOfContour = 2 * (coordinates[0].distanceSI(coordinates[1]) + coordinates[1].distanceSI(coordinates[2]))
145 + startWidth.getSI() + endWidth.getSI();
146 assertEquals("Length of contour is approximately " + approximateLengthOfContour, approximateLengthOfContour,
147 lane.getContour().getLengthSI(), 4);
148 assertEquals("There should be no GTUs on the lane", 0, lane.getGtuList().size());
149 assertEquals("LaneType should be " + laneType, laneType, lane.getLaneType());
150
151 Length startLateralPos2 = new Length(-8, METER);
152 Length endLateralPos2 = new Length(-5, METER);
153
154 Lane lane2 = new Lane(link, "lane.2", startLateralPos2, endLateralPos2, startWidth, endWidth, laneType, speedMap,
155 new OvertakingConditions.LeftAndRight());
156
157 assertEquals("PrevLanes should be empty", 0, lane2.prevLanes(gtuTypeCar).size());
158 assertEquals("NextLanes should be empty", 0, lane2.nextLanes(gtuTypeCar).size());
159 approximateLengthOfContour = 2 * (coordinates[0].distanceSI(coordinates[1]) + coordinates[1].distanceSI(coordinates[2]))
160 + startWidth.getSI() + endWidth.getSI();
161 assertEquals("Length of contour is approximately " + approximateLengthOfContour, approximateLengthOfContour,
162 lane2.getContour().getLengthSI(), 12);
163 assertEquals("There should be no GTUs on the lane", 0, lane2.getGtuList().size());
164 assertEquals("LaneType should be " + laneType, laneType, lane2.getLaneType());
165 }
166
167
168
169
170
171
172 @Test
173 public final void contourTest() throws Exception
174 {
175 final int[] startPositions = { 0, 1, -1, 20, -20 };
176 final double[] angles = { 0, Math.PI * 0.01, Math.PI / 3, Math.PI / 2, Math.PI * 2 / 3, Math.PI * 0.99, Math.PI,
177 Math.PI * 1.01, Math.PI * 4 / 3, Math.PI * 3 / 2, Math.PI * 1.99, Math.PI * 2, Math.PI * (-0.2) };
178 LaneType laneType = LaneType.TWO_WAY_LANE;
179 Map<GTUType, LongitudinalDirectionality> directionalityMap = new LinkedHashMap<>();
180 directionalityMap.put(GTUType.VEHICLE, LongitudinalDirectionality.DIR_PLUS);
181 Map<GTUType, Speed> speedMap = new LinkedHashMap<>();
182 speedMap.put(GTUType.VEHICLE, new Speed(50, KM_PER_HOUR));
183 int laneNum = 0;
184 for (int xStart : startPositions)
185 {
186 for (int yStart : startPositions)
187 {
188 for (double angle : angles)
189 {
190 Network network = new OTSNetwork("contour test network");
191 OTSNode start = new OTSNode(network, "start", new OTSPoint3D(xStart, yStart));
192 double linkLength = 1000;
193 double xEnd = xStart + linkLength * Math.cos(angle);
194 double yEnd = yStart + linkLength * Math.sin(angle);
195 OTSNode end = new OTSNode(network, "end", new OTSPoint3D(xEnd, yEnd));
196 OTSPoint3D[] coordinates = new OTSPoint3D[2];
197 coordinates[0] = start.getPoint();
198 coordinates[1] = end.getPoint();
199 OTSLine3D line = new OTSLine3D(coordinates);
200 DEVSSimulatorInterface.TimeDoubleUnit simulator = MockDEVSSimulator.createMock();
201 CrossSectionLink link = new CrossSectionLink(network, "A to B", start, end, LinkType.ROAD, line, simulator,
202 LaneKeepingPolicy.KEEP_RIGHT);
203 final int[] lateralOffsets = { -10, -3, -1, 0, 1, 3, 10 };
204 for (int startLateralOffset : lateralOffsets)
205 {
206 for (int endLateralOffset : lateralOffsets)
207 {
208 int startWidth = 4;
209 for (int endWidth : new int[] { 2, 4, 6 })
210 {
211
212
213 Lane lane = new Lane(link, "lane." + ++laneNum, new Length(startLateralOffset, METER),
214 new Length(endLateralOffset, METER), new Length(startWidth, METER),
215 new Length(endWidth, METER), laneType, speedMap,
216 new OvertakingConditions.LeftAndRight());
217 final Geometry geometry = lane.getContour().getLineString();
218 assertNotNull("geometry of the lane should not be null", geometry);
219
220
221 checkInside(lane, 1, startLateralOffset, true);
222
223 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset, true);
224
225 checkInside(lane, -1, startLateralOffset, false);
226
227 checkInside(lane, link.getLength().getSI() + 1, endLateralOffset, false);
228
229 checkInside(lane, 1, startLateralOffset - startWidth / 2 - 1, false);
230
231 checkInside(lane, 1, startLateralOffset + startWidth / 2 + 1, false);
232
233 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset - endWidth / 2 - 1, false);
234
235 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset + endWidth / 2 + 1, false);
236
237 DirectedPoint l = lane.getLocation();
238 Bounds bb = lane.getBounds();
239
240
241
242
243 Point2D.Double[] cornerPoints = new Point2D.Double[4];
244 cornerPoints[0] =
245 new Point2D.Double(xStart - (startLateralOffset + startWidth / 2) * Math.sin(angle),
246 yStart + (startLateralOffset + startWidth / 2) * Math.cos(angle));
247 cornerPoints[1] =
248 new Point2D.Double(xStart - (startLateralOffset - startWidth / 2) * Math.sin(angle),
249 yStart + (startLateralOffset - startWidth / 2) * Math.cos(angle));
250 cornerPoints[2] = new Point2D.Double(xEnd - (endLateralOffset + endWidth / 2) * Math.sin(angle),
251 yEnd + (endLateralOffset + endWidth / 2) * Math.cos(angle));
252 cornerPoints[3] = new Point2D.Double(xEnd - (endLateralOffset - endWidth / 2) * Math.sin(angle),
253 yEnd + (endLateralOffset - endWidth / 2) * Math.cos(angle));
254
255
256
257
258 double minX = cornerPoints[0].getX();
259 double maxX = cornerPoints[0].getX();
260 double minY = cornerPoints[0].getY();
261 double maxY = cornerPoints[0].getY();
262 for (int i = 1; i < cornerPoints.length; i++)
263 {
264 Point2D.Double p = cornerPoints[i];
265 minX = Math.min(minX, p.getX());
266 minY = Math.min(minY, p.getY());
267 maxX = Math.max(maxX, p.getX());
268 maxY = Math.max(maxY, p.getY());
269 }
270 Point3d bbLow = new Point3d();
271 ((BoundingBox) bb).getLower(bbLow);
272 Point3d bbHigh = new Point3d();
273 ((BoundingBox) bb).getUpper(bbHigh);
274
275
276
277 double boundsMinX = bbLow.x + l.x;
278 double boundsMinY = bbLow.y + l.y;
279 double boundsMaxX = bbHigh.x + l.x;
280 double boundsMaxY = bbHigh.y + l.y;
281 assertEquals("low x boundary", minX, boundsMinX, 0.1);
282 assertEquals("low y boundary", minY, boundsMinY, 0.1);
283 assertEquals("high x boundary", maxX, boundsMaxX, 0.1);
284 assertEquals("high y boundary", maxY, boundsMaxY, 0.1);
285 }
286 }
287 }
288 }
289 }
290 }
291 }
292
293
294
295
296
297
298
299
300
301
302
303
304
305 private void checkInside(final Lane lane, final double longitudinal, final double lateral, final boolean expectedResult)
306 {
307 CrossSectionLink parentLink = lane.getParentLink();
308 Node start = parentLink.getStartNode();
309 Node end = parentLink.getEndNode();
310 double startX = start.getPoint().x;
311 double startY = start.getPoint().y;
312 double endX = end.getPoint().x;
313 double endY = end.getPoint().y;
314 double length = Math.sqrt((endX - startX) * (endX - startX) + (endY - startY) * (endY - startY));
315 double ratio = longitudinal / length;
316 double designLineX = startX + (endX - startX) * ratio;
317 double designLineY = startY + (endY - startY) * ratio;
318 double lateralAngle = Math.atan2(endY - startY, endX - startX) + Math.PI / 2;
319 double px = designLineX + lateral * Math.cos(lateralAngle);
320 double py = designLineY + lateral * Math.sin(lateralAngle);
321 Geometry contour = lane.getContour().getLineString();
322 GeometryFactory factory = new GeometryFactory();
323 Geometry p = factory.createPoint(new Coordinate(px, py));
324
325
326 boolean result = contour.contains(p);
327 Coordinate[] polygon = contour.getCoordinates();
328 result = pointInsidePolygon(new Coordinate(px, py), polygon);
329 if (expectedResult)
330 {
331 assertTrue("Point at " + longitudinal + " along and " + lateral + " lateral is within lane", result);
332 }
333 else
334 {
335 assertFalse("Point at " + longitudinal + " along and " + lateral + " lateral is outside lane", result);
336 }
337 }
338
339
340
341
342
343
344
345
346
347
348 private boolean pointInsidePolygon(final Coordinate point, final Coordinate[] polygon)
349 {
350 boolean result = false;
351 for (int i = 0, j = polygon.length - 1; i < polygon.length; j = i++)
352 {
353 if ((polygon[i].y > point.y) != (polygon[j].y > point.y)
354 && point.x < (polygon[j].x - polygon[i].x) * (point.y - polygon[i].y) / (polygon[j].y - polygon[i].y)
355 + polygon[i].x)
356 {
357 result = !result;
358 }
359 }
360 return result;
361 }
362
363 }