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 org.djunits.unit.DurationUnit;
17 import org.djunits.unit.UNITS;
18 import org.djunits.value.vdouble.scalar.Duration;
19 import org.djunits.value.vdouble.scalar.Length;
20 import org.djunits.value.vdouble.scalar.Speed;
21 import org.djunits.value.vdouble.scalar.Time;
22 import org.junit.Test;
23 import org.locationtech.jts.geom.Coordinate;
24 import org.locationtech.jts.geom.Geometry;
25 import org.locationtech.jts.geom.GeometryFactory;
26 import org.opentrafficsim.core.compatibility.GTUCompatibility;
27 import org.opentrafficsim.core.dsol.AbstractOTSModel;
28 import org.opentrafficsim.core.dsol.OTSSimulator;
29 import org.opentrafficsim.core.dsol.OTSSimulatorInterface;
30 import org.opentrafficsim.core.geometry.OTSLine3D;
31 import org.opentrafficsim.core.geometry.OTSPoint3D;
32 import org.opentrafficsim.core.gtu.GTUType;
33 import org.opentrafficsim.core.network.LinkType;
34 import org.opentrafficsim.core.network.LongitudinalDirectionality;
35 import org.opentrafficsim.core.network.Node;
36 import org.opentrafficsim.core.network.OTSNode;
37 import org.opentrafficsim.road.network.OTSRoadNetwork;
38 import org.opentrafficsim.road.network.lane.changing.LaneKeepingPolicy;
39
40 import nl.tudelft.simulation.dsol.SimRuntimeException;
41 import nl.tudelft.simulation.language.d3.DirectedPoint;
42
43
44
45
46
47
48
49
50
51
52
53 public class LaneTest implements UNITS
54 {
55
56
57
58
59 @Test
60 public void laneConstructorTest() throws Exception
61 {
62
63 OTSRoadNetwork network = new OTSRoadNetwork("lane test network", true);
64 OTSNode nodeFrom = new OTSNode(network, "A", new OTSPoint3D(0, 0, 0));
65 OTSNode nodeTo = new OTSNode(network, "B", new OTSPoint3D(1000, 0, 0));
66
67 OTSPoint3D[] coordinates = new OTSPoint3D[2];
68 coordinates[0] = new OTSPoint3D(nodeFrom.getPoint().x, nodeFrom.getPoint().y, 0);
69 coordinates[1] = new OTSPoint3D(nodeTo.getPoint().x, nodeTo.getPoint().y, 0);
70 OTSSimulatorInterface simulator = new OTSSimulator();
71 Model model = new Model(simulator);
72 simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model);
73 CrossSectionLink link =
74 new CrossSectionLink(network, "A to B", nodeFrom, nodeTo, network.getLinkType(LinkType.DEFAULTS.ROAD),
75 new OTSLine3D(coordinates), simulator, LaneKeepingPolicy.KEEPRIGHT);
76 Length startLateralPos = new Length(2, METER);
77 Length endLateralPos = new Length(5, METER);
78 Length startWidth = new Length(3, METER);
79 Length endWidth = new Length(4, METER);
80 GTUType gtuTypeCar = network.getGtuType(GTUType.DEFAULTS.CAR);
81
82 GTUCompatibility<LaneType> gtuCompatibility = new GTUCompatibility<>((LaneType) null);
83 gtuCompatibility.addAllowedGTUType(network.getGtuType(GTUType.DEFAULTS.VEHICLE), LongitudinalDirectionality.DIR_PLUS);
84 LaneType laneType = new LaneType("One way", network.getLaneType(LaneType.DEFAULTS.FREEWAY), gtuCompatibility, network);
85 Map<GTUType, Speed> speedMap = new LinkedHashMap<>();
86 speedMap.put(network.getGtuType(GTUType.DEFAULTS.VEHICLE), new Speed(100, KM_PER_HOUR));
87
88
89 Lane lane = new Lane(link, "lane", startLateralPos, endLateralPos, startWidth, endWidth, laneType, speedMap);
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,
96 lane.getContour().getLengthSI(), 0.1);
97 assertEquals("Directionality should be " + LongitudinalDirectionality.DIR_PLUS, LongitudinalDirectionality.DIR_PLUS,
98 lane.getLaneType().getDirectionality(network.getGtuType(GTUType.DEFAULTS.VEHICLE)));
99 assertEquals("SpeedLimit should be " + (new Speed(100, KM_PER_HOUR)), new Speed(100, KM_PER_HOUR),
100 lane.getSpeedLimit(network.getGtuType(GTUType.DEFAULTS.VEHICLE)));
101 assertEquals("There should be no GTUs on the lane", 0, lane.getGtuList().size());
102 assertEquals("LaneType should be " + laneType, laneType, lane.getLaneType());
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135 coordinates = new OTSPoint3D[3];
136 coordinates[0] = new OTSPoint3D(nodeFrom.getPoint().x, nodeFrom.getPoint().y, 0);
137 coordinates[1] = new OTSPoint3D(200, 100);
138 coordinates[2] = new OTSPoint3D(nodeTo.getPoint().x, nodeTo.getPoint().y, 0);
139 link = new CrossSectionLink(network, "A to B with Kink", nodeFrom, nodeTo, network.getLinkType(LinkType.DEFAULTS.ROAD),
140 new OTSLine3D(coordinates), simulator, LaneKeepingPolicy.KEEPRIGHT);
141
142 lane = new Lane(link, "lane.1", startLateralPos, endLateralPos, startWidth, endWidth, laneType, speedMap);
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 = 2 * (coordinates[0].distanceSI(coordinates[1]) + coordinates[1].distanceSI(coordinates[2]))
147 + startWidth.getSI() + endWidth.getSI();
148 assertEquals("Length of contour is approximately " + approximateLengthOfContour, approximateLengthOfContour,
149 lane.getContour().getLengthSI(), 4);
150 assertEquals("There should be no GTUs on the lane", 0, lane.getGtuList().size());
151 assertEquals("LaneType should be " + laneType, laneType, lane.getLaneType());
152
153 Length startLateralPos2 = new Length(-8, METER);
154 Length endLateralPos2 = new Length(-5, METER);
155
156 Lane lane2 = new Lane(link, "lane.2", startLateralPos2, endLateralPos2, startWidth, endWidth, laneType, speedMap);
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 = 2 * (coordinates[0].distanceSI(coordinates[1]) + coordinates[1].distanceSI(coordinates[2]))
161 + startWidth.getSI() + endWidth.getSI();
162 assertEquals("Length of contour is approximately " + approximateLengthOfContour, approximateLengthOfContour,
163 lane2.getContour().getLengthSI(), 12);
164 assertEquals("There should be no GTUs on the lane", 0, lane2.getGtuList().size());
165 assertEquals("LaneType should be " + laneType, laneType, lane2.getLaneType());
166 }
167
168
169
170
171
172
173 @Test
174 public final void contourTest() throws Exception
175 {
176 final int[] startPositions = {0, 1, -1, 20, -20};
177 final double[] angles = {0, Math.PI * 0.01, Math.PI / 3, Math.PI / 2, Math.PI * 2 / 3, Math.PI * 0.99, Math.PI,
178 Math.PI * 1.01, Math.PI * 4 / 3, Math.PI * 3 / 2, Math.PI * 1.99, Math.PI * 2, Math.PI * (-0.2)};
179 int laneNum = 0;
180 for (int xStart : startPositions)
181 {
182 for (int yStart : startPositions)
183 {
184 for (double angle : angles)
185 {
186 OTSRoadNetwork network = new OTSRoadNetwork("contour test network", true);
187 LaneType laneType = network.getLaneType(LaneType.DEFAULTS.TWO_WAY_LANE);
188 Map<GTUType, LongitudinalDirectionality> directionalityMap = new LinkedHashMap<>();
189 directionalityMap.put(network.getGtuType(GTUType.DEFAULTS.VEHICLE), LongitudinalDirectionality.DIR_PLUS);
190 Map<GTUType, Speed> speedMap = new LinkedHashMap<>();
191 speedMap.put(network.getGtuType(GTUType.DEFAULTS.VEHICLE), new Speed(50, KM_PER_HOUR));
192 OTSNode start = new OTSNode(network, "start", new OTSPoint3D(xStart, yStart));
193 double linkLength = 1000;
194 double xEnd = xStart + linkLength * Math.cos(angle);
195 double yEnd = yStart + linkLength * Math.sin(angle);
196 OTSNode end = new OTSNode(network, "end", new OTSPoint3D(xEnd, yEnd));
197 OTSPoint3D[] coordinates = new OTSPoint3D[2];
198 coordinates[0] = start.getPoint();
199 coordinates[1] = end.getPoint();
200 OTSLine3D line = new OTSLine3D(coordinates);
201 OTSSimulatorInterface simulator = new OTSSimulator();
202 Model model = new Model(simulator);
203 simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model);
204 CrossSectionLink link = new CrossSectionLink(network, "A to B", start, end,
205 network.getLinkType(LinkType.DEFAULTS.ROAD), line, simulator, LaneKeepingPolicy.KEEPRIGHT);
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 = new Lane(link, "lane." + ++laneNum, new Length(startLateralOffset, METER),
217 new Length(endLateralOffset, METER), new Length(startWidth, METER),
218 new Length(endWidth, METER), laneType, speedMap);
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, false);
236
237 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset + endWidth / 2 + 1, false);
238
239 DirectedPoint l = lane.getLocation();
240 Bounds bb = lane.getBounds();
241
242
243
244
245 Point2D.Double[] cornerPoints = new Point2D.Double[4];
246 cornerPoints[0] =
247 new Point2D.Double(xStart - (startLateralOffset + startWidth / 2) * Math.sin(angle),
248 yStart + (startLateralOffset + startWidth / 2) * Math.cos(angle));
249 cornerPoints[1] =
250 new Point2D.Double(xStart - (startLateralOffset - startWidth / 2) * Math.sin(angle),
251 yStart + (startLateralOffset - startWidth / 2) * Math.cos(angle));
252 cornerPoints[2] = new Point2D.Double(xEnd - (endLateralOffset + endWidth / 2) * Math.sin(angle),
253 yEnd + (endLateralOffset + endWidth / 2) * Math.cos(angle));
254 cornerPoints[3] = new Point2D.Double(xEnd - (endLateralOffset - endWidth / 2) * Math.sin(angle),
255 yEnd + (endLateralOffset - endWidth / 2) * Math.cos(angle));
256
257
258
259
260 double minX = cornerPoints[0].getX();
261 double maxX = cornerPoints[0].getX();
262 double minY = cornerPoints[0].getY();
263 double maxY = cornerPoints[0].getY();
264 for (int i = 1; i < cornerPoints.length; i++)
265 {
266 Point2D.Double p = cornerPoints[i];
267 minX = Math.min(minX, p.getX());
268 minY = Math.min(minY, p.getY());
269 maxX = Math.max(maxX, p.getX());
270 maxY = Math.max(maxY, p.getY());
271 }
272 Point3d bbLow = new Point3d();
273 ((BoundingBox) bb).getLower(bbLow);
274 Point3d bbHigh = new Point3d();
275 ((BoundingBox) bb).getUpper(bbHigh);
276
277
278
279 double boundsMinX = bbLow.x + l.x;
280 double boundsMinY = bbLow.y + l.y;
281 double boundsMaxX = bbHigh.x + l.x;
282 double boundsMaxY = bbHigh.y + l.y;
283 assertEquals("low x boundary", minX, boundsMinX, 0.1);
284 assertEquals("low y boundary", minY, boundsMinY, 0.1);
285 assertEquals("high x boundary", maxX, boundsMaxX, 0.1);
286 assertEquals("high y boundary", maxY, boundsMaxY, 0.1);
287 }
288 }
289 }
290 }
291 }
292 }
293 }
294
295
296
297
298
299
300
301
302
303
304
305
306
307 private void checkInside(final Lane lane, final double longitudinal, final double lateral, final boolean expectedResult)
308 {
309 CrossSectionLink parentLink = lane.getParentLink();
310 Node start = parentLink.getStartNode();
311 Node end = parentLink.getEndNode();
312 double startX = start.getPoint().x;
313 double startY = start.getPoint().y;
314 double endX = end.getPoint().x;
315 double endY = end.getPoint().y;
316 double length = Math.sqrt((endX - startX) * (endX - startX) + (endY - startY) * (endY - startY));
317 double ratio = longitudinal / length;
318 double designLineX = startX + (endX - startX) * ratio;
319 double designLineY = startY + (endY - startY) * ratio;
320 double lateralAngle = Math.atan2(endY - startY, endX - startX) + Math.PI / 2;
321 double px = designLineX + lateral * Math.cos(lateralAngle);
322 double py = designLineY + lateral * Math.sin(lateralAngle);
323 Geometry contour = lane.getContour().getLineString();
324 GeometryFactory factory = new GeometryFactory();
325 Geometry p = factory.createPoint(new Coordinate(px, py));
326
327
328 boolean result = contour.contains(p);
329 Coordinate[] polygon = contour.getCoordinates();
330 result = pointInsidePolygon(new Coordinate(px, py), polygon);
331 if (expectedResult)
332 {
333 assertTrue("Point at " + longitudinal + " along and " + lateral + " lateral is within lane", result);
334 }
335 else
336 {
337 assertFalse("Point at " + longitudinal + " along and " + lateral + " lateral is outside lane", result);
338 }
339 }
340
341
342
343
344
345
346
347
348
349
350 private boolean pointInsidePolygon(final Coordinate point, final Coordinate[] polygon)
351 {
352 boolean result = false;
353 for (int i = 0, j = polygon.length - 1; i < polygon.length; j = i++)
354 {
355 if ((polygon[i].y > point.y) != (polygon[j].y > point.y)
356 && point.x < (polygon[j].x - polygon[i].x) * (point.y - polygon[i].y) / (polygon[j].y - polygon[i].y)
357 + polygon[i].x)
358 {
359 result = !result;
360 }
361 }
362 return result;
363 }
364
365
366 protected static class Model extends AbstractOTSModel
367 {
368
369 private static final long serialVersionUID = 20141027L;
370
371
372
373
374 public Model(final OTSSimulatorInterface simulator)
375 {
376 super(simulator);
377 }
378
379
380 @Override
381 public final void constructModel() throws SimRuntimeException
382 {
383
384 }
385
386
387 @Override
388 public final OTSRoadNetwork getNetwork()
389 {
390 return null;
391 }
392 }
393
394 }