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.OTSGeometryException;
19 import org.opentrafficsim.core.geometry.OTSLine3D;
20 import org.opentrafficsim.core.geometry.OTSPoint3D;
21 import org.opentrafficsim.core.gtu.GTUType;
22 import org.opentrafficsim.core.network.LateralDirectionality;
23 import org.opentrafficsim.core.network.LinkType;
24 import org.opentrafficsim.core.network.NetworkException;
25 import org.opentrafficsim.core.network.Node;
26 import org.opentrafficsim.core.network.OTSNode;
27 import org.opentrafficsim.road.network.OTSRoadNetwork;
28 import org.opentrafficsim.road.network.RoadNetwork;
29 import org.opentrafficsim.road.network.lane.CrossSectionLink;
30 import org.opentrafficsim.road.network.lane.Lane;
31 import org.opentrafficsim.road.network.lane.LaneType;
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 import nl.tudelft.simulation.dsol.simulators.DEVSSimulatorInterface;
38 import nl.tudelft.simulation.language.d3.DirectedPoint;
39
40 /**
41 * <p>
42 * Copyright (c) 2013-2019 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
43 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
44 * <p>
45 * $LastChangedDate: 2015-09-16 19:20:07 +0200 (Wed, 16 Sep 2015) $, @version $Revision: 1405 $, by $Author: averbraeck $,
46 * initial version 30 okt. 2014 <br>
47 * @author <a href="http://www.tudelft.nl/pknoppers">Peter Knoppers</a>
48 */
49 public final class LaneFactory
50 {
51
52 /** Stripe width. */
53 private static final Length STRIPE_WIDTH = Length.createSI(0.2);
54
55 /** Angle above which a Bezier curve is used over a straight line. */
56 private static final double BEZIER_MARGIN = Math.toRadians(0.5);
57
58 /** Link. */
59 private final CrossSectionLink link;
60
61 /** Offset for next cross section elements. Left side of lane when building left to right, and vice versa. */
62 private Length offset;
63
64 /** Lane width to use (negative when building left to right). */
65 private Length laneWidth0;
66
67 /** Lane type to use. */
68 private LaneType laneType0;
69
70 /** Speed limit to use. */
71 private Speed speedLimit0;
72
73 /** created lanes. */
74 private final List<Lane> lanes = new ArrayList<>();
75
76 /**
77 * @param network OTSRoadNetwork; network
78 * @param from OTSNode; from node
79 * @param to OTSNode; to node
80 * @param type LinkType; link type
81 * @param simulator OTSSimulatorInterface; simulator
82 * @param policy LaneKeepingPolicy; lane keeping policy
83 * @throws OTSGeometryException if no valid line can be created
84 * @throws NetworkException if the link exists, or a node does not exist, in the network
85 */
86 public LaneFactory(final OTSRoadNetwork network, final OTSNode from, final OTSNode to, final LinkType type,
87 final OTSSimulatorInterface simulator, final LaneKeepingPolicy policy) throws OTSGeometryException, NetworkException
88 {
89 this(network, from, to, type, simulator, policy, makeLine(from, to));
90 }
91
92 /**
93 * @param network OTSRoadNetwork; network
94 * @param from OTSNode; from node
95 * @param to OTSNode; to node
96 * @param type LinkType; link type
97 * @param simulator OTSSimulatorInterface; simulator
98 * @param policy LaneKeepingPolicy; lane keeping policy
99 * @param line OTSLine3D; line
100 * @throws NetworkException if the link exists, or a node does not exist, in the network
101 */
102 public LaneFactory(final OTSRoadNetwork network, final OTSNode from, final OTSNode to, final LinkType type,
103 final OTSSimulatorInterface simulator, final LaneKeepingPolicy policy, final OTSLine3D line) throws NetworkException
104 {
105 this.link = new CrossSectionLink(network, from.getId() + to.getId(), from, to, type, line, simulator, policy);
106 }
107
108 /**
109 * Creates a line between two nodes. If the nodes and their directions are on a straight line, a straight line is created.
110 * Otherwise a default Bezier curve is created.
111 * @param from OTSNode; from node
112 * @param to OTSNode; to node
113 * @return OTSLine3D; line
114 * @throws OTSGeometryException if no valid line can be created
115 */
116 private static OTSLine3D makeLine(final OTSNode from, final OTSNode to) throws OTSGeometryException
117 {
118 // Straight or bezier?
119 double rotCrow = Math.atan2(to.getLocation().y - from.getLocation().y, to.getLocation().x - from.getLocation().x);
120 double dRot = from.getLocation().getRotZ() - rotCrow;
121 while (dRot < -Math.PI)
122 {
123 dRot += 2.0 * Math.PI;
124 }
125 while (dRot > Math.PI)
126 {
127 dRot -= 2.0 * Math.PI;
128 }
129 OTSLine3D line;
130 if (from.getLocation().getRotZ() != to.getLocation().getRotZ() || Math.abs(dRot) > BEZIER_MARGIN)
131 {
132 line = Bezier.cubic(from.getLocation(), to.getLocation());
133 }
134 else
135 {
136 line = new OTSLine3D(from.getPoint(), to.getPoint());
137 }
138 return line;
139 }
140
141 /**
142 * Prepare the factory to add lanes from left to right.
143 * @param leftLanes double; number of lanes left from the link design line
144 * @param laneWidth Length; lane width
145 * @param laneType LaneType; lane type
146 * @param speedLimit Speed; speed limit
147 * @return LaneFactory this lane factory for method chaining
148 */
149 public LaneFactory leftToRight(final double leftLanes, final Length laneWidth, final LaneType laneType,
150 final Speed speedLimit)
151 {
152 this.offset = laneWidth.multiplyBy(leftLanes);
153 this.laneWidth0 = laneWidth.neg();
154 this.laneType0 = laneType;
155 this.speedLimit0 = speedLimit;
156 Try.execute(() -> new Stripe(this.link, this.offset, this.offset, STRIPE_WIDTH),
157 "Unexpected exception while building link.");
158 return this;
159 }
160
161 /**
162 * Prepare the factory to add lanes from right to left.
163 * @param rightLanes double; number of lanes right from the link design line
164 * @param laneWidth Length; lane width
165 * @param laneType LaneType; lane type
166 * @param speedLimit Speed; speed limit
167 * @return LaneFactory this lane factory for method chaining
168 */
169 public LaneFactory rightToLeft(final double rightLanes, final Length laneWidth, final LaneType laneType,
170 final Speed speedLimit)
171 {
172 this.offset = laneWidth.multiplyBy(-rightLanes);
173 this.laneWidth0 = laneWidth;
174 this.laneType0 = laneType;
175 this.speedLimit0 = speedLimit;
176 Try.execute(() -> new Stripe(this.link, this.offset, this.offset, STRIPE_WIDTH),
177 "Unexpected exception while building link.");
178 return this;
179 }
180
181 /**
182 * Adds a lane pair for each permeable, where the permeable determines the right-hand side line when building from left to
183 * right and vice versa. The left-most line is created in {@code leftToRight()}, meaning that each permeable describes
184 * permeablility between a lane and it's right-hand neighbor, when building left to right (and vice versa). For no allowed
185 * lane changes use {@code null}. This method internally adds {@code null} to create the final continuous stripe.
186 * @param permeable Permeable...; permeable per lane pair, for N lanes N-1 should be provided
187 * @return this LaneFactory this lane factory for method chaining
188 */
189 public LaneFactory addLanes(final Permeable... permeable)
190 {
191 List<Permeable> list = new ArrayList<>(Arrays.asList(permeable));
192 list.add(null);
193 for (Permeable perm : list)
194 {
195 this.lanes.add(Try.assign(() -> new Lane(this.link, "Lane " + (this.lanes.size() + 1),
196 this.offset.plus(this.laneWidth0.multiplyBy(0.5)), this.laneWidth0.abs(), this.laneType0, this.speedLimit0),
197 "Unexpected exception while building link."));
198 this.offset = this.offset.plus(this.laneWidth0);
199 Stripe stripe = Try.assign(() -> new Stripe(this.link, this.offset, this.offset, STRIPE_WIDTH),
200 "Unexpected exception while building link.");
201 if (perm != null)
202 {
203 stripe.addPermeability(this.link.getNetwork().getGtuType(GTUType.DEFAULTS.VEHICLE), perm);
204 }
205 }
206 return this;
207 }
208
209 /**
210 * Adds 1 or 2 shoulders to the current set of lanes.
211 * @param width Length; width of the shoulder
212 * @param lat LateralDirectionality; side of shoulder, use {@code null} or {@code NONE} for both
213 * @return LaneFactory this lane factory for method chaining
214 * @throws IllegalStateException if no lanes are defined
215 */
216 public LaneFactory addShoulder(final Length width, final LateralDirectionality lat)
217 {
218 Throw.when(this.lanes.isEmpty(), IllegalStateException.class, "Lanes should be defined before adding shoulder(s).");
219 if (lat == null || lat.isNone() || lat.isLeft())
220 {
221 Length startOffset = null;
222 Length endOffset = null;
223 for (Lane lane : this.lanes)
224 {
225 if (startOffset == null
226 || lane.getDesignLineOffsetAtBegin().plus(lane.getBeginWidth().multiplyBy(0.5)).gt(startOffset))
227 {
228 startOffset = lane.getDesignLineOffsetAtBegin().plus(lane.getBeginWidth().multiplyBy(0.5));
229 }
230 if (endOffset == null || lane.getDesignLineOffsetAtEnd().plus(lane.getEndWidth().multiplyBy(0.5)).gt(endOffset))
231 {
232 endOffset = lane.getDesignLineOffsetAtEnd().plus(lane.getEndWidth().multiplyBy(0.5));
233 }
234 }
235 Length start = startOffset.plus(width.multiplyBy(0.5));
236 Length end = endOffset.plus(width.multiplyBy(0.5));
237 Try.assign(() -> new Shoulder(this.link, "Left shoulder", start, end, width, width),
238 "Unexpected exception while building link.");
239 }
240 if (lat == null || lat.isNone() || lat.isRight())
241 {
242 Length startOffset = null;
243 Length endOffset = null;
244 for (Lane lane : this.lanes)
245 {
246 if (startOffset == null
247 || lane.getDesignLineOffsetAtBegin().minus(lane.getBeginWidth().multiplyBy(0.5)).lt(startOffset))
248 {
249 startOffset = lane.getDesignLineOffsetAtBegin().minus(lane.getBeginWidth().multiplyBy(0.5));
250 }
251 if (endOffset == null
252 || lane.getDesignLineOffsetAtEnd().minus(lane.getEndWidth().multiplyBy(0.5)).lt(endOffset))
253 {
254 endOffset = lane.getDesignLineOffsetAtEnd().minus(lane.getEndWidth().multiplyBy(0.5));
255 }
256 }
257 Length start = startOffset.minus(width.multiplyBy(0.5));
258 Length end = endOffset.minus(width.multiplyBy(0.5));
259 Try.assign(() -> new Shoulder(this.link, "Right shoulder", start, end, width, width),
260 "Unexpected exception while building link.");
261 }
262 return this;
263 }
264
265 /**
266 * Returns the created lanes in build order.
267 * @return List<Lane> created lanes in build order
268 */
269 public List<Lane> getLanes()
270 {
271 return this.lanes;
272 }
273
274 /**
275 * Create a Link along intermediate coordinates from one Node to another.
276 * @param network RoadNetwork; the network
277 * @param name String; name of the new Link
278 * @param from Node; start Node of the new Link
279 * @param to Node; end Node of the new Link
280 * @param intermediatePoints OTSPoint3D[]; array of intermediate coordinates (may be null); the intermediate points may
281 * contain the coordinates of the from node and to node
282 * @param simulator OTSSimulatorInterface; the simulator for this network
283 * @return Link; the newly constructed Link
284 * @throws OTSGeometryException when the design line is degenerate (only one point or duplicate point)
285 * @throws NetworkException if link already exists in the network, if name of the link is not unique, or if the start node
286 * or the end node of the link are not registered in the network.
287 */
288 public static CrossSectionLink makeLink(final RoadNetwork network, final String name, final Node from, final Node to,
289 final OTSPoint3D[] intermediatePoints, final OTSSimulatorInterface simulator)
290 throws OTSGeometryException, NetworkException
291 {
292 List<OTSPoint3D> pointList =
293 intermediatePoints == null ? new ArrayList<>() : new ArrayList<>(Arrays.asList(intermediatePoints));
294 if (pointList.size() == 0 || !from.getPoint().equals(pointList.get(0)))
295 {
296 pointList.add(0, from.getPoint());
297 }
298 if (pointList.size() == 0 || !to.getPoint().equals(pointList.get(pointList.size() - 1)))
299 {
300 pointList.add(to.getPoint());
301 }
302
303 /*-
304 // see if an intermediate point needs to be created to the start of the link in the right direction
305 OTSPoint3D s1 = pointList.get(0);
306 OTSPoint3D s2 = pointList.get(1);
307 double dy = s2.y - s1.y;
308 double dx = s2.x - s1.x;
309 double a = from.getLocation().getRotZ();
310 if (Math.abs(a - Math.atan2(dy, dx)) > 1E-6)
311 {
312 double r = Math.min(1.0, Math.sqrt(dy * dy + dx * dx) / 4.0);
313 OTSPoint3D extra = new OTSPoint3D(s1.x + r * Math.cos(a), s1.y + r * Math.sin(a), s1.z);
314 pointList.add(1, extra);
315 }
316
317 // see if an intermediate point needs to be created to the end of the link in the right direction
318 s1 = pointList.get(pointList.size() - 2);
319 s2 = pointList.get(pointList.size() - 1);
320 dy = s2.y - s1.y;
321 dx = s2.x - s1.x;
322 a = to.getLocation().getRotZ() - Math.PI;
323 if (Math.abs(a - Math.atan2(dy, dx)) > 1E-6)
324 {
325 double r = Math.min(1.0, Math.sqrt(dy * dy + dx * dx) / 4.0);
326 OTSPoint3D extra = new OTSPoint3D(s2.x + r * Math.cos(a), s2.y + r * Math.sin(a), s2.z);
327 pointList.add(pointList.size() - 2, extra);
328 }
329 */
330
331 OTSLine3D designLine = new OTSLine3D(pointList);
332 CrossSectionLink link = new CrossSectionLink(network, name, from, to, network.getLinkType(LinkType.DEFAULTS.ROAD),
333 designLine, simulator, LaneKeepingPolicy.KEEPRIGHT);
334 return link;
335 }
336
337 /**
338 * Create one Lane.
339 * @param link CrossSectionLink; the link that owns the new Lane
340 * @param id String; the id of this lane, should be unique within the link
341 * @param laneType LaneType; the type of the new Lane
342 * @param latPosAtStart Length; the lateral position of the new Lane with respect to the design line of the link at the
343 * start of the link
344 * @param latPosAtEnd Length; the lateral position of the new Lane with respect to the design line of the link at the end of
345 * the link
346 * @param width Length; the width of the new Lane
347 * @param speedLimit Speed; the speed limit on the new Lane
348 * @param simulator DEVSSimulatorInterface.TimeDoubleUnit; the simulator
349 * @return Lane
350 * @throws NetworkException on network inconsistency
351 * @throws OTSGeometryException when creation of center line or contour fails
352 */
353 @SuppressWarnings("checkstyle:parameternumber")
354 private static Lane makeLane(final CrossSectionLink link, final String id, final LaneType laneType,
355 final Length latPosAtStart, final Length latPosAtEnd, final Length width, final Speed speedLimit,
356 final DEVSSimulatorInterface.TimeDoubleUnit simulator) throws NetworkException, OTSGeometryException
357 {
358 Map<GTUType, Speed> speedMap = new LinkedHashMap<>();
359 speedMap.put(link.getNetwork().getGtuType(GTUType.DEFAULTS.VEHICLE), speedLimit);
360 Lane result = new Lane(link, id, latPosAtStart, latPosAtEnd, width, width, laneType, speedMap);
361 return result;
362 }
363
364 /**
365 * Create a simple Lane.
366 * @param network RoadNetwork; the network
367 * @param name String; name of the Lane (and also of the Link that owns it)
368 * @param from OTSNode; starting node of the new Lane
369 * @param to OTSNode; ending node of the new Lane
370 * @param intermediatePoints OTSPoint3D[]; intermediate coordinates or null to create a straight road; the intermediate
371 * points may contain the coordinates of the from node and to node
372 * @param laneType LaneType; type of the new Lane
373 * @param speedLimit Speed; the speed limit on the new Lane
374 * @param simulator OTSSimulatorInterface; the simulator
375 * @return Lane; the new Lane
376 * @throws NetworkException on network inconsistency
377 * @throws OTSGeometryException when creation of center line or contour fails
378 */
379 public static Lane makeLane(final RoadNetwork network, final String name, final OTSNode from, final OTSNode to,
380 final OTSPoint3D[] intermediatePoints, final LaneType laneType, final Speed speedLimit,
381 final OTSSimulatorInterface simulator) throws NetworkException, OTSGeometryException
382 {
383 Length width = new Length(4.0, LengthUnit.METER);
384 final CrossSectionLink link = makeLink(network, name, from, to, intermediatePoints, simulator);
385 Length latPos = new Length(0.0, LengthUnit.METER);
386 return makeLane(link, "lane", laneType, latPos, latPos, width, speedLimit, simulator);
387 }
388
389 /**
390 * Create a simple road with the specified number of Lanes.<br>
391 * This method returns an array of Lane. These lanes are embedded in a Link that can be accessed through the getParentLink
392 * method of the Lane.
393 * @param network RoadNetwork; the network
394 * @param name String; name of the Link
395 * @param from OTSNode; starting node of the new Lane
396 * @param to OTSNode; ending node of the new Lane
397 * @param intermediatePoints OTSPoint3D[]; intermediate coordinates or null to create a straight road; the intermediate
398 * points may contain the coordinates of the from node and to node
399 * @param laneCount int; number of lanes in the road
400 * @param laneOffsetAtStart int; extra offset from design line in lane widths at start of link
401 * @param laneOffsetAtEnd int; extra offset from design line in lane widths at end of link
402 * @param laneType LaneType; type of the new Lanes
403 * @param speedLimit Speed; the speed limit on all lanes
404 * @param simulator OTSSimulatorInterface; the simulator
405 * @return Lane<String, String>[]; array containing the new Lanes
406 * @throws NetworkException on topological problems
407 * @throws OTSGeometryException when creation of center line or contour fails
408 */
409 @SuppressWarnings("checkstyle:parameternumber")
410 public static Lane[] makeMultiLane(final RoadNetwork network, final String name, final OTSNode from, final OTSNode to,
411 final OTSPoint3D[] intermediatePoints, final int laneCount, final int laneOffsetAtStart, final int laneOffsetAtEnd,
412 final LaneType laneType, final Speed speedLimit, final OTSSimulatorInterface simulator)
413 throws NetworkException, OTSGeometryException
414 {
415 final CrossSectionLink link = makeLink(network, name, from, to, intermediatePoints, simulator);
416 Lane[] result = new Lane[laneCount];
417 Length width = new Length(4.0, LengthUnit.METER);
418 for (int laneIndex = 0; laneIndex < laneCount; laneIndex++)
419 {
420 // Be ware! LEFT is lateral positive, RIGHT is lateral negative.
421 Length latPosAtStart = new Length((-0.5 - laneIndex - laneOffsetAtStart) * width.getSI(), LengthUnit.SI);
422 Length latPosAtEnd = new Length((-0.5 - laneIndex - laneOffsetAtEnd) * width.getSI(), LengthUnit.SI);
423 result[laneIndex] =
424 makeLane(link, "lane." + laneIndex, laneType, latPosAtStart, latPosAtEnd, width, speedLimit, simulator);
425 }
426 return result;
427 }
428
429 /**
430 * Create a simple road with the specified number of Lanes.<br>
431 * This method returns an array of Lane. These lanes are embedded in a Link that can be accessed through the getParentLink
432 * method of the Lane.
433 * @param network RoadNetwork; the network
434 * @param name String; name of the Link
435 * @param from OTSNode; starting node of the new Lane
436 * @param to OTSNode; ending node of the new Lane
437 * @param intermediatePoints OTSPoint3D[]; intermediate coordinates or null to create a straight road; the intermediate
438 * points may contain the coordinates of the from node and to node
439 * @param laneCount int; number of lanes in the road
440 * @param laneType LaneType; type of the new Lanes
441 * @param speedLimit Speed; Speed the speed limit (applies to all generated lanes)
442 * @param simulator OTSSimulatorInterface; the simulator
443 * @return Lane<String, String>[]; array containing the new Lanes
444 * @throws NamingException when names cannot be registered for animation
445 * @throws NetworkException on topological problems
446 * @throws OTSGeometryException when creation of center line or contour fails
447 */
448 @SuppressWarnings("checkstyle:parameternumber")
449 public static Lane[] makeMultiLane(final RoadNetwork network, final String name, final OTSNode from, final OTSNode to,
450 final OTSPoint3D[] intermediatePoints, final int laneCount, final LaneType laneType, final Speed speedLimit,
451 final OTSSimulatorInterface simulator) throws NamingException, NetworkException, OTSGeometryException
452 {
453 return makeMultiLane(network, name, from, to, intermediatePoints, laneCount, 0, 0, laneType, speedLimit, simulator);
454 }
455
456 /**
457 * Create a simple road with the specified number of Lanes, based on a Bezier curve.<br>
458 * This method returns an array of Lane. These lanes are embedded in a Link that can be accessed through the getParentLink
459 * method of the Lane.
460 * @param network RoadNetwork; the network
461 * @param name String; name of the Link
462 * @param n1 OTSNode; control node for the start direction
463 * @param n2 OTSNode; starting node of the new Lane
464 * @param n3 OTSNode; ending node of the new Lane
465 * @param n4 OTSNode; control node for the end direction
466 * @param laneCount int; number of lanes in the road
467 * @param laneOffsetAtStart int; extra offset from design line in lane widths at start of link
468 * @param laneOffsetAtEnd int; extra offset from design line in lane widths at end of link
469 * @param laneType LaneType; type of the new Lanes
470 * @param speedLimit Speed; the speed limit on all lanes
471 * @param simulator OTSSimulatorInterface; the simulator
472 * @return Lane<String, String>[]; array containing the new Lanes
473 * @throws NamingException when names cannot be registered for animation
474 * @throws NetworkException on topological problems
475 * @throws OTSGeometryException when creation of center line or contour fails
476 */
477 @SuppressWarnings("checkstyle:parameternumber")
478 public static Lane[] makeMultiLaneBezier(final RoadNetwork network, final String name, final OTSNode n1, final OTSNode n2,
479 final OTSNode n3, final OTSNode n4, final int laneCount, final int laneOffsetAtStart, final int laneOffsetAtEnd,
480 final LaneType laneType, final Speed speedLimit, final OTSSimulatorInterface simulator)
481 throws NamingException, NetworkException, OTSGeometryException
482 {
483 OTSLine3D bezier = makeBezier(n1, n2, n3, n4);
484 final CrossSectionLink link = makeLink(network, name, n2, n3, bezier.getPoints(), simulator);
485 Lane[] result = new Lane[laneCount];
486 Length width = new Length(4.0, LengthUnit.METER);
487 for (int laneIndex = 0; laneIndex < laneCount; laneIndex++)
488 {
489 // Be ware! LEFT is lateral positive, RIGHT is lateral negative.
490 Length latPosAtStart = new Length((-0.5 - laneIndex - laneOffsetAtStart) * width.getSI(), LengthUnit.SI);
491 Length latPosAtEnd = new Length((-0.5 - laneIndex - laneOffsetAtEnd) * width.getSI(), LengthUnit.SI);
492 result[laneIndex] =
493 makeLane(link, "lane." + laneIndex, laneType, latPosAtStart, latPosAtEnd, width, speedLimit, simulator);
494 }
495 return result;
496 }
497
498 /**
499 * @param n1 OTSNode; node 1
500 * @param n2 OTSNode; node 2
501 * @param n3 OTSNode; node 3
502 * @param n4 OTSNode; node 4
503 * @return line between n2 and n3 with start-direction n1-->n2 and end-direction n3-->n4
504 * @throws OTSGeometryException on failure of Bezier curve creation
505 */
506 public static OTSLine3D makeBezier(final OTSNode n1, final OTSNode n2, final OTSNode n3, final OTSNode n4)
507 throws OTSGeometryException
508 {
509 OTSPoint3D p1 = n1.getPoint();
510 OTSPoint3D p2 = n2.getPoint();
511 OTSPoint3D p3 = n3.getPoint();
512 OTSPoint3D p4 = n4.getPoint();
513 DirectedPoint dp1 = new DirectedPoint(p2.x, p2.y, p2.z, 0.0, 0.0, Math.atan2(p2.y - p1.y, p2.x - p1.x));
514 DirectedPoint dp2 = new DirectedPoint(p3.x, p3.y, p3.z, 0.0, 0.0, Math.atan2(p4.y - p3.y, p4.x - p3.x));
515 return Bezier.cubic(dp1, dp2);
516 }
517 }