1 package org.opentrafficsim.core.geometry;
2
3 import nl.tudelft.simulation.language.d3.DirectedPoint;
4
5 /**
6 * Generation of Bézier curves. <br>
7 * The class implements the cubic(...) method to generate a cubic Bézier curve using the following formula: B(t) = (1 -
8 * 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>
9 * 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
10 * points. <br>
11 * For a smooth movement, one of the standard implementations if the cubic(...) function offered is the case where P<sub>1</sub>
12 * 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>,
13 * 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
14 * of P<sub>0</sub>.<br>
15 * Finally, an n-point generalization of the Bézier curve is implemented with the bezier(...) function.
16 * <p>
17 * Copyright (c) 2013-2015 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br>
18 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>.
19 * </p>
20 * $LastChangedDate: 2015-07-24 02:58:59 +0200 (Fri, 24 Jul 2015) $, @version $Revision: 1147 $, by $Author: averbraeck $,
21 * initial version Nov 14, 2015 <br>
22 * @author <a href="http://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a>
23 * @author <a href="http://www.tudelft.nl/pknoppers">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,
32 39916800L, 479001600L, 6227020800L, 87178291200L, 1307674368000L, 20922789888000L, 355687428096000L,
33 6402373705728000L, 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 the number of points for the Bézier curve
44 * @param start the start point of the Bézier curve
45 * @param control1 the first control point
46 * @param control2 the second control point
47 * @param end 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 OTSPoint3D[] points = new OTSPoint3D[numPoints];
56 for (int n = 0; n < numPoints; n++)
57 {
58 double t = n / (numPoints - 1.0);
59 double x = B3(t, start.x, control1.x, control2.x, end.x);
60 double y = B3(t, start.y, control1.y, control2.y, end.y);
61 double z = B3(t, start.z, control1.z, control2.z, end.z);
62 points[n] = new OTSPoint3D(x, y, z);
63 }
64 return new OTSLine3D(points);
65 }
66
67 /**
68 * Construct a cubic Bézier curve from start to end with two generated control points at half the distance between
69 * start and end. The z-value is interpolated in a linear way.
70 * @param numPoints the number of points for the Bézier curve
71 * @param start the directed start point of the Bézier curve
72 * @param end the directed end point of the Bézier curve
73 * @return a cubic Bézier curve between start and end, with the two provided control points
74 * @throws OTSGeometryException in case the number of points is less than 2 or the Bézier curve could not be
75 * constructed
76 */
77 public static OTSLine3D cubic(final int numPoints, final DirectedPoint start, final DirectedPoint end)
78 throws OTSGeometryException
79 {
80 double distance2 =
81 Math.sqrt((end.x - start.x) * (end.x - start.x) + (end.y - start.y) * (end.y - start.y)) / 2.0;
82 OTSPoint3D control1 =
83 new OTSPoint3D(start.x + distance2 * Math.cos(start.getRotZ()), start.y + distance2
84 * Math.sin(start.getRotZ()), start.z);
85 OTSPoint3D control2 =
86 new OTSPoint3D(end.x - distance2 * Math.cos(end.getRotZ()), end.y - distance2 * Math.sin(end.getRotZ()),
87 end.z);
88 // return cubic(numPoints, new OTSPoint3D(start), control1, control2, new OTSPoint3D(end));
89 return bezier(numPoints, new OTSPoint3D(start), control1, control2, new OTSPoint3D(end));
90 }
91
92 /**
93 * Construct a cubic Bézier curve from start to end with two generated control points at half the distance between
94 * start and end. The z-value is interpolated in a linear way.
95 * @param start the directed start point of the Bézier curve
96 * @param end the directed end point of the Bézier curve
97 * @return a cubic Bézier curve between start and end, with the two provided 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 DirectedPoint start, final DirectedPoint end) throws OTSGeometryException
102 {
103 return cubic(DEFAULT_NUM_POINTS, start, end);
104 }
105
106 /**
107 * Calculate the cubic Bézier point with B(t) = (1 - t)<sup>3</sup>P<sub>0</sub> + 3t(1 - t)<sup>2</sup>
108 * P<sub>1</sub> + 3t<sup>2</sup> (1 - t) P<sub>2</sub> + t<sup>3</sup> P<sub>3</sub>.
109 * @param t the fraction
110 * @param p0 the first point of the curve
111 * @param p1 the first control point
112 * @param p2 the second control point
113 * @param p3 the end point of the curve
114 * @return the cubic bezier value B(t)
115 */
116 @SuppressWarnings("checkstyle:methodname")
117 private static double B3(final double t, final double p0, final double p1, final double p2, final double p3)
118 {
119 double t2 = t * t;
120 double t3 = t2 * t;
121 double m = (1.0 - t);
122 double m2 = m * m;
123 double m3 = m2 * m;
124 return m3 * p0 + 3.0 * t * m2 * p1 + 3.0 * t2 * m * p2 + t3 * p3;
125 }
126
127 /**
128 * Construct a Bézier curve of degree n.
129 * @param numPoints the number of points for the Bézier curve to be constructed
130 * @param points the points of the curve, where the first and last are begin and end point, and the intermediate ones are
131 * control points. There should be at least two points.
132 * @return the Bézier value B(t) of degree n, where n is the number of points in the array
133 * @throws OTSGeometryException in case the number of points is less than 2 or the Bézier curve could not be
134 * constructed
135 */
136 public static OTSLine3D bezier(final int numPoints, final OTSPoint3D... points) throws OTSGeometryException
137 {
138 OTSPoint3D[] result = new OTSPoint3D[numPoints];
139 double[] px = new double[points.length];
140 double[] py = new double[points.length];
141 double[] pz = new double[points.length];
142 for (int i = 0; i < points.length; i++)
143 {
144 px[i] = points[i].x;
145 py[i] = points[i].y;
146 pz[i] = points[i].z;
147 }
148 for (int n = 0; n < numPoints; n++)
149 {
150 double t = n / (numPoints - 1.0);
151 double x = Bn(t, px);
152 double y = Bn(t, py);
153 double z = Bn(t, pz);
154 result[n] = new OTSPoint3D(x, y, z);
155 }
156 return new OTSLine3D(result);
157 }
158
159 /**
160 * Construct a Bézier curve of degree n.
161 * @param points the points of the curve, where the first and last are begin and end point, and the intermediate ones are
162 * control points. There should be at least two points.
163 * @return the Bézier value B(t) of degree n, where n is the number of points in the array
164 * @throws OTSGeometryException in case the number of points is less than 2 or the Bézier curve could not be
165 * constructed
166 */
167 public static OTSLine3D bezier(final OTSPoint3D... points) throws OTSGeometryException
168 {
169 return bezier(DEFAULT_NUM_POINTS, points);
170 }
171
172 /**
173 * 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>
174 * P<sub>i</sub>], where C(n, k) is the binomial coefficient defined by n! / ( k! (n-k)! ), ! being the factorial operator.
175 * @param t the fraction
176 * @param p the points of the curve, where the first and last are begin and end point, and the intermediate ones are control
177 * points
178 * @return the Bézier value B(t) of degree n, where n is the number of points in the array
179 */
180 @SuppressWarnings("checkstyle:methodname")
181 private static double Bn(final double t, final double... p)
182 {
183 double b = 0.0;
184 double m = (1.0 - t);
185 int n = p.length - 1;
186 double fn = factorial(n);
187 for (int i = 0; i <= n; i++)
188 {
189 double c = fn / (factorial(i) * (factorial(n - i)));
190 b += c * Math.pow(m, n - i) * Math.pow(t, i) * p[i];
191 }
192 return b;
193 }
194
195 /**
196 * Calculate factorial(k), which is k * (k-1) * (k-2) * ... * 1. For factorials up to 20, a lookup table is used.
197 * @param k the parameter
198 * @return factorial(k)
199 */
200 private static double factorial(final int k)
201 {
202 if (k < fact.length)
203 {
204 return fact[k];
205 }
206 double f = 1;
207 for (int i = 2; i <= k; i++)
208 {
209 f = f * i;
210 }
211 return f;
212 }
213
214 /**
215 * @param args args
216 * @throws OTSGeometryException ne
217 */
218 public static void main(final String[] args) throws OTSGeometryException
219 {
220 // DirectedPoint s = new DirectedPoint(0, 0, 0, 0, 0, -Math.PI/2.0);
221 // DirectedPoint e = new DirectedPoint(10, 10, 20, 0, 0, Math.PI);
222 // OTSLine3D b1 = Bezier.cubic(s, e);
223 // for (OTSPoint3D p : b1.getPoints())
224 // {
225 // System.out.println(p.x + "\t" + p.y + "\t" + p.z);
226 // }
227
228 OTSPoint3D s = new OTSPoint3D(0, 0, 0);
229 OTSPoint3D s1 = new OTSPoint3D(10, 0, 0);
230 OTSPoint3D m1 = new OTSPoint3D(25, 5, 0);
231 OTSPoint3D m2 = new OTSPoint3D(-15, 5, 0);
232 OTSPoint3D e0 = new OTSPoint3D(0, 10, 20);
233 OTSPoint3D e = new OTSPoint3D(10, 10, 20);
234 OTSLine3D b1 = Bezier.bezier(s, s1, m1, m2, e0, e);
235 for (OTSPoint3D p : b1.getPoints())
236 {
237 System.out.println(p.x + "\t" + p.y + "\t" + p.z);
238 }
239
240 }
241 }