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