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