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