1 package org.opentrafficsim.core.geometry;
2
3 import java.awt.geom.Line2D;
4
5 import org.djutils.draw.point.OrientedPoint2d;
6 import org.djutils.draw.point.Point2d;
7 import org.djutils.exceptions.Throw;
8
9 /**
10 * Generation of Bézier curves. <br>
11 * The class implements the cubic(...) method to generate a cubic Bézier curve using the following formula: B(t) = (1 -
12 * t)<sup>3</sup>P<sub>0</sub> + 3t(1 - t)<sup>2</sup> P<sub>1</sub> + 3t<sup>2</sup> (1 - t) P<sub>2</sub> + t<sup>3</sup>
13 * P<sub>3</sub> where P<sub>0</sub> and P<sub>3</sub> are the end points, and P<sub>1</sub> and P<sub>2</sub> the control
14 * points. <br>
15 * For a smooth movement, one of the standard implementations if the cubic(...) function offered is the case where P<sub>1</sub>
16 * is positioned halfway between P<sub>0</sub> and P<sub>3</sub> starting from P<sub>0</sub> in the direction of P<sub>3</sub>,
17 * and P<sub>2</sub> is positioned halfway between P<sub>3</sub> and P<sub>0</sub> starting from P<sub>3</sub> in the direction
18 * of P<sub>0</sub>.<br>
19 * Finally, an n-point generalization of the Bézier curve is implemented with the bezier(...) function.
20 * <p>
21 * Copyright (c) 2013-2024 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
22 * BSD-style license. See <a href="https://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
23 * </p>
24 * @author <a href="https://github.com/averbraeck">Alexander Verbraeck</a>
25 * @author <a href="https://tudelft.nl/staff/p.knoppers-1">Peter Knoppers</a>
26 */
27 public final class Bezier
28 {
29 /** The default number of points to use to construct a Bézier curve. */
30 private static final int DEFAULT_NUM_POINTS = 64;
31
32 /** Cached factorial values. */
33 private static long[] fact = new long[] {1L, 1L, 2L, 6L, 24L, 120L, 720L, 5040L, 40320L, 362880L, 3628800L, 39916800L,
34 479001600L, 6227020800L, 87178291200L, 1307674368000L, 20922789888000L, 355687428096000L, 6402373705728000L,
35 121645100408832000L, 2432902008176640000L};
36
37 /** Utility class. */
38 private Bezier()
39 {
40 // do not instantiate
41 }
42
43 /**
44 * Construct a cubic Bézier curve from start to end with two control points.
45 * @param numPoints int; the number of points for the Bézier curve
46 * @param start Point2d; the start point of the Bézier curve
47 * @param control1 Point2d; the first control point
48 * @param control2 Point2d; the second control point
49 * @param end Point2d; the end point of the Bézier curve
50 * @return a cubic Bézier curve between start and end, with the two provided control points
51 * @throws OtsGeometryException in case the number of points is less than 2 or the Bézier curve could not be
52 * constructed
53 */
54 public static OtsLine2d cubic(final int numPoints, final Point2d start, final Point2d control1,
55 final Point2d control2, final Point2d end) throws OtsGeometryException
56 {
57 Throw.when(numPoints < 2, OtsGeometryException.class, "Number of points too small (got %d; minimum value is 2)",
58 numPoints);
59 Point2d[] points = new Point2d[numPoints];
60 for (int n = 0; n < numPoints; n++)
61 {
62 double t = n / (numPoints - 1.0);
63 double x = B3(t, start.x, control1.x, control2.x, end.x);
64 double y = B3(t, start.y, control1.y, control2.y, end.y);
65 points[n] = new Point2d(x, y);
66 }
67 return new OtsLine2d(points);
68 }
69
70 /**
71 * Construct a cubic Bézier curve from start to end with two generated control points at half the distance between
72 * start and end. The z-value is interpolated in a linear way.
73 * @param numPoints int; the number of points for the Bézier curve
74 * @param start OrientedPoint2d; the directed start point of the Bézier curve
75 * @param end OrientedPoint2d; the directed end point of the Bézier curve
76 * @return a cubic Bézier curve between start and end, with the two provided control points
77 * @throws OtsGeometryException in case the number of points is less than 2 or the Bézier curve could not be
78 * constructed
79 */
80 public static OtsLine2d cubic(final int numPoints, final OrientedPoint2d start, final OrientedPoint2d end)
81 throws OtsGeometryException
82 {
83 return cubic(numPoints, start, end, 1.0);
84 }
85
86 /**
87 * Construct a cubic Bézier curve from start to end with two generated control points at half the distance between
88 * start and end. The z-value is interpolated in a linear way.
89 * @param numPoints int; the number of points for the Bézier curve
90 * @param start OrientedPoint2d; the directed start point of the Bézier curve
91 * @param end OrientedPoint2d; the directed end point of the Bézier curve
92 * @param shape shape factor; 1 = control points at half the distance between start and end, > 1 results in a pointier
93 * shape, < 1 results in a flatter shape, value should be above 0
94 * @return a cubic Bézier curve between start and end, with the two determined control points
95 * @throws OtsGeometryException in case the number of points is less than 2 or the Bézier curve could not be
96 * constructed
97 */
98 public static OtsLine2d cubic(final int numPoints, final OrientedPoint2d start, final OrientedPoint2d end, final double shape)
99 throws OtsGeometryException
100 {
101 return cubic(numPoints, start, end, shape, false);
102 }
103
104 /**
105 * Construct a cubic Bézier curve from start to end with two generated control points at half the distance between
106 * start and end. The z-value is interpolated in a linear way.
107 * @param numPoints int; the number of points for the Bézier curve
108 * @param start OrientedPoint2d; the directed start point of the Bézier curve
109 * @param end OrientedPoint2d; the directed end point of the Bézier curve
110 * @param shape double; shape factor; 1 = control points at half the distance between start and end, > 1 results in a
111 * pointier shape, < 1 results in a flatter shape, value should be above 0
112 * @param weighted boolean; control point distance relates to distance to projected point on extended line from other end
113 * @return a cubic Bézier curve between start and end, with the two determined control points
114 * @throws OtsGeometryException in case the number of points is less than 2 or the Bézier curve could not be
115 * constructed
116 */
117 public static OtsLine2d cubic(final int numPoints, final OrientedPoint2d start, final OrientedPoint2d end, final double shape,
118 final boolean weighted) throws OtsGeometryException
119 {
120 return bezier(cubicControlPoints(start, end, shape, weighted));
121 }
122
123 /**
124 * Construct control points for a cubic Bézier curve from start to end with two generated control points at half the
125 * distance between start and end.
126 * @param start OrientedPoint2d; the directed start point of the Bézier curve
127 * @param end OrientedPoint2d; the directed end point of the Bézier curve
128 * @param shape double; shape factor; 1 = control points at half the distance between start and end, > 1 results in a
129 * pointier shape, < 1 results in a flatter shape, value should be above 0
130 * @param weighted boolean; control point distance relates to distance to projected point on extended line from other end
131 * @return a cubic Bézier curve between start and end, with the two determined control points
132 */
133 public static Point2d[] cubicControlPoints(final OrientedPoint2d start, final OrientedPoint2d end, final double shape,
134 final boolean weighted)
135 {
136 Throw.when(shape <= 0.0, IllegalArgumentException.class, "Shape factor must be above 0.0.");
137 Point2d control1;
138 Point2d control2;
139
140 if (weighted)
141 {
142 // each control point is 'w' * the distance between the end-points away from the respective end point
143 // 'w' is a weight given by the distance from the end point to the extended line of the other end point
144 double dx = end.x - start.x;
145 double dy = end.y - start.y;
146 double distance = shape * Math.hypot(dx, dy);
147 double cosEnd = Math.cos(end.getDirZ());
148 double sinEnd = Math.sin(end.getDirZ());
149 double dStart = Line2D.ptLineDist(end.x, end.y, end.x + cosEnd, end.y + sinEnd, start.x, start.y);
150 double cosStart = Math.cos(start.getDirZ());
151 double sinStart = Math.sin(start.getDirZ());
152 double dEnd = Line2D.ptLineDist(start.x, start.y, start.x + cosStart, start.y + sinStart, end.x, end.y);
153 double wStart = dStart / (dStart + dEnd);
154 double wEnd = dEnd / (dStart + dEnd);
155 double wStartDistance = wStart * distance;
156 double wEndDistance = wEnd * distance;
157 control1 = new Point2d(start.x + wStartDistance * cosStart, start.y + wStartDistance * sinStart);
158 // - (minus) as the angle is where the line leaves, i.e. from shape point to end
159 control2 = new Point2d(end.x - wEndDistance * cosEnd, end.y - wEndDistance * sinEnd);
160 }
161 else
162 {
163 // each control point is half the distance between the end-points away from the respective end point
164 double dx = end.x - start.x;
165 double dy = end.y - start.y;
166 double distance2 = shape * .5 * Math.hypot(dx, dy);
167 control1 = new Point2d(start.x + distance2 * Math.cos(start.getDirZ()),
168 start.y + distance2 * Math.sin(start.getDirZ()));
169 control2 = new Point2d(end.x - distance2 * Math.cos(end.getDirZ()), end.y - distance2 * Math.sin(end.getDirZ()));
170 }
171
172 return new Point2d[] {start, control1, control2, end};
173 }
174
175 /**
176 * Construct a cubic Bézier curve from start to end with two generated control points at half the distance between
177 * start and end. The z-value is interpolated in a linear way.
178 * @param start OrientedPoint2d; the directed start point of the Bézier curve
179 * @param end OrientedPoint2d; the directed end point of the Bézier curve
180 * @return a cubic Bézier curve between start and end, with the two provided control points
181 * @throws OtsGeometryException in case the number of points is less than 2 or the Bézier curve could not be
182 * constructed
183 */
184 public static OtsLine2d cubic(final OrientedPoint2d start, final OrientedPoint2d end) throws OtsGeometryException
185 {
186 return cubic(DEFAULT_NUM_POINTS, start, end);
187 }
188
189 /**
190 * Calculate the cubic Bézier point with B(t) = (1 - t)<sup>3</sup>P<sub>0</sub> + 3t(1 - t)<sup>2</sup>
191 * P<sub>1</sub> + 3t<sup>2</sup> (1 - t) P<sub>2</sub> + t<sup>3</sup> P<sub>3</sub>.
192 * @param t double; the fraction
193 * @param p0 double; the first point of the curve
194 * @param p1 double; the first control point
195 * @param p2 double; the second control point
196 * @param p3 double; the end point of the curve
197 * @return the cubic bezier value B(t)
198 */
199 @SuppressWarnings("checkstyle:methodname")
200 private static double B3(final double t, final double p0, final double p1, final double p2, final double p3)
201 {
202 double t2 = t * t;
203 double t3 = t2 * t;
204 double m = (1.0 - t);
205 double m2 = m * m;
206 double m3 = m2 * m;
207 return m3 * p0 + 3.0 * t * m2 * p1 + 3.0 * t2 * m * p2 + t3 * p3;
208 }
209
210 /**
211 * Construct a Bézier curve of degree n.
212 * @param numPoints int; the number of points for the Bézier curve to be constructed
213 * @param points Point2d...; the points of the curve, where the first and last are begin and end point, and the
214 * intermediate ones are control points. There should be at least two points.
215 * @return the Bézier value B(t) of degree n, where n is the number of points in the array
216 * @throws OtsGeometryException in case the number of points is less than 2 or the Bézier curve could not be
217 * constructed
218 */
219 public static OtsLine2d bezier(final int numPoints, final Point2d... points) throws OtsGeometryException
220 {
221 Point2d[] result = new Point2d[numPoints];
222 double[] px = new double[points.length];
223 double[] py = new double[points.length];
224 for (int i = 0; i < points.length; i++)
225 {
226 px[i] = points[i].x;
227 py[i] = points[i].y;
228 }
229 for (int n = 0; n < numPoints; n++)
230 {
231 double t = n / (numPoints - 1.0);
232 double x = Bn(t, px);
233 double y = Bn(t, py);
234 result[n] = new Point2d(x, y);
235 }
236 return new OtsLine2d(result);
237 }
238
239 /**
240 * Construct a Bézier curve of degree n.
241 * @param points Point2d...; the points of the curve, where the first and last are begin and end point, and the
242 * intermediate ones are control points. There should be at least two points.
243 * @return the Bézier value B(t) of degree n, where n is the number of points in the array
244 * @throws OtsGeometryException in case the number of points is less than 2 or the Bézier curve could not be
245 * constructed
246 */
247 public static OtsLine2d bezier(final Point2d... points) throws OtsGeometryException
248 {
249 return bezier(DEFAULT_NUM_POINTS, points);
250 }
251
252 /**
253 * Calculate the Bézier point of degree n, with B(t) = Sum(i = 0..n) [C(n, i) * (1 - t)<sup>n-i</sup> t<sup>i</sup>
254 * P<sub>i</sub>], where C(n, k) is the binomial coefficient defined by n! / ( k! (n-k)! ), ! being the factorial operator.
255 * @param t double; the fraction
256 * @param p double...; the points of the curve, where the first and last are begin and end point, and the intermediate ones
257 * are control points
258 * @return the Bézier value B(t) of degree n, where n is the number of points in the array
259 */
260 @SuppressWarnings("checkstyle:methodname")
261 static double Bn(final double t, final double... p)
262 {
263 double b = 0.0;
264 double m = (1.0 - t);
265 int n = p.length - 1;
266 double fn = factorial(n);
267 for (int i = 0; i <= n; i++)
268 {
269 double c = fn / (factorial(i) * (factorial(n - i)));
270 b += c * Math.pow(m, n - i) * Math.pow(t, i) * p[i];
271 }
272 return b;
273 }
274
275 /**
276 * Calculate factorial(k), which is k * (k-1) * (k-2) * ... * 1. For factorials up to 20, a lookup table is used.
277 * @param k int; the parameter
278 * @return factorial(k)
279 */
280 private static double factorial(final int k)
281 {
282 if (k < fact.length)
283 {
284 return fact[k];
285 }
286 double f = 1;
287 for (int i = 2; i <= k; i++)
288 {
289 f = f * i;
290 }
291 return f;
292 }
293
294 }