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
80 OTSRoadNetwork network = new OTSRoadNetwork("lane test network", true);
81 OTSRoadNode nodeFrom = new OTSRoadNode(network, "A", new OTSPoint3D(0, 0, 0), Direction.ZERO);
82 OTSRoadNode nodeTo = new OTSRoadNode(network, "B", new OTSPoint3D(1000, 0, 0), Direction.ZERO);
83
84 OTSPoint3D[] coordinates = new OTSPoint3D[2];
85 coordinates[0] = new OTSPoint3D(nodeFrom.getPoint().x, nodeFrom.getPoint().y, 0);
86 coordinates[1] = new OTSPoint3D(nodeTo.getPoint().x, nodeTo.getPoint().y, 0);
87 OTSSimulatorInterface simulator = new OTSSimulator("LaneTest");
88 Model model = new Model(simulator);
89 simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model);
90 CrossSectionLink link =
91 new CrossSectionLink(network, "A to B", nodeFrom, nodeTo, network.getLinkType(LinkType.DEFAULTS.FREEWAY),
92 new OTSLine3D(coordinates), simulator, 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), simulator,
158 LaneKeepingPolicy.KEEPRIGHT);
159
160 lane = new Lane(link, "lane.1", startLateralPos, endLateralPos, startWidth, endWidth, laneType, speedMap);
161
162 assertEquals("PrevLanes should contain one lane from the other link", 1, lane.prevLanes(gtuTypeCar).size());
163 assertEquals("NextLanes should contain one lane from the other link", 1, lane.nextLanes(gtuTypeCar).size());
164 approximateLengthOfContour = 2 * (coordinates[0].distanceSI(coordinates[1]) + coordinates[1].distanceSI(coordinates[2]))
165 + startWidth.getSI() + endWidth.getSI();
166
167
168 assertEquals("Length of contour is approximately " + approximateLengthOfContour, approximateLengthOfContour,
169 lane.getContour().getLengthSI(), 4);
170 assertEquals("There should be no GTUs on the lane", 0, lane.getGtuList().size());
171 assertEquals("LaneType should be " + laneType, laneType, lane.getLaneType());
172
173 Length startLateralPos2 = new Length(-8, METER);
174 Length endLateralPos2 = new Length(-5, METER);
175
176 Lane lane2 = new Lane(link, "lane.2", startLateralPos2, endLateralPos2, startWidth, endWidth, laneType, speedMap);
177
178 assertEquals("PrevLanes should be empty", 0, lane2.prevLanes(gtuTypeCar).size());
179 assertEquals("NextLanes should be empty", 0, lane2.nextLanes(gtuTypeCar).size());
180 approximateLengthOfContour = 2 * (coordinates[0].distanceSI(coordinates[1]) + coordinates[1].distanceSI(coordinates[2]))
181 + startWidth.getSI() + endWidth.getSI();
182 assertEquals("Length of contour is approximately " + approximateLengthOfContour, approximateLengthOfContour,
183 lane2.getContour().getLengthSI(), 12);
184 assertEquals("There should be no GTUs on the lane", 0, lane2.getGtuList().size());
185 assertEquals("LaneType should be " + laneType, laneType, lane2.getLaneType());
186
187
188 try
189 {
190 new Lane(link, "lanex", null, laneType, speedMap);
191 fail("null pointer for CrossSectionSlices should have thrown a NullPointerException");
192 }
193 catch (NullPointerException npe)
194 {
195
196 }
197 List<CrossSectionSlice> crossSectionSlices = new ArrayList<>();
198 try
199 {
200 new Lane(link, "lanex", crossSectionSlices, laneType, speedMap);
201 fail("empty CrossSectionSlices should have thrown a NetworkException");
202 }
203 catch (NetworkException ne)
204 {
205
206 }
207 crossSectionSlices.add(new CrossSectionSlice(Length.ZERO, startLateralPos, startWidth));
208 lane = new Lane(link, "lanex", crossSectionSlices, laneType, new Speed(100, KM_PER_HOUR));
209 sensorTest(lane);
210 }
211
212
213
214
215
216
217 public final void sensorTest(final Lane lane) throws NetworkException
218 {
219 assertEquals("List of sensor is initially empty", 0, lane.getSensors().size());
220 Listener listener = new Listener();
221 double length = lane.getLength().si;
222 lane.addListener(listener, Lane.SENSOR_ADD_EVENT);
223 lane.addListener(listener, Lane.SENSOR_REMOVE_EVENT);
224 assertEquals("event list is initially empty", 0, listener.events.size());
225 SingleSensor sensor1 = new MockSensor("sensor1", Length.instantiateSI(length / 4)).getMock();
226 lane.addSensor(sensor1);
227 assertEquals("event list now contains one event", 1, listener.events.size());
228 assertEquals("event indicates that a sensor got added", listener.events.get(0).getType(), Lane.SENSOR_ADD_EVENT);
229 assertEquals("lane now contains one sensor", 1, lane.getSensors().size());
230 assertEquals("sensor on lane is sensor1", sensor1, lane.getSensors().get(0));
231 SingleSensor sensor2 = new MockSensor("sensor2", Length.instantiateSI(length / 2)).getMock();
232 lane.addSensor(sensor2);
233 assertEquals("event list now contains two events", 2, listener.events.size());
234 assertEquals("event indicates that a sensor got added", listener.events.get(1).getType(), Lane.SENSOR_ADD_EVENT);
235 List<SingleSensor> sensors = lane.getSensors();
236 assertEquals("lane now contains two sensors", 2, sensors.size());
237 assertTrue("sensor list contains sensor1", sensors.contains(sensor1));
238 assertTrue("sensor list contains sensor2", sensors.contains(sensor2));
239 sensors = lane.getSensors(Length.ZERO, Length.instantiateSI(length / 3),
240 lane.getParentLink().getNetwork().getGtuType(GTUType.DEFAULTS.VEHICLE), GTUDirectionality.DIR_PLUS);
241 assertEquals("first third of lane contains 1 sensor", 1, sensors.size());
242 assertTrue("sensor list contains sensor1", sensors.contains(sensor1));
243 sensors = lane.getSensors(Length.instantiateSI(length / 3), Length.instantiateSI(length),
244 lane.getParentLink().getNetwork().getGtuType(GTUType.DEFAULTS.VEHICLE), GTUDirectionality.DIR_PLUS);
245 assertEquals("last two-thirds of lane contains 1 sensor", 1, sensors.size());
246 assertTrue("sensor list contains sensor2", sensors.contains(sensor2));
247 sensors = lane.getSensors(lane.getParentLink().getNetwork().getGtuType(GTUType.DEFAULTS.VEHICLE),
248 GTUDirectionality.DIR_PLUS);
249
250 assertEquals("sensor list contains two sensors", 2, sensors.size());
251 assertTrue("sensor list contains sensor1", sensors.contains(sensor1));
252 assertTrue("sensor list contains sensor2", sensors.contains(sensor2));
253 sensors = lane.getSensors(lane.getParentLink().getNetwork().getGtuType(GTUType.DEFAULTS.VEHICLE),
254 GTUDirectionality.DIR_MINUS);
255
256 assertEquals("sensor list contains two sensors", 2, sensors.size());
257 assertTrue("sensor list contains sensor1", sensors.contains(sensor1));
258 assertTrue("sensor list contains sensor2", sensors.contains(sensor2));
259 SortedMap<Double, List<SingleSensor>> sensorMap = lane.getSensorMap(
260 lane.getParentLink().getNetwork().getGtuType(GTUType.DEFAULTS.VEHICLE), GTUDirectionality.DIR_PLUS);
261 assertEquals("sensor map contains two entries", 2, sensorMap.size());
262 for (Double d : sensorMap.keySet())
263 {
264 List<SingleSensor> sensorsAtD = sensorMap.get(d);
265 assertEquals("There is one sensor at position d", 1, sensorsAtD.size());
266 assertEquals("Sensor map contains the correct sensor at the correct distance", d < length / 3 ? sensor1 : sensor2,
267 sensorsAtD.get(0));
268 }
269
270 lane.removeSensor(sensor1);
271 assertEquals("event list now contains three events", 3, listener.events.size());
272 assertEquals("event indicates that a sensor got removed", listener.events.get(2).getType(), Lane.SENSOR_REMOVE_EVENT);
273 sensors = lane.getSensors();
274 assertEquals("lane now contains one sensor", 1, sensors.size());
275 assertTrue("sensor list contains sensor2", sensors.contains(sensor2));
276 try
277 {
278 lane.removeSensor(sensor1);
279 fail("Removing a sensor twice should have thrown a NetworkException");
280 }
281 catch (NetworkException ne)
282 {
283
284 }
285 try
286 {
287 lane.addSensor(sensor2);
288 fail("Adding a sensor twice should have thrown a NetworkException");
289 }
290 catch (NetworkException ne)
291 {
292
293 }
294 SingleSensor badSensor = new MockSensor("sensor3", Length.instantiateSI(-0.1)).getMock();
295 try
296 {
297 lane.addSensor(badSensor);
298 fail("Adding a sensor at negative position should have thrown a NetworkException");
299 }
300 catch (NetworkException ne)
301 {
302
303 }
304 badSensor = new MockSensor("sensor4", Length.instantiateSI(length + 0.1)).getMock();
305 try
306 {
307 lane.addSensor(badSensor);
308 fail("Adding a sensor at position beyond the end of the lane should have thrown a NetworkException");
309 }
310 catch (NetworkException ne)
311 {
312
313 }
314 lane.removeSensor(sensor2);
315 List<LaneBasedObject> lboList = lane.getLaneBasedObjects();
316 assertEquals("lane initially contains zero lane based objects", 0, lboList.size());
317 LaneBasedObject lbo1 = new MockLaneBasedObject("lbo1", Length.instantiateSI(length / 4)).getMock();
318 listener.getEvents().clear();
319 lane.addListener(listener, Lane.OBJECT_ADD_EVENT);
320 lane.addListener(listener, Lane.OBJECT_REMOVE_EVENT);
321 lane.addLaneBasedObject(lbo1);
322 assertEquals("adding a lane based object cause the lane to emit an event", 1, listener.getEvents().size());
323 assertEquals("The emitted event was a OBJECT_ADD_EVENT", Lane.OBJECT_ADD_EVENT, listener.getEvents().get(0).getType());
324 LaneBasedObject lbo2 = new MockLaneBasedObject("lbo2", Length.instantiateSI(3 * length / 4)).getMock();
325 lane.addLaneBasedObject(lbo2);
326 lboList = lane.getLaneBasedObjects();
327 assertEquals("lane based object list now contains two objects", 2, lboList.size());
328 assertTrue("lane base object list contains lbo1", lboList.contains(lbo1));
329 assertTrue("lane base object list contains lbo2", lboList.contains(lbo2));
330 lboList = lane.getLaneBasedObjects(Length.ZERO, Length.instantiateSI(length / 2));
331 assertEquals("first half of lane contains one object", 1, lboList.size());
332 assertEquals("object in first haf of lane is lbo1", lbo1, lboList.get(0));
333 lboList = lane.getLaneBasedObjects(Length.instantiateSI(length / 2), Length.instantiateSI(length));
334 assertEquals("second half of lane contains one object", 1, lboList.size());
335 assertEquals("object in second haf of lane is lbo2", lbo2, lboList.get(0));
336 SortedMap<Double, List<LaneBasedObject>> sortedMap = lane.getLaneBasedObjectMap();
337 assertEquals("sorted map contains two objects", 2, sortedMap.size());
338 for (Double d : sortedMap.keySet())
339 {
340 List<LaneBasedObject> objectsAtD = sortedMap.get(d);
341 assertEquals("There is one object at position d", 1, objectsAtD.size());
342 assertEquals("Object at position d is the expected one", d < length / 2 ? lbo1 : lbo2, objectsAtD.get(0));
343 }
344
345 for (double fraction : new double[] { -0.5, 0, 0.2, 0.5, 0.9, 1.0, 2 })
346 {
347 double positionSI = length * fraction;
348 double fractionSI = lane.fractionSI(positionSI);
349 assertEquals("fractionSI matches fraction", fraction, fractionSI, 0.0001);
350
351 LaneBasedObject nextObject = positionSI < lbo1.getLongitudinalPosition().si ? lbo1
352 : positionSI < lbo2.getLongitudinalPosition().si ? lbo2 : null;
353 List<LaneBasedObject> expected = null;
354 if (null != nextObject)
355 {
356 expected = new ArrayList<>();
357 expected.add(nextObject);
358 }
359 List<LaneBasedObject> got = lane.getObjectAhead(Length.instantiateSI(positionSI), GTUDirectionality.DIR_PLUS);
360 assertEquals("First bunch of objects ahead of d", expected, got);
361
362 nextObject = positionSI > lbo2.getLongitudinalPosition().si ? lbo2
363 : positionSI > lbo1.getLongitudinalPosition().si ? lbo1 : null;
364 expected = null;
365 if (null != nextObject)
366 {
367 expected = new ArrayList<>();
368 expected.add(nextObject);
369 }
370 got = lane.getObjectAhead(Length.instantiateSI(positionSI), GTUDirectionality.DIR_MINUS);
371 assertEquals("First bunch of objects behind d", expected, got);
372 }
373
374 lane.removeLaneBasedObject(lbo1);
375 assertEquals("removing a lane based object caused the lane to emit an event", 3, listener.getEvents().size());
376 assertEquals("removing a lane based object caused the lane to emit OBJECT_REMOVE_EVENT", Lane.OBJECT_REMOVE_EVENT,
377 listener.getEvents().get(2).getType());
378 try
379 {
380 lane.removeLaneBasedObject(lbo1);
381 fail("Removing a lane bases object that was already removed should have caused a NetworkException");
382 }
383 catch (NetworkException ne)
384 {
385
386 }
387 try
388 {
389 lane.addLaneBasedObject(lbo2);
390 fail("Adding a lane base object that was already added should have caused a NetworkException");
391 }
392 catch (NetworkException ne)
393 {
394
395 }
396 LaneBasedObject badLBO = new MockLaneBasedObject("badLBO", Length.instantiateSI(-0.1)).getMock();
397 try
398 {
399 lane.addLaneBasedObject(badLBO);
400 fail("Adding a lane based object at negative position should have thrown a NetworkException");
401 }
402 catch (NetworkException ne)
403 {
404
405 }
406 badLBO = new MockLaneBasedObject("badLBO", Length.instantiateSI(length + 0.1)).getMock();
407 try
408 {
409 lane.addLaneBasedObject(badLBO);
410 fail("Adding a lane based object at position beyond end of lane should have thrown a NetworkException");
411 }
412 catch (NetworkException ne)
413 {
414
415 }
416 }
417
418
419
420
421 class Listener implements EventListenerInterface
422 {
423
424 private List<EventInterface> events = new ArrayList<>();
425
426 @Override
427 public void notify(final EventInterface event) throws RemoteException
428 {
429 events.add(event);
430 }
431
432
433
434
435
436 public List<EventInterface> getEvents()
437 {
438 return this.events;
439 }
440
441 }
442
443
444
445
446 class MockSensor
447 {
448
449 private final SingleSensor mockSensor;
450
451
452 private final String id;
453
454
455 private final Length position;
456
457
458 private final OTSSimulatorInterface simulator = MockDEVSSimulator.createMock();
459
460
461
462
463
464
465 MockSensor(final String id, final Length position)
466 {
467 this.mockSensor = Mockito.mock(SingleSensor.class);
468 this.id = id;
469 this.position = position;
470 Mockito.when(this.mockSensor.getId()).thenReturn(this.id);
471 Mockito.when(this.mockSensor.getLongitudinalPosition()).thenReturn(this.position);
472 Mockito.when(this.mockSensor.getSimulator()).thenReturn(this.simulator);
473 Mockito.when(this.mockSensor.getFullId()).thenReturn(this.id);
474 Mockito.when(this.mockSensor.isCompatible(Mockito.any(), Mockito.any())).thenReturn(true);
475 }
476
477
478
479
480
481 public SingleSensor getMock()
482 {
483 return this.mockSensor;
484 }
485
486
487
488
489
490 public Length getLongitudinalPosition()
491 {
492 return this.position;
493 }
494
495 @Override
496 public String toString()
497 {
498 return "MockSensor [mockSensor=" + mockSensor + ", id=" + id + ", position=" + position + "]";
499 }
500
501 }
502
503
504
505
506 class MockLaneBasedObject
507 {
508
509 private final LaneBasedObject mockLaneBasedObject;
510
511
512 private final String id;
513
514
515 private final Length position;
516
517
518
519
520
521
522 MockLaneBasedObject(final String id, final Length position)
523 {
524 this.mockLaneBasedObject = Mockito.mock(SingleSensor.class);
525 this.id = id;
526 this.position = position;
527 Mockito.when(this.mockLaneBasedObject.getId()).thenReturn(this.id);
528 Mockito.when(this.mockLaneBasedObject.getLongitudinalPosition()).thenReturn(this.position);
529 Mockito.when(this.mockLaneBasedObject.getFullId()).thenReturn(this.id);
530 }
531
532
533
534
535
536 public LaneBasedObject getMock()
537 {
538 return this.mockLaneBasedObject;
539 }
540
541
542
543
544
545 public Length getLongitudinalPosition()
546 {
547 return this.position;
548 }
549
550 @Override
551 public String toString()
552 {
553 return "MockLaneBasedObject [mockLaneBasedObject=" + mockLaneBasedObject + ", id=" + id + ", position=" + position
554 + "]";
555 }
556
557 }
558
559
560
561
562
563
564
565
566
567 @Test
568 public final void lateralOffsetTest() throws NetworkException, SimRuntimeException, NamingException, OTSGeometryException
569 {
570 OTSPoint3D from = new OTSPoint3D(10, 10, 0);
571 OTSPoint3D to = new OTSPoint3D(1010, 10, 0);
572 OTSRoadNetwork network = new OTSRoadNetwork("contour test network", true);
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 OTSSimulatorInterface simulator = new OTSSimulator("LaneTest");
585 Model model = new Model(simulator);
586 simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model);
587 CrossSectionLink link = new CrossSectionLink(network, "A to B", start, end, network.getLinkType(LinkType.DEFAULTS.ROAD),
588 line, simulator, LaneKeepingPolicy.KEEPRIGHT);
589 Length offsetAtStart = Length.instantiateSI(5);
590 Length offsetAtEnd = Length.instantiateSI(15);
591 Length width = Length.instantiateSI(4);
592 Lane lane = new Lane(link, "lane", offsetAtStart, offsetAtEnd, width, width, laneType, speedMap, true);
593 OTSLine3D laneCenterLine = lane.getCenterLine();
594
595 OTSPoint3D[] points = laneCenterLine.getPoints();
596 double prev = offsetAtStart.si + from.y;
597 double prevRatio = 0;
598 double prevDirection = 0;
599 for (int i = 0; i < points.length; i++)
600 {
601 OTSPoint3D p = points[i];
602 double relativeLength = p.x - from.x;
603 double ratio = relativeLength / (to.x - from.x);
604 double actualOffset = p.y;
605 if (0 == i)
606 {
607 assertEquals("first point must have offset at start", offsetAtStart.si + from.y, actualOffset, 0.001);
608 }
609 if (points.length - 1 == i)
610 {
611 assertEquals("last point must have offset at end", offsetAtEnd.si + from.y, actualOffset, 0.001);
612 }
613
614 double delta = actualOffset - prev;
615 assertTrue("delta must be nonnegative", delta >= 0);
616 if (i > 0)
617 {
618 OTSPoint3D prevPoint = points[i - 1];
619 double direction = Math.atan2(p.y - prevPoint.y, p.x - prevPoint.x);
620
621 assertTrue("Direction of lane center line is > 0", direction > 0);
622 if (ratio < 0.5)
623 {
624 assertTrue("in first half direction is increasing", direction > prevDirection);
625 }
626 else if (prevRatio > 0.5)
627 {
628 assertTrue("in second half direction is decreasing", direction < prevDirection);
629 }
630 prevDirection = direction;
631 prevRatio = ratio;
632 }
633 }
634 }
635
636
637
638
639
640
641 @Test
642 public final void contourTest() throws Exception
643 {
644 final int[] startPositions = { 0, 1, -1, 20, -20 };
645 final double[] angles = { 0, Math.PI * 0.01, Math.PI / 3, Math.PI / 2, Math.PI * 2 / 3, Math.PI * 0.99, Math.PI,
646 Math.PI * 1.01, Math.PI * 4 / 3, Math.PI * 3 / 2, Math.PI * 1.99, Math.PI * 2, Math.PI * (-0.2) };
647 int laneNum = 0;
648 for (int xStart : startPositions)
649 {
650 for (int yStart : startPositions)
651 {
652 for (double angle : angles)
653 {
654 OTSRoadNetwork network = new OTSRoadNetwork("contour test network", true);
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 = new OTSRoadNode(network, "end", new OTSPoint3D(xEnd, yEnd), Direction.instantiateSI(angle));
666 OTSPoint3D[] coordinates = new OTSPoint3D[2];
667 coordinates[0] = start.getPoint();
668 coordinates[1] = end.getPoint();
669 OTSLine3D line = new OTSLine3D(coordinates);
670 OTSSimulatorInterface simulator = new OTSSimulator("LaneTest");
671 Model model = new Model(simulator);
672 simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model);
673 CrossSectionLink link = new CrossSectionLink(network, "A to B", start, end,
674 network.getLinkType(LinkType.DEFAULTS.ROAD), line, simulator, 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 }