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
60
61
62
63
64
65
66
67
68 public class LaneTest implements UNITS
69 {
70
71
72
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
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
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
103
104 Lane lane = new Lane(link, "lane", startLateralPos, endLateralPos, startWidth, endWidth, laneType, speedMap);
105
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());
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
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
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
149
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
157 lane = new Lane(link, "lane.1", startLateralPos, endLateralPos, startWidth, endWidth, laneType, speedMap);
158
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
164
165 assertEquals("Length of contour is approximately " + approximateLengthOfContour, approximateLengthOfContour,
166 lane.getContour().getLengthSI(), 4);
167 assertEquals("There should be no GTUs on the lane", 0, lane.getGtuList().size());
168 assertEquals("LaneType should be " + laneType, laneType, lane.getLaneType());
169
170 Length startLateralPos2 = new Length(-8, METER);
171 Length endLateralPos2 = new Length(-5, METER);
172
173 Lane lane2 = new Lane(link, "lane.2", startLateralPos2, endLateralPos2, startWidth, endWidth, laneType, speedMap);
174
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);
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
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
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
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
211
212
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
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
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
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
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
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
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
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
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
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
412 }
413 }
414
415
416
417
418 class Listener implements EventListenerInterface
419 {
420
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
431
432
433 public List<EventInterface> getEvents()
434 {
435 return this.events;
436 }
437
438 }
439
440
441
442
443 class MockSensor
444 {
445
446 private final SingleSensor mockSensor;
447
448
449 private final String id;
450
451
452 private final Length position;
453
454
455 private final OTSSimulatorInterface simulator = MockDEVSSimulator.createMock();
456
457
458
459
460
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
476
477
478 public SingleSensor getMock()
479 {
480 return this.mockSensor;
481 }
482
483
484
485
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
502
503 class MockLaneBasedObject
504 {
505
506 private final LaneBasedObject mockLaneBasedObject;
507
508
509 private final String id;
510
511
512 private final Length position;
513
514
515
516
517
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
531
532
533 public LaneBasedObject getMock()
534 {
535 return this.mockLaneBasedObject;
536 }
537
538
539
540
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
558
559
560
561
562
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
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
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
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
635
636
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;
679 for (int endWidth : new int[] { 2, 4, 6 })
680 {
681
682
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
689
690 checkInside(lane, 1, startLateralOffset, true);
691
692 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset, true);
693
694 checkInside(lane, -1, startLateralOffset, false);
695
696 checkInside(lane, link.getLength().getSI() + 1, endLateralOffset, false);
697
698 checkInside(lane, 1, startLateralOffset - startWidth / 2 - 1, false);
699
700 checkInside(lane, 1, startLateralOffset + startWidth / 2 + 1, false);
701
702 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset - endWidth / 2 - 1, false);
703
704 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset + endWidth / 2 + 1, false);
705
706 DirectedPoint l = lane.getLocation();
707 Bounds bb = lane.getBounds();
708
709
710
711
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
724
725
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
740
741
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
760
761
762
763
764
765
766
767
768
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
790
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
806
807
808
809
810
811
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
829 protected static class Model extends AbstractOTSModel
830 {
831
832 private static final long serialVersionUID = 20141027L;
833
834
835
836
837 public Model(final OTSSimulatorInterface simulator)
838 {
839 super(simulator);
840 }
841
842
843 @Override
844 public final void constructModel() throws SimRuntimeException
845 {
846
847 }
848
849
850 @Override
851 public final OTSRoadNetwork getNetwork()
852 {
853 return null;
854 }
855
856
857 @Override
858 public Serializable getSourceId()
859 {
860 return "LaneTest.Model";
861 }
862 }
863
864 }