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