Bezier.java

  1. package org.opentrafficsim.core.geometry;

  3. import java.awt.geom.Line2D;

  5. import org.djutils.exceptions.Throw;

  7. import nl.tudelft.simulation.language.d3.DirectedPoint;

  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-2020 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;

  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};

  39.     /** Utility class. */
  40.     private Bezier()
  41.     {
  42.         // do not instantiate
  43.     }

  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.     }

  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.     }

  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.     }

  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;

  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.         }

  156.         // Limit control points to not intersect with the other (infinite) line
  157.         OTSPoint3D s = new OTSPoint3D(start);
  158.         OTSPoint3D e = new OTSPoint3D(end);

  160.         // return cubic(numPoints, new OTSPoint3D(start), control1, control2, new OTSPoint3D(end));
  161.         return bezier(numPoints, s, control1, control2, e);
  162.     }

  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.     }

  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.     }

  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.     }

  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.     }

  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.     }

  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.     }

  286. }
Created with JaCoCo 0.8.6.202009150832