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