View Javadoc
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&eacute;zier curves. <br>
11   * The class implements the cubic(...) method to generate a cubic B&eacute;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&eacute;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&eacute;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&eacute;zier curve from start to end with two control points.
47       * @param numPoints int; the number of points for the B&eacute;zier curve
48       * @param start OTSPoint3D; the start point of the B&eacute;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&eacute;zier curve
52       * @return a cubic B&eacute;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&eacute;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&eacute;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&eacute;zier curve
77       * @param start DirectedPoint; the directed start point of the B&eacute;zier curve
78       * @param end DirectedPoint; the directed end point of the B&eacute;zier curve
79       * @return a cubic B&eacute;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&eacute;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&eacute;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&eacute;zier curve
93       * @param start DirectedPoint; the directed start point of the B&eacute;zier curve
94       * @param end DirectedPoint; the directed end point of the B&eacute;zier curve
95       * @param shape shape factor; 1 = control points at half the distance between start and end, &gt; 1 results in a pointier
96       *            shape, &lt; 1 results in a flatter shape, value should be above 0
97       * @return a cubic B&eacute;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&eacute;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&eacute;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&eacute;zier curve
111      * @param start DirectedPoint; the directed start point of the B&eacute;zier curve
112      * @param end DirectedPoint; the directed end point of the B&eacute;zier curve
113      * @param shape shape factor; 1 = control points at half the distance between start and end, &gt; 1 results in a pointier
114      *            shape, &lt; 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&eacute;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&eacute;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&eacute;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&eacute;zier curve
168      * @param end DirectedPoint; the directed end point of the B&eacute;zier curve
169      * @return a cubic B&eacute;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&eacute;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&eacute;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&eacute;zier curve of degree n.
201      * @param numPoints int; the number of points for the B&eacute;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&eacute;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&eacute;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&eacute;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&eacute;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&eacute;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&eacute;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&eacute;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 }