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