1 package org.opentrafficsim.road.network.factory;
2
3 import java.awt.Color;
4 import java.awt.geom.Rectangle2D;
5 import java.io.IOException;
6 import java.net.SocketException;
7 import java.net.URL;
8 import java.rmi.RemoteException;
9 import java.util.ArrayList;
10 import java.util.Arrays;
11 import java.util.HashSet;
12 import java.util.List;
13 import java.util.Set;
14
15 import javax.naming.NamingException;
16 import javax.swing.JPanel;
17 import javax.swing.SwingUtilities;
18 import javax.xml.parsers.ParserConfigurationException;
19
20 import nl.javel.gisbeans.io.esri.CoordinateTransform;
21 import nl.tudelft.simulation.dsol.SimRuntimeException;
22 import nl.tudelft.simulation.dsol.animation.D2.GisRenderable2D;
23 import nl.tudelft.simulation.dsol.animation.D2.Renderable2D;
24 import nl.tudelft.simulation.dsol.simulators.SimulatorInterface;
25 import nl.tudelft.simulation.jstats.distributions.DistConstant;
26 import nl.tudelft.simulation.jstats.distributions.DistExponential;
27 import nl.tudelft.simulation.jstats.distributions.DistTriangular;
28 import nl.tudelft.simulation.jstats.distributions.DistUniform;
29 import nl.tudelft.simulation.jstats.streams.MersenneTwister;
30 import nl.tudelft.simulation.jstats.streams.StreamInterface;
31 import nl.tudelft.simulation.language.d3.DirectedPoint;
32 import nl.tudelft.simulation.language.io.URLResource;
33
34 import org.djunits.unit.AccelerationUnit;
35 import org.djunits.unit.LengthUnit;
36 import org.djunits.unit.SpeedUnit;
37 import org.djunits.unit.TimeUnit;
38 import org.djunits.value.vdouble.scalar.Acceleration;
39 import org.djunits.value.vdouble.scalar.DoubleScalar;
40 import org.djunits.value.vdouble.scalar.Length;
41 import org.djunits.value.vdouble.scalar.Speed;
42 import org.djunits.value.vdouble.scalar.Time;
43 import org.opentrafficsim.core.dsol.OTSDEVSSimulatorInterface;
44 import org.opentrafficsim.core.dsol.OTSModelInterface;
45 import org.opentrafficsim.core.dsol.OTSSimTimeDouble;
46 import org.opentrafficsim.core.geometry.Bezier;
47 import org.opentrafficsim.core.geometry.OTSGeometryException;
48 import org.opentrafficsim.core.geometry.OTSLine3D;
49 import org.opentrafficsim.core.geometry.OTSPoint3D;
50 import org.opentrafficsim.core.gtu.GTUDirectionality;
51 import org.opentrafficsim.core.gtu.GTUException;
52 import org.opentrafficsim.core.gtu.GTUType;
53 import org.opentrafficsim.core.gtu.animation.AccelerationGTUColorer;
54 import org.opentrafficsim.core.gtu.animation.GTUColorer;
55 import org.opentrafficsim.core.gtu.animation.IDGTUColorer;
56 import org.opentrafficsim.core.gtu.animation.SwitchableGTUColorer;
57 import org.opentrafficsim.core.gtu.animation.VelocityGTUColorer;
58 import org.opentrafficsim.core.network.Link;
59 import org.opentrafficsim.core.network.LinkType;
60 import org.opentrafficsim.core.network.LongitudinalDirectionality;
61 import org.opentrafficsim.core.network.NetworkException;
62 import org.opentrafficsim.core.network.Node;
63 import org.opentrafficsim.core.network.OTSNetwork;
64 import org.opentrafficsim.core.network.OTSNode;
65 import org.opentrafficsim.core.network.route.CompleteRoute;
66 import org.opentrafficsim.core.network.route.Route;
67 import org.opentrafficsim.core.units.distributions.ContinuousDistDoubleScalar;
68 import org.opentrafficsim.road.gtu.lane.LaneBasedIndividualGTU;
69 import org.opentrafficsim.road.gtu.lane.driver.LaneBasedBehavioralCharacteristics;
70 import org.opentrafficsim.road.gtu.lane.perception.LanePerceptionFull;
71 import org.opentrafficsim.road.gtu.lane.tactical.LaneBasedGTUFollowingLaneChangeTacticalPlanner;
72 import org.opentrafficsim.road.gtu.lane.tactical.following.IDMPlusOld;
73 import org.opentrafficsim.road.gtu.lane.tactical.lanechangemobil.Altruistic;
74 import org.opentrafficsim.road.gtu.strategical.LaneBasedStrategicalPlanner;
75 import org.opentrafficsim.road.gtu.strategical.route.LaneBasedStrategicalRoutePlanner;
76 import org.opentrafficsim.road.network.factory.opendrive.LaneAnimationOD;
77 import org.opentrafficsim.road.network.factory.opendrive.OpenDriveNetworkLaneParser;
78 import org.opentrafficsim.road.network.factory.opendrive.communicationRTI.ReceiverThread;
79 import org.opentrafficsim.road.network.lane.CrossSectionElement;
80 import org.opentrafficsim.road.network.lane.CrossSectionLink;
81 import org.opentrafficsim.road.network.lane.DirectedLanePosition;
82 import org.opentrafficsim.road.network.lane.Lane;
83 import org.opentrafficsim.road.network.lane.NoTrafficLane;
84 import org.opentrafficsim.road.network.lane.changing.LaneKeepingPolicy;
85 import org.opentrafficsim.simulationengine.AbstractWrappableAnimation;
86 import org.opentrafficsim.simulationengine.OTSSimulationException;
87 import org.opentrafficsim.simulationengine.properties.AbstractProperty;
88 import org.xml.sax.SAXException;
89
90
91
92
93
94
95
96
97
98
99 public class TestOpenDriveParserNoRTINew extends AbstractWrappableAnimation
100 {
101
102
103
104
105
106 public static void main(final String[] args) throws SimRuntimeException
107 {
108 SwingUtilities.invokeLater(new Runnable()
109 {
110 @Override
111 public void run()
112 {
113 try
114 {
115 TestOpenDriveParserNoRTINew xmlModel = new TestOpenDriveParserNoRTINew();
116
117 xmlModel.buildAnimator(new Time.Abs(0.0, TimeUnit.SECOND), new Time.Rel(0.0, TimeUnit.SECOND),
118 new Time.Rel(60.0, TimeUnit.MINUTE), new ArrayList<AbstractProperty<?>>(), null, true);
119 }
120 catch (SimRuntimeException | NamingException | OTSSimulationException exception)
121 {
122 exception.printStackTrace();
123 }
124 }
125 });
126 }
127
128
129 @Override
130 public final String shortName()
131 {
132 return "TestOpenDriveModel";
133 }
134
135
136 @Override
137 public final String description()
138 {
139 return "TestOpenDriveModel";
140 }
141
142
143 @Override
144 public final void stopTimersThreads()
145 {
146 super.stopTimersThreads();
147 }
148
149
150 @Override
151 protected final JPanel makeCharts()
152 {
153 return null;
154 }
155
156
157 @Override
158 protected final OTSModelInterface makeModel(final GTUColorer colorer)
159 {
160 return new TestOpenDriveModel();
161 }
162
163
164 @Override
165 protected final Rectangle2D.Double makeAnimationRectangle()
166 {
167 return new Rectangle2D.Double(-1000, -1000, 2000, 2000);
168 }
169
170
171
172
173
174
175
176
177
178
179
180
181
182 class TestOpenDriveModel implements OTSModelInterface
183 {
184
185 private static final long serialVersionUID = 20150811L;
186
187
188 private OTSDEVSSimulatorInterface simulator;
189
190 private List<LaneBasedIndividualGTU> rtiCars;
191
192
193 private ContinuousDistDoubleScalar.Rel<Speed, SpeedUnit> initialSpeedDist;
194
195
196 private ContinuousDistDoubleScalar.Rel<Time.Rel, TimeUnit> iatDist;
197
198
199 private ContinuousDistDoubleScalar.Rel<Length.Rel, LengthUnit> lengthDist;
200
201
202 private ContinuousDistDoubleScalar.Rel<Length.Rel, LengthUnit> widthDist;
203
204
205 private ContinuousDistDoubleScalar.Rel<Speed, SpeedUnit> maxSpeedDist;
206
207
208 private ContinuousDistDoubleScalar.Rel<Length.Rel, LengthUnit> initialPosDist;
209
210
211 private GTUType carType;
212
213
214 private int lastId = 0;
215
216
217 private StreamInterface stream;
218
219
220
221
222 public TestOpenDriveModel()
223 {
224 super();
225 this.stream = new MersenneTwister(1);
226 this.initialSpeedDist = new ContinuousDistDoubleScalar.Rel<>(new DistConstant(this.stream, 0.0), SpeedUnit.SI);
227 this.iatDist = new ContinuousDistDoubleScalar.Rel<>(new DistExponential(this.stream, 30.0), TimeUnit.SECOND);
228 this.lengthDist = new ContinuousDistDoubleScalar.Rel<>(new DistUniform(this.stream, 4.0, 5.0), LengthUnit.METER);
229 this.widthDist = new ContinuousDistDoubleScalar.Rel<>(new DistConstant(this.stream, 2.0), LengthUnit.METER);
230 this.maxSpeedDist =
231 new ContinuousDistDoubleScalar.Rel<>(new DistTriangular(this.stream, 30.0, 35.0, 40.0),
232 SpeedUnit.MILE_PER_HOUR);
233 this.initialPosDist =
234 new ContinuousDistDoubleScalar.Rel<>(new DistUniform(this.stream, 0.0, 1.0), LengthUnit.METER);
235 this.carType = GTUType.makeGTUType("Car");
236 }
237
238
239 @Override
240 public final
241 void
242 constructModel(
243 final SimulatorInterface<DoubleScalar.Abs<TimeUnit>, DoubleScalar.Rel<TimeUnit>, OTSSimTimeDouble> pSimulator)
244 throws SimRuntimeException
245 {
246 this.simulator = (OTSDEVSSimulatorInterface) pSimulator;
247
248 this.rtiCars = new ArrayList<LaneBasedIndividualGTU>();
249
250
251 URL url = URLResource.getResource("/testod.xodr");
252 this.simulator.setPauseOnError(false);
253 OpenDriveNetworkLaneParser nlp = new OpenDriveNetworkLaneParser(this.simulator);
254 OTSNetwork network = null;
255 try
256 {
257 network = nlp.build(url);
258 }
259 catch (NetworkException | ParserConfigurationException | SAXException | IOException | NamingException
260 | GTUException | OTSGeometryException exception)
261 {
262 exception.printStackTrace();
263 }
264
265 URL gisURL = URLResource.getResource("/gis/map.xml");
266 System.err.println("GIS-map file: " + gisURL.toString());
267
268 double latCenter = nlp.getHeaderTag().getOriginLat().si, lonCenter = nlp.getHeaderTag().getOriginLong().si;
269
270 CoordinateTransform latLonToXY = new CoordinateTransformLonLatToXY(lonCenter, latCenter);
271 new GisRenderable2D(this.simulator, gisURL, latLonToXY);
272
273
274
275
276 try
277 {
278 destroyLink(nlp, network, "3766109");
279 destroyLink(nlp, network, "3766110");
280 destroyLink(nlp, network, "3766111");
281
282 Lane lane109 =
283 makeLane(network, "3766068.1", "3766068.0", "3", "3766059.7", "3766059.150", "2", "3766109", "-1",
284 LinkType.ALL, LaneKeepingPolicy.KEEP_LANE);
285 Renderable2D animation109 = new LaneAnimationOD(lane109, this.simulator, Color.gray);
286 nlp.animationMap.put(lane109, animation109);
287
288 Lane lane110 =
289 makeLane(network, "3766068.1", "3766068.0", "4", "3766059.7", "3766059.150", "3", "3766110", "-1",
290 LinkType.ALL, LaneKeepingPolicy.KEEP_LANE);
291 Renderable2D animation110 = new LaneAnimationOD(lane110, this.simulator, Color.gray);
292 nlp.animationMap.put(lane110, animation110);
293
294 Lane lane111 =
295 makeLane(network, "3766068.1", "3766068.0", "5", "3766059.7", "3766059.150", "4", "3766111", "-1",
296 LinkType.ALL, LaneKeepingPolicy.KEEP_LANE);
297 Renderable2D animation111 = new LaneAnimationOD(lane111, this.simulator, Color.gray);
298 nlp.animationMap.put(lane111, animation111);
299
300 destroyLink(nlp, network, "3766175");
301 destroyLink(nlp, network, "3766176");
302 destroyLink(nlp, network, "3766177");
303
304 Lane lane175 =
305 makeLane(network, "3766059.1", "3766059.0", "3", "3766054.5", "3766054.191", "2", "3766175", "-1",
306 LinkType.ALL, LaneKeepingPolicy.KEEP_LANE);
307 Renderable2D animation175 = new LaneAnimationOD(lane175, this.simulator, Color.gray);
308 nlp.animationMap.put(lane175, animation175);
309
310 Lane lane176 =
311 makeLane(network, "3766059.1", "3766059.0", "4", "3766054.5", "3766054.191", "3", "3766176", "-1",
312 LinkType.ALL, LaneKeepingPolicy.KEEP_LANE);
313 Renderable2D animation176 = new LaneAnimationOD(lane176, this.simulator, Color.gray);
314 nlp.animationMap.put(lane176, animation176);
315
316 Lane lane177 =
317 makeLane(network, "3766059.1", "3766059.0", "5", "3766054.5", "3766054.191", "4", "3766177", "-1",
318 LinkType.ALL, LaneKeepingPolicy.KEEP_LANE);
319 Renderable2D animation177 = new LaneAnimationOD(lane177, this.simulator, Color.gray);
320 nlp.animationMap.put(lane177, animation177);
321
322 Lane lane191x =
323 makeLane(network, "3766054.5", "3766054.191", "-6", "3766059.1", "3766059.0", "-4", "3766191x",
324 "-1", LinkType.ALL, LaneKeepingPolicy.KEEP_LANE);
325 Renderable2D animation191x = new LaneAnimationOD(lane191x, this.simulator, Color.gray);
326 nlp.animationMap.put(lane191x, animation191x);
327
328 }
329 catch (OTSGeometryException | NetworkException | NamingException | RemoteException e)
330 {
331 e.printStackTrace();
332 }
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391 network.buildGraph(this.carType);
392
393
394 try
395 {
396 CrossSectionLink link70_1 = (CrossSectionLink) network.getLink("3766070.1");
397 Lane lane70_1m1 = getLane(link70_1, "-1");
398 Lane lane70_1p1 = getLane(link70_1, "1");
399
400
401 Node n70_0 = network.getNode("3766070.0");
402 Node n70_23 = network.getNode("3766070.23");
403 Node n45_0 = network.getNode("3766045.0");
404 Node n45_22 = network.getNode("3766045.22");
405 Node n65_0 = network.getNode("3766065.0");
406 Node n65_8 = network.getNode("3766065.8");
407 Route route7070r =
408 new CompleteRoute("Right around NRC-SV", this.carType, network.getShortestRouteBetween(
409 this.carType, n70_0, n70_0, Arrays.asList(new Node[]{n45_0, n65_0})).getNodes());
410 putCar(lane70_1m1, route7070r, network, GTUDirectionality.DIR_PLUS);
411 Route route7070l =
412 new CompleteRoute("Left around NRC-SV", this.carType, network.getShortestRouteBetween(this.carType,
413 n70_0, n70_0, Arrays.asList(new Node[]{n65_8, n45_22, n70_23})).getNodes());
414 putCar(lane70_1p1, route7070l, network, GTUDirectionality.DIR_MINUS);
415 }
416 catch (NetworkException | GTUException | NamingException | OTSGeometryException exception)
417 {
418 exception.printStackTrace();
419 }
420
421
422 try
423 {
424 List<CrossSectionLink> links68 = new ArrayList<>();
425 links68.add((CrossSectionLink) network.getLink("3766068.1"));
426 links68.add((CrossSectionLink) network.getLink("3766068.2"));
427 links68.add((CrossSectionLink) network.getLink("3766068.3"));
428 links68.add((CrossSectionLink) network.getLink("3766068.4"));
429 links68.add((CrossSectionLink) network.getLink("3766068.5"));
430
431
432 Node n68_0 = network.getNode("3766068.0");
433 Node n38_88 = network.getNode("3766038.88");
434 Node n43_0 = network.getNode("3766043.0");
435 Node n45_0 = network.getNode("3766045.0");
436 Node n65_0 = network.getNode("3766065.0");
437 Node n64_17 = network.getNode("3766064.17");
438 Route route6868r =
439 new CompleteRoute("Right top", this.carType, network.getShortestRouteBetween(
440 this.carType, n68_0, n68_0, Arrays.asList(new Node[]{n38_88, n43_0, n45_0, n65_0, n64_17})).getNodes());
441 System.out.println(route6868r);
442 for (int i=0; i<5; i++)
443 {
444 putCar(randLane(links68, LongitudinalDirectionality.DIR_PLUS), route6868r, network, GTUDirectionality.DIR_PLUS);
445 }
446
447
448 Node n38_0 = network.getNode("3766038.0");
449 Node n43_53 = network.getNode("3766043.53");
450 Node n45_22 = network.getNode("3766045.22");
451 Node n65_8 = network.getNode("3766065.8");
452 Node n64_0 = network.getNode("3766064.0");
453 Node n68_158 = network.getNode("3766068.158");
454 Route route6868l =
455 new CompleteRoute("Right top", this.carType, network.getShortestRouteBetween(
456 this.carType, n68_158, n68_158, Arrays.asList(new Node[]{n64_0, n65_8, n45_22, n43_53, n38_0})).getNodes());
457 System.out.println(route6868l);
458 for (int i=0; i<5; i++)
459 {
460 putCar(randLane(links68, LongitudinalDirectionality.DIR_MINUS), route6868l, network, GTUDirectionality.DIR_MINUS);
461 }
462
463 }
464 catch (NetworkException | GTUException | NamingException | OTSGeometryException exception)
465 {
466 exception.printStackTrace();
467 }
468
469
470
471 while (this.rtiCars.size() < 52)
472 {
473 this.rtiCars.add(this.rtiCars.get(this.rtiCars.size() - 1));
474
475 }
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658 try
659 {
660 new Thread(new ReceiverThread(this.simulator, this.carType, this.rtiCars, network)).start();
661 }
662 catch (SocketException exception1)
663 {
664 exception1.printStackTrace();
665 }
666
667 }
668
669 private Lane randLane(final List<CrossSectionLink> links, final LongitudinalDirectionality dir)
670 {
671
672 CrossSectionLink link = links.get(this.stream.nextInt(0, links.size()-1));
673 List<Lane> lanes = new ArrayList<>();
674 for (CrossSectionElement cse : link.getCrossSectionElementList())
675 {
676 if (cse instanceof Lane)
677 {
678 Lane lane = (Lane) cse;
679 if (lane.getDirectionality(this.carType).equals(dir) || lane.getDirectionality(this.carType).isBoth())
680 {
681 lanes.add(lane);
682 }
683 }
684 }
685 for (int i=0; i<10; i++)
686 {
687 Lane lane = lanes.get(this.stream.nextInt(0, lanes.size()-1));
688 if (lane.getGtuList().isEmpty())
689 {
690 return lane;
691 }
692 }
693 return lanes.get(this.stream.nextInt(0, lanes.size()-1));
694 }
695
696 private void putCar(Lane lane, Route route, OTSNetwork network, GTUDirectionality dir) throws GTUException,
697 NamingException, NetworkException, SimRuntimeException, OTSGeometryException
698 {
699 LaneBasedBehavioralCharacteristics drivingCharacteristics =
700 new LaneBasedBehavioralCharacteristics(new IDMPlusOld(), new Altruistic());
701 LaneBasedStrategicalPlanner sPlanner =
702 new LaneBasedStrategicalRoutePlanner(drivingCharacteristics,
703 new LaneBasedGTUFollowingLaneChangeTacticalPlanner(), route);
704 LanePerceptionFull perception = new LanePerceptionFull();
705
706 DirectedLanePosition directedLanePosition =
707 new DirectedLanePosition(lane, this.initialPosDist.draw()
708 .multiplyBy(lane.getCenterLine().getLengthSI()), dir);
709 Set<DirectedLanePosition> lanepositionSet = new HashSet<DirectedLanePosition>();
710 lanepositionSet.add(directedLanePosition);
711 Length.Rel carLength = this.lengthDist.draw();
712 LaneBasedIndividualGTU car =
713 new LaneBasedIndividualGTU("" + (++this.lastId), this.carType, lanepositionSet, new Speed(0.0,
714 SpeedUnit.METER_PER_SECOND), carLength, this.widthDist.draw(), this.maxSpeedDist.draw(),
715 this.simulator, sPlanner, perception, network);
716 this.rtiCars.add(car);
717 }
718
719 private Lane getLane(CrossSectionLink link, String id) throws NetworkException
720 {
721 for (CrossSectionElement cse : link.getCrossSectionElementList())
722 {
723 if (cse instanceof Lane && !(cse instanceof NoTrafficLane) && cse.getId().equals(id))
724 {
725 return (Lane) cse;
726 }
727 }
728 throw new NetworkException("Could not find Lane " + id + " in link " + link);
729 }
730
731
732
733
734
735
736
737
738
739 private void destroyLink(final OpenDriveNetworkLaneParser nlp, final OTSNetwork network, final String linkId)
740 throws NamingException, NetworkException
741 {
742 Link link = network.getLink(linkId);
743 link.getStartNode().removeLink(link);
744 link.getEndNode().removeLink(link);
745 network.removeLink(link);
746 if (link instanceof CrossSectionLink)
747 {
748 for (CrossSectionElement cse : ((CrossSectionLink) link).getCrossSectionElementList())
749 {
750 if (nlp.animationMap.containsKey(cse))
751 {
752 nlp.animationMap.get(cse).destroy();
753 }
754 }
755 }
756 if (nlp.animationMap.containsKey(link))
757 {
758 nlp.animationMap.get(link).destroy();
759 }
760 }
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779 private Lane
780 makeLane(final OTSNetwork network, final String sLinkStr, final String sNodeStr, final String sLaneStr,
781 final String eLinkStr, final String eNodeStr, final String eLaneStr, final String linkId,
782 final String laneId, final LinkType linkType, final LaneKeepingPolicy laneKeepingPolicy)
783 throws OTSGeometryException, NetworkException
784 {
785 CrossSectionLink sLink = (CrossSectionLink) network.getLink(sLinkStr);
786 OTSNode sNode = (OTSNode) network.getNode(sNodeStr);
787 Lane sLane = (Lane) sLink.getCrossSectionElement(sLaneStr);
788 CrossSectionLink eLink = (CrossSectionLink) network.getLink(eLinkStr);
789 OTSNode eNode = (OTSNode) network.getNode(eNodeStr);
790 Lane eLane = (Lane) eLink.getCrossSectionElement(eLaneStr);
791 DirectedPoint sp, ep;
792 Length.Rel beginWidth, endWidth;
793 if (sLink.getStartNode().equals(sNode))
794 {
795 OTSPoint3D p1 = sLane.getCenterLine().get(1);
796 OTSPoint3D p2 = sLane.getCenterLine().get(0);
797 sp = new DirectedPoint(p2.x, p2.y, p2.z, 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
798 beginWidth = sLane.getBeginWidth();
799 }
800 else
801 {
802 OTSPoint3D p1 = sLane.getCenterLine().get(sLane.getCenterLine().size() - 2);
803 OTSPoint3D p2 = sLane.getCenterLine().get(sLane.getCenterLine().size() - 1);
804 sp = new DirectedPoint(p2.x, p2.y, p2.z, 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
805 beginWidth = sLane.getEndWidth();
806 }
807 if (eLink.getStartNode().equals(eNode))
808 {
809 OTSPoint3D p1 = eLane.getCenterLine().get(1);
810 OTSPoint3D p2 = eLane.getCenterLine().get(0);
811 ep = new DirectedPoint(p2.x, p2.y, p2.z, 0.0, 0.0, Math.atan2(p1.y - p2.y, p1.x - p2.x));
812 endWidth = eLane.getBeginWidth();
813 }
814 else
815 {
816 OTSPoint3D p1 = eLane.getCenterLine().get(eLane.getCenterLine().size() - 2);
817 OTSPoint3D p2 = eLane.getCenterLine().get(eLane.getCenterLine().size() - 1);
818 ep = new DirectedPoint(p2.x, p2.y, p2.z, 0.0, 0.0, Math.atan2(p1.y - p2.y, p1.x - p2.x));
819 endWidth = eLane.getEndWidth();
820 }
821 OTSLine3D designLine = Bezier.cubic(64, sp, ep);
822 CrossSectionLink newLink =
823 new CrossSectionLink(linkId, sNode, eNode, linkType, designLine, laneKeepingPolicy);
824 newLink.addDirectionality(GTUType.ALL, LongitudinalDirectionality.DIR_PLUS);
825 Lane newLane =
826 new Lane(newLink, laneId, Length.Rel.ZERO, Length.Rel.ZERO, beginWidth, endWidth, sLane.getLaneType(),
827 LongitudinalDirectionality.DIR_PLUS, sLane.getSpeedLimit(GTUType.ALL),
828 sLane.getOvertakingConditions());
829 network.addLink(newLink);
830 return newLane;
831 }
832
833
834 @Override
835 public SimulatorInterface<DoubleScalar.Abs<TimeUnit>, DoubleScalar.Rel<TimeUnit>, OTSSimTimeDouble>
836 getSimulator()
837
838 {
839 return this.simulator;
840 }
841
842
843
844
845 private GTUColorer makeSwitchableGTUColorer()
846 {
847 GTUColorer[] gtuColorers =
848 new GTUColorer[]{
849 new IDGTUColorer(),
850 new VelocityGTUColorer(new Speed(100.0, SpeedUnit.KM_PER_HOUR)),
851 new AccelerationGTUColorer(new Acceleration(1.0, AccelerationUnit.METER_PER_SECOND_2),
852 new Acceleration(1.0, AccelerationUnit.METER_PER_SECOND_2))};
853 return new SwitchableGTUColorer(0, gtuColorers);
854 }
855 }
856
857 }