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