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