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