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