1 package org.opentrafficsim.road.network.lane;
2
3 import static org.junit.jupiter.api.Assertions.assertEquals;
4 import static org.junit.jupiter.api.Assertions.assertFalse;
5 import static org.junit.jupiter.api.Assertions.assertTrue;
6 import static org.junit.jupiter.api.Assertions.fail;
7
8 import java.awt.geom.Point2D;
9 import java.util.ArrayList;
10 import java.util.LinkedHashMap;
11 import java.util.List;
12 import java.util.Map;
13 import java.util.SortedMap;
14
15 import javax.naming.NamingException;
16
17 import org.djunits.unit.DurationUnit;
18 import org.djunits.unit.util.UNITS;
19 import org.djunits.value.vdouble.scalar.Direction;
20 import org.djunits.value.vdouble.scalar.Duration;
21 import org.djunits.value.vdouble.scalar.Length;
22 import org.djunits.value.vdouble.scalar.Speed;
23 import org.djutils.draw.bounds.Bounds;
24 import org.djutils.draw.function.ContinuousPiecewiseLinearFunction;
25 import org.djutils.draw.line.Polygon2d;
26 import org.djutils.draw.point.DirectedPoint2d;
27 import org.djutils.draw.point.Point2d;
28 import org.djutils.event.Event;
29 import org.djutils.event.EventListener;
30 import org.junit.jupiter.api.Test;
31 import org.mockito.Mockito;
32 import org.opentrafficsim.base.geometry.OtsLine2d;
33 import org.opentrafficsim.core.definitions.DefaultsNl;
34 import org.opentrafficsim.core.dsol.AbstractOtsModel;
35 import org.opentrafficsim.core.dsol.OtsSimulator;
36 import org.opentrafficsim.core.dsol.OtsSimulatorInterface;
37 import org.opentrafficsim.core.gtu.GtuType;
38 import org.opentrafficsim.core.network.LateralDirectionality;
39 import org.opentrafficsim.core.network.NetworkException;
40 import org.opentrafficsim.core.network.Node;
41 import org.opentrafficsim.core.perception.HistoryManagerDevs;
42 import org.opentrafficsim.road.definitions.DefaultsRoadNl;
43 import org.opentrafficsim.road.mock.MockDevsSimulator;
44 import org.opentrafficsim.road.network.CrossSectionGeometry;
45 import org.opentrafficsim.road.network.CrossSectionLink;
46 import org.opentrafficsim.road.network.Lane;
47 import org.opentrafficsim.road.network.LaneGeometryUtil;
48 import org.opentrafficsim.road.network.LaneKeepingPolicy;
49 import org.opentrafficsim.road.network.LaneType;
50 import org.opentrafficsim.road.network.RoadNetwork;
51 import org.opentrafficsim.road.network.object.LaneBasedObject;
52 import org.opentrafficsim.road.network.object.detector.LaneDetector;
53 import org.opentrafficsim.road.network.speed.LaneSpeedLimits;
54
55 import nl.tudelft.simulation.dsol.SimRuntimeException;
56
57
58
59
60
61
62
63
64
65 public final class LaneTest implements UNITS
66 {
67
68
69 private LaneTest()
70 {
71
72 }
73
74
75
76
77
78 @Test
79 public void laneConstructorTest() throws Exception
80 {
81 OtsSimulatorInterface simulator = new OtsSimulator("LaneTest");
82 RoadNetwork network = new RoadNetwork("lane test network", simulator);
83 Model model = new Model(simulator);
84 simulator.initialize(Duration.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model,
85 HistoryManagerDevs.noHistory(simulator));
86
87 Node nodeFrom = new Node(network, "A", new Point2d(0, 0), Direction.ZERO);
88 Node nodeTo = new Node(network, "B", new Point2d(1000, 0), Direction.ZERO);
89
90 Point2d[] coordinates = new Point2d[2];
91 coordinates[0] = nodeFrom.getPoint();
92 coordinates[1] = nodeTo.getPoint();
93 CrossSectionLink link = new CrossSectionLink(network, "A to B", nodeFrom, nodeTo, DefaultsNl.FREEWAY,
94 new OtsLine2d(coordinates), null, LaneKeepingPolicy.KEEPRIGHT);
95 Length startLateralPos = new Length(2, METER);
96 Length endLateralPos = new Length(5, METER);
97 Length startWidth = new Length(3, METER);
98 Length endWidth = new Length(4, METER);
99 GtuType gtuTypeCar = DefaultsNl.CAR;
100
101 LaneType laneType = new LaneType("One way", DefaultsRoadNl.FREEWAY);
102 laneType.addCompatibleGtuType(DefaultsNl.VEHICLE);
103 Map<GtuType, Speed> speedMap = new LinkedHashMap<>();
104 speedMap.put(DefaultsNl.VEHICLE, new Speed(100, KM_PER_HOUR));
105
106
107 Lane lane = LaneGeometryUtil.createStraightLane(link, "lane", startLateralPos, endLateralPos, startWidth, endWidth,
108 laneType, new LaneSpeedLimits(speedMap));
109
110 assertEquals(network, link.getNetwork(), "Link returns network");
111 assertEquals(network, lane.getNetwork(), "Lane returns network");
112 assertEquals(0, lane.prevLanes(gtuTypeCar).size(), "PrevLanes should be empty");
113 assertEquals(0, lane.nextLanes(gtuTypeCar).size(), "NextLanes should be empty");
114 double approximateLengthOfContour =
115 2 * nodeFrom.getPoint().distance(nodeTo.getPoint()) + startWidth.getSI() + endWidth.getSI();
116 assertEquals(approximateLengthOfContour, lane.getAbsoluteContour().getLength(), 0.1,
117 "Length of contour is approximately " + approximateLengthOfContour);
118 assertEquals(new Speed(100, KM_PER_HOUR), lane.getSpeedLimits(DefaultsNl.VEHICLE).gtuTypeSpeedLimit().speed(),
119 "SpeedLimit should be " + (new Speed(100, KM_PER_HOUR)));
120 assertEquals(0, lane.getGtuList().size(), "There should be no GTUs on the lane");
121 assertEquals(laneType, lane.getType(), "LaneType should be " + laneType);
122
123 for (int i = 0; i < 10; i++)
124 {
125 double expectedLateralCenterOffset =
126 startLateralPos.getSI() + (endLateralPos.getSI() - startLateralPos.getSI()) * i / 10;
127 assertEquals(expectedLateralCenterOffset, lane.getLateralCenterPosition(i / 10.0).getSI(), 0.01,
128 String.format("Lateral offset at %d%% should be %.3fm", 10 * i, expectedLateralCenterOffset));
129 Length longitudinalPosition = new Length(lane.getLength().getSI() * i / 10, METER);
130 assertEquals(expectedLateralCenterOffset, lane.getLateralCenterPosition(longitudinalPosition).getSI(), 0.01,
131 "Lateral offset at " + longitudinalPosition + " should be " + expectedLateralCenterOffset);
132 double expectedWidth = startWidth.getSI() + (endWidth.getSI() - startWidth.getSI()) * i / 10;
133 assertEquals(expectedWidth, lane.getWidth(i / 10.0).getSI(), 0.0001,
134 String.format("Width at %d%% should be %.3fm", 10 * i, expectedWidth));
135 assertEquals(expectedWidth, lane.getWidth(longitudinalPosition).getSI(), 0.0001,
136 "Width at " + longitudinalPosition + " should be " + expectedWidth);
137 double expectedLeftOffset = expectedLateralCenterOffset - expectedWidth / 2;
138
139 assertEquals(expectedLeftOffset, lane.getLateralBoundaryPosition(LateralDirectionality.LEFT, i / 10.0).getSI(),
140 0.001, String.format("Left edge at %d%% should be %.3fm", 10 * i, expectedLeftOffset));
141 assertEquals(expectedLeftOffset,
142 lane.getLateralBoundaryPosition(LateralDirectionality.LEFT, longitudinalPosition).getSI(), 0.001,
143 "Left edge at " + longitudinalPosition + " should be " + expectedLeftOffset);
144 double expectedRightOffset = expectedLateralCenterOffset + expectedWidth / 2;
145 assertEquals(expectedRightOffset, lane.getLateralBoundaryPosition(LateralDirectionality.RIGHT, i / 10.0).getSI(),
146 0.001, String.format("Right edge at %d%% should be %.3fm", 10 * i, expectedRightOffset));
147 assertEquals(expectedRightOffset,
148 lane.getLateralBoundaryPosition(LateralDirectionality.RIGHT, longitudinalPosition).getSI(), 0.001,
149 "Right edge at " + longitudinalPosition + " should be " + expectedRightOffset);
150 }
151
152
153
154 coordinates = new Point2d[3];
155 coordinates[0] = new Point2d(nodeFrom.getPoint().x, nodeFrom.getPoint().y);
156 coordinates[1] = new Point2d(200, 100);
157 coordinates[2] = new Point2d(nodeTo.getPoint().x, nodeTo.getPoint().y);
158 link = new CrossSectionLink(network, "A to B with Kink", nodeFrom, nodeTo, DefaultsNl.FREEWAY,
159 new OtsLine2d(coordinates), null, LaneKeepingPolicy.KEEPRIGHT);
160 lane = LaneGeometryUtil.createStraightLane(link, "lane.1", startLateralPos, endLateralPos, startWidth, endWidth,
161 laneType, new LaneSpeedLimits(speedMap));
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193 OtsLine2d centerLine = new OtsLine2d(new Point2d(0.0, 0.0), new Point2d(100.0, 0.0));
194 Polygon2d contour = new Polygon2d(new Point2d(0.0, -1.75), new Point2d(100.0, -1.75), new Point2d(100.0, 1.75),
195 new Point2d(0.0, -1.75));
196 ContinuousPiecewiseLinearFunction offsetFunc = ContinuousPiecewiseLinearFunction.of(0.0, startLateralPos.si);
197 ContinuousPiecewiseLinearFunction widthFunc = ContinuousPiecewiseLinearFunction.of(0.0, startWidth.si);
198 lane = new Lane(link, "lanex", new CrossSectionGeometry(centerLine, contour, offsetFunc, widthFunc), laneType,
199 new LaneSpeedLimits(speedMap));
200 sensorTest(lane);
201 }
202
203
204
205
206
207
208 public void sensorTest(final Lane lane) throws NetworkException
209 {
210 assertEquals(0, lane.getDetectors().size(), "List of sensor is initially empty");
211 Listener listener = new Listener();
212 double length = lane.getLength().si;
213 lane.addListener(listener, Lane.DETECTOR_ADD_EVENT);
214 lane.addListener(listener, Lane.DETECTOR_REMOVE_EVENT);
215 assertEquals(0, listener.events.size(), "event list is initially empty");
216 LaneDetector sensor1 = new MockSensor("sensor1", Length.ofSI(length / 4)).getMock();
217 lane.addDetector(sensor1);
218 assertEquals(1, listener.events.size(), "event list now contains one event");
219 assertEquals(listener.events.get(0).getType(), Lane.DETECTOR_ADD_EVENT, "event indicates that a sensor got added");
220 assertEquals(1, lane.getDetectors().size(), "lane now contains one sensor");
221 assertEquals(sensor1, lane.getDetectors().get(0), "sensor on lane is sensor1");
222 LaneDetector sensor2 = new MockSensor("sensor2", Length.ofSI(length / 2)).getMock();
223 lane.addDetector(sensor2);
224 assertEquals(2, listener.events.size(), "event list now contains two events");
225 assertEquals(listener.events.get(1).getType(), Lane.DETECTOR_ADD_EVENT, "event indicates that a sensor got added");
226 List<LaneDetector> sensors = lane.getDetectors();
227 assertEquals(2, sensors.size(), "lane now contains two sensors");
228 assertTrue(sensors.contains(sensor1), "sensor list contains sensor1");
229 assertTrue(sensors.contains(sensor2), "sensor list contains sensor2");
230 sensors = lane.getDetectors(Length.ZERO, Length.ofSI(length / 3), DefaultsNl.VEHICLE);
231 assertEquals(1, sensors.size(), "first third of lane contains 1 sensor");
232 assertTrue(sensors.contains(sensor1), "sensor list contains sensor1");
233 sensors = lane.getDetectors(Length.ofSI(length / 3), Length.ofSI(length), DefaultsNl.VEHICLE);
234 assertEquals(1, sensors.size(), "last two-thirds of lane contains 1 sensor");
235 assertTrue(sensors.contains(sensor2), "sensor list contains sensor2");
236 sensors = lane.getDetectors(DefaultsNl.VEHICLE);
237
238 assertEquals(2, sensors.size(), "sensor list contains two sensors");
239 assertTrue(sensors.contains(sensor1), "sensor list contains sensor1");
240 assertTrue(sensors.contains(sensor2), "sensor list contains sensor2");
241 sensors = lane.getDetectors(DefaultsNl.VEHICLE);
242
243 assertEquals(2, sensors.size(), "sensor list contains two sensors");
244 assertTrue(sensors.contains(sensor1), "sensor list contains sensor1");
245 assertTrue(sensors.contains(sensor2), "sensor list contains sensor2");
246 SortedMap<Double, List<LaneDetector>> sensorMap = lane.getDetectorMap(DefaultsNl.VEHICLE);
247 assertEquals(2, sensorMap.size(), "sensor map contains two entries");
248 for (Double d : sensorMap.keySet())
249 {
250 List<LaneDetector> sensorsAtD = sensorMap.get(d);
251 assertEquals(1, sensorsAtD.size(), "There is one sensor at position d");
252 assertEquals(d < length / 3 ? sensor1 : sensor2, sensorsAtD.get(0),
253 "Sensor map contains the correct sensor at the correct distance");
254 }
255
256 lane.removeDetector(sensor1);
257 assertEquals(3, listener.events.size(), "event list now contains three events");
258 assertEquals(listener.events.get(2).getType(), Lane.DETECTOR_REMOVE_EVENT, "event indicates that a sensor got removed");
259 sensors = lane.getDetectors();
260 assertEquals(1, sensors.size(), "lane now contains one sensor");
261 assertTrue(sensors.contains(sensor2), "sensor list contains sensor2");
262 try
263 {
264 lane.removeDetector(sensor1);
265 fail("Removing a sensor twice should have thrown a NetworkException");
266 }
267 catch (NetworkException ne)
268 {
269
270 }
271 try
272 {
273 lane.addDetector(sensor2);
274 fail("Adding a sensor twice should have thrown a NetworkException");
275 }
276 catch (NetworkException ne)
277 {
278
279 }
280 LaneDetector badSensor = new MockSensor("sensor3", Length.ofSI(-0.1)).getMock();
281 try
282 {
283 lane.addDetector(badSensor);
284 fail("Adding a sensor at negative position should have thrown a NetworkException");
285 }
286 catch (NetworkException ne)
287 {
288
289 }
290 badSensor = new MockSensor("sensor4", Length.ofSI(length + 0.1)).getMock();
291 try
292 {
293 lane.addDetector(badSensor);
294 fail("Adding a sensor at position beyond the end of the lane should have thrown a NetworkException");
295 }
296 catch (NetworkException ne)
297 {
298
299 }
300 lane.removeDetector(sensor2);
301 List<LaneBasedObject> lboList = lane.getLaneBasedObjects();
302 assertEquals(0, lboList.size(), "lane initially contains zero lane based objects");
303 LaneBasedObject lbo1 = new MockLaneBasedObject("lbo1", Length.ofSI(length / 4)).getMock();
304 listener.getEvents().clear();
305 lane.addListener(listener, Lane.OBJECT_ADD_EVENT);
306 lane.addListener(listener, Lane.OBJECT_REMOVE_EVENT);
307 lane.addLaneBasedObject(lbo1);
308 assertEquals(1, listener.getEvents().size(), "adding a lane based object cause the lane to emit an event");
309 assertEquals(Lane.OBJECT_ADD_EVENT, listener.getEvents().get(0).getType(), "The emitted event was a OBJECT_ADD_EVENT");
310 LaneBasedObject lbo2 = new MockLaneBasedObject("lbo2", Length.ofSI(3 * length / 4)).getMock();
311 lane.addLaneBasedObject(lbo2);
312 lboList = lane.getLaneBasedObjects();
313 assertEquals(2, lboList.size(), "lane based object list now contains two objects");
314 assertTrue(lboList.contains(lbo1), "lane base object list contains lbo1");
315 assertTrue(lboList.contains(lbo2), "lane base object list contains lbo2");
316 lboList = lane.getLaneBasedObjects(Length.ZERO, Length.ofSI(length / 2));
317 assertEquals(1, lboList.size(), "first half of lane contains one object");
318 assertEquals(lbo1, lboList.get(0), "object in first haf of lane is lbo1");
319 lboList = lane.getLaneBasedObjects(Length.ofSI(length / 2), Length.ofSI(length));
320 assertEquals(1, lboList.size(), "second half of lane contains one object");
321 assertEquals(lbo2, lboList.get(0), "object in second haf of lane is lbo2");
322 SortedMap<Double, List<LaneBasedObject>> sortedMap = lane.getLaneBasedObjectMap();
323 assertEquals(2, sortedMap.size(), "sorted map contains two objects");
324 for (Double d : sortedMap.keySet())
325 {
326 List<LaneBasedObject> objectsAtD = sortedMap.get(d);
327 assertEquals(1, objectsAtD.size(), "There is one object at position d");
328 assertEquals(d < length / 2 ? lbo1 : lbo2, objectsAtD.get(0), "Object at position d is the expected one");
329 }
330
331 for (double fraction : new double[] {-0.5, 0, 0.2, 0.5, 0.9, 1.0, 2})
332 {
333 double positionSI = length * fraction;
334 double fractionSI = lane.fractionSI(positionSI);
335 assertEquals(fraction, fractionSI, 0.0001, "fractionSI matches fraction");
336
337 LaneBasedObject nextObject = positionSI < lbo1.getLongitudinalPosition().si ? lbo1
338 : positionSI < lbo2.getLongitudinalPosition().si ? lbo2 : null;
339 List<LaneBasedObject> expected = new ArrayList<>();
340 if (null != nextObject)
341 {
342 expected.add(nextObject);
343 }
344 List<LaneBasedObject> got = lane.getObjectAhead(Length.ofSI(positionSI));
345 assertEquals(expected, got, "First bunch of objects ahead of d");
346
347 nextObject = positionSI > lbo2.getLongitudinalPosition().si ? lbo2
348 : positionSI > lbo1.getLongitudinalPosition().si ? lbo1 : null;
349 expected = new ArrayList<>();
350 if (null != nextObject)
351 {
352 expected.add(nextObject);
353 }
354 got = lane.getObjectBehind(Length.ofSI(positionSI));
355 assertEquals(expected, got, "First bunch of objects behind d");
356 }
357
358 lane.removeLaneBasedObject(lbo1);
359 assertEquals(3, listener.getEvents().size(), "removing a lane based object caused the lane to emit an event");
360 assertEquals(Lane.OBJECT_REMOVE_EVENT, listener.getEvents().get(2).getType(),
361 "removing a lane based object caused the lane to emit OBJECT_REMOVE_EVENT");
362 try
363 {
364 lane.removeLaneBasedObject(lbo1);
365 fail("Removing a lane bases object that was already removed should have caused a NetworkException");
366 }
367 catch (NetworkException ne)
368 {
369
370 }
371 try
372 {
373 lane.addLaneBasedObject(lbo2);
374 fail("Adding a lane base object that was already added should have caused a NetworkException");
375 }
376 catch (NetworkException ne)
377 {
378
379 }
380 LaneBasedObject badLBO = new MockLaneBasedObject("badLBO", Length.ofSI(-0.1)).getMock();
381 try
382 {
383 lane.addLaneBasedObject(badLBO);
384 fail("Adding a lane based object at negative position should have thrown a NetworkException");
385 }
386 catch (NetworkException ne)
387 {
388
389 }
390 badLBO = new MockLaneBasedObject("badLBO", Length.ofSI(length + 0.1)).getMock();
391 try
392 {
393 lane.addLaneBasedObject(badLBO);
394 fail("Adding a lane based object at position beyond end of lane should have thrown a NetworkException");
395 }
396 catch (NetworkException ne)
397 {
398
399 }
400 }
401
402
403
404
405 class Listener implements EventListener
406 {
407
408 private List<Event> events = new ArrayList<>();
409
410
411
412
413 Listener()
414 {
415
416 }
417
418 @Override
419 public void notify(final Event event)
420 {
421 this.events.add(event);
422 }
423
424
425
426
427
428 public List<Event> getEvents()
429 {
430 return this.events;
431 }
432
433 }
434
435
436
437
438 class MockSensor
439 {
440
441 private final LaneDetector mockSensor;
442
443
444 private final String id;
445
446
447 private final Length position;
448
449
450 private final OtsSimulatorInterface simulator = MockDevsSimulator.createMock();
451
452
453
454
455
456
457 MockSensor(final String id, final Length position)
458 {
459 this.mockSensor = Mockito.mock(LaneDetector.class);
460 this.id = id;
461 this.position = position;
462 Mockito.when(this.mockSensor.getId()).thenReturn(this.id);
463 Mockito.when(this.mockSensor.getLongitudinalPosition()).thenReturn(this.position);
464 Mockito.when(this.mockSensor.getSimulator()).thenReturn(this.simulator);
465 Mockito.when(this.mockSensor.getFullId()).thenReturn(this.id);
466 Mockito.when(this.mockSensor.isCompatible(Mockito.any())).thenReturn(true);
467 }
468
469
470
471
472
473 public LaneDetector getMock()
474 {
475 return this.mockSensor;
476 }
477
478
479
480
481
482 public Length getLongitudinalPosition()
483 {
484 return this.position;
485 }
486
487 @Override
488 public String toString()
489 {
490 return "MockSensor [mockSensor=" + this.mockSensor + ", id=" + this.id + ", position=" + this.position + "]";
491 }
492
493 }
494
495
496
497
498 class MockLaneBasedObject
499 {
500
501 private final LaneBasedObject mockLaneBasedObject;
502
503
504 private final String id;
505
506
507 private final Length position;
508
509
510
511
512
513
514 MockLaneBasedObject(final String id, final Length position)
515 {
516 this.mockLaneBasedObject = Mockito.mock(LaneDetector.class);
517 this.id = id;
518 this.position = position;
519 Mockito.when(this.mockLaneBasedObject.getId()).thenReturn(this.id);
520 Mockito.when(this.mockLaneBasedObject.getLongitudinalPosition()).thenReturn(this.position);
521 Mockito.when(this.mockLaneBasedObject.getFullId()).thenReturn(this.id);
522 }
523
524
525
526
527
528 public LaneBasedObject getMock()
529 {
530 return this.mockLaneBasedObject;
531 }
532
533
534
535
536
537 public Length getLongitudinalPosition()
538 {
539 return this.position;
540 }
541
542 @Override
543 public String toString()
544 {
545 return "MockLaneBasedObject [mockLaneBasedObject=" + this.mockLaneBasedObject + ", id=" + this.id + ", position="
546 + this.position + "]";
547 }
548
549 }
550
551
552
553
554
555
556
557
558 @Test
559 public final void lateralOffsetTest() throws NetworkException, SimRuntimeException, NamingException
560 {
561 Point2d from = new Point2d(10, 10);
562 Point2d to = new Point2d(1010, 10);
563 OtsSimulatorInterface simulator = new OtsSimulator("LaneTest");
564 Model model = new Model(simulator);
565 simulator.initialize(Duration.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model,
566 HistoryManagerDevs.noHistory(simulator));
567 RoadNetwork network = new RoadNetwork("contour test network", simulator);
568 LaneType laneType = DefaultsRoadNl.TWO_WAY_LANE;
569 laneType.addCompatibleGtuType(DefaultsNl.VEHICLE);
570 Map<GtuType, Speed> speedMap = new LinkedHashMap<>();
571 speedMap.put(DefaultsNl.VEHICLE, new Speed(50, KM_PER_HOUR));
572 Node start = new Node(network, "start", from, Direction.ZERO);
573 Node end = new Node(network, "end", to, Direction.ZERO);
574 Point2d[] coordinates = new Point2d[2];
575 coordinates[0] = start.getPoint();
576 coordinates[1] = end.getPoint();
577 OtsLine2d line = new OtsLine2d(coordinates);
578 CrossSectionLink link =
579 new CrossSectionLink(network, "A to B", start, end, DefaultsNl.ROAD, line, null, LaneKeepingPolicy.KEEPRIGHT);
580 Length offsetAtStart = Length.ofSI(5);
581 Length offsetAtEnd = Length.ofSI(15);
582 Length width = Length.ofSI(4);
583 Lane lane = LaneGeometryUtil.createStraightLane(link, "lane", offsetAtStart, offsetAtEnd, width, width, laneType,
584 new LaneSpeedLimits(speedMap));
585 OtsLine2d laneCenterLine = lane.getCenterLine();
586
587 List<Point2d> points = laneCenterLine.getPointList();
588 double prev = offsetAtStart.si + from.y;
589 double prevRatio = 0;
590 double prevDirection = 0;
591 for (int i = 0; i < points.size(); i++)
592 {
593 Point2d p = points.get(i);
594 double relativeLength = p.x - from.x;
595 double ratio = relativeLength / (to.x - from.x);
596 double actualOffset = p.y;
597 if (0 == i)
598 {
599 assertEquals(offsetAtStart.si + from.y, actualOffset, 0.001, "first point must have offset at start");
600 }
601 if (points.size() - 1 == i)
602 {
603 assertEquals(offsetAtEnd.si + from.y, actualOffset, 0.001, "last point must have offset at end");
604 }
605
606 double delta = actualOffset - prev;
607 assertTrue(delta >= 0, "delta must be nonnegative");
608 if (i > 0)
609 {
610 Point2d prevPoint = points.get(i - 1);
611 double direction = Math.atan2(p.y - prevPoint.y, p.x - prevPoint.x);
612
613 assertTrue(direction > 0, "Direction of lane center line is > 0");
614 if (ratio < 0.5)
615 {
616 assertTrue(direction > prevDirection, "in first half direction is increasing");
617 }
618 else if (prevRatio > 0.5)
619 {
620 assertTrue(direction < prevDirection, "in second half direction is decreasing");
621 }
622 prevDirection = direction;
623 prevRatio = ratio;
624 }
625 }
626 }
627
628
629
630
631
632
633 @Test
634 public final void contourTest() throws Exception
635 {
636 final int[] startPositions = {0, 1, -1, 20, -20};
637 final double[] angles = {0, Math.PI * 0.01, Math.PI / 3, Math.PI / 2, Math.PI * 2 / 3, Math.PI * 0.99, Math.PI,
638 Math.PI * 1.01, Math.PI * 4 / 3, Math.PI * 3 / 2, Math.PI * 1.99, Math.PI * 2, Math.PI * (-0.2)};
639 int laneNum = 0;
640 for (int xStart : startPositions)
641 {
642 for (int yStart : startPositions)
643 {
644 for (double angle : angles)
645 {
646 OtsSimulatorInterface simulator = new OtsSimulator("LaneTest");
647 Model model = new Model(simulator);
648 simulator.initialize(Duration.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model,
649 HistoryManagerDevs.noHistory(simulator));
650 RoadNetwork network = new RoadNetwork("contour test network", simulator);
651 LaneType laneType = DefaultsRoadNl.TWO_WAY_LANE;
652 laneType.addCompatibleGtuType(DefaultsNl.VEHICLE);
653 Map<GtuType, Speed> speedMap = new LinkedHashMap<>();
654 speedMap.put(DefaultsNl.VEHICLE, new Speed(50, KM_PER_HOUR));
655 Node start = new Node(network, "start", new Point2d(xStart, yStart), Direction.ofSI(angle));
656 double linkLength = 1000;
657 double xEnd = xStart + linkLength * Math.cos(angle);
658 double yEnd = yStart + linkLength * Math.sin(angle);
659 Node end = new Node(network, "end", new Point2d(xEnd, yEnd), Direction.ofSI(angle));
660 Point2d[] coordinates = new Point2d[2];
661 coordinates[0] = start.getPoint();
662 coordinates[1] = end.getPoint();
663 OtsLine2d line = new OtsLine2d(coordinates);
664 CrossSectionLink link = new CrossSectionLink(network, "A to B", start, end, DefaultsNl.ROAD, line, null,
665 LaneKeepingPolicy.KEEPRIGHT);
666 final int[] lateralOffsets = {-10, -3, -1, 0, 1, 3, 10};
667 for (int startLateralOffset : lateralOffsets)
668 {
669 for (int endLateralOffset : lateralOffsets)
670 {
671 int startWidth = 4;
672 for (int endWidth : new int[] {2, 4, 6})
673 {
674
675
676 Lane lane = LaneGeometryUtil.createStraightLane(link, "lane." + ++laneNum,
677 new Length(startLateralOffset, METER), new Length(endLateralOffset, METER),
678 new Length(startWidth, METER), new Length(endWidth, METER), laneType,
679 new LaneSpeedLimits(speedMap));
680
681
682 checkInside(lane, 1, startLateralOffset, true);
683
684 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset, true);
685
686 checkInside(lane, -1, startLateralOffset, false);
687
688 checkInside(lane, link.getLength().getSI() + 1, endLateralOffset, false);
689
690 checkInside(lane, 1, startLateralOffset - startWidth / 2 - 1, false);
691
692 checkInside(lane, 1, startLateralOffset + startWidth / 2 + 1, false);
693
694 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset - endWidth / 2 - 1, false);
695
696 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset + endWidth / 2 + 1, false);
697
698 DirectedPoint2d l = lane.getLocation();
699
700
701
702
703 Point2D.Double[] cornerPoints = new Point2D.Double[4];
704 cornerPoints[0] =
705 new Point2D.Double(xStart - (startLateralOffset + startWidth / 2) * Math.sin(angle),
706 yStart + (startLateralOffset + startWidth / 2) * Math.cos(angle));
707 cornerPoints[1] =
708 new Point2D.Double(xStart - (startLateralOffset - startWidth / 2) * Math.sin(angle),
709 yStart + (startLateralOffset - startWidth / 2) * Math.cos(angle));
710 cornerPoints[2] = new Point2D.Double(xEnd - (endLateralOffset + endWidth / 2) * Math.sin(angle),
711 yEnd + (endLateralOffset + endWidth / 2) * Math.cos(angle));
712 cornerPoints[3] = new Point2D.Double(xEnd - (endLateralOffset - endWidth / 2) * Math.sin(angle),
713 yEnd + (endLateralOffset - endWidth / 2) * Math.cos(angle));
714
715
716
717
718 double minX = cornerPoints[0].getX();
719 double maxX = cornerPoints[0].getX();
720 double minY = cornerPoints[0].getY();
721 double maxY = cornerPoints[0].getY();
722 for (int i = 1; i < cornerPoints.length; i++)
723 {
724 Point2D.Double p = cornerPoints[i];
725 minX = Math.min(minX, p.getX());
726 minY = Math.min(minY, p.getY());
727 maxX = Math.max(maxX, p.getX());
728 maxY = Math.max(maxY, p.getY());
729 }
730
731
732
733 Bounds<?, ?> bb = lane.getAbsoluteContour().getAbsoluteBounds();
734 double boundsMinX = bb.getMinX();
735 double boundsMinY = bb.getMinY();
736 double boundsMaxX = bb.getMaxX();
737 double boundsMaxY = bb.getMaxY();
738 assertEquals(minX, boundsMinX, 0.1, "low x boundary");
739 assertEquals(minY, boundsMinY, 0.1, "low y boundary");
740 assertEquals(maxX, boundsMaxX, 0.1, "high x boundary");
741 assertEquals(maxY, boundsMaxY, 0.1, "high y boundary");
742 }
743 }
744 }
745 }
746 }
747 }
748 }
749
750
751
752
753
754
755
756
757
758
759
760
761
762 private void checkInside(final Lane lane, final double longitudinal, final double lateral, final boolean expectedResult)
763 {
764 CrossSectionLink parentLink = lane.getLink();
765 Node start = parentLink.getStartNode();
766 Node end = parentLink.getEndNode();
767 double startX = start.getPoint().x;
768 double startY = start.getPoint().y;
769 double endX = end.getPoint().x;
770 double endY = end.getPoint().y;
771 double length = Math.sqrt((endX - startX) * (endX - startX) + (endY - startY) * (endY - startY));
772 double ratio = longitudinal / length;
773 double designLineX = startX + (endX - startX) * ratio;
774 double designLineY = startY + (endY - startY) * ratio;
775 double lateralAngle = Math.atan2(endY - startY, endX - startX) + Math.PI / 2;
776 double px = designLineX + lateral * Math.cos(lateralAngle);
777 double py = designLineY + lateral * Math.sin(lateralAngle);
778 Polygon2d contour = lane.getAbsoluteContour();
779
780
781 Point2d p = new Point2d(px, py);
782
783
784 boolean result = contour.contains(p);
785 if (expectedResult)
786 {
787 assertTrue(result, "Point at " + longitudinal + " along and " + lateral + " lateral is within lane");
788 }
789 else
790 {
791 assertFalse(result, "Point at " + longitudinal + " along and " + lateral + " lateral is outside lane");
792 }
793 }
794
795
796 protected static class Model extends AbstractOtsModel
797 {
798
799
800
801
802 public Model(final OtsSimulatorInterface simulator)
803 {
804 super(simulator);
805 }
806
807 @Override
808 public final void constructModel() throws SimRuntimeException
809 {
810
811 }
812
813 @Override
814 public final RoadNetwork getNetwork()
815 {
816 return null;
817 }
818 }
819
820 }