1 package org.opentrafficsim.road.network.lane;
2
3 import static org.junit.Assert.assertEquals;
4 import static org.junit.Assert.assertFalse;
5 import static org.junit.Assert.assertNotNull;
6 import static org.junit.Assert.assertTrue;
7 import static org.junit.Assert.fail;
8
9 import java.awt.geom.Point2D;
10 import java.rmi.RemoteException;
11 import java.util.ArrayList;
12 import java.util.LinkedHashMap;
13 import java.util.List;
14 import java.util.Map;
15 import java.util.SortedMap;
16
17 import javax.naming.NamingException;
18
19 import org.djunits.unit.DurationUnit;
20 import org.djunits.unit.util.UNITS;
21 import org.djunits.value.vdouble.scalar.Direction;
22 import org.djunits.value.vdouble.scalar.Duration;
23 import org.djunits.value.vdouble.scalar.Length;
24 import org.djunits.value.vdouble.scalar.Speed;
25 import org.djunits.value.vdouble.scalar.Time;
26 import org.djutils.event.Event;
27 import org.djutils.event.EventListener;
28 import org.junit.Test;
29 import org.locationtech.jts.geom.Coordinate;
30 import org.locationtech.jts.geom.Geometry;
31 import org.locationtech.jts.geom.GeometryFactory;
32 import org.mockito.Mockito;
33 import org.opentrafficsim.core.compatibility.GtuCompatibility;
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.Bounds;
39 import org.opentrafficsim.core.geometry.DirectedPoint;
40 import org.opentrafficsim.core.geometry.OtsGeometryException;
41 import org.opentrafficsim.core.geometry.OtsLine3d;
42 import org.opentrafficsim.core.geometry.OtsPoint3d;
43 import org.opentrafficsim.core.gtu.GtuType;
44 import org.opentrafficsim.core.network.LateralDirectionality;
45 import org.opentrafficsim.core.network.NetworkException;
46 import org.opentrafficsim.core.network.Node;
47 import org.opentrafficsim.road.definitions.DefaultsRoadNl;
48 import org.opentrafficsim.road.mock.MockDevsSimulator;
49 import org.opentrafficsim.road.network.RoadNetwork;
50 import org.opentrafficsim.road.network.lane.changing.LaneKeepingPolicy;
51 import org.opentrafficsim.road.network.lane.object.LaneBasedObject;
52 import org.opentrafficsim.road.network.lane.object.detector.LaneDetector;
53
54 import nl.tudelft.simulation.dsol.SimRuntimeException;
55
56
57
58
59
60
61
62
63
64 public class LaneTest implements UNITS
65 {
66
67
68
69
70 @Test
71 public void laneConstructorTest() throws Exception
72 {
73 OtsSimulatorInterface simulator = new OtsSimulator("LaneTest");
74 RoadNetwork network = new RoadNetwork("lane test network", simulator);
75 Model model = new Model(simulator);
76 simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model);
77
78 Node nodeFrom = new Node(network, "A", new OtsPoint3d(0, 0, 0), Direction.ZERO);
79 Node nodeTo = new Node(network, "B", new OtsPoint3d(1000, 0, 0), Direction.ZERO);
80
81 OtsPoint3d[] coordinates = new OtsPoint3d[2];
82 coordinates[0] = new OtsPoint3d(nodeFrom.getPoint().x, nodeFrom.getPoint().y, 0);
83 coordinates[1] = new OtsPoint3d(nodeTo.getPoint().x, nodeTo.getPoint().y, 0);
84 CrossSectionLink link = new CrossSectionLink(network, "A to B", nodeFrom, nodeTo, DefaultsNl.FREEWAY,
85 new OtsLine3d(coordinates), LaneKeepingPolicy.KEEPRIGHT);
86 Length startLateralPos = new Length(2, METER);
87 Length endLateralPos = new Length(5, METER);
88 Length startWidth = new Length(3, METER);
89 Length endWidth = new Length(4, METER);
90 GtuType gtuTypeCar = DefaultsNl.CAR;
91
92 LaneType laneType = new LaneType("One way", DefaultsRoadNl.FREEWAY);
93 laneType.addCompatibleGtuType(DefaultsNl.VEHICLE);
94 Map<GtuType, Speed> speedMap = new LinkedHashMap<>();
95 speedMap.put(DefaultsNl.VEHICLE, new Speed(100, KM_PER_HOUR));
96
97
98 Lane lane = new Lane(link, "lane", startLateralPos, endLateralPos, startWidth, endWidth, laneType, speedMap, false);
99
100 assertEquals("Link returns network", network, link.getNetwork());
101 assertEquals("Lane returns network", network, lane.getNetwork());
102 assertEquals("PrevLanes should be empty", 0, lane.prevLanes(gtuTypeCar).size());
103 assertEquals("NextLanes should be empty", 0, lane.nextLanes(gtuTypeCar).size());
104 double approximateLengthOfContour =
105 2 * nodeFrom.getPoint().distanceSI(nodeTo.getPoint()) + startWidth.getSI() + endWidth.getSI();
106 assertEquals("Length of contour is approximately " + approximateLengthOfContour, approximateLengthOfContour,
107 lane.getContour().getLengthSI(), 0.1);
108 assertEquals("SpeedLimit should be " + (new Speed(100, KM_PER_HOUR)), new Speed(100, KM_PER_HOUR),
109 lane.getSpeedLimit(DefaultsNl.VEHICLE));
110 assertEquals("There should be no GTUs on the lane", 0, lane.getGtuList().size());
111 assertEquals("LaneType should be " + laneType, laneType, lane.getType());
112
113 for (int i = 0; i < 10; i++)
114 {
115 double expectedLateralCenterOffset =
116 startLateralPos.getSI() + (endLateralPos.getSI() - startLateralPos.getSI()) * i / 10;
117 assertEquals(String.format("Lateral offset at %d%% should be %.3fm", 10 * i, expectedLateralCenterOffset),
118 expectedLateralCenterOffset, lane.getLateralCenterPosition(i / 10.0).getSI(), 0.01);
119 Length longitudinalPosition = new Length(lane.getLength().getSI() * i / 10, METER);
120 assertEquals("Lateral offset at " + longitudinalPosition + " should be " + expectedLateralCenterOffset,
121 expectedLateralCenterOffset, lane.getLateralCenterPosition(longitudinalPosition).getSI(), 0.01);
122 double expectedWidth = startWidth.getSI() + (endWidth.getSI() - startWidth.getSI()) * i / 10;
123 assertEquals(String.format("Width at %d%% should be %.3fm", 10 * i, expectedWidth), expectedWidth,
124 lane.getWidth(i / 10.0).getSI(), 0.0001);
125 assertEquals("Width at " + longitudinalPosition + " should be " + expectedWidth, expectedWidth,
126 lane.getWidth(longitudinalPosition).getSI(), 0.0001);
127 double expectedLeftOffset = expectedLateralCenterOffset - expectedWidth / 2;
128
129 assertEquals(String.format("Left edge at %d%% should be %.3fm", 10 * i, expectedLeftOffset), expectedLeftOffset,
130 lane.getLateralBoundaryPosition(LateralDirectionality.LEFT, i / 10.0).getSI(), 0.001);
131 assertEquals("Left edge at " + longitudinalPosition + " should be " + expectedLeftOffset, expectedLeftOffset,
132 lane.getLateralBoundaryPosition(LateralDirectionality.LEFT, longitudinalPosition).getSI(), 0.001);
133 double expectedRightOffset = expectedLateralCenterOffset + expectedWidth / 2;
134 assertEquals(String.format("Right edge at %d%% should be %.3fm", 10 * i, expectedRightOffset), expectedRightOffset,
135 lane.getLateralBoundaryPosition(LateralDirectionality.RIGHT, i / 10.0).getSI(), 0.001);
136 assertEquals("Right edge at " + longitudinalPosition + " should be " + expectedRightOffset, expectedRightOffset,
137 lane.getLateralBoundaryPosition(LateralDirectionality.RIGHT, longitudinalPosition).getSI(), 0.001);
138 }
139
140
141
142 coordinates = new OtsPoint3d[3];
143 coordinates[0] = new OtsPoint3d(nodeFrom.getPoint().x, nodeFrom.getPoint().y, 0);
144 coordinates[1] = new OtsPoint3d(200, 100);
145 coordinates[2] = new OtsPoint3d(nodeTo.getPoint().x, nodeTo.getPoint().y, 0);
146 link = new CrossSectionLink(network, "A to B with Kink", nodeFrom, nodeTo, DefaultsNl.FREEWAY,
147 new OtsLine3d(coordinates), LaneKeepingPolicy.KEEPRIGHT);
148 lane = new Lane(link, "lane.1", startLateralPos, endLateralPos, startWidth, endWidth, laneType, speedMap, false);
149
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 try
181 {
182 new Lane(link, "lanex", null, laneType, speedMap);
183 fail("null pointer for CrossSectionSlices should have thrown a NullPointerException");
184 }
185 catch (NullPointerException npe)
186 {
187
188 }
189 List<CrossSectionSlice> crossSectionSlices = new ArrayList<>();
190 try
191 {
192 new Lane(link, "lanex", crossSectionSlices, laneType, speedMap);
193 fail("empty CrossSectionSlices should have thrown a NetworkException");
194 }
195 catch (NetworkException ne)
196 {
197
198 }
199 crossSectionSlices.add(new CrossSectionSlice(Length.ZERO, startLateralPos, startWidth));
200 lane = new Lane(link, "lanex", crossSectionSlices, laneType, speedMap);
201 sensorTest(lane);
202 }
203
204
205
206
207
208
209 public final void sensorTest(final Lane lane) throws NetworkException
210 {
211 assertEquals("List of sensor is initially empty", 0, lane.getDetectors().size());
212 Listener listener = new Listener();
213 double length = lane.getLength().si;
214 lane.addListener(listener, Lane.DETECTOR_ADD_EVENT);
215 lane.addListener(listener, Lane.DETECTOR_REMOVE_EVENT);
216 assertEquals("event list is initially empty", 0, listener.events.size());
217 LaneDetector sensor1 = new MockSensor("sensor1", Length.instantiateSI(length / 4)).getMock();
218 lane.addDetector(sensor1);
219 assertEquals("event list now contains one event", 1, listener.events.size());
220 assertEquals("event indicates that a sensor got added", listener.events.get(0).getType(), Lane.DETECTOR_ADD_EVENT);
221 assertEquals("lane now contains one sensor", 1, lane.getDetectors().size());
222 assertEquals("sensor on lane is sensor1", sensor1, lane.getDetectors().get(0));
223 LaneDetector sensor2 = new MockSensor("sensor2", Length.instantiateSI(length / 2)).getMock();
224 lane.addDetector(sensor2);
225 assertEquals("event list now contains two events", 2, listener.events.size());
226 assertEquals("event indicates that a sensor got added", listener.events.get(1).getType(), Lane.DETECTOR_ADD_EVENT);
227 List<LaneDetector> sensors = lane.getDetectors();
228 assertEquals("lane now contains two sensors", 2, sensors.size());
229 assertTrue("sensor list contains sensor1", sensors.contains(sensor1));
230 assertTrue("sensor list contains sensor2", sensors.contains(sensor2));
231 sensors = lane.getDetectors(Length.ZERO, Length.instantiateSI(length / 3), DefaultsNl.VEHICLE);
232 assertEquals("first third of lane contains 1 sensor", 1, sensors.size());
233 assertTrue("sensor list contains sensor1", sensors.contains(sensor1));
234 sensors = lane.getDetectors(Length.instantiateSI(length / 3), Length.instantiateSI(length), DefaultsNl.VEHICLE);
235 assertEquals("last two-thirds of lane contains 1 sensor", 1, sensors.size());
236 assertTrue("sensor list contains sensor2", sensors.contains(sensor2));
237 sensors = lane.getDetectors(DefaultsNl.VEHICLE);
238
239 assertEquals("sensor list contains two sensors", 2, sensors.size());
240 assertTrue("sensor list contains sensor1", sensors.contains(sensor1));
241 assertTrue("sensor list contains sensor2", sensors.contains(sensor2));
242 sensors = lane.getDetectors(DefaultsNl.VEHICLE);
243
244 assertEquals("sensor list contains two sensors", 2, sensors.size());
245 assertTrue("sensor list contains sensor1", sensors.contains(sensor1));
246 assertTrue("sensor list contains sensor2", sensors.contains(sensor2));
247 SortedMap<Double, List<LaneDetector>> sensorMap = lane.getDetectorMap(DefaultsNl.VEHICLE);
248 assertEquals("sensor map contains two entries", 2, sensorMap.size());
249 for (Double d : sensorMap.keySet())
250 {
251 List<LaneDetector> sensorsAtD = sensorMap.get(d);
252 assertEquals("There is one sensor at position d", 1, sensorsAtD.size());
253 assertEquals("Sensor map contains the correct sensor at the correct distance", d < length / 3 ? sensor1 : sensor2,
254 sensorsAtD.get(0));
255 }
256
257 lane.removeDetector(sensor1);
258 assertEquals("event list now contains three events", 3, listener.events.size());
259 assertEquals("event indicates that a sensor got removed", listener.events.get(2).getType(), Lane.DETECTOR_REMOVE_EVENT);
260 sensors = lane.getDetectors();
261 assertEquals("lane now contains one sensor", 1, sensors.size());
262 assertTrue("sensor list contains sensor2", sensors.contains(sensor2));
263 try
264 {
265 lane.removeDetector(sensor1);
266 fail("Removing a sensor twice should have thrown a NetworkException");
267 }
268 catch (NetworkException ne)
269 {
270
271 }
272 try
273 {
274 lane.addDetector(sensor2);
275 fail("Adding a sensor twice should have thrown a NetworkException");
276 }
277 catch (NetworkException ne)
278 {
279
280 }
281 LaneDetector badSensor = new MockSensor("sensor3", Length.instantiateSI(-0.1)).getMock();
282 try
283 {
284 lane.addDetector(badSensor);
285 fail("Adding a sensor at negative position should have thrown a NetworkException");
286 }
287 catch (NetworkException ne)
288 {
289
290 }
291 badSensor = new MockSensor("sensor4", Length.instantiateSI(length + 0.1)).getMock();
292 try
293 {
294 lane.addDetector(badSensor);
295 fail("Adding a sensor at position beyond the end of the lane should have thrown a NetworkException");
296 }
297 catch (NetworkException ne)
298 {
299
300 }
301 lane.removeDetector(sensor2);
302 List<LaneBasedObject> lboList = lane.getLaneBasedObjects();
303 assertEquals("lane initially contains zero lane based objects", 0, lboList.size());
304 LaneBasedObject lbo1 = new MockLaneBasedObject("lbo1", Length.instantiateSI(length / 4)).getMock();
305 listener.getEvents().clear();
306 lane.addListener(listener, Lane.OBJECT_ADD_EVENT);
307 lane.addListener(listener, Lane.OBJECT_REMOVE_EVENT);
308 lane.addLaneBasedObject(lbo1);
309 assertEquals("adding a lane based object cause the lane to emit an event", 1, listener.getEvents().size());
310 assertEquals("The emitted event was a OBJECT_ADD_EVENT", Lane.OBJECT_ADD_EVENT, listener.getEvents().get(0).getType());
311 LaneBasedObject lbo2 = new MockLaneBasedObject("lbo2", Length.instantiateSI(3 * length / 4)).getMock();
312 lane.addLaneBasedObject(lbo2);
313 lboList = lane.getLaneBasedObjects();
314 assertEquals("lane based object list now contains two objects", 2, lboList.size());
315 assertTrue("lane base object list contains lbo1", lboList.contains(lbo1));
316 assertTrue("lane base object list contains lbo2", lboList.contains(lbo2));
317 lboList = lane.getLaneBasedObjects(Length.ZERO, Length.instantiateSI(length / 2));
318 assertEquals("first half of lane contains one object", 1, lboList.size());
319 assertEquals("object in first haf of lane is lbo1", lbo1, lboList.get(0));
320 lboList = lane.getLaneBasedObjects(Length.instantiateSI(length / 2), Length.instantiateSI(length));
321 assertEquals("second half of lane contains one object", 1, lboList.size());
322 assertEquals("object in second haf of lane is lbo2", lbo2, lboList.get(0));
323 SortedMap<Double, List<LaneBasedObject>> sortedMap = lane.getLaneBasedObjectMap();
324 assertEquals("sorted map contains two objects", 2, sortedMap.size());
325 for (Double d : sortedMap.keySet())
326 {
327 List<LaneBasedObject> objectsAtD = sortedMap.get(d);
328 assertEquals("There is one object at position d", 1, objectsAtD.size());
329 assertEquals("Object at position d is the expected one", d < length / 2 ? lbo1 : lbo2, objectsAtD.get(0));
330 }
331
332 for (double fraction : new double[] {-0.5, 0, 0.2, 0.5, 0.9, 1.0, 2})
333 {
334 double positionSI = length * fraction;
335 double fractionSI = lane.fractionSI(positionSI);
336 assertEquals("fractionSI matches fraction", fraction, fractionSI, 0.0001);
337
338 LaneBasedObject nextObject = positionSI < lbo1.getLongitudinalPosition().si ? lbo1
339 : positionSI < lbo2.getLongitudinalPosition().si ? lbo2 : null;
340 List<LaneBasedObject> expected = null;
341 if (null != nextObject)
342 {
343 expected = new ArrayList<>();
344 expected.add(nextObject);
345 }
346 List<LaneBasedObject> got = lane.getObjectAhead(Length.instantiateSI(positionSI));
347 assertEquals("First bunch of objects ahead of d", expected, got);
348
349 nextObject = positionSI > lbo2.getLongitudinalPosition().si ? lbo2
350 : positionSI > lbo1.getLongitudinalPosition().si ? lbo1 : null;
351 expected = null;
352 if (null != nextObject)
353 {
354 expected = new ArrayList<>();
355 expected.add(nextObject);
356 }
357 got = lane.getObjectBehind(Length.instantiateSI(positionSI));
358 assertEquals("First bunch of objects behind d", expected, got);
359 }
360
361 lane.removeLaneBasedObject(lbo1);
362 assertEquals("removing a lane based object caused the lane to emit an event", 3, listener.getEvents().size());
363 assertEquals("removing a lane based object caused the lane to emit OBJECT_REMOVE_EVENT", Lane.OBJECT_REMOVE_EVENT,
364 listener.getEvents().get(2).getType());
365 try
366 {
367 lane.removeLaneBasedObject(lbo1);
368 fail("Removing a lane bases object that was already removed should have caused a NetworkException");
369 }
370 catch (NetworkException ne)
371 {
372
373 }
374 try
375 {
376 lane.addLaneBasedObject(lbo2);
377 fail("Adding a lane base object that was already added should have caused a NetworkException");
378 }
379 catch (NetworkException ne)
380 {
381
382 }
383 LaneBasedObject badLBO = new MockLaneBasedObject("badLBO", Length.instantiateSI(-0.1)).getMock();
384 try
385 {
386 lane.addLaneBasedObject(badLBO);
387 fail("Adding a lane based object at negative position should have thrown a NetworkException");
388 }
389 catch (NetworkException ne)
390 {
391
392 }
393 badLBO = new MockLaneBasedObject("badLBO", Length.instantiateSI(length + 0.1)).getMock();
394 try
395 {
396 lane.addLaneBasedObject(badLBO);
397 fail("Adding a lane based object at position beyond end of lane should have thrown a NetworkException");
398 }
399 catch (NetworkException ne)
400 {
401
402 }
403 }
404
405
406
407
408 class Listener implements EventListener
409 {
410
411 private List<Event> events = new ArrayList<>();
412
413 @Override
414 public void notify(final Event event) throws RemoteException
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
554 @Test
555 public final void lateralOffsetTest() throws NetworkException, SimRuntimeException, NamingException, OtsGeometryException
556 {
557 OtsPoint3d from = new OtsPoint3d(10, 10, 0);
558 OtsPoint3d to = new OtsPoint3d(1010, 10, 0);
559 OtsSimulatorInterface simulator = new OtsSimulator("LaneTest");
560 Model model = new Model(simulator);
561 simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model);
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 OtsPoint3d[] coordinates = new OtsPoint3d[2];
570 coordinates[0] = start.getPoint();
571 coordinates[1] = end.getPoint();
572 OtsLine3d line = new OtsLine3d(coordinates);
573 CrossSectionLink link =
574 new CrossSectionLink(network, "A to B", start, end, DefaultsNl.ROAD, line, LaneKeepingPolicy.KEEPRIGHT);
575 Length offsetAtStart = Length.instantiateSI(5);
576 Length offsetAtEnd = Length.instantiateSI(15);
577 Length width = Length.instantiateSI(4);
578 Lane lane = new Lane(link, "lane", offsetAtStart, offsetAtEnd, width, width, laneType, speedMap, true);
579 OtsLine3d laneCenterLine = lane.getCenterLine();
580
581 OtsPoint3d[] points = laneCenterLine.getPoints();
582 double prev = offsetAtStart.si + from.y;
583 double prevRatio = 0;
584 double prevDirection = 0;
585 for (int i = 0; i < points.length; i++)
586 {
587 OtsPoint3d p = points[i];
588 double relativeLength = p.x - from.x;
589 double ratio = relativeLength / (to.x - from.x);
590 double actualOffset = p.y;
591 if (0 == i)
592 {
593 assertEquals("first point must have offset at start", offsetAtStart.si + from.y, actualOffset, 0.001);
594 }
595 if (points.length - 1 == i)
596 {
597 assertEquals("last point must have offset at end", offsetAtEnd.si + from.y, actualOffset, 0.001);
598 }
599
600 double delta = actualOffset - prev;
601 assertTrue("delta must be nonnegative", delta >= 0);
602 if (i > 0)
603 {
604 OtsPoint3d prevPoint = points[i - 1];
605 double direction = Math.atan2(p.y - prevPoint.y, p.x - prevPoint.x);
606
607 assertTrue("Direction of lane center line is > 0", direction > 0);
608 if (ratio < 0.5)
609 {
610 assertTrue("in first half direction is increasing", direction > prevDirection);
611 }
612 else if (prevRatio > 0.5)
613 {
614 assertTrue("in second half direction is decreasing", direction < prevDirection);
615 }
616 prevDirection = direction;
617 prevRatio = ratio;
618 }
619 }
620 }
621
622
623
624
625
626
627 @Test
628 public final void contourTest() throws Exception
629 {
630 final int[] startPositions = {0, 1, -1, 20, -20};
631 final double[] angles = {0, Math.PI * 0.01, Math.PI / 3, Math.PI / 2, Math.PI * 2 / 3, Math.PI * 0.99, Math.PI,
632 Math.PI * 1.01, Math.PI * 4 / 3, Math.PI * 3 / 2, Math.PI * 1.99, Math.PI * 2, Math.PI * (-0.2)};
633 int laneNum = 0;
634 for (int xStart : startPositions)
635 {
636 for (int yStart : startPositions)
637 {
638 for (double angle : angles)
639 {
640 OtsSimulatorInterface simulator = new OtsSimulator("LaneTest");
641 Model model = new Model(simulator);
642 simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model);
643 RoadNetwork network = new RoadNetwork("contour test network", simulator);
644 LaneType laneType = DefaultsRoadNl.TWO_WAY_LANE;
645 laneType.addCompatibleGtuType(DefaultsNl.VEHICLE);
646 Map<GtuType, Speed> speedMap = new LinkedHashMap<>();
647 speedMap.put(DefaultsNl.VEHICLE, new Speed(50, KM_PER_HOUR));
648 Node start =
649 new Node(network, "start", new OtsPoint3d(xStart, yStart), Direction.instantiateSI(angle));
650 double linkLength = 1000;
651 double xEnd = xStart + linkLength * Math.cos(angle);
652 double yEnd = yStart + linkLength * Math.sin(angle);
653 Node end =
654 new Node(network, "end", new OtsPoint3d(xEnd, yEnd), Direction.instantiateSI(angle));
655 OtsPoint3d[] coordinates = new OtsPoint3d[2];
656 coordinates[0] = start.getPoint();
657 coordinates[1] = end.getPoint();
658 OtsLine3d line = new OtsLine3d(coordinates);
659 CrossSectionLink link = new CrossSectionLink(network, "A to B", start, end, DefaultsNl.ROAD, line,
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 = new Lane(link, "lane." + ++laneNum, new Length(startLateralOffset, METER),
672 new Length(endLateralOffset, METER), new Length(startWidth, METER),
673 new Length(endWidth, METER), laneType, speedMap, false);
674 final Geometry geometry = lane.getContour().getLineString();
675 assertNotNull("geometry of the lane should not be null", geometry);
676
677
678 checkInside(lane, 1, startLateralOffset, true);
679
680 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset, true);
681
682 checkInside(lane, -1, startLateralOffset, false);
683
684 checkInside(lane, link.getLength().getSI() + 1, endLateralOffset, false);
685
686 checkInside(lane, 1, startLateralOffset - startWidth / 2 - 1, false);
687
688 checkInside(lane, 1, startLateralOffset + startWidth / 2 + 1, false);
689
690 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset - endWidth / 2 - 1, false);
691
692 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset + endWidth / 2 + 1, false);
693
694 DirectedPoint l = lane.getLocation();
695 Bounds bb = lane.getBounds();
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 double boundsMinX = bb.getMinX() + l.x;
731 double boundsMinY = bb.getMinY() + l.y;
732 double boundsMaxX = bb.getMaxX() + l.x;
733 double boundsMaxY = bb.getMaxY() + l.y;
734 assertEquals("low x boundary", minX, boundsMinX, 0.1);
735 assertEquals("low y boundary", minY, boundsMinY, 0.1);
736 assertEquals("high x boundary", maxX, boundsMaxX, 0.1);
737 assertEquals("high y boundary", maxY, boundsMaxY, 0.1);
738 }
739 }
740 }
741 }
742 }
743 }
744 }
745
746
747
748
749
750
751
752
753
754
755
756
757
758 private void checkInside(final Lane lane, final double longitudinal, final double lateral, final boolean expectedResult)
759 {
760 CrossSectionLink parentLink = lane.getParentLink();
761 Node start = parentLink.getStartNode();
762 Node end = parentLink.getEndNode();
763 double startX = start.getPoint().x;
764 double startY = start.getPoint().y;
765 double endX = end.getPoint().x;
766 double endY = end.getPoint().y;
767 double length = Math.sqrt((endX - startX) * (endX - startX) + (endY - startY) * (endY - startY));
768 double ratio = longitudinal / length;
769 double designLineX = startX + (endX - startX) * ratio;
770 double designLineY = startY + (endY - startY) * ratio;
771 double lateralAngle = Math.atan2(endY - startY, endX - startX) + Math.PI / 2;
772 double px = designLineX + lateral * Math.cos(lateralAngle);
773 double py = designLineY + lateral * Math.sin(lateralAngle);
774 Geometry contour = lane.getContour().getLineString();
775 GeometryFactory factory = new GeometryFactory();
776 Geometry p = factory.createPoint(new Coordinate(px, py));
777
778
779 boolean result = contour.contains(p);
780 Coordinate[] polygon = contour.getCoordinates();
781 result = pointInsidePolygon(new Coordinate(px, py), polygon);
782 if (expectedResult)
783 {
784 assertTrue("Point at " + longitudinal + " along and " + lateral + " lateral is within lane", result);
785 }
786 else
787 {
788 assertFalse("Point at " + longitudinal + " along and " + lateral + " lateral is outside lane", result);
789 }
790 }
791
792
793
794
795
796
797
798
799
800
801 private boolean pointInsidePolygon(final Coordinate point, final Coordinate[] polygon)
802 {
803 boolean result = false;
804 for (int i = 0, j = polygon.length - 1; i < polygon.length; j = i++)
805 {
806 if ((polygon[i].y > point.y) != (polygon[j].y > point.y)
807 && point.x < (polygon[j].x - polygon[i].x) * (point.y - polygon[i].y) / (polygon[j].y - polygon[i].y)
808 + polygon[i].x)
809 {
810 result = !result;
811 }
812 }
813 return result;
814 }
815
816
817 protected static class Model extends AbstractOtsModel
818 {
819
820 private static final long serialVersionUID = 20141027L;
821
822
823
824
825 public Model(final OtsSimulatorInterface simulator)
826 {
827 super(simulator);
828 }
829
830
831 @Override
832 public final void constructModel() throws SimRuntimeException
833 {
834
835 }
836
837
838 @Override
839 public final RoadNetwork getNetwork()
840 {
841 return null;
842 }
843 }
844
845 }