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.djunits.unit.UNITS;
19 import org.djunits.value.vdouble.scalar.Length;
20 import org.djunits.value.vdouble.scalar.Speed;
21 import org.junit.Test;
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.LateralDirectionality;
26 import org.opentrafficsim.core.network.LinkType;
27 import org.opentrafficsim.core.network.LongitudinalDirectionality;
28 import org.opentrafficsim.core.network.Node;
29 import org.opentrafficsim.core.network.OTSNode;
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 UNITS
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, LinkType.ALL, new OTSLine3D(coordinates),
65 LongitudinalDirectionality.DIR_PLUS, LaneKeepingPolicy.KEEP_RIGHT);
66 Length.Rel startLateralPos = new Length.Rel(2, METER);
67 Length.Rel endLateralPos = new Length.Rel(5, METER);
68 Length.Rel startWidth = new Length.Rel(3, METER);
69 Length.Rel endWidth = new Length.Rel(4, METER);
70 GTUType gtuTypeCar = GTUType.makeGTUType("Car");
71 GTUType gtuTypeTruck = GTUType.makeGTUType("Truck");
72 LaneType laneType = new LaneType("Car");
73 laneType.addCompatibility(gtuTypeCar);
74 laneType.addCompatibility(gtuTypeTruck);
75 Map<GTUType, LongitudinalDirectionality> directionalityMap = new LinkedHashMap<>();
76 directionalityMap.put(GTUType.ALL, LongitudinalDirectionality.DIR_PLUS);
77 Map<GTUType, Speed> speedMap = new LinkedHashMap<>();
78 speedMap.put(GTUType.ALL, new Speed(100, KM_PER_HOUR));
79
80
81 Lane lane =
82 new Lane(link, "lane", startLateralPos, endLateralPos, startWidth, endWidth, laneType, directionalityMap,
83 speedMap, new OvertakingConditions.LeftAndRight());
84
85 assertEquals("PrevLanes should be empty", 0, lane.prevLanes(gtuTypeCar).size());
86 assertEquals("NextLanes should be empty", 0, lane.nextLanes(gtuTypeCar).size());
87 double approximateLengthOfContour =
88 2 * nodeFrom.getPoint().distanceSI(nodeTo.getPoint()) + startWidth.getSI() + endWidth.getSI();
89 assertEquals("Length of contour is approximately " + approximateLengthOfContour, approximateLengthOfContour,
90 lane.getContour().getLengthSI(), 0.1);
91 assertEquals("Directionality should be " + LongitudinalDirectionality.DIR_PLUS,
92 LongitudinalDirectionality.DIR_PLUS, lane.getDirectionality(GTUType.ALL));
93 assertEquals("SpeedLimit should be " + (new Speed(100, KM_PER_HOUR)), new Speed(100, KM_PER_HOUR), lane
94 .getSpeedLimit(GTUType.ALL));
95 assertEquals("There should be no GTUs on the lane", 0, lane.getGtuList().size());
96 assertEquals("LaneType should be " + laneType, laneType, lane.getLaneType());
97 for (int i = 0; i < 10; i++)
98 {
99 double expectedLateralCenterOffset =
100 startLateralPos.getSI() + (endLateralPos.getSI() - startLateralPos.getSI()) * i / 10;
101 assertEquals(String.format("Lateral offset at %d%% should be %.3fm", 10 * i, expectedLateralCenterOffset),
102 expectedLateralCenterOffset, lane.getLateralCenterPosition(i / 10.0).getSI(), 0.01);
103 Length.Rel longitudinalPosition = new Length.Rel(lane.getLength().getSI() * i / 10, METER);
104 assertEquals("Lateral offset at " + longitudinalPosition + " should be " + expectedLateralCenterOffset,
105 expectedLateralCenterOffset, lane.getLateralCenterPosition(longitudinalPosition).getSI(), 0.01);
106 double expectedWidth = startWidth.getSI() + (endWidth.getSI() - startWidth.getSI()) * i / 10;
107 assertEquals(String.format("Width at %d%% should be %.3fm", 10 * i, expectedWidth), expectedWidth, lane
108 .getWidth(i / 10.0).getSI(), 0.0001);
109 assertEquals("Width at " + longitudinalPosition + " should be " + expectedWidth, expectedWidth, lane
110 .getWidth(longitudinalPosition).getSI(), 0.0001);
111 double expectedLeftOffset = expectedLateralCenterOffset - expectedWidth / 2;
112
113 assertEquals(String.format("Left edge at %d%% should be %.3fm", 10 * i, expectedLeftOffset),
114 expectedLeftOffset, lane.getLateralBoundaryPosition(LateralDirectionality.LEFT, i / 10.0).getSI(),
115 0.001);
116 assertEquals("Left edge at " + longitudinalPosition + " should be " + expectedLeftOffset,
117 expectedLeftOffset, lane.getLateralBoundaryPosition(LateralDirectionality.LEFT, longitudinalPosition)
118 .getSI(), 0.001);
119 double expectedRightOffset = expectedLateralCenterOffset + expectedWidth / 2;
120 assertEquals(String.format("Right edge at %d%% should be %.3fm", 10 * i, expectedRightOffset),
121 expectedRightOffset, lane.getLateralBoundaryPosition(LateralDirectionality.RIGHT, i / 10.0).getSI(),
122 0.001);
123 assertEquals("Right edge at " + longitudinalPosition + " should be " + expectedRightOffset,
124 expectedRightOffset, lane.getLateralBoundaryPosition(LateralDirectionality.RIGHT, longitudinalPosition)
125 .getSI(), 0.001);
126 }
127
128
129
130 coordinates = new OTSPoint3D[3];
131 coordinates[0] = new OTSPoint3D(nodeFrom.getPoint().x, nodeFrom.getPoint().y, 0);
132 coordinates[1] = new OTSPoint3D(200, 100);
133 coordinates[2] = new OTSPoint3D(nodeTo.getPoint().x, nodeTo.getPoint().y, 0);
134 link =
135 new CrossSectionLink("A to B with Kink", nodeFrom, nodeTo, LinkType.ALL, new OTSLine3D(coordinates),
136 LongitudinalDirectionality.DIR_PLUS, LaneKeepingPolicy.KEEP_RIGHT);
137
138 lane =
139 new Lane(link, "lane.1", startLateralPos, endLateralPos, startWidth, endWidth, laneType, directionalityMap,
140 speedMap, 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 =
145 2 * (coordinates[0].distanceSI(coordinates[1]) + coordinates[1].distanceSI(coordinates[2]))
146 + startWidth.getSI() + endWidth.getSI();
147 assertEquals("Length of contour is approximately " + approximateLengthOfContour, approximateLengthOfContour,
148 lane.getContour().getLengthSI(), 4);
149 assertEquals("There should be no GTUs on the lane", 0, lane.getGtuList().size());
150 assertEquals("LaneType should be " + laneType, laneType, lane.getLaneType());
151
152 Length.Rel startLateralPos2 = new Length.Rel(-8, METER);
153 Length.Rel endLateralPos2 = new Length.Rel(-5, METER);
154
155 Lane lane2 =
156 new Lane(link, "lane.2", startLateralPos2, endLateralPos2, startWidth, endWidth, laneType,
157 directionalityMap, speedMap, new OvertakingConditions.LeftAndRight());
158
159 assertEquals("PrevLanes should be empty", 0, lane2.prevLanes(gtuTypeCar).size());
160 assertEquals("NextLanes should be empty", 0, lane2.nextLanes(gtuTypeCar).size());
161 approximateLengthOfContour =
162 2 * (coordinates[0].distanceSI(coordinates[1]) + coordinates[1].distanceSI(coordinates[2]))
163 + startWidth.getSI() + endWidth.getSI();
164 assertEquals("Length of contour is approximately " + approximateLengthOfContour, approximateLengthOfContour,
165 lane2.getContour().getLengthSI(), 12);
166 assertEquals("There should be no GTUs on the lane", 0, lane2.getGtuList().size());
167 assertEquals("LaneType should be " + laneType, laneType, lane2.getLaneType());
168 }
169
170
171
172
173
174
175 @Test
176 public void contourTest() throws Exception
177 {
178 final int[] startPositions = {0, 1, -1, 20, -20};
179 final double[] angles =
180 {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,
181 Math.PI * 4 / 3, Math.PI * 3 / 2, Math.PI * 1.99, Math.PI * 2, Math.PI * (-0.2)};
182 LaneType laneType = new LaneType("Car");
183 Map<GTUType, LongitudinalDirectionality> directionalityMap = new LinkedHashMap<>();
184 directionalityMap.put(GTUType.ALL, LongitudinalDirectionality.DIR_PLUS);
185 Map<GTUType, Speed> speedMap = new LinkedHashMap<>();
186 speedMap.put(GTUType.ALL, new Speed(50, KM_PER_HOUR));
187 int laneNum = 0;
188 for (int xStart : startPositions)
189 {
190 for (int yStart : startPositions)
191 {
192 for (double angle : angles)
193 {
194 OTSNode start = new OTSNode("start", new OTSPoint3D(xStart, yStart));
195 double linkLength = 1000;
196 double xEnd = xStart + linkLength * Math.cos(angle);
197 double yEnd = yStart + linkLength * Math.sin(angle);
198 OTSNode end = new OTSNode("end", new OTSPoint3D(xEnd, yEnd));
199 OTSPoint3D[] coordinates = new OTSPoint3D[2];
200 coordinates[0] = start.getPoint();
201 coordinates[1] = end.getPoint();
202 OTSLine3D line = new OTSLine3D(coordinates);
203 CrossSectionLink link =
204 new CrossSectionLink("A to B", start, end, LinkType.ALL, line,
205 LongitudinalDirectionality.DIR_PLUS, LaneKeepingPolicy.KEEP_RIGHT);
206 final int[] lateralOffsets = {-10, -3, -1, 0, 1, 3, 10};
207 for (int startLateralOffset : lateralOffsets)
208 {
209 for (int endLateralOffset : lateralOffsets)
210 {
211 int startWidth = 4;
212 for (int endWidth : new int[]{2, 4, 6})
213 {
214
215
216 Lane lane =
217 new Lane(link, "lane." + ++laneNum, new Length.Rel(startLateralOffset, METER),
218 new Length.Rel(endLateralOffset, METER), new Length.Rel(startWidth, METER),
219 new Length.Rel(endWidth, METER), laneType, directionalityMap, speedMap,
220 new OvertakingConditions.LeftAndRight());
221 final Geometry geometry = lane.getContour().getLineString();
222 assertNotNull("geometry of the lane should not be null", geometry);
223
224
225 checkInside(lane, 1, startLateralOffset, true);
226
227 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset, true);
228
229 checkInside(lane, -1, startLateralOffset, false);
230
231 checkInside(lane, link.getLength().getSI() + 1, endLateralOffset, false);
232
233 checkInside(lane, 1, startLateralOffset - startWidth / 2 - 1, false);
234
235 checkInside(lane, 1, startLateralOffset + startWidth / 2 + 1, false);
236
237 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset - endWidth / 2 - 1,
238 false);
239
240 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset + endWidth / 2 + 1,
241 false);
242
243 DirectedPoint l = lane.getLocation();
244 Bounds bb = lane.getBounds();
245
246
247
248
249 Point2D.Double[] cornerPoints = new Point2D.Double[4];
250 cornerPoints[0] =
251 new Point2D.Double(
252 xStart - (startLateralOffset + startWidth / 2) * Math.sin(angle), yStart
253 + (startLateralOffset + startWidth / 2) * Math.cos(angle));
254 cornerPoints[1] =
255 new Point2D.Double(
256 xStart - (startLateralOffset - startWidth / 2) * Math.sin(angle), yStart
257 + (startLateralOffset - startWidth / 2) * Math.cos(angle));
258 cornerPoints[2] =
259 new Point2D.Double(xEnd - (endLateralOffset + endWidth / 2) * Math.sin(angle), yEnd
260 + (endLateralOffset + endWidth / 2) * Math.cos(angle));
261 cornerPoints[3] =
262 new Point2D.Double(xEnd - (endLateralOffset - endWidth / 2) * Math.sin(angle), yEnd
263 + (endLateralOffset - endWidth / 2) * Math.cos(angle));
264 for (int i = 0; i < cornerPoints.length; i++)
265 {
266
267 }
268 double minX = cornerPoints[0].getX();
269 double maxX = cornerPoints[0].getX();
270 double minY = cornerPoints[0].getY();
271 double maxY = cornerPoints[0].getY();
272 for (int i = 1; i < cornerPoints.length; i++)
273 {
274 Point2D.Double p = cornerPoints[i];
275 minX = Math.min(minX, p.getX());
276 minY = Math.min(minY, p.getY());
277 maxX = Math.max(maxX, p.getX());
278 maxY = Math.max(maxY, p.getY());
279 }
280 Point3d bbLow = new Point3d();
281 ((BoundingBox) bb).getLower(bbLow);
282 Point3d bbHigh = new Point3d();
283 ((BoundingBox) bb).getUpper(bbHigh);
284
285
286
287 double boundsMinX = bbLow.x + l.x;
288 double boundsMinY = bbLow.y + l.y;
289 double boundsMaxX = bbHigh.x + l.x;
290 double boundsMaxY = bbHigh.y + l.y;
291 assertEquals("low x boundary", minX, boundsMinX, 0.1);
292 assertEquals("low y boundary", minY, boundsMinY, 0.1);
293 assertEquals("high x boundary", maxX, boundsMaxX, 0.1);
294 assertEquals("high y boundary", maxY, boundsMaxY, 0.1);
295 }
296 }
297 }
298 }
299 }
300 }
301 }
302
303
304
305
306
307
308
309
310
311
312
313
314
315 private void checkInside(final Lane lane, final double longitudinal, final double lateral,
316 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 }