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 import static org.opentrafficsim.core.gtu.GTUType.CAR;
8
9 import java.awt.geom.Point2D;
10 import java.util.LinkedHashMap;
11 import java.util.Map;
12
13 import javax.media.j3d.BoundingBox;
14 import javax.media.j3d.Bounds;
15 import javax.vecmath.Point3d;
16
17 import org.djunits.unit.DurationUnit;
18 import org.djunits.unit.UNITS;
19 import org.djunits.value.vdouble.scalar.Duration;
20 import org.djunits.value.vdouble.scalar.Length;
21 import org.djunits.value.vdouble.scalar.Speed;
22 import org.djunits.value.vdouble.scalar.Time;
23 import org.junit.Test;
24 import org.locationtech.jts.geom.Coordinate;
25 import org.locationtech.jts.geom.Geometry;
26 import org.locationtech.jts.geom.GeometryFactory;
27 import org.opentrafficsim.core.compatibility.GTUCompatibility;
28 import org.opentrafficsim.core.dsol.AbstractOTSModel;
29 import org.opentrafficsim.core.dsol.OTSSimulator;
30 import org.opentrafficsim.core.dsol.OTSSimulatorInterface;
31 import org.opentrafficsim.core.geometry.OTSLine3D;
32 import org.opentrafficsim.core.geometry.OTSPoint3D;
33 import org.opentrafficsim.core.gtu.GTUType;
34 import org.opentrafficsim.core.network.LinkType;
35 import org.opentrafficsim.core.network.LongitudinalDirectionality;
36 import org.opentrafficsim.core.network.Network;
37 import org.opentrafficsim.core.network.Node;
38 import org.opentrafficsim.core.network.OTSNetwork;
39 import org.opentrafficsim.core.network.OTSNode;
40 import org.opentrafficsim.road.network.lane.changing.LaneKeepingPolicy;
41 import org.opentrafficsim.road.network.lane.changing.OvertakingConditions;
42
43 import nl.tudelft.simulation.dsol.SimRuntimeException;
44 import nl.tudelft.simulation.language.d3.DirectedPoint;
45
46
47
48
49
50
51
52
53
54
55
56 public class LaneTest implements UNITS
57 {
58
59
60
61
62 @Test
63 public void laneConstructorTest() throws Exception
64 {
65
66 Network network = new OTSNetwork("lane test network");
67 OTSNode nodeFrom = new OTSNode(network, "A", new OTSPoint3D(0, 0, 0));
68 OTSNode nodeTo = new OTSNode(network, "B", new OTSPoint3D(1000, 0, 0));
69
70 OTSPoint3D[] coordinates = new OTSPoint3D[2];
71 coordinates[0] = new OTSPoint3D(nodeFrom.getPoint().x, nodeFrom.getPoint().y, 0);
72 coordinates[1] = new OTSPoint3D(nodeTo.getPoint().x, nodeTo.getPoint().y, 0);
73 OTSSimulatorInterface simulator = new OTSSimulator();
74 Model model = new Model(simulator);
75 simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model);
76 CrossSectionLink link = new CrossSectionLink(network, "A to B", nodeFrom, nodeTo, LinkType.ROAD,
77 new OTSLine3D(coordinates), simulator, LaneKeepingPolicy.KEEPRIGHT);
78 Length startLateralPos = new Length(2, METER);
79 Length endLateralPos = new Length(5, METER);
80 Length startWidth = new Length(3, METER);
81 Length endWidth = new Length(4, METER);
82 GTUType gtuTypeCar = CAR;
83
84 GTUCompatibility<LaneType> gtuCompatibility = new GTUCompatibility<>((LaneType) null);
85 gtuCompatibility.addAllowedGTUType(GTUType.VEHICLE, LongitudinalDirectionality.DIR_PLUS);
86 LaneType laneType = new LaneType("One way", LaneType.FREEWAY, gtuCompatibility);
87 Map<GTUType, Speed> speedMap = new LinkedHashMap<>();
88 speedMap.put(GTUType.VEHICLE, new Speed(100, KM_PER_HOUR));
89
90
91 Lane lane = new Lane(link, "lane", startLateralPos, endLateralPos, startWidth, endWidth, laneType, speedMap,
92 new OvertakingConditions.LeftAndRight());
93
94 assertEquals("PrevLanes should be empty", 0, lane.prevLanes(gtuTypeCar).size());
95 assertEquals("NextLanes should be empty", 0, lane.nextLanes(gtuTypeCar).size());
96 double approximateLengthOfContour =
97 2 * nodeFrom.getPoint().distanceSI(nodeTo.getPoint()) + startWidth.getSI() + endWidth.getSI();
98 assertEquals("Length of contour is approximately " + approximateLengthOfContour, approximateLengthOfContour,
99 lane.getContour().getLengthSI(), 0.1);
100 assertEquals("Directionality should be " + LongitudinalDirectionality.DIR_PLUS, LongitudinalDirectionality.DIR_PLUS,
101 lane.getLaneType().getDirectionality(GTUType.VEHICLE));
102 assertEquals("SpeedLimit should be " + (new Speed(100, KM_PER_HOUR)), new Speed(100, KM_PER_HOUR),
103 lane.getSpeedLimit(GTUType.VEHICLE));
104 assertEquals("There should be no GTUs on the lane", 0, lane.getGtuList().size());
105 assertEquals("LaneType should be " + laneType, laneType, lane.getLaneType());
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
136
137
138 coordinates = new OTSPoint3D[3];
139 coordinates[0] = new OTSPoint3D(nodeFrom.getPoint().x, nodeFrom.getPoint().y, 0);
140 coordinates[1] = new OTSPoint3D(200, 100);
141 coordinates[2] = new OTSPoint3D(nodeTo.getPoint().x, nodeTo.getPoint().y, 0);
142 link = new CrossSectionLink(network, "A to B with Kink", nodeFrom, nodeTo, LinkType.ROAD, new OTSLine3D(coordinates),
143 simulator, LaneKeepingPolicy.KEEPRIGHT);
144
145 lane = new Lane(link, "lane.1", startLateralPos, endLateralPos, startWidth, endWidth, laneType, speedMap,
146 new OvertakingConditions.LeftAndRight());
147
148 assertEquals("PrevLanes should be empty", 0, lane.prevLanes(gtuTypeCar).size());
149 assertEquals("NextLanes should be empty", 0, lane.nextLanes(gtuTypeCar).size());
150 approximateLengthOfContour = 2 * (coordinates[0].distanceSI(coordinates[1]) + coordinates[1].distanceSI(coordinates[2]))
151 + startWidth.getSI() + endWidth.getSI();
152 assertEquals("Length of contour is approximately " + approximateLengthOfContour, approximateLengthOfContour,
153 lane.getContour().getLengthSI(), 4);
154 assertEquals("There should be no GTUs on the lane", 0, lane.getGtuList().size());
155 assertEquals("LaneType should be " + laneType, laneType, lane.getLaneType());
156
157 Length startLateralPos2 = new Length(-8, METER);
158 Length endLateralPos2 = new Length(-5, METER);
159
160 Lane lane2 = new Lane(link, "lane.2", startLateralPos2, endLateralPos2, startWidth, endWidth, laneType, speedMap,
161 new OvertakingConditions.LeftAndRight());
162
163 assertEquals("PrevLanes should be empty", 0, lane2.prevLanes(gtuTypeCar).size());
164 assertEquals("NextLanes should be empty", 0, lane2.nextLanes(gtuTypeCar).size());
165 approximateLengthOfContour = 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 final void contourTest() throws Exception
180 {
181 final int[] startPositions = { 0, 1, -1, 20, -20 };
182 final double[] angles = { 0, Math.PI * 0.01, Math.PI / 3, Math.PI / 2, Math.PI * 2 / 3, Math.PI * 0.99, Math.PI,
183 Math.PI * 1.01, Math.PI * 4 / 3, Math.PI * 3 / 2, Math.PI * 1.99, Math.PI * 2, Math.PI * (-0.2) };
184 LaneType laneType = LaneType.TWO_WAY_LANE;
185 Map<GTUType, LongitudinalDirectionality> directionalityMap = new LinkedHashMap<>();
186 directionalityMap.put(GTUType.VEHICLE, LongitudinalDirectionality.DIR_PLUS);
187 Map<GTUType, Speed> speedMap = new LinkedHashMap<>();
188 speedMap.put(GTUType.VEHICLE, new Speed(50, KM_PER_HOUR));
189 int laneNum = 0;
190 for (int xStart : startPositions)
191 {
192 for (int yStart : startPositions)
193 {
194 for (double angle : angles)
195 {
196 Network network = new OTSNetwork("contour test network");
197 OTSNode start = new OTSNode(network, "start", new OTSPoint3D(xStart, yStart));
198 double linkLength = 1000;
199 double xEnd = xStart + linkLength * Math.cos(angle);
200 double yEnd = yStart + linkLength * Math.sin(angle);
201 OTSNode end = new OTSNode(network, "end", new OTSPoint3D(xEnd, yEnd));
202 OTSPoint3D[] coordinates = new OTSPoint3D[2];
203 coordinates[0] = start.getPoint();
204 coordinates[1] = end.getPoint();
205 OTSLine3D line = new OTSLine3D(coordinates);
206 OTSSimulatorInterface simulator = new OTSSimulator();
207 Model model = new Model(simulator);
208 simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model);
209 CrossSectionLink link = new CrossSectionLink(network, "A to B", start, end, LinkType.ROAD, line, simulator,
210 LaneKeepingPolicy.KEEPRIGHT);
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 = new Lane(link, "lane." + ++laneNum, new Length(startLateralOffset, METER),
222 new Length(endLateralOffset, METER), new Length(startWidth, METER),
223 new Length(endWidth, METER), laneType, speedMap,
224 new OvertakingConditions.LeftAndRight());
225 final Geometry geometry = lane.getContour().getLineString();
226 assertNotNull("geometry of the lane should not be null", geometry);
227
228
229 checkInside(lane, 1, startLateralOffset, true);
230
231 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset, true);
232
233 checkInside(lane, -1, startLateralOffset, false);
234
235 checkInside(lane, link.getLength().getSI() + 1, endLateralOffset, false);
236
237 checkInside(lane, 1, startLateralOffset - startWidth / 2 - 1, false);
238
239 checkInside(lane, 1, startLateralOffset + startWidth / 2 + 1, false);
240
241 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset - endWidth / 2 - 1, false);
242
243 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset + endWidth / 2 + 1, false);
244
245 DirectedPoint l = lane.getLocation();
246 Bounds bb = lane.getBounds();
247
248
249
250
251 Point2D.Double[] cornerPoints = new Point2D.Double[4];
252 cornerPoints[0] =
253 new Point2D.Double(xStart - (startLateralOffset + startWidth / 2) * Math.sin(angle),
254 yStart + (startLateralOffset + startWidth / 2) * Math.cos(angle));
255 cornerPoints[1] =
256 new Point2D.Double(xStart - (startLateralOffset - startWidth / 2) * Math.sin(angle),
257 yStart + (startLateralOffset - startWidth / 2) * Math.cos(angle));
258 cornerPoints[2] = new Point2D.Double(xEnd - (endLateralOffset + endWidth / 2) * Math.sin(angle),
259 yEnd + (endLateralOffset + endWidth / 2) * Math.cos(angle));
260 cornerPoints[3] = new Point2D.Double(xEnd - (endLateralOffset - endWidth / 2) * Math.sin(angle),
261 yEnd + (endLateralOffset - endWidth / 2) * Math.cos(angle));
262
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, final boolean expectedResult)
314 {
315 CrossSectionLink parentLink = lane.getParentLink();
316 Node start = parentLink.getStartNode();
317 Node end = parentLink.getEndNode();
318 double startX = start.getPoint().x;
319 double startY = start.getPoint().y;
320 double endX = end.getPoint().x;
321 double endY = end.getPoint().y;
322 double length = Math.sqrt((endX - startX) * (endX - startX) + (endY - startY) * (endY - startY));
323 double ratio = longitudinal / length;
324 double designLineX = startX + (endX - startX) * ratio;
325 double designLineY = startY + (endY - startY) * ratio;
326 double lateralAngle = Math.atan2(endY - startY, endX - startX) + Math.PI / 2;
327 double px = designLineX + lateral * Math.cos(lateralAngle);
328 double py = designLineY + lateral * Math.sin(lateralAngle);
329 Geometry contour = lane.getContour().getLineString();
330 GeometryFactory factory = new GeometryFactory();
331 Geometry p = factory.createPoint(new Coordinate(px, py));
332
333
334 boolean result = contour.contains(p);
335 Coordinate[] polygon = contour.getCoordinates();
336 result = pointInsidePolygon(new Coordinate(px, py), polygon);
337 if (expectedResult)
338 {
339 assertTrue("Point at " + longitudinal + " along and " + lateral + " lateral is within lane", result);
340 }
341 else
342 {
343 assertFalse("Point at " + longitudinal + " along and " + lateral + " lateral is outside lane", result);
344 }
345 }
346
347
348
349
350
351
352
353
354
355
356 private boolean pointInsidePolygon(final Coordinate point, final Coordinate[] polygon)
357 {
358 boolean result = false;
359 for (int i = 0, j = polygon.length - 1; i < polygon.length; j = i++)
360 {
361 if ((polygon[i].y > point.y) != (polygon[j].y > point.y)
362 && point.x < (polygon[j].x - polygon[i].x) * (point.y - polygon[i].y) / (polygon[j].y - polygon[i].y)
363 + polygon[i].x)
364 {
365 result = !result;
366 }
367 }
368 return result;
369 }
370
371
372 protected static class Model extends AbstractOTSModel
373 {
374
375 private static final long serialVersionUID = 20141027L;
376
377
378
379
380 public Model(final OTSSimulatorInterface simulator)
381 {
382 super(simulator);
383 }
384
385
386 @Override
387 public final void constructModel() throws SimRuntimeException
388 {
389
390 }
391
392
393 @Override
394 public final OTSNetwork getNetwork()
395 {
396 return null;
397 }
398 }
399
400 }