View Javadoc
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.io.Serializable;
11  import java.rmi.RemoteException;
12  import java.util.ArrayList;
13  import java.util.LinkedHashMap;
14  import java.util.List;
15  import java.util.Map;
16  import java.util.SortedMap;
17  
18  import javax.naming.NamingException;
19  
20  import org.djunits.unit.DurationUnit;
21  import org.djunits.unit.util.UNITS;
22  import org.djunits.value.vdouble.scalar.Direction;
23  import org.djunits.value.vdouble.scalar.Duration;
24  import org.djunits.value.vdouble.scalar.Length;
25  import org.djunits.value.vdouble.scalar.Speed;
26  import org.djunits.value.vdouble.scalar.Time;
27  import org.djutils.event.EventInterface;
28  import org.djutils.event.EventListenerInterface;
29  import org.junit.Test;
30  import org.locationtech.jts.geom.Coordinate;
31  import org.locationtech.jts.geom.Geometry;
32  import org.locationtech.jts.geom.GeometryFactory;
33  import org.mockito.Mockito;
34  import org.opentrafficsim.core.compatibility.GTUCompatibility;
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.GTUDirectionality;
44  import org.opentrafficsim.core.gtu.GTUType;
45  import org.opentrafficsim.core.network.LateralDirectionality;
46  import org.opentrafficsim.core.network.LinkType;
47  import org.opentrafficsim.core.network.LongitudinalDirectionality;
48  import org.opentrafficsim.core.network.NetworkException;
49  import org.opentrafficsim.core.network.Node;
50  import org.opentrafficsim.road.mock.MockDEVSSimulator;
51  import org.opentrafficsim.road.network.OTSRoadNetwork;
52  import org.opentrafficsim.road.network.lane.changing.LaneKeepingPolicy;
53  import org.opentrafficsim.road.network.lane.object.LaneBasedObject;
54  import org.opentrafficsim.road.network.lane.object.sensor.SingleSensor;
55  
56  import nl.tudelft.simulation.dsol.SimRuntimeException;
57  
58  /**
59   * Test the Lane class.
60   * <p>
61   * Copyright (c) 2013-2022 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
62   * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
63   * <p>
64   * $LastChangedDate: 2015-09-16 19:20:07 +0200 (Wed, 16 Sep 2015) $, @version $Revision: 1405 $, by $Author: averbraeck $,
65   * initial version 21 jan. 2015 <br>
66   * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
67   */
68  public class LaneTest implements UNITS
69  {
70      /**
71       * Test the constructor.
72       * @throws Exception when something goes wrong (should not happen)
73       */
74      @Test
75      public void laneConstructorTest() throws Exception
76      {
77          OTSSimulatorInterface simulator = new OTSSimulator("LaneTest");
78          OTSRoadNetwork network = new OTSRoadNetwork("lane test network", true, simulator);
79          Model model = new Model(simulator);
80          simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model);
81          // First we need two Nodes
82          OTSRoadNode nodeFrom = new OTSRoadNode(network, "A", new OTSPoint3D(0, 0, 0), Direction.ZERO);
83          OTSRoadNode nodeTo = new OTSRoadNode(network, "B", new OTSPoint3D(1000, 0, 0), Direction.ZERO);
84          // Now we can make a Link
85          OTSPoint3D[] coordinates = new OTSPoint3D[2];
86          coordinates[0] = new OTSPoint3D(nodeFrom.getPoint().x, nodeFrom.getPoint().y, 0);
87          coordinates[1] = new OTSPoint3D(nodeTo.getPoint().x, nodeTo.getPoint().y, 0);
88          CrossSectionLink link =
89                  new CrossSectionLink(network, "A to B", nodeFrom, nodeTo, network.getLinkType(LinkType.DEFAULTS.FREEWAY),
90                          new OTSLine3D(coordinates), LaneKeepingPolicy.KEEPRIGHT);
91          Length startLateralPos = new Length(2, METER);
92          Length endLateralPos = new Length(5, METER);
93          Length startWidth = new Length(3, METER);
94          Length endWidth = new Length(4, METER);
95          GTUType gtuTypeCar = network.getGtuType(GTUType.DEFAULTS.CAR);
96  
97          GTUCompatibility<LaneType> gtuCompatibility = new GTUCompatibility<>((LaneType) null);
98          gtuCompatibility.addAllowedGTUType(network.getGtuType(GTUType.DEFAULTS.VEHICLE), LongitudinalDirectionality.DIR_PLUS);
99          LaneType laneType = new LaneType("One way", network.getLaneType(LaneType.DEFAULTS.FREEWAY), gtuCompatibility, network);
100         Map<GTUType, Speed> speedMap = new LinkedHashMap<>();
101         speedMap.put(network.getGtuType(GTUType.DEFAULTS.VEHICLE), new Speed(100, KM_PER_HOUR));
102         // Now we can construct a Lane
103         // FIXME what overtaking conditions do we want to test in this unit test?
104         Lane lane = new Lane(link, "lane", startLateralPos, endLateralPos, startWidth, endWidth, laneType, speedMap);
105         // Verify the easy bits
106         assertEquals("Link returns network", network, link.getNetwork());
107         assertEquals("Lane returns network", network, lane.getNetwork());
108         assertEquals("PrevLanes should be empty", 0, lane.prevLanes(gtuTypeCar).size()); // this one caught a bug!
109         assertEquals("NextLanes should be empty", 0, lane.nextLanes(gtuTypeCar).size());
110         double approximateLengthOfContour =
111                 2 * nodeFrom.getPoint().distanceSI(nodeTo.getPoint()) + startWidth.getSI() + endWidth.getSI();
112         assertEquals("Length of contour is approximately " + approximateLengthOfContour, approximateLengthOfContour,
113                 lane.getContour().getLengthSI(), 0.1);
114         assertEquals("Directionality should be " + LongitudinalDirectionality.DIR_PLUS, LongitudinalDirectionality.DIR_PLUS,
115                 lane.getLaneType().getDirectionality(network.getGtuType(GTUType.DEFAULTS.VEHICLE)));
116         assertEquals("SpeedLimit should be " + (new Speed(100, KM_PER_HOUR)), new Speed(100, KM_PER_HOUR),
117                 lane.getSpeedLimit(network.getGtuType(GTUType.DEFAULTS.VEHICLE)));
118         assertEquals("There should be no GTUs on the lane", 0, lane.getGtuList().size());
119         assertEquals("LaneType should be " + laneType, laneType, lane.getLaneType());
120         // TODO: This test for expectedLateralCenterOffset fails
121         for (int i = 0; i < 10; i++)
122         {
123             double expectedLateralCenterOffset =
124                     startLateralPos.getSI() + (endLateralPos.getSI() - startLateralPos.getSI()) * i / 10;
125             assertEquals(String.format("Lateral offset at %d%% should be %.3fm", 10 * i, expectedLateralCenterOffset),
126                     expectedLateralCenterOffset, lane.getLateralCenterPosition(i / 10.0).getSI(), 0.01);
127             Length longitudinalPosition = new Length(lane.getLength().getSI() * i / 10, METER);
128             assertEquals("Lateral offset at " + longitudinalPosition + " should be " + expectedLateralCenterOffset,
129                     expectedLateralCenterOffset, lane.getLateralCenterPosition(longitudinalPosition).getSI(), 0.01);
130             double expectedWidth = startWidth.getSI() + (endWidth.getSI() - startWidth.getSI()) * i / 10;
131             assertEquals(String.format("Width at %d%% should be %.3fm", 10 * i, expectedWidth), expectedWidth,
132                     lane.getWidth(i / 10.0).getSI(), 0.0001);
133             assertEquals("Width at " + longitudinalPosition + " should be " + expectedWidth, expectedWidth,
134                     lane.getWidth(longitudinalPosition).getSI(), 0.0001);
135             double expectedLeftOffset = expectedLateralCenterOffset - expectedWidth / 2;
136             // The next test caught a bug
137             assertEquals(String.format("Left edge at %d%% should be %.3fm", 10 * i, expectedLeftOffset), expectedLeftOffset,
138                     lane.getLateralBoundaryPosition(LateralDirectionality.LEFT, i / 10.0).getSI(), 0.001);
139             assertEquals("Left edge at " + longitudinalPosition + " should be " + expectedLeftOffset, expectedLeftOffset,
140                     lane.getLateralBoundaryPosition(LateralDirectionality.LEFT, longitudinalPosition).getSI(), 0.001);
141             double expectedRightOffset = expectedLateralCenterOffset + expectedWidth / 2;
142             assertEquals(String.format("Right edge at %d%% should be %.3fm", 10 * i, expectedRightOffset), expectedRightOffset,
143                     lane.getLateralBoundaryPosition(LateralDirectionality.RIGHT, i / 10.0).getSI(), 0.001);
144             assertEquals("Right edge at " + longitudinalPosition + " should be " + expectedRightOffset, expectedRightOffset,
145                     lane.getLateralBoundaryPosition(LateralDirectionality.RIGHT, longitudinalPosition).getSI(), 0.001);
146         }
147 
148         // Harder case; create a Link with form points along the way
149         // System.out.println("Constructing Link and Lane with one form point");
150         coordinates = new OTSPoint3D[3];
151         coordinates[0] = new OTSPoint3D(nodeFrom.getPoint().x, nodeFrom.getPoint().y, 0);
152         coordinates[1] = new OTSPoint3D(200, 100);
153         coordinates[2] = new OTSPoint3D(nodeTo.getPoint().x, nodeTo.getPoint().y, 0);
154         link = new CrossSectionLink(network, "A to B with Kink", nodeFrom, nodeTo,
155                 network.getLinkType(LinkType.DEFAULTS.FREEWAY), new OTSLine3D(coordinates), LaneKeepingPolicy.KEEPRIGHT);
156         // FIXME what overtaking conditions do we want to test in this unit test?
157         lane = new Lane(link, "lane.1", startLateralPos, endLateralPos, startWidth, endWidth, laneType, speedMap);
158         // Verify the easy bits
159         assertEquals("PrevLanes should contain one lane from the other link", 1, lane.prevLanes(gtuTypeCar).size());
160         assertEquals("NextLanes should contain one lane from the other link", 1, lane.nextLanes(gtuTypeCar).size());
161         approximateLengthOfContour = 2 * (coordinates[0].distanceSI(coordinates[1]) + coordinates[1].distanceSI(coordinates[2]))
162                 + startWidth.getSI() + endWidth.getSI();
163         // System.out.println("contour of lane is " + lane.getContour());
164         // System.out.println(lane.getContour().toPlot());
165         assertEquals("Length of contour is approximately " + approximateLengthOfContour, approximateLengthOfContour,
166                 lane.getContour().getLengthSI(), 4); // This lane takes a path that is about 3m longer than the design line
167         assertEquals("There should be no GTUs on the lane", 0, lane.getGtuList().size());
168         assertEquals("LaneType should be " + laneType, laneType, lane.getLaneType());
169         // System.out.println("Add another Lane at the inside of the corner in the design line");
170         Length startLateralPos2 = new Length(-8, METER);
171         Length endLateralPos2 = new Length(-5, METER);
172         // FIXME what overtaking conditions do we ant to test in this unit test?
173         Lane lane2 = new Lane(link, "lane.2", startLateralPos2, endLateralPos2, startWidth, endWidth, laneType, speedMap);
174         // Verify the easy bits
175         assertEquals("PrevLanes should be empty", 0, lane2.prevLanes(gtuTypeCar).size());
176         assertEquals("NextLanes should be empty", 0, lane2.nextLanes(gtuTypeCar).size());
177         approximateLengthOfContour = 2 * (coordinates[0].distanceSI(coordinates[1]) + coordinates[1].distanceSI(coordinates[2]))
178                 + startWidth.getSI() + endWidth.getSI();
179         assertEquals("Length of contour is approximately " + approximateLengthOfContour, approximateLengthOfContour,
180                 lane2.getContour().getLengthSI(), 12); // This lane takes a path that is about 11 meters shorter
181         assertEquals("There should be no GTUs on the lane", 0, lane2.getGtuList().size());
182         assertEquals("LaneType should be " + laneType, laneType, lane2.getLaneType());
183 
184         // Construct a lane using CrossSectionSlices
185         try
186         {
187             new Lane(link, "lanex", null, laneType, speedMap);
188             fail("null pointer for CrossSectionSlices should have thrown a NullPointerException");
189         }
190         catch (NullPointerException npe)
191         {
192             // Ignore expected exception
193         }
194         List<CrossSectionSlice> crossSectionSlices = new ArrayList<>();
195         try
196         {
197             new Lane(link, "lanex", crossSectionSlices, laneType, speedMap);
198             fail("empty CrossSectionSlices should have thrown a NetworkException");
199         }
200         catch (NetworkException ne)
201         {
202             // Ignore expected exception
203         }
204         crossSectionSlices.add(new CrossSectionSlice(Length.ZERO, startLateralPos, startWidth));
205         lane = new Lane(link, "lanex", crossSectionSlices, laneType, new Speed(100, KM_PER_HOUR));
206         sensorTest(lane);
207     }
208 
209     /**
210      * Add/Remove some sensor to/from a lane and see if the expected events occur.
211      * @param lane Lane; the lane to manipulate
212      * @throws NetworkException when this happens uncaught; this test has failed
213      */
214     public final void sensorTest(final Lane lane) throws NetworkException
215     {
216         assertEquals("List of sensor is initially empty", 0, lane.getSensors().size());
217         Listener listener = new Listener();
218         double length = lane.getLength().si;
219         lane.addListener(listener, Lane.SENSOR_ADD_EVENT);
220         lane.addListener(listener, Lane.SENSOR_REMOVE_EVENT);
221         assertEquals("event list is initially empty", 0, listener.events.size());
222         SingleSensor sensor1 = new MockSensor("sensor1", Length.instantiateSI(length / 4)).getMock();
223         lane.addSensor(sensor1);
224         assertEquals("event list now contains one event", 1, listener.events.size());
225         assertEquals("event indicates that a sensor got added", listener.events.get(0).getType(), Lane.SENSOR_ADD_EVENT);
226         assertEquals("lane now contains one sensor", 1, lane.getSensors().size());
227         assertEquals("sensor on lane is sensor1", sensor1, lane.getSensors().get(0));
228         SingleSensor sensor2 = new MockSensor("sensor2", Length.instantiateSI(length / 2)).getMock();
229         lane.addSensor(sensor2);
230         assertEquals("event list now contains two events", 2, listener.events.size());
231         assertEquals("event indicates that a sensor got added", listener.events.get(1).getType(), Lane.SENSOR_ADD_EVENT);
232         List<SingleSensor> sensors = lane.getSensors();
233         assertEquals("lane now contains two sensors", 2, sensors.size());
234         assertTrue("sensor list contains sensor1", sensors.contains(sensor1));
235         assertTrue("sensor list contains sensor2", sensors.contains(sensor2));
236         sensors = lane.getSensors(Length.ZERO, Length.instantiateSI(length / 3),
237                 lane.getParentLink().getNetwork().getGtuType(GTUType.DEFAULTS.VEHICLE), GTUDirectionality.DIR_PLUS);
238         assertEquals("first third of lane contains 1 sensor", 1, sensors.size());
239         assertTrue("sensor list contains sensor1", sensors.contains(sensor1));
240         sensors = lane.getSensors(Length.instantiateSI(length / 3), Length.instantiateSI(length),
241                 lane.getParentLink().getNetwork().getGtuType(GTUType.DEFAULTS.VEHICLE), GTUDirectionality.DIR_PLUS);
242         assertEquals("last two-thirds of lane contains 1 sensor", 1, sensors.size());
243         assertTrue("sensor list contains sensor2", sensors.contains(sensor2));
244         sensors = lane.getSensors(lane.getParentLink().getNetwork().getGtuType(GTUType.DEFAULTS.VEHICLE),
245                 GTUDirectionality.DIR_PLUS);
246         // NB. The mocked sensor is compatible with all GTU types in all directions.
247         assertEquals("sensor list contains two sensors", 2, sensors.size());
248         assertTrue("sensor list contains sensor1", sensors.contains(sensor1));
249         assertTrue("sensor list contains sensor2", sensors.contains(sensor2));
250         sensors = lane.getSensors(lane.getParentLink().getNetwork().getGtuType(GTUType.DEFAULTS.VEHICLE),
251                 GTUDirectionality.DIR_MINUS);
252         // NB. The mocked sensor is compatible with all GTU types in all directions.
253         assertEquals("sensor list contains two sensors", 2, sensors.size());
254         assertTrue("sensor list contains sensor1", sensors.contains(sensor1));
255         assertTrue("sensor list contains sensor2", sensors.contains(sensor2));
256         SortedMap<Double, List<SingleSensor>> sensorMap = lane.getSensorMap(
257                 lane.getParentLink().getNetwork().getGtuType(GTUType.DEFAULTS.VEHICLE), GTUDirectionality.DIR_PLUS);
258         assertEquals("sensor map contains two entries", 2, sensorMap.size());
259         for (Double d : sensorMap.keySet())
260         {
261             List<SingleSensor> sensorsAtD = sensorMap.get(d);
262             assertEquals("There is one sensor at position d", 1, sensorsAtD.size());
263             assertEquals("Sensor map contains the correct sensor at the correct distance", d < length / 3 ? sensor1 : sensor2,
264                     sensorsAtD.get(0));
265         }
266 
267         lane.removeSensor(sensor1);
268         assertEquals("event list now contains three events", 3, listener.events.size());
269         assertEquals("event indicates that a sensor got removed", listener.events.get(2).getType(), Lane.SENSOR_REMOVE_EVENT);
270         sensors = lane.getSensors();
271         assertEquals("lane now contains one sensor", 1, sensors.size());
272         assertTrue("sensor list contains sensor2", sensors.contains(sensor2));
273         try
274         {
275             lane.removeSensor(sensor1);
276             fail("Removing a sensor twice should have thrown a NetworkException");
277         }
278         catch (NetworkException ne)
279         {
280             // Ignore expected exception
281         }
282         try
283         {
284             lane.addSensor(sensor2);
285             fail("Adding a sensor twice should have thrown a NetworkException");
286         }
287         catch (NetworkException ne)
288         {
289             // Ignore expected exception
290         }
291         SingleSensor badSensor = new MockSensor("sensor3", Length.instantiateSI(-0.1)).getMock();
292         try
293         {
294             lane.addSensor(badSensor);
295             fail("Adding a sensor at negative position should have thrown a NetworkException");
296         }
297         catch (NetworkException ne)
298         {
299             // Ignore expected exception
300         }
301         badSensor = new MockSensor("sensor4", Length.instantiateSI(length + 0.1)).getMock();
302         try
303         {
304             lane.addSensor(badSensor);
305             fail("Adding a sensor at position beyond the end of the lane should have thrown a NetworkException");
306         }
307         catch (NetworkException ne)
308         {
309             // Ignore expected exception
310         }
311         lane.removeSensor(sensor2);
312         List<LaneBasedObject> lboList = lane.getLaneBasedObjects();
313         assertEquals("lane initially contains zero lane based objects", 0, lboList.size());
314         LaneBasedObject lbo1 = new MockLaneBasedObject("lbo1", Length.instantiateSI(length / 4)).getMock();
315         listener.getEvents().clear();
316         lane.addListener(listener, Lane.OBJECT_ADD_EVENT);
317         lane.addListener(listener, Lane.OBJECT_REMOVE_EVENT);
318         lane.addLaneBasedObject(lbo1);
319         assertEquals("adding a lane based object cause the lane to emit an event", 1, listener.getEvents().size());
320         assertEquals("The emitted event was a OBJECT_ADD_EVENT", Lane.OBJECT_ADD_EVENT, listener.getEvents().get(0).getType());
321         LaneBasedObject lbo2 = new MockLaneBasedObject("lbo2", Length.instantiateSI(3 * length / 4)).getMock();
322         lane.addLaneBasedObject(lbo2);
323         lboList = lane.getLaneBasedObjects();
324         assertEquals("lane based object list now contains two objects", 2, lboList.size());
325         assertTrue("lane base object list contains lbo1", lboList.contains(lbo1));
326         assertTrue("lane base object list contains lbo2", lboList.contains(lbo2));
327         lboList = lane.getLaneBasedObjects(Length.ZERO, Length.instantiateSI(length / 2));
328         assertEquals("first half of lane contains one object", 1, lboList.size());
329         assertEquals("object in first haf of lane is lbo1", lbo1, lboList.get(0));
330         lboList = lane.getLaneBasedObjects(Length.instantiateSI(length / 2), Length.instantiateSI(length));
331         assertEquals("second half of lane contains one object", 1, lboList.size());
332         assertEquals("object in second haf of lane is lbo2", lbo2, lboList.get(0));
333         SortedMap<Double, List<LaneBasedObject>> sortedMap = lane.getLaneBasedObjectMap();
334         assertEquals("sorted map contains two objects", 2, sortedMap.size());
335         for (Double d : sortedMap.keySet())
336         {
337             List<LaneBasedObject> objectsAtD = sortedMap.get(d);
338             assertEquals("There is one object at position d", 1, objectsAtD.size());
339             assertEquals("Object at position d is the expected one", d < length / 2 ? lbo1 : lbo2, objectsAtD.get(0));
340         }
341 
342         for (double fraction : new double[] { -0.5, 0, 0.2, 0.5, 0.9, 1.0, 2 })
343         {
344             double positionSI = length * fraction;
345             double fractionSI = lane.fractionSI(positionSI);
346             assertEquals("fractionSI matches fraction", fraction, fractionSI, 0.0001);
347 
348             LaneBasedObject nextObject = positionSI < lbo1.getLongitudinalPosition().si ? lbo1
349                     : positionSI < lbo2.getLongitudinalPosition().si ? lbo2 : null;
350             List<LaneBasedObject> expected = null;
351             if (null != nextObject)
352             {
353                 expected = new ArrayList<>();
354                 expected.add(nextObject);
355             }
356             List<LaneBasedObject> got = lane.getObjectAhead(Length.instantiateSI(positionSI), GTUDirectionality.DIR_PLUS);
357             assertEquals("First bunch of objects ahead of d", expected, got);
358 
359             nextObject = positionSI > lbo2.getLongitudinalPosition().si ? lbo2
360                     : positionSI > lbo1.getLongitudinalPosition().si ? lbo1 : null;
361             expected = null;
362             if (null != nextObject)
363             {
364                 expected = new ArrayList<>();
365                 expected.add(nextObject);
366             }
367             got = lane.getObjectAhead(Length.instantiateSI(positionSI), GTUDirectionality.DIR_MINUS);
368             assertEquals("First bunch of objects behind d", expected, got);
369         }
370 
371         lane.removeLaneBasedObject(lbo1);
372         assertEquals("removing a lane based object caused the lane to emit an event", 3, listener.getEvents().size());
373         assertEquals("removing a lane based object caused the lane to emit OBJECT_REMOVE_EVENT", Lane.OBJECT_REMOVE_EVENT,
374                 listener.getEvents().get(2).getType());
375         try
376         {
377             lane.removeLaneBasedObject(lbo1);
378             fail("Removing a lane bases object that was already removed should have caused a NetworkException");
379         }
380         catch (NetworkException ne)
381         {
382             // Ignore expected exception
383         }
384         try
385         {
386             lane.addLaneBasedObject(lbo2);
387             fail("Adding a lane base object that was already added should have caused a NetworkException");
388         }
389         catch (NetworkException ne)
390         {
391             // Ignore expected exception
392         }
393         LaneBasedObject badLBO = new MockLaneBasedObject("badLBO", Length.instantiateSI(-0.1)).getMock();
394         try
395         {
396             lane.addLaneBasedObject(badLBO);
397             fail("Adding a lane based object at negative position should have thrown a NetworkException");
398         }
399         catch (NetworkException ne)
400         {
401             // Ignore expected exception
402         }
403         badLBO = new MockLaneBasedObject("badLBO", Length.instantiateSI(length + 0.1)).getMock();
404         try
405         {
406             lane.addLaneBasedObject(badLBO);
407             fail("Adding a lane based object at position beyond end of lane should have thrown a NetworkException");
408         }
409         catch (NetworkException ne)
410         {
411             // Ignore expected exception
412         }
413     }
414 
415     /**
416      * Simple event listener that collects events in a list.
417      */
418     class Listener implements EventListenerInterface
419     {
420         /** Collect the received events. */
421         private List<EventInterface> events = new ArrayList<>();
422 
423         @Override
424         public void notify(final EventInterface event) throws RemoteException
425         {
426             this.events.add(event);
427         }
428 
429         /**
430          * Retrieve the collected events.
431          * @return List&lt;EventInterface&gt;; the events
432          */
433         public List<EventInterface> getEvents()
434         {
435             return this.events;
436         }
437 
438     }
439 
440     /**
441      * Mock a SingleSensor.
442      */
443     class MockSensor
444     {
445         /** The mocked sensor. */
446         private final SingleSensor mockSensor;
447 
448         /** Id of the mocked sensor. */
449         private final String id;
450 
451         /** The position along the lane of the sensor. */
452         private final Length position;
453 
454         /** Faked simulator. */
455         private final OTSSimulatorInterface simulator = MockDEVSSimulator.createMock();
456 
457         /**
458          * Construct a new Mocked SingleSensor.
459          * @param id String; result of the getId() method of the mocked SingleSensor
460          * @param position Length; result of the getLongitudinalPosition of the mocked SingleSensor
461          */
462         MockSensor(final String id, final Length position)
463         {
464             this.mockSensor = Mockito.mock(SingleSensor.class);
465             this.id = id;
466             this.position = position;
467             Mockito.when(this.mockSensor.getId()).thenReturn(this.id);
468             Mockito.when(this.mockSensor.getLongitudinalPosition()).thenReturn(this.position);
469             Mockito.when(this.mockSensor.getSimulator()).thenReturn(this.simulator);
470             Mockito.when(this.mockSensor.getFullId()).thenReturn(this.id);
471             Mockito.when(this.mockSensor.isCompatible(Mockito.any(), Mockito.any())).thenReturn(true);
472         }
473 
474         /**
475          * Retrieve the mocked sensor.
476          * @return SingleSensor; the mocked sensor
477          */
478         public SingleSensor getMock()
479         {
480             return this.mockSensor;
481         }
482 
483         /**
484          * Retrieve the position of the mocked sensor.
485          * @return Length; the longitudinal position of the mocked sensor
486          */
487         public Length getLongitudinalPosition()
488         {
489             return this.position;
490         }
491 
492         @Override
493         public String toString()
494         {
495             return "MockSensor [mockSensor=" + this.mockSensor + ", id=" + this.id + ", position=" + this.position + "]";
496         }
497 
498     }
499 
500     /**
501      * Mock a LaneBasedObject.
502      */
503     class MockLaneBasedObject
504     {
505         /** The mocked sensor. */
506         private final LaneBasedObject mockLaneBasedObject;
507 
508         /** Id of the mocked sensor. */
509         private final String id;
510 
511         /** The position along the lane of the sensor. */
512         private final Length position;
513 
514         /**
515          * Construct a new Mocked SingleSensor.
516          * @param id String; result of the getId() method of the mocked SingleSensor
517          * @param position Length; result of the getLongitudinalPosition of the mocked SingleSensor
518          */
519         MockLaneBasedObject(final String id, final Length position)
520         {
521             this.mockLaneBasedObject = Mockito.mock(SingleSensor.class);
522             this.id = id;
523             this.position = position;
524             Mockito.when(this.mockLaneBasedObject.getId()).thenReturn(this.id);
525             Mockito.when(this.mockLaneBasedObject.getLongitudinalPosition()).thenReturn(this.position);
526             Mockito.when(this.mockLaneBasedObject.getFullId()).thenReturn(this.id);
527         }
528 
529         /**
530          * Retrieve the mocked LaneBasedObject.
531          * @return LaneBasedObject; the mocked LaneBasedObject
532          */
533         public LaneBasedObject getMock()
534         {
535             return this.mockLaneBasedObject;
536         }
537 
538         /**
539          * Retrieve the position of the mocked sensor.
540          * @return Length; the longitudinal position of the mocked sensor
541          */
542         public Length getLongitudinalPosition()
543         {
544             return this.position;
545         }
546 
547         @Override
548         public String toString()
549         {
550             return "MockLaneBasedObject [mockLaneBasedObject=" + this.mockLaneBasedObject + ", id=" + this.id + ", position=" + this.position
551                     + "]";
552         }
553 
554     }
555 
556     /**
557      * Test that gradually varying lateral offsets have gradually increasing angles (with respect to the design line) in the
558      * first half and gradually decreasing angles in the second half.
559      * @throws NetworkException when that happens uncaught; this test has failed
560      * @throws NamingException when that happens uncaught; this test has failed
561      * @throws SimRuntimeException when that happens uncaught; this test has failed
562      * @throws OTSGeometryException when that happens uncaught; this test has failed
563      */
564     @Test
565     public final void lateralOffsetTest() throws NetworkException, SimRuntimeException, NamingException, OTSGeometryException
566     {
567         OTSPoint3D from = new OTSPoint3D(10, 10, 0);
568         OTSPoint3D to = new OTSPoint3D(1010, 10, 0);
569         OTSSimulatorInterface simulator = new OTSSimulator("LaneTest");
570         Model model = new Model(simulator);
571         simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model);
572         OTSRoadNetwork network = new OTSRoadNetwork("contour test network", true, simulator);
573         LaneType laneType = network.getLaneType(LaneType.DEFAULTS.TWO_WAY_LANE);
574         Map<GTUType, LongitudinalDirectionality> directionalityMap = new LinkedHashMap<>();
575         directionalityMap.put(network.getGtuType(GTUType.DEFAULTS.VEHICLE), LongitudinalDirectionality.DIR_PLUS);
576         Map<GTUType, Speed> speedMap = new LinkedHashMap<>();
577         speedMap.put(network.getGtuType(GTUType.DEFAULTS.VEHICLE), new Speed(50, KM_PER_HOUR));
578         OTSRoadNode start = new OTSRoadNode(network, "start", from, Direction.ZERO);
579         OTSRoadNode end = new OTSRoadNode(network, "end", to, Direction.ZERO);
580         OTSPoint3D[] coordinates = new OTSPoint3D[2];
581         coordinates[0] = start.getPoint();
582         coordinates[1] = end.getPoint();
583         OTSLine3D line = new OTSLine3D(coordinates);
584         CrossSectionLink link = new CrossSectionLink(network, "A to B", start, end, network.getLinkType(LinkType.DEFAULTS.ROAD),
585                 line, LaneKeepingPolicy.KEEPRIGHT);
586         Length offsetAtStart = Length.instantiateSI(5);
587         Length offsetAtEnd = Length.instantiateSI(15);
588         Length width = Length.instantiateSI(4);
589         Lane lane = new Lane(link, "lane", offsetAtStart, offsetAtEnd, width, width, laneType, speedMap, true);
590         OTSLine3D laneCenterLine = lane.getCenterLine();
591         // System.out.println("Center line is " + laneCenterLine);
592         OTSPoint3D[] points = laneCenterLine.getPoints();
593         double prev = offsetAtStart.si + from.y;
594         double prevRatio = 0;
595         double prevDirection = 0;
596         for (int i = 0; i < points.length; i++)
597         {
598             OTSPoint3D p = points[i];
599             double relativeLength = p.x - from.x;
600             double ratio = relativeLength / (to.x - from.x);
601             double actualOffset = p.y;
602             if (0 == i)
603             {
604                 assertEquals("first point must have offset at start", offsetAtStart.si + from.y, actualOffset, 0.001);
605             }
606             if (points.length - 1 == i)
607             {
608                 assertEquals("last point must have offset at end", offsetAtEnd.si + from.y, actualOffset, 0.001);
609             }
610             // Other offsets must grow smoothly
611             double delta = actualOffset - prev;
612             assertTrue("delta must be nonnegative", delta >= 0);
613             if (i > 0)
614             {
615                 OTSPoint3D prevPoint = points[i - 1];
616                 double direction = Math.atan2(p.y - prevPoint.y, p.x - prevPoint.x);
617                 // System.out.println(String.format("p=%30s: ratio=%7.5f, direction=%10.7f", p, ratio, direction));
618                 assertTrue("Direction of lane center line is > 0", direction > 0);
619                 if (ratio < 0.5)
620                 {
621                     assertTrue("in first half direction is increasing", direction > prevDirection);
622                 }
623                 else if (prevRatio > 0.5)
624                 {
625                     assertTrue("in second half direction is decreasing", direction < prevDirection);
626                 }
627                 prevDirection = direction;
628                 prevRatio = ratio;
629             }
630         }
631     }
632 
633     /**
634      * Test that the contour of a constructed lane covers the expected area. Tests are only performed for straight lanes, but
635      * the orientation of the link and the offset of the lane from the link is varied in many ways.
636      * @throws Exception when something goes wrong (should not happen)
637      */
638     @Test
639     public final void contourTest() throws Exception
640     {
641         final int[] startPositions = { 0, 1, -1, 20, -20 };
642         final double[] angles = { 0, Math.PI * 0.01, Math.PI / 3, Math.PI / 2, Math.PI * 2 / 3, Math.PI * 0.99, Math.PI,
643                 Math.PI * 1.01, Math.PI * 4 / 3, Math.PI * 3 / 2, Math.PI * 1.99, Math.PI * 2, Math.PI * (-0.2) };
644         int laneNum = 0;
645         for (int xStart : startPositions)
646         {
647             for (int yStart : startPositions)
648             {
649                 for (double angle : angles)
650                 {
651                     OTSSimulatorInterface simulator = new OTSSimulator("LaneTest");
652                     Model model = new Model(simulator);
653                     simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model);
654                     OTSRoadNetwork network = new OTSRoadNetwork("contour test network", true, simulator);
655                     LaneType laneType = network.getLaneType(LaneType.DEFAULTS.TWO_WAY_LANE);
656                     Map<GTUType, LongitudinalDirectionality> directionalityMap = new LinkedHashMap<>();
657                     directionalityMap.put(network.getGtuType(GTUType.DEFAULTS.VEHICLE), LongitudinalDirectionality.DIR_PLUS);
658                     Map<GTUType, Speed> speedMap = new LinkedHashMap<>();
659                     speedMap.put(network.getGtuType(GTUType.DEFAULTS.VEHICLE), new Speed(50, KM_PER_HOUR));
660                     OTSRoadNode start =
661                             new OTSRoadNode(network, "start", new OTSPoint3D(xStart, yStart), Direction.instantiateSI(angle));
662                     double linkLength = 1000;
663                     double xEnd = xStart + linkLength * Math.cos(angle);
664                     double yEnd = yStart + linkLength * Math.sin(angle);
665                     OTSRoadNode end =
666                             new OTSRoadNode(network, "end", new OTSPoint3D(xEnd, yEnd), Direction.instantiateSI(angle));
667                     OTSPoint3D[] coordinates = new OTSPoint3D[2];
668                     coordinates[0] = start.getPoint();
669                     coordinates[1] = end.getPoint();
670                     OTSLine3D line = new OTSLine3D(coordinates);
671                     CrossSectionLink link = new CrossSectionLink(network, "A to B", start, end,
672                             network.getLinkType(LinkType.DEFAULTS.ROAD), line, LaneKeepingPolicy.KEEPRIGHT);
673                     final int[] lateralOffsets = { -10, -3, -1, 0, 1, 3, 10 };
674                     for (int startLateralOffset : lateralOffsets)
675                     {
676                         for (int endLateralOffset : lateralOffsets)
677                         {
678                             int startWidth = 4; // This one is not varied
679                             for (int endWidth : new int[] { 2, 4, 6 })
680                             {
681                                 // Now we can construct a Lane
682                                 // FIXME what overtaking conditions do we want to test in this unit test?
683                                 Lane lane = new Lane(link, "lane." + ++laneNum, new Length(startLateralOffset, METER),
684                                         new Length(endLateralOffset, METER), new Length(startWidth, METER),
685                                         new Length(endWidth, METER), laneType, speedMap);
686                                 final Geometry geometry = lane.getContour().getLineString();
687                                 assertNotNull("geometry of the lane should not be null", geometry);
688                                 // Verify a couple of points that should be inside the contour of the Lane
689                                 // One meter along the lane design line
690                                 checkInside(lane, 1, startLateralOffset, true);
691                                 // One meter before the end along the lane design line
692                                 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset, true);
693                                 // One meter before the start of the lane along the lane design line
694                                 checkInside(lane, -1, startLateralOffset, false);
695                                 // One meter beyond the end of the lane along the lane design line
696                                 checkInside(lane, link.getLength().getSI() + 1, endLateralOffset, false);
697                                 // One meter along the lane design line, left outside the lane
698                                 checkInside(lane, 1, startLateralOffset - startWidth / 2 - 1, false);
699                                 // One meter along the lane design line, right outside the lane
700                                 checkInside(lane, 1, startLateralOffset + startWidth / 2 + 1, false);
701                                 // One meter before the end, left outside the lane
702                                 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset - endWidth / 2 - 1, false);
703                                 // One meter before the end, right outside the lane
704                                 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset + endWidth / 2 + 1, false);
705                                 // Check the result of getBounds.
706                                 DirectedPoint l = lane.getLocation();
707                                 Bounds bb = lane.getBounds();
708                                 // System.out.println("bb is " + bb);
709                                 // System.out.println("l is " + l.x + "," + l.y + "," + l.z);
710                                 // System.out.println("start is at " + start.getX() + ", " + start.getY());
711                                 // System.out.println(" end is at " + end.getX() + ", " + end.getY());
712                                 Point2D.Double[] cornerPoints = new Point2D.Double[4];
713                                 cornerPoints[0] =
714                                         new Point2D.Double(xStart - (startLateralOffset + startWidth / 2) * Math.sin(angle),
715                                                 yStart + (startLateralOffset + startWidth / 2) * Math.cos(angle));
716                                 cornerPoints[1] =
717                                         new Point2D.Double(xStart - (startLateralOffset - startWidth / 2) * Math.sin(angle),
718                                                 yStart + (startLateralOffset - startWidth / 2) * Math.cos(angle));
719                                 cornerPoints[2] = new Point2D.Double(xEnd - (endLateralOffset + endWidth / 2) * Math.sin(angle),
720                                         yEnd + (endLateralOffset + endWidth / 2) * Math.cos(angle));
721                                 cornerPoints[3] = new Point2D.Double(xEnd - (endLateralOffset - endWidth / 2) * Math.sin(angle),
722                                         yEnd + (endLateralOffset - endWidth / 2) * Math.cos(angle));
723                                 // for (int i = 0; i < cornerPoints.length; i++)
724                                 // {
725                                 // System.out.println("p" + i + ": " + cornerPoints[i].x + "," + cornerPoints[i].y);
726                                 // }
727                                 double minX = cornerPoints[0].getX();
728                                 double maxX = cornerPoints[0].getX();
729                                 double minY = cornerPoints[0].getY();
730                                 double maxY = cornerPoints[0].getY();
731                                 for (int i = 1; i < cornerPoints.length; i++)
732                                 {
733                                     Point2D.Double p = cornerPoints[i];
734                                     minX = Math.min(minX, p.getX());
735                                     minY = Math.min(minY, p.getY());
736                                     maxX = Math.max(maxX, p.getX());
737                                     maxY = Math.max(maxY, p.getY());
738                                 }
739                                 // System.out.println(" my bbox is " + minX + "," + minY + " - " + maxX + "," + maxY);
740                                 // System.out.println("the bbox is " + (bbLow.x + l.x) + "," + (bbLow.y + l.y) + " - "
741                                 // + (bbHigh.x + l.x) + "," + (bbHigh.y + l.y));
742                                 double boundsMinX = bb.getMinX() + l.x;
743                                 double boundsMinY = bb.getMinY() + l.y;
744                                 double boundsMaxX = bb.getMaxX() + l.x;
745                                 double boundsMaxY = bb.getMaxY() + l.y;
746                                 assertEquals("low x boundary", minX, boundsMinX, 0.1);
747                                 assertEquals("low y boundary", minY, boundsMinY, 0.1);
748                                 assertEquals("high x boundary", maxX, boundsMaxX, 0.1);
749                                 assertEquals("high y boundary", maxY, boundsMaxY, 0.1);
750                             }
751                         }
752                     }
753                 }
754             }
755         }
756     }
757 
758     /**
759      * Verify that a point at specified distance along and across from the design line of the parent Link of a Lane is inside
760      * c.q. outside the contour of a Lane. The test uses an implementation that is as independent as possible of the Geometry
761      * class methods.
762      * @param lane Lane; the lane
763      * @param longitudinal double; the longitudinal position along the design line of the parent Link of the Lane. This design
764      *            line is expected to be straight and the longitudinal position may be negative (indicating a point before the
765      *            start of the Link) and it may exceed the length of the Link (indicating a point beyond the end of the Link)
766      * @param lateral double; the lateral offset from the design line of the link (positive is left, negative is right)
767      * @param expectedResult boolean; true if the calling method expects the point to be within the contour of the Lane, false
768      *            if the calling method expects the point to be outside the contour of the Lane
769      */
770     private void checkInside(final Lane lane, final double longitudinal, final double lateral, final boolean expectedResult)
771     {
772         CrossSectionLink parentLink = lane.getParentLink();
773         Node start = parentLink.getStartNode();
774         Node end = parentLink.getEndNode();
775         double startX = start.getPoint().x;
776         double startY = start.getPoint().y;
777         double endX = end.getPoint().x;
778         double endY = end.getPoint().y;
779         double length = Math.sqrt((endX - startX) * (endX - startX) + (endY - startY) * (endY - startY));
780         double ratio = longitudinal / length;
781         double designLineX = startX + (endX - startX) * ratio;
782         double designLineY = startY + (endY - startY) * ratio;
783         double lateralAngle = Math.atan2(endY - startY, endX - startX) + Math.PI / 2;
784         double px = designLineX + lateral * Math.cos(lateralAngle);
785         double py = designLineY + lateral * Math.sin(lateralAngle);
786         Geometry contour = lane.getContour().getLineString();
787         GeometryFactory factory = new GeometryFactory();
788         Geometry p = factory.createPoint(new Coordinate(px, py));
789         // CrossSectionElement.printCoordinates("contour: ", contour);
790         // System.out.println("p: " + p);
791         boolean result = contour.contains(p);
792         Coordinate[] polygon = contour.getCoordinates();
793         result = pointInsidePolygon(new Coordinate(px, py), polygon);
794         if (expectedResult)
795         {
796             assertTrue("Point at " + longitudinal + " along and " + lateral + " lateral is within lane", result);
797         }
798         else
799         {
800             assertFalse("Point at " + longitudinal + " along and " + lateral + " lateral is outside lane", result);
801         }
802     }
803 
804     /**
805      * Algorithm of W. Randolph Franklin http://www.ecse.rpi.edu/Homepages/wrf/Research/Short_Notes/pnpoly.html, found via
806      * stackoverflow.com: http://stackoverflow.com/questions/217578/point-in-polygon-aka-hit-test.
807      * @param point Coordinate; the point
808      * @param polygon OTSPoint3D[]; the polygon (last coordinate is allowed to be identical to the first, but his is not a
809      *            requirement)
810      * @return boolean; true if the point is inside the polygon; false if it is outside the polygon; if the point lies <b>on</b>
811      *         an vertex or edge of the polygon the result is (of course) undefined
812      */
813     private boolean pointInsidePolygon(final Coordinate point, final Coordinate[] polygon)
814     {
815         boolean result = false;
816         for (int i = 0, j = polygon.length - 1; i < polygon.length; j = i++)
817         {
818             if ((polygon[i].y > point.y) != (polygon[j].y > point.y)
819                     && point.x < (polygon[j].x - polygon[i].x) * (point.y - polygon[i].y) / (polygon[j].y - polygon[i].y)
820                             + polygon[i].x)
821             {
822                 result = !result;
823             }
824         }
825         return result;
826     }
827 
828     /** The helper model. */
829     protected static class Model extends AbstractOTSModel
830     {
831         /** */
832         private static final long serialVersionUID = 20141027L;
833 
834         /**
835          * @param simulator the simulator to use
836          */
837         public Model(final OTSSimulatorInterface simulator)
838         {
839             super(simulator);
840         }
841 
842         /** {@inheritDoc} */
843         @Override
844         public final void constructModel() throws SimRuntimeException
845         {
846             //
847         }
848 
849         /** {@inheritDoc} */
850         @Override
851         public final OTSRoadNetwork getNetwork()
852         {
853             return null;
854         }
855 
856         /** {@inheritDoc} */
857         @Override
858         public Serializable getSourceId()
859         {
860             return "LaneTest.Model";
861         }
862     }
863 
864 }