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.Network;
31 import org.opentrafficsim.core.network.Node;
32 import org.opentrafficsim.core.network.OTSNetwork;
33 import org.opentrafficsim.core.network.OTSNode;
34 import org.opentrafficsim.road.network.lane.changing.LaneKeepingPolicy;
35 import org.opentrafficsim.road.network.lane.changing.OvertakingConditions;
36
37 import com.vividsolutions.jts.geom.Coordinate;
38 import com.vividsolutions.jts.geom.Geometry;
39 import com.vividsolutions.jts.geom.GeometryFactory;
40
41
42
43
44
45
46
47
48
49
50
51 public class LaneTest implements UNITS
52 {
53
54
55
56
57 @Test
58 public void laneConstructorTest() throws Exception
59 {
60
61 Network network = new OTSNetwork("lane test network");
62 OTSNode nodeFrom = new OTSNode(network, "A", new OTSPoint3D(0, 0, 0));
63 OTSNode nodeTo = new OTSNode(network, "B", new OTSPoint3D(1000, 0, 0));
64
65 OTSPoint3D[] coordinates = new OTSPoint3D[2];
66 coordinates[0] = new OTSPoint3D(nodeFrom.getPoint().x, nodeFrom.getPoint().y, 0);
67 coordinates[1] = new OTSPoint3D(nodeTo.getPoint().x, nodeTo.getPoint().y, 0);
68 CrossSectionLink link =
69 new CrossSectionLink(network, "A to B", nodeFrom, nodeTo, LinkType.ALL, new OTSLine3D(coordinates),
70 LongitudinalDirectionality.DIR_PLUS, LaneKeepingPolicy.KEEP_RIGHT);
71 Length startLateralPos = new Length(2, METER);
72 Length endLateralPos = new Length(5, METER);
73 Length startWidth = new Length(3, METER);
74 Length endWidth = new Length(4, METER);
75 GTUType gtuTypeCar = new GTUType("Car");
76 GTUType gtuTypeTruck = new GTUType("Truck");
77 Set<GTUType> compatibility = new HashSet<GTUType>();
78 compatibility.add(gtuTypeCar);
79 compatibility.add(gtuTypeTruck);
80 LaneType laneType = new LaneType("CarLane", compatibility);
81 Map<GTUType, LongitudinalDirectionality> directionalityMap = new LinkedHashMap<>();
82 directionalityMap.put(GTUType.ALL, LongitudinalDirectionality.DIR_PLUS);
83 Map<GTUType, Speed> speedMap = new LinkedHashMap<>();
84 speedMap.put(GTUType.ALL, new Speed(100, KM_PER_HOUR));
85
86
87 Lane lane =
88 new Lane(link, "lane", startLateralPos, endLateralPos, startWidth, endWidth, laneType, directionalityMap,
89 speedMap, new OvertakingConditions.LeftAndRight());
90
91 assertEquals("PrevLanes should be empty", 0, lane.prevLanes(gtuTypeCar).size());
92 assertEquals("NextLanes should be empty", 0, lane.nextLanes(gtuTypeCar).size());
93 double approximateLengthOfContour =
94 2 * nodeFrom.getPoint().distanceSI(nodeTo.getPoint()) + startWidth.getSI() + endWidth.getSI();
95 assertEquals("Length of contour is approximately " + approximateLengthOfContour, approximateLengthOfContour, lane
96 .getContour().getLengthSI(), 0.1);
97 assertEquals("Directionality should be " + LongitudinalDirectionality.DIR_PLUS, LongitudinalDirectionality.DIR_PLUS,
98 lane.getDirectionality(GTUType.ALL));
99 assertEquals("SpeedLimit should be " + (new Speed(100, KM_PER_HOUR)), new Speed(100, KM_PER_HOUR),
100 lane.getSpeedLimit(GTUType.ALL));
101 assertEquals("There should be no GTUs on the lane", 0, lane.getGtuList().size());
102 assertEquals("LaneType should be " + laneType, laneType, lane.getLaneType());
103 for (int i = 0; i < 10; i++)
104 {
105 double expectedLateralCenterOffset =
106 startLateralPos.getSI() + (endLateralPos.getSI() - startLateralPos.getSI()) * i / 10;
107 assertEquals(String.format("Lateral offset at %d%% should be %.3fm", 10 * i, expectedLateralCenterOffset),
108 expectedLateralCenterOffset, lane.getLateralCenterPosition(i / 10.0).getSI(), 0.01);
109 Length longitudinalPosition = new Length(lane.getLength().getSI() * i / 10, METER);
110 assertEquals("Lateral offset at " + longitudinalPosition + " should be " + expectedLateralCenterOffset,
111 expectedLateralCenterOffset, lane.getLateralCenterPosition(longitudinalPosition).getSI(), 0.01);
112 double expectedWidth = startWidth.getSI() + (endWidth.getSI() - startWidth.getSI()) * i / 10;
113 assertEquals(String.format("Width at %d%% should be %.3fm", 10 * i, expectedWidth), expectedWidth,
114 lane.getWidth(i / 10.0).getSI(), 0.0001);
115 assertEquals("Width at " + longitudinalPosition + " should be " + expectedWidth, expectedWidth,
116 lane.getWidth(longitudinalPosition).getSI(), 0.0001);
117 double expectedLeftOffset = expectedLateralCenterOffset - expectedWidth / 2;
118
119 assertEquals(String.format("Left edge at %d%% should be %.3fm", 10 * i, expectedLeftOffset), expectedLeftOffset,
120 lane.getLateralBoundaryPosition(LateralDirectionality.LEFT, i / 10.0).getSI(), 0.001);
121 assertEquals("Left edge at " + longitudinalPosition + " should be " + expectedLeftOffset, expectedLeftOffset, lane
122 .getLateralBoundaryPosition(LateralDirectionality.LEFT, longitudinalPosition).getSI(), 0.001);
123 double expectedRightOffset = expectedLateralCenterOffset + expectedWidth / 2;
124 assertEquals(String.format("Right edge at %d%% should be %.3fm", 10 * i, expectedRightOffset), expectedRightOffset,
125 lane.getLateralBoundaryPosition(LateralDirectionality.RIGHT, i / 10.0).getSI(), 0.001);
126 assertEquals("Right edge at " + longitudinalPosition + " should be " + expectedRightOffset, expectedRightOffset,
127 lane.getLateralBoundaryPosition(LateralDirectionality.RIGHT, longitudinalPosition).getSI(), 0.001);
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 =
137 new CrossSectionLink(network, "A to B with Kink", nodeFrom, nodeTo, LinkType.ALL, new OTSLine3D(coordinates),
138 LongitudinalDirectionality.DIR_PLUS, LaneKeepingPolicy.KEEP_RIGHT);
139
140 lane =
141 new Lane(link, "lane.1", startLateralPos, endLateralPos, startWidth, endWidth, laneType, directionalityMap,
142 speedMap, new OvertakingConditions.LeftAndRight());
143
144 assertEquals("PrevLanes should be empty", 0, lane.prevLanes(gtuTypeCar).size());
145 assertEquals("NextLanes should be empty", 0, lane.nextLanes(gtuTypeCar).size());
146 approximateLengthOfContour =
147 2 * (coordinates[0].distanceSI(coordinates[1]) + coordinates[1].distanceSI(coordinates[2]))
148 + startWidth.getSI() + endWidth.getSI();
149 assertEquals("Length of contour is approximately " + approximateLengthOfContour, approximateLengthOfContour, lane
150 .getContour().getLengthSI(), 4);
151 assertEquals("There should be no GTUs on the lane", 0, lane.getGtuList().size());
152 assertEquals("LaneType should be " + laneType, laneType, lane.getLaneType());
153
154 Length startLateralPos2 = new Length(-8, METER);
155 Length endLateralPos2 = new Length(-5, METER);
156
157 Lane lane2 =
158 new Lane(link, "lane.2", startLateralPos2, endLateralPos2, startWidth, endWidth, laneType, directionalityMap,
159 speedMap, new OvertakingConditions.LeftAndRight());
160
161 assertEquals("PrevLanes should be empty", 0, lane2.prevLanes(gtuTypeCar).size());
162 assertEquals("NextLanes should be empty", 0, lane2.nextLanes(gtuTypeCar).size());
163 approximateLengthOfContour =
164 2 * (coordinates[0].distanceSI(coordinates[1]) + coordinates[1].distanceSI(coordinates[2]))
165 + startWidth.getSI() + endWidth.getSI();
166 assertEquals("Length of contour is approximately " + approximateLengthOfContour, approximateLengthOfContour, lane2
167 .getContour().getLengthSI(), 12);
168 assertEquals("There should be no GTUs on the lane", 0, lane2.getGtuList().size());
169 assertEquals("LaneType should be " + laneType, laneType, lane2.getLaneType());
170 }
171
172
173
174
175
176
177 @Test
178 public void contourTest() throws Exception
179 {
180 final int[] startPositions = { 0, 1, -1, 20, -20 };
181 final double[] angles =
182 { 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,
183 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 CrossSectionLink link =
209 new CrossSectionLink(network, "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), new Length(
223 endLateralOffset, METER), new Length(startWidth, METER), new Length(endWidth,
224 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, false);
243
244 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset + endWidth / 2 + 1, false);
245
246 DirectedPoint l = lane.getLocation();
247 Bounds bb = lane.getBounds();
248
249
250
251
252 Point2D.Double[] cornerPoints = new Point2D.Double[4];
253 cornerPoints[0] =
254 new Point2D.Double(xStart - (startLateralOffset + startWidth / 2) * Math.sin(angle),
255 yStart + (startLateralOffset + startWidth / 2) * Math.cos(angle));
256 cornerPoints[1] =
257 new Point2D.Double(xStart - (startLateralOffset - startWidth / 2) * Math.sin(angle),
258 yStart + (startLateralOffset - startWidth / 2) * Math.cos(angle));
259 cornerPoints[2] =
260 new Point2D.Double(xEnd - (endLateralOffset + endWidth / 2) * Math.sin(angle), yEnd
261 + (endLateralOffset + endWidth / 2) * Math.cos(angle));
262 cornerPoints[3] =
263 new Point2D.Double(xEnd - (endLateralOffset - endWidth / 2) * Math.sin(angle), yEnd
264 + (endLateralOffset - endWidth / 2) * Math.cos(angle));
265 for (int i = 0; i < cornerPoints.length; i++)
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 }