1 package org.opentrafficsim.road.network.factory;
2
3 import java.util.ArrayList;
4 import java.util.Arrays;
5 import java.util.LinkedHashMap;
6 import java.util.List;
7 import java.util.Map;
8
9 import javax.naming.NamingException;
10
11 import org.djunits.unit.LengthUnit;
12 import org.djunits.value.vdouble.scalar.Length;
13 import org.djunits.value.vdouble.scalar.Speed;
14 import org.djutils.exceptions.Throw;
15 import org.djutils.exceptions.Try;
16 import org.opentrafficsim.core.dsol.OTSSimulatorInterface;
17 import org.opentrafficsim.core.geometry.Bezier;
18 import org.opentrafficsim.core.geometry.DirectedPoint;
19 import org.opentrafficsim.core.geometry.OTSGeometryException;
20 import org.opentrafficsim.core.geometry.OTSLine3D;
21 import org.opentrafficsim.core.geometry.OTSPoint3D;
22 import org.opentrafficsim.core.gtu.GTUType;
23 import org.opentrafficsim.core.network.LateralDirectionality;
24 import org.opentrafficsim.core.network.LinkType;
25 import org.opentrafficsim.core.network.NetworkException;
26 import org.opentrafficsim.core.network.OTSNode;
27 import org.opentrafficsim.road.network.OTSRoadNetwork;
28 import org.opentrafficsim.road.network.lane.CrossSectionLink;
29 import org.opentrafficsim.road.network.lane.Lane;
30 import org.opentrafficsim.road.network.lane.LaneType;
31 import org.opentrafficsim.road.network.lane.OTSRoadNode;
32 import org.opentrafficsim.road.network.lane.Shoulder;
33 import org.opentrafficsim.road.network.lane.Stripe;
34 import org.opentrafficsim.road.network.lane.Stripe.Permeable;
35 import org.opentrafficsim.road.network.lane.changing.LaneKeepingPolicy;
36
37
38
39
40
41
42
43
44
45
46 public final class LaneFactory
47 {
48
49
50 private static final Length STRIPE_WIDTH = Length.instantiateSI(0.2);
51
52
53 private static final double BEZIER_MARGIN = Math.toRadians(0.5);
54
55
56 private final CrossSectionLink link;
57
58
59 private Length offset;
60
61
62 private Length laneWidth0;
63
64
65 private Length offsetStart = Length.ZERO;
66
67
68 private Length offsetEnd = Length.ZERO;
69
70
71 private LaneType laneType0;
72
73
74 private Speed speedLimit0;
75
76
77 private final List<Lane> lanes = new ArrayList<>();
78
79
80
81
82
83
84
85
86
87
88
89 public LaneFactory(final OTSRoadNetwork network, final OTSRoadNode from, final OTSRoadNode to, final LinkType type,
90 final OTSSimulatorInterface simulator, final LaneKeepingPolicy policy) throws OTSGeometryException, NetworkException
91 {
92 this(network, from, to, type, simulator, policy, makeLine(from, to));
93 }
94
95
96
97
98
99
100
101
102
103
104
105 public LaneFactory(final OTSRoadNetwork network, final OTSRoadNode from, final OTSRoadNode to, final LinkType type,
106 final OTSSimulatorInterface simulator, final LaneKeepingPolicy policy, final OTSLine3D line) throws NetworkException
107 {
108 this.link = new CrossSectionLink(network, from.getId() + to.getId(), from, to, type, line, policy);
109 }
110
111
112
113
114
115
116
117
118
119 private static OTSLine3D makeLine(final OTSNode from, final OTSNode to) throws OTSGeometryException
120 {
121
122 double rotCrow = Math.atan2(to.getLocation().y - from.getLocation().y, to.getLocation().x - from.getLocation().x);
123 double dRot = from.getLocation().getRotZ() - rotCrow;
124 while (dRot < -Math.PI)
125 {
126 dRot += 2.0 * Math.PI;
127 }
128 while (dRot > Math.PI)
129 {
130 dRot -= 2.0 * Math.PI;
131 }
132 OTSLine3D line;
133 if (from.getLocation().getRotZ() != to.getLocation().getRotZ() || Math.abs(dRot) > BEZIER_MARGIN)
134 {
135 line = Bezier.cubic(from.getLocation(), to.getLocation());
136 }
137 else
138 {
139 line = new OTSLine3D(from.getPoint(), to.getPoint());
140 }
141 return line;
142 }
143
144
145
146
147
148
149
150
151
152 public LaneFactory leftToRight(final double leftLanes, final Length laneWidth, final LaneType laneType,
153 final Speed speedLimit)
154 {
155 this.offset = laneWidth.times(leftLanes);
156 this.laneWidth0 = laneWidth.neg();
157 this.laneType0 = laneType;
158 this.speedLimit0 = speedLimit;
159 Try.execute(
160 () -> new Stripe(this.link, this.offset.plus(this.offsetStart), this.offset.plus(this.offsetEnd), STRIPE_WIDTH),
161 "Unexpected exception while building link.");
162 return this;
163 }
164
165
166
167
168
169
170
171
172
173 public LaneFactory rightToLeft(final double rightLanes, final Length laneWidth, final LaneType laneType,
174 final Speed speedLimit)
175 {
176 this.offset = laneWidth.times(-rightLanes);
177 this.laneWidth0 = laneWidth;
178 this.laneType0 = laneType;
179 this.speedLimit0 = speedLimit;
180 Try.execute(() -> new Stripe(this.link, this.offset, this.offset, STRIPE_WIDTH),
181 "Unexpected exception while building link.");
182 return this;
183 }
184
185
186
187
188
189
190 public LaneFactory setOffsetStart(final Length startOffset)
191 {
192 this.offsetStart = startOffset;
193 return this;
194 }
195
196
197
198
199
200
201 public LaneFactory setOffsetEnd(final Length endOffset)
202 {
203 this.offsetEnd = endOffset;
204 return this;
205 }
206
207
208
209
210
211
212
213
214
215 public LaneFactory addLanes(final Permeable... permeable)
216 {
217 List<Permeable> list = new ArrayList<>(Arrays.asList(permeable));
218 list.add(null);
219 for (Permeable perm : list)
220 {
221 Length startOffset = this.offset.plus(this.laneWidth0.times(0.5)).plus(this.offsetStart);
222 Length endOffset = this.offset.plus(this.laneWidth0.times(0.5)).plus(this.offsetEnd);
223 this.lanes.add(Try.assign(
224 () -> new Lane(this.link, "Lane " + (this.lanes.size() + 1), startOffset, endOffset, this.laneWidth0.abs(),
225 this.laneWidth0.abs(), this.laneType0, this.speedLimit0),
226 "Unexpected exception while building link."));
227 this.offset = this.offset.plus(this.laneWidth0);
228 Stripe stripe = Try.assign(() -> new Stripe(this.link, this.offset.plus(this.offsetStart),
229 this.offset.plus(this.offsetEnd), STRIPE_WIDTH), "Unexpected exception while building link.");
230 if (perm != null)
231 {
232 stripe.addPermeability(this.link.getNetwork().getGtuType(GTUType.DEFAULTS.VEHICLE), perm);
233 }
234 }
235 return this;
236 }
237
238
239
240
241
242
243
244
245 public LaneFactory addShoulder(final Length width, final LateralDirectionality lat)
246 {
247 Throw.when(this.lanes.isEmpty(), IllegalStateException.class, "Lanes should be defined before adding shoulder(s).");
248 if (lat == null || lat.isNone() || lat.isLeft())
249 {
250 Length startOffset = null;
251 Length endOffset = null;
252 for (Lane lane : this.lanes)
253 {
254 if (startOffset == null
255 || lane.getDesignLineOffsetAtBegin().plus(lane.getBeginWidth().times(0.5)).gt(startOffset))
256 {
257 startOffset = lane.getDesignLineOffsetAtBegin().plus(lane.getBeginWidth().times(0.5));
258 }
259 if (endOffset == null || lane.getDesignLineOffsetAtEnd().plus(lane.getEndWidth().times(0.5)).gt(endOffset))
260 {
261 endOffset = lane.getDesignLineOffsetAtEnd().plus(lane.getEndWidth().times(0.5));
262 }
263 }
264 Length start = startOffset.plus(width.times(0.5));
265 Length end = endOffset.plus(width.times(0.5));
266 Try.assign(() -> new Shoulder(this.link, "Left shoulder", start, end, width, width),
267 "Unexpected exception while building link.");
268 }
269 if (lat == null || lat.isNone() || lat.isRight())
270 {
271 Length startOffset = null;
272 Length endOffset = null;
273 for (Lane lane : this.lanes)
274 {
275 if (startOffset == null
276 || lane.getDesignLineOffsetAtBegin().minus(lane.getBeginWidth().times(0.5)).lt(startOffset))
277 {
278 startOffset = lane.getDesignLineOffsetAtBegin().minus(lane.getBeginWidth().times(0.5));
279 }
280 if (endOffset == null
281 || lane.getDesignLineOffsetAtEnd().minus(lane.getEndWidth().times(0.5)).lt(endOffset))
282 {
283 endOffset = lane.getDesignLineOffsetAtEnd().minus(lane.getEndWidth().times(0.5));
284 }
285 }
286 Length start = startOffset.minus(width.times(0.5));
287 Length end = endOffset.minus(width.times(0.5));
288 Try.assign(() -> new Shoulder(this.link, "Right shoulder", start, end, width, width),
289 "Unexpected exception while building link.");
290 }
291 return this;
292 }
293
294
295
296
297
298 public List<Lane> getLanes()
299 {
300 return this.lanes;
301 }
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317 public static CrossSectionLink makeLink(final OTSRoadNetwork network, final String name, final OTSRoadNode from,
318 final OTSRoadNode to, final OTSPoint3D[] intermediatePoints, final OTSSimulatorInterface simulator)
319 throws OTSGeometryException, NetworkException
320 {
321 List<OTSPoint3D> pointList =
322 intermediatePoints == null ? new ArrayList<>() : new ArrayList<>(Arrays.asList(intermediatePoints));
323 if (pointList.size() == 0 || !from.getPoint().equals(pointList.get(0)))
324 {
325 pointList.add(0, from.getPoint());
326 }
327 if (pointList.size() == 0 || !to.getPoint().equals(pointList.get(pointList.size() - 1)))
328 {
329 pointList.add(to.getPoint());
330 }
331
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 OTSLine3D designLine = new OTSLine3D(pointList);
361 CrossSectionLink link = new CrossSectionLink(network, name, from, to, network.getLinkType(LinkType.DEFAULTS.ROAD),
362 designLine, LaneKeepingPolicy.KEEPRIGHT);
363 return link;
364 }
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382 @SuppressWarnings("checkstyle:parameternumber")
383 private static Lane makeLane(final CrossSectionLink link, final String id, final LaneType laneType,
384 final Length latPosAtStart, final Length latPosAtEnd, final Length width, final Speed speedLimit,
385 final OTSSimulatorInterface simulator) throws NetworkException, OTSGeometryException
386 {
387 Map<GTUType, Speed> speedMap = new LinkedHashMap<>();
388 speedMap.put(link.getNetwork().getGtuType(GTUType.DEFAULTS.VEHICLE), speedLimit);
389 Lane result = new Lane(link, id, latPosAtStart, latPosAtEnd, width, width, laneType, speedMap);
390 return result;
391 }
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408 public static Lane makeLane(final OTSRoadNetwork network, final String name, final OTSRoadNode from, final OTSRoadNode to,
409 final OTSPoint3D[] intermediatePoints, final LaneType laneType, final Speed speedLimit,
410 final OTSSimulatorInterface simulator) throws NetworkException, OTSGeometryException
411 {
412 Length width = new Length(4.0, LengthUnit.METER);
413 final CrossSectionLink link = makeLink(network, name, from, to, intermediatePoints, simulator);
414 Length latPos = new Length(0.0, LengthUnit.METER);
415 return makeLane(link, "lane", laneType, latPos, latPos, width, speedLimit, simulator);
416 }
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438 @SuppressWarnings("checkstyle:parameternumber")
439 public static Lane[] makeMultiLane(final OTSRoadNetwork network, final String name, final OTSRoadNode from,
440 final OTSRoadNode to, final OTSPoint3D[] intermediatePoints, final int laneCount, final int laneOffsetAtStart,
441 final int laneOffsetAtEnd, final LaneType laneType, final Speed speedLimit, final OTSSimulatorInterface simulator)
442 throws NetworkException, OTSGeometryException
443 {
444 final CrossSectionLink link = makeLink(network, name, from, to, intermediatePoints, simulator);
445 Lane[] result = new Lane[laneCount];
446 Length width = new Length(4.0, LengthUnit.METER);
447 for (int laneIndex = 0; laneIndex < laneCount; laneIndex++)
448 {
449
450 Length latPosAtStart = new Length((-0.5 - laneIndex - laneOffsetAtStart) * width.getSI(), LengthUnit.SI);
451 Length latPosAtEnd = new Length((-0.5 - laneIndex - laneOffsetAtEnd) * width.getSI(), LengthUnit.SI);
452 result[laneIndex] =
453 makeLane(link, "lane." + laneIndex, laneType, latPosAtStart, latPosAtEnd, width, speedLimit, simulator);
454 }
455 return result;
456 }
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477 @SuppressWarnings("checkstyle:parameternumber")
478 public static Lane[] makeMultiLane(final OTSRoadNetwork network, final String name, final OTSRoadNode from, final OTSRoadNode to,
479 final OTSPoint3D[] intermediatePoints, final int laneCount, final LaneType laneType, final Speed speedLimit,
480 final OTSSimulatorInterface simulator) throws NamingException, NetworkException, OTSGeometryException
481 {
482 return makeMultiLane(network, name, from, to, intermediatePoints, laneCount, 0, 0, laneType, speedLimit, simulator);
483 }
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506 @SuppressWarnings("checkstyle:parameternumber")
507 public static Lane[] makeMultiLaneBezier(final OTSRoadNetwork network, final String name, final OTSRoadNode n1,
508 final OTSRoadNode n2, final OTSRoadNode n3, final OTSRoadNode n4, final int laneCount, final int laneOffsetAtStart,
509 final int laneOffsetAtEnd, final LaneType laneType, final Speed speedLimit, final OTSSimulatorInterface simulator)
510 throws NamingException, NetworkException, OTSGeometryException
511 {
512 OTSLine3D bezier = makeBezier(n1, n2, n3, n4);
513 final CrossSectionLink link = makeLink(network, name, n2, n3, bezier.getPoints(), simulator);
514 Lane[] result = new Lane[laneCount];
515 Length width = new Length(4.0, LengthUnit.METER);
516 for (int laneIndex = 0; laneIndex < laneCount; laneIndex++)
517 {
518
519 Length latPosAtStart = new Length((-0.5 - laneIndex - laneOffsetAtStart) * width.getSI(), LengthUnit.SI);
520 Length latPosAtEnd = new Length((-0.5 - laneIndex - laneOffsetAtEnd) * width.getSI(), LengthUnit.SI);
521 result[laneIndex] =
522 makeLane(link, "lane." + laneIndex, laneType, latPosAtStart, latPosAtEnd, width, speedLimit, simulator);
523 }
524 return result;
525 }
526
527
528
529
530
531
532
533
534
535 public static OTSLine3D makeBezier(final OTSNode n1, final OTSNode n2, final OTSNode n3, final OTSNode n4)
536 throws OTSGeometryException
537 {
538 OTSPoint3D p1 = n1.getPoint();
539 OTSPoint3D p2 = n2.getPoint();
540 OTSPoint3D p3 = n3.getPoint();
541 OTSPoint3D p4 = n4.getPoint();
542 DirectedPoint dp1 = new DirectedPoint(p2.x, p2.y, p2.z, 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
543 DirectedPoint dp2 = new DirectedPoint(p3.x, p3.y, p3.z, 0.0, 0.0, Math.atan2(p4.y - p3.y, p4.x - p3.x));
544 return Bezier.cubic(dp1, dp2);
545 }
546 }