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