1 package org.opentrafficsim.road.network.lane;
2
3 import static org.junit.Assert.assertEquals;
4 import static org.junit.Assert.assertFalse;
5 import static org.junit.Assert.assertNotNull;
6 import static org.junit.Assert.assertTrue;
7 import static org.junit.Assert.fail;
8
9 import java.awt.geom.Point2D;
10 import java.rmi.RemoteException;
11 import java.util.ArrayList;
12 import java.util.LinkedHashMap;
13 import java.util.List;
14 import java.util.Map;
15 import java.util.SortedMap;
16
17 import javax.media.j3d.BoundingBox;
18 import javax.media.j3d.Bounds;
19 import javax.naming.NamingException;
20 import javax.vecmath.Point3d;
21
22 import org.djunits.unit.DurationUnit;
23 import org.djunits.unit.util.UNITS;
24 import org.djunits.value.vdouble.scalar.Direction;
25 import org.djunits.value.vdouble.scalar.Duration;
26 import org.djunits.value.vdouble.scalar.Length;
27 import org.djunits.value.vdouble.scalar.Speed;
28 import org.djunits.value.vdouble.scalar.Time;
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.OTSGeometryException;
39 import org.opentrafficsim.core.geometry.OTSLine3D;
40 import org.opentrafficsim.core.geometry.OTSPoint3D;
41 import org.opentrafficsim.core.gtu.GTUDirectionality;
42 import org.opentrafficsim.core.gtu.GTUType;
43 import org.opentrafficsim.core.network.LinkType;
44 import org.opentrafficsim.core.network.LongitudinalDirectionality;
45 import org.opentrafficsim.core.network.NetworkException;
46 import org.opentrafficsim.core.network.Node;
47 import org.opentrafficsim.road.mock.MockDEVSSimulator;
48 import org.opentrafficsim.road.network.OTSRoadNetwork;
49 import org.opentrafficsim.road.network.lane.changing.LaneKeepingPolicy;
50 import org.opentrafficsim.road.network.lane.object.LaneBasedObject;
51 import org.opentrafficsim.road.network.lane.object.sensor.SingleSensor;
52
53 import nl.tudelft.simulation.dsol.SimRuntimeException;
54 import nl.tudelft.simulation.event.EventInterface;
55 import nl.tudelft.simulation.event.EventListenerInterface;
56 import nl.tudelft.simulation.language.d3.DirectedPoint;
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
78 OTSRoadNetwork network = new OTSRoadNetwork("lane test network", true);
79 OTSRoadNode nodeFrom = new OTSRoadNode(network, "A", new OTSPoint3D(0, 0, 0), Direction.ZERO);
80 OTSRoadNode nodeTo = new OTSRoadNode(network, "B", new OTSPoint3D(1000, 0, 0), Direction.ZERO);
81
82 OTSPoint3D[] coordinates = new OTSPoint3D[2];
83 coordinates[0] = new OTSPoint3D(nodeFrom.getPoint().x, nodeFrom.getPoint().y, 0);
84 coordinates[1] = new OTSPoint3D(nodeTo.getPoint().x, nodeTo.getPoint().y, 0);
85 OTSSimulatorInterface simulator = new OTSSimulator();
86 Model model = new Model(simulator);
87 simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model);
88 CrossSectionLink link =
89 new CrossSectionLink(network, "A to B", nodeFrom, nodeTo, network.getLinkType(LinkType.DEFAULTS.ROAD),
90 new OTSLine3D(coordinates), simulator, 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
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
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, network.getLinkType(LinkType.DEFAULTS.ROAD),
157 new OTSLine3D(coordinates), simulator, LaneKeepingPolicy.KEEPRIGHT);
158
159 lane = new Lane(link, "lane.1", startLateralPos, endLateralPos, startWidth, endWidth, laneType, speedMap);
160
161 assertEquals("PrevLanes should be empty", 0, lane.prevLanes(gtuTypeCar).size());
162 assertEquals("NextLanes should be empty", 0, lane.nextLanes(gtuTypeCar).size());
163 approximateLengthOfContour = 2 * (coordinates[0].distanceSI(coordinates[1]) + coordinates[1].distanceSI(coordinates[2]))
164 + startWidth.getSI() + endWidth.getSI();
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 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=" + mockSensor + ", id=" + id + ", position=" + 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=" + mockLaneBasedObject + ", id=" + id + ", position=" + 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 OTSRoadNetwork network = new OTSRoadNetwork("contour test network", true);
570 LaneType laneType = network.getLaneType(LaneType.DEFAULTS.TWO_WAY_LANE);
571 Map<GTUType, LongitudinalDirectionality> directionalityMap = new LinkedHashMap<>();
572 directionalityMap.put(network.getGtuType(GTUType.DEFAULTS.VEHICLE), LongitudinalDirectionality.DIR_PLUS);
573 Map<GTUType, Speed> speedMap = new LinkedHashMap<>();
574 speedMap.put(network.getGtuType(GTUType.DEFAULTS.VEHICLE), new Speed(50, KM_PER_HOUR));
575 OTSRoadNode start = new OTSRoadNode(network, "start", from, Direction.ZERO);
576 OTSRoadNode end = new OTSRoadNode(network, "end", to, Direction.ZERO);
577 OTSPoint3D[] coordinates = new OTSPoint3D[2];
578 coordinates[0] = start.getPoint();
579 coordinates[1] = end.getPoint();
580 OTSLine3D line = new OTSLine3D(coordinates);
581 OTSSimulatorInterface simulator = new OTSSimulator();
582 Model model = new Model(simulator);
583 simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model);
584 CrossSectionLink link = new CrossSectionLink(network, "A to B", start, end, network.getLinkType(LinkType.DEFAULTS.ROAD),
585 line, simulator, 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 OTSRoadNetwork network = new OTSRoadNetwork("contour test network", true);
652 LaneType laneType = network.getLaneType(LaneType.DEFAULTS.TWO_WAY_LANE);
653 Map<GTUType, LongitudinalDirectionality> directionalityMap = new LinkedHashMap<>();
654 directionalityMap.put(network.getGtuType(GTUType.DEFAULTS.VEHICLE), LongitudinalDirectionality.DIR_PLUS);
655 Map<GTUType, Speed> speedMap = new LinkedHashMap<>();
656 speedMap.put(network.getGtuType(GTUType.DEFAULTS.VEHICLE), new Speed(50, KM_PER_HOUR));
657 OTSRoadNode start =
658 new OTSRoadNode(network, "start", new OTSPoint3D(xStart, yStart), Direction.instantiateSI(angle));
659 double linkLength = 1000;
660 double xEnd = xStart + linkLength * Math.cos(angle);
661 double yEnd = yStart + linkLength * Math.sin(angle);
662 OTSRoadNode end = new OTSRoadNode(network, "end", new OTSPoint3D(xEnd, yEnd), Direction.instantiateSI(angle));
663 OTSPoint3D[] coordinates = new OTSPoint3D[2];
664 coordinates[0] = start.getPoint();
665 coordinates[1] = end.getPoint();
666 OTSLine3D line = new OTSLine3D(coordinates);
667 OTSSimulatorInterface simulator = new OTSSimulator();
668 Model model = new Model(simulator);
669 simulator.initialize(Time.ZERO, Duration.ZERO, new Duration(3600.0, DurationUnit.SECOND), model);
670 CrossSectionLink link = new CrossSectionLink(network, "A to B", start, end,
671 network.getLinkType(LinkType.DEFAULTS.ROAD), line, simulator, LaneKeepingPolicy.KEEPRIGHT);
672 final int[] lateralOffsets = { -10, -3, -1, 0, 1, 3, 10 };
673 for (int startLateralOffset : lateralOffsets)
674 {
675 for (int endLateralOffset : lateralOffsets)
676 {
677 int startWidth = 4;
678 for (int endWidth : new int[] { 2, 4, 6 })
679 {
680
681
682 Lane lane = new Lane(link, "lane." + ++laneNum, new Length(startLateralOffset, METER),
683 new Length(endLateralOffset, METER), new Length(startWidth, METER),
684 new Length(endWidth, METER), laneType, speedMap);
685 final Geometry geometry = lane.getContour().getLineString();
686 assertNotNull("geometry of the lane should not be null", geometry);
687
688
689 checkInside(lane, 1, startLateralOffset, true);
690
691 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset, true);
692
693 checkInside(lane, -1, startLateralOffset, false);
694
695 checkInside(lane, link.getLength().getSI() + 1, endLateralOffset, false);
696
697 checkInside(lane, 1, startLateralOffset - startWidth / 2 - 1, false);
698
699 checkInside(lane, 1, startLateralOffset + startWidth / 2 + 1, false);
700
701 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset - endWidth / 2 - 1, false);
702
703 checkInside(lane, link.getLength().getSI() - 1, endLateralOffset + endWidth / 2 + 1, false);
704
705 DirectedPoint l = lane.getLocation();
706 Bounds bb = lane.getBounds();
707
708
709
710
711 Point2D.Double[] cornerPoints = new Point2D.Double[4];
712 cornerPoints[0] =
713 new Point2D.Double(xStart - (startLateralOffset + startWidth / 2) * Math.sin(angle),
714 yStart + (startLateralOffset + startWidth / 2) * Math.cos(angle));
715 cornerPoints[1] =
716 new Point2D.Double(xStart - (startLateralOffset - startWidth / 2) * Math.sin(angle),
717 yStart + (startLateralOffset - startWidth / 2) * Math.cos(angle));
718 cornerPoints[2] = new Point2D.Double(xEnd - (endLateralOffset + endWidth / 2) * Math.sin(angle),
719 yEnd + (endLateralOffset + endWidth / 2) * Math.cos(angle));
720 cornerPoints[3] = new Point2D.Double(xEnd - (endLateralOffset - endWidth / 2) * Math.sin(angle),
721 yEnd + (endLateralOffset - endWidth / 2) * Math.cos(angle));
722
723
724
725
726 double minX = cornerPoints[0].getX();
727 double maxX = cornerPoints[0].getX();
728 double minY = cornerPoints[0].getY();
729 double maxY = cornerPoints[0].getY();
730 for (int i = 1; i < cornerPoints.length; i++)
731 {
732 Point2D.Double p = cornerPoints[i];
733 minX = Math.min(minX, p.getX());
734 minY = Math.min(minY, p.getY());
735 maxX = Math.max(maxX, p.getX());
736 maxY = Math.max(maxY, p.getY());
737 }
738 Point3d bbLow = new Point3d();
739 ((BoundingBox) bb).getLower(bbLow);
740 Point3d bbHigh = new Point3d();
741 ((BoundingBox) bb).getUpper(bbHigh);
742
743
744
745 double boundsMinX = bbLow.x + l.x;
746 double boundsMinY = bbLow.y + l.y;
747 double boundsMaxX = bbHigh.x + l.x;
748 double boundsMaxY = bbHigh.y + l.y;
749 assertEquals("low x boundary", minX, boundsMinX, 0.1);
750 assertEquals("low y boundary", minY, boundsMinY, 0.1);
751 assertEquals("high x boundary", maxX, boundsMaxX, 0.1);
752 assertEquals("high y boundary", maxY, boundsMaxY, 0.1);
753 }
754 }
755 }
756 }
757 }
758 }
759 }
760
761
762
763
764
765
766
767
768
769
770
771
772
773 private void checkInside(final Lane lane, final double longitudinal, final double lateral, final boolean expectedResult)
774 {
775 CrossSectionLink parentLink = lane.getParentLink();
776 Node start = parentLink.getStartNode();
777 Node end = parentLink.getEndNode();
778 double startX = start.getPoint().x;
779 double startY = start.getPoint().y;
780 double endX = end.getPoint().x;
781 double endY = end.getPoint().y;
782 double length = Math.sqrt((endX - startX) * (endX - startX) + (endY - startY) * (endY - startY));
783 double ratio = longitudinal / length;
784 double designLineX = startX + (endX - startX) * ratio;
785 double designLineY = startY + (endY - startY) * ratio;
786 double lateralAngle = Math.atan2(endY - startY, endX - startX) + Math.PI / 2;
787 double px = designLineX + lateral * Math.cos(lateralAngle);
788 double py = designLineY + lateral * Math.sin(lateralAngle);
789 Geometry contour = lane.getContour().getLineString();
790 GeometryFactory factory = new GeometryFactory();
791 Geometry p = factory.createPoint(new Coordinate(px, py));
792
793
794 boolean result = contour.contains(p);
795 Coordinate[] polygon = contour.getCoordinates();
796 result = pointInsidePolygon(new Coordinate(px, py), polygon);
797 if (expectedResult)
798 {
799 assertTrue("Point at " + longitudinal + " along and " + lateral + " lateral is within lane", result);
800 }
801 else
802 {
803 assertFalse("Point at " + longitudinal + " along and " + lateral + " lateral is outside lane", result);
804 }
805 }
806
807
808
809
810
811
812
813
814
815
816 private boolean pointInsidePolygon(final Coordinate point, final Coordinate[] polygon)
817 {
818 boolean result = false;
819 for (int i = 0, j = polygon.length - 1; i < polygon.length; j = i++)
820 {
821 if ((polygon[i].y > point.y) != (polygon[j].y > point.y)
822 && point.x < (polygon[j].x - polygon[i].x) * (point.y - polygon[i].y) / (polygon[j].y - polygon[i].y)
823 + polygon[i].x)
824 {
825 result = !result;
826 }
827 }
828 return result;
829 }
830
831
832 protected static class Model extends AbstractOTSModel
833 {
834
835 private static final long serialVersionUID = 20141027L;
836
837
838
839
840 public Model(final OTSSimulatorInterface simulator)
841 {
842 super(simulator);
843 }
844
845
846 @Override
847 public final void constructModel() throws SimRuntimeException
848 {
849
850 }
851
852
853 @Override
854 public final OTSRoadNetwork getNetwork()
855 {
856 return null;
857 }
858 }
859
860 }