1 package org.djutils.draw.line; 2 3 import java.util.Map; 4 import java.util.NavigableMap; 5 import java.util.TreeMap; 6 7 import org.djutils.draw.DrawException; 8 import org.djutils.draw.DrawRuntimeException; 9 import org.djutils.draw.Transform2d; 10 import org.djutils.draw.point.Point2d; 11 import org.djutils.draw.point.Point3d; 12 import org.djutils.exceptions.Throw; 13 14 /** 15 * Generation of Bézier curves. <br> 16 * The class implements the cubic(...) method to generate a cubic Bézier curve using the following formula: B(t) = (1 - 17 * 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> 18 * 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 19 * points. <br> 20 * For a smooth movement, one of the standard implementations if the cubic(...) function offered is the case where P<sub>1</sub> 21 * 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>, 22 * 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 23 * of P<sub>0</sub>.<br> 24 * Finally, an n-point generalization of the Bézier curve is implemented with the bezier(...) function. 25 * <p> 26 * Copyright (c) 2013-2021 Delft University of Technology, PO Box 5, 2600 AA, Delft, the Netherlands. All rights reserved. <br> 27 * BSD-style license. See <a href="http://opentrafficsim.org/docs/license.html">OpenTrafficSim License</a>. 28 * </p> 29 * @author <a href="https://www.tbm.tudelft.nl/averbraeck">Alexander Verbraeck</a> 30 * @author <a href="https://www.tudelft.nl/pknoppers">Peter Knoppers</a> 31 */ 32 public final class Bezier 33 { 34 /** The default number of points to use to construct a Bézier curve. */ 35 public static final int DEFAULT_BEZIER_SIZE = 64; 36 37 /** Cached factorial values. */ 38 private static long[] fact = new long[] {1L, 1L, 2L, 6L, 24L, 120L, 720L, 5040L, 40320L, 362880L, 3628800L, 39916800L, 39 479001600L, 6227020800L, 87178291200L, 1307674368000L, 20922789888000L, 355687428096000L, 6402373705728000L, 40 121645100408832000L, 2432902008176640000L}; 41 42 /** Utility class. */ 43 private Bezier() 44 { 45 // do not instantiate 46 } 47 48 /** 49 * Approximate a cubic Bézier curve from start to end with two control points. 50 * @param size int; the number of points of the Bézier curve 51 * @param start Point2d; the start point of the Bézier curve 52 * @param control1 Point2d; the first control point 53 * @param control2 Point2d; the second control point 54 * @param end Point2d; the end point of the Bézier curve 55 * @return PolyLine2d; an approximation of a cubic Bézier curve between start and end, using the two provided control 56 * points 57 * @throws DrawRuntimeException in case the number of points is less than 2 or the Bézier curve could not be 58 * constructed 59 */ 60 public static PolyLine2d cubic(final int size, final Point2dtml#Point2d">Point2dd.html#Point2d">Point2d start, final Point2dtml#Point2d">Point2d control1, final Point2d control2, 61 final Point2d end) throws DrawRuntimeException 62 { 63 Throw.when(size < 2, DrawRuntimeException.class, "Too few points (specified %d; minimum is 2)", size); 64 Point2dhtml#Point2d">Point2d[] points = new Point2d[size]; 65 for (int n = 0; n < size; n++) 66 { 67 double t = n / (size - 1.0); 68 double x = B3(t, start.x, control1.x, control2.x, end.x); 69 double y = B3(t, start.y, control1.y, control2.y, end.y); 70 points[n] = new Point2d(x, y); 71 } 72 return new PolyLine2d(points); 73 } 74 75 /** 76 * Approximate a cubic Bézier curve from start to end with two control points with a specified precision. 77 * @param epsilon double; the precision. 78 * @param start Point2d; the start point of the Bézier curve 79 * @param control1 Point2d; the first control point 80 * @param control2 Point2d; the second control point 81 * @param end Point2d; the end point of the Bézier curve 82 * @return PolyLine2d; an approximation of a cubic Bézier curve between start and end, using the two provided control 83 * points 84 * @throws DrawRuntimeException in case the number of points is less than 2, or the Bézier curve could not be 85 * constructed 86 */ 87 public static PolyLine2d cubic(final double epsilon, final Point2dtml#Point2d">Point2dd.html#Point2d">Point2d start, final Point2dtml#Point2d">Point2d control1, final Point2d control2, 88 final Point2d end) throws DrawRuntimeException 89 { 90 return bezier(epsilon, start, control1, control2, end); 91 } 92 93 /** 94 * Approximate a cubic Bézier curve from start to end with two generated control points at half the distance between 95 * start and end. TODO change start and end to Ray3d 96 * @param size int; the number of points of the Bézier curve 97 * @param start Ray2d; the start point and start direction of the Bézier curve 98 * @param end Ray2d; the end point and end direction of the Bézier curve 99 * @return PolyLine2d; an approximation of a cubic Bézier curve between start and end, using the directions of those 100 * points at start and end 101 * @throws DrawRuntimeException in case the number of points is less than 2 or the Bézier curve could not be 102 * constructed 103 */ 104 public static PolyLine2d cubic(final int size, final Ray2d.html#Ray2d">Ray2d start, final Ray2d end) throws DrawRuntimeException 105 { 106 return cubic(size, start, end, 1.0); 107 } 108 109 /** 110 * Approximate a cubic Bézier curve from start to end with two generated control points at half the distance between 111 * start and end with specified precision. 112 * @param epsilon double; the precision. 113 * @param start Ray2d; the start point and start direction of the Bézier curve 114 * @param end Ray2d; the end point and end direction of the Bézier curve 115 * @return PolyLine2d; an approximation of a cubic Bézier curve between start and end, using the directions of those 116 * points at start and end 117 * @throws DrawRuntimeException in case the number of points is less than 2 or the Bézier curve could not be 118 * constructed 119 */ 120 public static PolyLine2d cubic(final double epsilon, final Ray2d.html#Ray2d">Ray2d start, final Ray2d end) throws DrawRuntimeException 121 { 122 return cubic(epsilon, start, end, 1.0); 123 } 124 125 /** 126 * Approximate a cubic Bézier curve from start to end with two generated control points at half the distance between 127 * start and end. 128 * @param size int; the number of points for the Bézier curve 129 * @param start Ray2d; the start point and start direction of the Bézier curve 130 * @param end Ray2d; the end point and end direction of the Bézier curve 131 * @param shape shape factor; 1 = control points at half the distance between start and end, > 1 results in a pointier 132 * shape, < 1 results in a flatter shape, value should be above 0 and finite 133 * @return PolyLine2d; an approximation of a cubic Bézier curve between start and end, using the directions of those 134 * points at start and end 135 * @throws DrawRuntimeException in case the number of points is less than 2 or the Bézier curve could not be 136 * constructed 137 */ 138 public static PolyLine2d cubic(final int size, final Ray2d.html#Ray2d">Ray2d start, final Ray2d end, final double shape) 139 throws DrawRuntimeException 140 { 141 Throw.when(Double.isNaN(shape) || Double.isInfinite(shape) || shape <= 0, DrawRuntimeException.class, 142 "shape must be a finite, positive value"); 143 return cubic(size, start, end, shape, false); 144 } 145 146 /** 147 * Approximate a cubic Bézier curve from start to end with two generated control points at half the distance between 148 * start and end with specified precision. 149 * @param epsilon double; the precision. 150 * @param start Ray2d; the start point and start direction of the Bézier curve 151 * @param end Ray2d; the end point and end direction of the Bézier curve 152 * @param shape shape factor; 1 = control points at half the distance between start and end, > 1 results in a pointier 153 * shape, < 1 results in a flatter shape, value should be above 0 and finite 154 * @return PolyLine2d; an approximation of a cubic Bézier curve between start and end, using the directions of those 155 * points at start and end 156 * @throws DrawRuntimeException in case the number of points is less than 2 or the Bézier curve could not be 157 * constructed 158 */ 159 public static PolyLine2d cubic(final double epsilon, final Ray2d.html#Ray2d">Ray2d start, final Ray2d end, final double shape) 160 throws DrawRuntimeException 161 { 162 Throw.when(Double.isNaN(shape) || Double.isInfinite(shape) || shape <= 0, DrawRuntimeException.class, 163 "shape must be a finite, positive value"); 164 return cubic(epsilon, start, end, shape, false); 165 } 166 167 /** 168 * Approximate a cubic Bézier curve from start to end with two generated control points at half the distance between 169 * start and end. 170 * @param size int; the number of points for the Bézier curve 171 * @param start Ray2d; the start point and start direction of the Bézier curve 172 * @param end Ray2d; the end point and end direction of the Bézier curve 173 * @param shape shape factor; 1 = control points at half the distance between start and end, > 1 results in a pointier 174 * shape, < 1 results in a flatter shape, value should be above 0, finite and not NaN 175 * @param weighted boolean; control point distance relates to distance to projected point on extended line from other end 176 * @return PolyLine2d; an approximation of a cubic Bézier curve between start and end, with the two determined 177 * control points 178 * @throws NullPointerException when start or end is null 179 * @throws DrawRuntimeException in case size is less than 2, start is at the same location as end, shape is invalid, or the 180 * Bézier curve could not be constructed 181 */ 182 public static PolyLine2d cubic(final int size, final Ray2d.html#Ray2d">Ray2d start, final Ray2d end, final double shape, 183 final boolean weighted) throws NullPointerException, DrawRuntimeException 184 { 185 Point2d[] points = createControlPoints(start, end, shape, weighted); 186 return cubic(size, points[0], points[1], points[2], points[3]); 187 } 188 189 /** 190 * Approximate a cubic Bézier curve from start to end with two generated control points at half the distance between 191 * start and end with specified precision. 192 * @param epsilon double; the precision. 193 * @param start Ray2d; the start point and start direction of the Bézier curve 194 * @param end Ray2d; the end point and end direction of the Bézier curve 195 * @param shape shape factor; 1 = control points at half the distance between start and end, > 1 results in a pointier 196 * shape, < 1 results in a flatter shape, value should be above 0, finite and not NaN 197 * @param weighted boolean; control point distance relates to distance to projected point on extended line from other end 198 * @return PolyLine2d; an approximation of a cubic Bézier curve between start and end, with the two determined 199 * control points 200 * @throws NullPointerException when start or end is null 201 * @throws DrawRuntimeException in case size is less than 2, start is at the same location as end, shape is invalid 202 */ 203 public static PolyLine2d cubic(final double epsilon, final Ray2d.html#Ray2d">Ray2d start, final Ray2d end, final double shape, 204 final boolean weighted) throws NullPointerException, DrawRuntimeException 205 { 206 Point2d[] points = createControlPoints(start, end, shape, weighted); 207 return cubic(epsilon, points[0], points[1], points[2], points[3]); 208 } 209 210 /** Unit vector for transformations in createControlPoints. */ 211 private static final Point2dPoint2d">Point2d UNIT_VECTOR2D = new Point2d(1, 0); 212 213 /** 214 * Create control points for a cubic Bézier curve defined by two Rays. 215 * @param start Ray2d; the start point (and direction) 216 * @param end Ray2d; the end point (and direction) 217 * @param shape double; the shape; higher values put the generated control points further away from end and result in a 218 * pointier Bézier curve 219 * @param weighted boolean; 220 * @return Point2d[]; an array of four Point2d elements: start, the first control point, the second control point, end. 221 * @throws DrawRuntimeException when shape is invalid 222 */ 223 private static Point2d[] createControlPoints(final Ray2d.html#Ray2d">Ray2d start, final Ray2d end, final double shape, final boolean weighted) 224 throws DrawRuntimeException 225 { 226 Throw.whenNull(start, "start point may not be null"); 227 Throw.whenNull(end, "end point may not be null"); 228 Throw.when(start.distanceSquared(end) == 0, DrawRuntimeException.class, 229 "Cannot create control points if start and end points coincide"); 230 Throw.when(Double.isNaN(shape) || shape <= 0 || Double.isInfinite(shape), DrawRuntimeException.class, 231 "shape must be a finite, positive value"); 232 233 Point2d control1; 234 Point2d control2; 235 if (weighted) 236 { 237 // each control point is 'w' * the distance between the end-points away from the respective end point 238 // 'w' is a weight given by the distance from the end point to the extended line of the other end point 239 double distance = shape * start.distance(end); 240 double dStart = start.distance(start.closestPointOnLine(end)); 241 double dEnd = end.distance(end.closestPointOnLine(start)); 242 double wStart = dStart / (dStart + dEnd); 243 double wEnd = dEnd / (dStart + dEnd); 244 control1 = new Transform2d().translate(start).rotation(start.phi).scale(distance * wStart, distance * wStart) 245 .transform(UNIT_VECTOR2D); 246 // - (minus) as the angle is where the line leaves, i.e. from shape point to end 247 control2 = new Transform2d().translate(end).rotation(end.phi + Math.PI).scale(distance * wEnd, distance * wEnd) 248 .transform(UNIT_VECTOR2D); 249 } 250 else 251 { 252 // each control point is half the distance between the end-points away from the respective end point 253 double distance = shape * start.distance(end) / 2.0; 254 control1 = start.getLocation(distance); 255 // new Transform2d().translate(start).rotation(start.phi).scale(distance, distance).transform(UNIT_VECTOR2D); 256 control2 = end.getLocationExtended(-distance); 257 // new Transform2d().translate(end).rotation(end.phi + Math.PI).scale(distance, distance).transform(UNIT_VECTOR2D); 258 } 259 return new Point2d[] {start, control1, control2, end}; 260 } 261 262 /** 263 * Approximate a cubic Bézier curve from start to end with two generated control points at half the distance between 264 * start and end. The size of the constructed curve is <code>DEFAULT_BEZIER_SIZE</code>. 265 * @param start Ray2d; the start point and start direction of the Bézier curve 266 * @param end Ray2d; the end point and end direction of the Bézier curve 267 * @return PolyLine2d; an approximation of a cubic Bézier curve between start and end, following the directions of 268 * those points at start and end 269 * @throws DrawRuntimeException in case the number of points is less than 2 or the Bézier curve could not be 270 * constructed 271 */ 272 public static PolyLine2d cubic(final Ray2d.html#Ray2d">Ray2d start, final Ray2d end) throws DrawRuntimeException 273 { 274 return cubic(DEFAULT_BEZIER_SIZE, start, end); 275 } 276 277 /** 278 * Approximate a Bézier curve of degree n. 279 * @param size int; the number of points for the Bézier curve to be constructed 280 * @param points Point2d...; the points of the curve, where the first and last are begin and end point, and the intermediate 281 * ones are control points. There should be at least two points. 282 * @return PolyLine2d; an approximation of a cubic Bézier curve between start and end, using the provided control 283 * points 284 * @throws NullPointerException when points contains a null 285 * @throws DrawRuntimeException in case the number of points is less than 2, size is less than 2, or the Bézier curve 286 * could not be constructed 287 */ 288 public static PolyLine2d bezier(final int size, final Point2d... points) throws NullPointerException, DrawRuntimeException 289 { 290 Throw.when(points.length < 2, DrawRuntimeException.class, "Too few points; need at least two"); 291 Throw.when(size < 2, DrawRuntimeException.class, "size too small (must be at least 2)"); 292 Point2dhtml#Point2d">Point2d[] result = new Point2d[size]; 293 double[] px = new double[points.length]; 294 double[] py = new double[points.length]; 295 for (int i = 0; i < points.length; i++) 296 { 297 Point2d p = points[i]; 298 Throw.whenNull(p, "points contains a null value"); 299 px[i] = p.x; 300 py[i] = p.y; 301 } 302 for (int n = 0; n < size; n++) 303 { 304 double t = n / (size - 1.0); 305 double x = Bn(t, px); 306 double y = Bn(t, py); 307 result[n] = new Point2d(x, y); 308 } 309 return new PolyLine2d(result); 310 } 311 312 /** 313 * Approximate a Bézier curve of degree n using <code>DEFAULT_BEZIER_SIZE</code> points. 314 * @param points Point2d...; the points of the curve, where the first and last are begin and end point, and the intermediate 315 * ones are control points. There should be at least two points. 316 * @return PolyLine2d; an approximation of a cubic Bézier curve between start and end, using the provided control 317 * points 318 * @throws NullPointerException when points contains a null value 319 * @throws DrawRuntimeException in case the number of points is less than 2 or the Bézier curve could not be 320 * constructed 321 */ 322 public static PolyLine2d bezier(final Point2d... points) throws NullPointerException, DrawRuntimeException 323 { 324 return bezier(DEFAULT_BEZIER_SIZE, points); 325 } 326 327 /** 328 * Approximate a Bézier curve of degree n with a specified precision. 329 * @param epsilon double; the precision. 330 * @param points Point2d...; the points of the curve, where the first and last are begin and end point, and the intermediate 331 * ones are control points. There should be at least two points. 332 * @return PolyLine2d; an approximation of a cubic Bézier curve between start and end, with the provided control 333 * points 334 * @throws NullPointerException when points contains a null value 335 * @throws DrawRuntimeException in case the number of points is less than 2 or the Bézier curve could not be 336 * constructed 337 */ 338 public static PolyLine2d bezier(final double epsilon, final Point2d... points) 339 throws NullPointerException, DrawRuntimeException 340 { 341 Throw.when(points.length < 2, DrawRuntimeException.class, "Too few points; need at least two"); 342 Throw.when(Double.isNaN(epsilon) || epsilon <= 0, DrawRuntimeException.class, 343 "epsilonPosition must be a positive number"); 344 NavigableMap<Double, Point2d> result = new TreeMap<>(); 345 double[] px = new double[points.length]; 346 double[] py = new double[points.length]; 347 for (int i = 0; i < points.length; i++) 348 { 349 Point2d p = points[i]; 350 Throw.whenNull(p, "points contains a null value"); 351 px[i] = p.x; 352 py[i] = p.y; 353 } 354 int initialSize = points.length - 1; 355 for (int n = 0; n < initialSize; n++) 356 { 357 double t = n / (initialSize - 1.0); 358 double x = Bn(t, px); 359 double y = Bn(t, py); 360 result.put(t, new Point2d(x, y)); 361 } 362 // Walk along all point pairs and see if additional points need to be inserted 363 Double prevT = result.firstKey(); 364 Point2d prevPoint = result.get(prevT); 365 Map.Entry<Double, Point2d> entry; 366 while ((entry = result.higherEntry(prevT)) != null) 367 { 368 Double nextT = entry.getKey(); 369 Point2d nextPoint = entry.getValue(); 370 double medianT = (prevT + nextT) / 2; 371 double x = Bn(medianT, px); 372 double y = Bn(medianT, py); 373 Point2dl#Point2d">Point2d medianPoint = new Point2d(x, y); 374 Point2d projectedPoint = medianPoint.closestPointOnSegment(prevPoint, nextPoint); 375 double errorPosition = medianPoint.distance(projectedPoint); 376 if (errorPosition >= epsilon) 377 { 378 // We need to insert another point 379 result.put(medianT, medianPoint); 380 continue; 381 } 382 if (prevPoint.distance(nextPoint) > epsilon) 383 { 384 // Check for an inflection point by creating additional points at one quarter and three quarters. If these 385 // are on opposite sides of the line from prevPoint to nextPoint; there must be an inflection point. 386 // https://stackoverflow.com/questions/1560492/how-to-tell-whether-a-point-is-to-the-right-or-left-side-of-a-line 387 double quarterT = (prevT + medianT) / 2; 388 double quarterX = Bn(quarterT, px); 389 double quarterY = Bn(quarterT, py); 390 int sign1 = (int) Math.signum((nextPoint.x - prevPoint.x) * (quarterY - prevPoint.y) 391 - (nextPoint.y - prevPoint.y) * (quarterX - prevPoint.x)); 392 double threeQuarterT = (nextT + medianT) / 2; 393 double threeQuarterX = Bn(threeQuarterT, px); 394 double threeQuarterY = Bn(threeQuarterT, py); 395 int sign2 = (int) Math.signum((nextPoint.x - prevPoint.x) * (threeQuarterY - prevPoint.y) 396 - (nextPoint.y - prevPoint.y) * (threeQuarterX - prevPoint.x)); 397 if (sign1 != sign2) 398 { 399 // There is an inflection point 400 System.out.println("Detected inflection point between " + prevPoint + " and " + nextPoint); 401 // Inserting the halfway point should take care of this 402 result.put(medianT, medianPoint); 403 continue; 404 } 405 } 406 // TODO check angles 407 prevT = nextT; 408 prevPoint = nextPoint; 409 } 410 try 411 { 412 return new PolyLine2d(result.values().iterator()); 413 } 414 catch (NullPointerException | DrawException e) 415 { 416 // Cannot happen? Really? 417 e.printStackTrace(); 418 throw new DrawRuntimeException(e); 419 } 420 } 421 422 /** 423 * Approximate a cubic Bézier curve from start to end with two control points. 424 * @param size int; the number of points for the Bézier curve 425 * @param start Point3d; the start point of the Bézier curve 426 * @param control1 Point3d; the first control point 427 * @param control2 Point3d; the second control point 428 * @param end Point3d; the end point of the Bézier curve 429 * @return PolyLine3d; an approximation of a cubic Bézier curve between start and end, with the two provided control 430 * points 431 * @throws DrawRuntimeException in case the number of points is less than 2 or the Bézier curve could not be 432 * constructed 433 */ 434 public static PolyLine3d cubic(final int size, final Point3dtml#Point3d">Point3dd.html#Point3d">Point3d start, final Point3dtml#Point3d">Point3d control1, final Point3d control2, 435 final Point3d end) throws DrawRuntimeException 436 { 437 return bezier(size, start, control1, control2, end); 438 } 439 440 /** 441 * Approximate a cubic Bézier curve from start to end with two control points with a specified precision. 442 * @param epsilon double; the precision. 443 * @param start Point3d; the start point of the Bézier curve 444 * @param control1 Point3d; the first control point 445 * @param control2 Point3d; the second control point 446 * @param end Point3d; the end point of the Bézier curve 447 * @return PolyLine3d; an approximation of a cubic Bézier curve between start and end, with the two provided control 448 * points 449 * @throws DrawRuntimeException in case the number of points is less than 2 or the Bézier curve could not be 450 * constructed 451 */ 452 public static PolyLine3d cubic(final double epsilon, final Point3dtml#Point3d">Point3dd.html#Point3d">Point3d start, final Point3dtml#Point3d">Point3d control1, final Point3d control2, 453 final Point3d end) throws DrawRuntimeException 454 { 455 return bezier(epsilon, start, control1, control2, end); 456 } 457 458 /** 459 * Approximate a cubic Bézier curve from start to end with two generated control points at half the distance between 460 * start and end. 461 * @param size int; the number of points for the Bézier curve 462 * @param start Ray3d; the start point and start direction of the Bézier curve 463 * @param end Ray3d; the end point and end direction of the Bézier curve 464 * @return PolyLine2d; an approximation of a cubic Bézier curve between start and end, with the two provided control 465 * points 466 * @throws DrawRuntimeException in case the number of points is less than 2 or the Bézier curve could not be 467 * constructed 468 */ 469 public static PolyLine3d cubic(final int size, final Ray3d.html#Ray3d">Ray3d start, final Ray3d end) throws DrawRuntimeException 470 { 471 return cubic(size, start, end, 1.0); 472 } 473 474 /** 475 * Approximate a cubic Bézier curve from start to end with two generated control points at half the distance between 476 * start and end with specified precision. 477 * @param epsilon double; the precision. 478 * @param start Ray3d; the start point and start direction of the Bézier curve 479 * @param end Ray3d; the end point and end direction of the Bézier curve 480 * @return PolyLine2d; an approximation of a cubic Bézier curve between start and end, with the two provided control 481 * points 482 * @throws DrawRuntimeException in case the number of points is less than 2 or the Bézier curve could not be 483 * constructed 484 */ 485 public static PolyLine3d cubic(final double epsilon, final Ray3d.html#Ray3d">Ray3d start, final Ray3d end) throws DrawRuntimeException 486 { 487 return cubic(epsilon, start, end, 1.0); 488 } 489 490 /** 491 * Approximate a cubic Bézier curve from start to end with two generated control points at half the distance between 492 * start and end. 493 * @param size int; the number of points for the Bézier curve 494 * @param start Ray3d; the start point and start direction of the Bézier curve 495 * @param end Ray3d; the end point and end direction of the Bézier curve 496 * @param shape shape factor; 1 = control points at half the distance between start and end, > 1 results in a pointier 497 * shape, < 1 results in a flatter shape, value should be above 0 and finite 498 * @return a cubic Bézier curve between start and end, with the two determined control points 499 * @throws DrawRuntimeException in case the number of points is less than 2 or the Bézier curve could not be 500 * constructed 501 */ 502 public static PolyLine3d cubic(final int size, final Ray3d.html#Ray3d">Ray3d start, final Ray3d end, final double shape) 503 throws DrawRuntimeException 504 { 505 Throw.when(Double.isNaN(shape) || Double.isInfinite(shape) || shape <= 0, DrawRuntimeException.class, 506 "shape must be a finite, positive value"); 507 return cubic(size, start, end, shape, false); 508 } 509 510 /** 511 * Approximate a cubic Bézier curve from start to end with two generated control points at half the distance between 512 * start and end with specified precision. 513 * @param epsilon double; the precision. 514 * @param start Ray3d; the start point and start direction of the Bézier curve 515 * @param end Ray3d; the end point and end direction of the Bézier curve 516 * @param shape shape factor; 1 = control points at half the distance between start and end, > 1 results in a pointier 517 * shape, < 1 results in a flatter shape, value should be above 0 and finite 518 * @return a cubic Bézier curve between start and end, with the two determined control points 519 * @throws DrawRuntimeException in case the number of points is less than 2 or the Bézier curve could not be 520 * constructed 521 */ 522 public static PolyLine3d cubic(final double epsilon, final Ray3d.html#Ray3d">Ray3d start, final Ray3d end, final double shape) 523 throws DrawRuntimeException 524 { 525 Throw.when(Double.isNaN(shape) || Double.isInfinite(shape) || shape <= 0, DrawRuntimeException.class, 526 "shape must be a finite, positive value"); 527 return cubic(epsilon, start, end, shape, false); 528 } 529 530 /** 531 * Approximate a cubic Bézier curve from start to end with two generated control points at half the distance between 532 * start and end. The z-value is interpolated in a linear way. 533 * @param size int; the number of points for the Bézier curve 534 * @param start Ray3d; the start point and start direction of the Bézier curve 535 * @param end Ray3d; the end point and end direction of the Bézier curve 536 * @param shape shape factor; 1 = control points at half the distance between start and end, > 1 results in a pointier 537 * shape, < 1 results in a flatter shape, value should be above 0 538 * @param weighted boolean; control point distance relates to distance to projected point on extended line from other end 539 * @return a cubic Bézier curve between start and end, with the two determined control points 540 * @throws NullPointerException when start or end is null 541 * @throws DrawRuntimeException in case size is less than 2, start is at the same location as end, shape is invalid, or the 542 * Bézier curve could not be constructed 543 */ 544 public static PolyLine3d cubic(final int size, final Ray3d.html#Ray3d">Ray3d start, final Ray3d end, final double shape, 545 final boolean weighted) throws NullPointerException, DrawRuntimeException 546 { 547 Point3d[] points = createControlPoints(start, end, shape, weighted); 548 return cubic(size, points[0], points[1], points[2], points[3]); 549 } 550 551 /** 552 * Approximate a cubic Bézier curve from start to end with two generated control points at half the distance between 553 * start and end with specified precision. 554 * @param epsilon double; the precision. 555 * @param start Ray3d; the start point and start direction of the Bézier curve 556 * @param end Ray3d; the end point and end direction of the Bézier curve 557 * @param shape shape factor; 1 = control points at half the distance between start and end, > 1 results in a pointier 558 * shape, < 1 results in a flatter shape, value should be above 0, finite and not NaN 559 * @param weighted boolean; control point distance relates to distance to projected point on extended line from other end 560 * @return PolyLine3d; an approximation of a cubic Bézier curve between start and end, with the two determined 561 * control points 562 * @throws NullPointerException when start or end is null 563 * @throws DrawRuntimeException in case size is less than 2, start is at the same location as end, shape is invalid, or the 564 * Bézier curve could not be constructed 565 */ 566 public static PolyLine3d cubic(final double epsilon, final Ray3d.html#Ray3d">Ray3d start, final Ray3d end, final double shape, 567 final boolean weighted) throws NullPointerException, DrawRuntimeException 568 { 569 Point3d[] points = createControlPoints(start, end, shape, weighted); 570 return cubic(epsilon, points[0], points[1], points[2], points[3]); 571 } 572 573 /** 574 * Create control points for a cubic Bézier curve defined by two Rays. 575 * @param start Ray3d; the start point (and direction) 576 * @param end Ray3d; the end point (and direction) 577 * @param shape double; the shape; higher values put the generated control points further away from end and result in a 578 * pointier Bézier curve 579 * @param weighted boolean; 580 * @return Point3d[]; an array of four Point3d elements: start, the first control point, the second control point, end. 581 */ 582 private static Point3d[] createControlPoints(final Ray3d.html#Ray3d">Ray3d start, final Ray3d end, final double shape, final boolean weighted) 583 { 584 Throw.whenNull(start, "start point may not be null"); 585 Throw.whenNull(end, "end point may not be null"); 586 Throw.when(start.distanceSquared(end) == 0, DrawRuntimeException.class, 587 "Cannot create control points if start and end points coincide"); 588 Throw.when(Double.isNaN(shape) || shape <= 0 || Double.isInfinite(shape), DrawRuntimeException.class, 589 "shape must be a finite, positive value"); 590 591 Point3d control1; 592 Point3d control2; 593 if (weighted) 594 { 595 // each control point is 'w' * the distance between the end-points away from the respective end point 596 // 'w' is a weight given by the distance from the end point to the extended line of the other end point 597 double distance = shape * start.distance(end); 598 double dStart = start.distance(start.closestPointOnLine(end)); 599 double dEnd = end.distance(end.closestPointOnLine(start)); 600 double wStart = dStart / (dStart + dEnd); 601 double wEnd = dEnd / (dStart + dEnd); 602 control1 = start.getLocation(distance * wStart); 603 control2 = end.getLocationExtended(-distance * wEnd); 604 } 605 else 606 { 607 // each control point is half the distance between the end-points away from the respective end point 608 double distance = shape * start.distance(end) / 2.0; 609 control1 = start.getLocation(distance); 610 control2 = end.getLocationExtended(-distance); 611 } 612 return new Point3d[] {start, control1, control2, end}; 613 } 614 615 /** 616 * Construct a cubic Bézier curve from start to end with two generated control points at half the distance between 617 * start and end. The z-value is interpolated in a linear way. The size of the constructed curve is 618 * <code>DEFAULT_BEZIER_SIZE</code>. TODO change start en end to Ray3d 619 * @param start Ray3d; the start point and orientation of the Bézier curve 620 * @param end Ray3d; the end point and orientation of the Bézier curve 621 * @return a cubic Bézier curve between start and end, with the two provided control points 622 * @throws DrawRuntimeException in case the number of points is less than 2 or the Bézier curve could not be 623 * constructed 624 */ 625 public static PolyLine3d cubic(final Ray3d.html#Ray3d">Ray3d start, final Ray3d end) throws DrawRuntimeException 626 { 627 return cubic(DEFAULT_BEZIER_SIZE, start, end); 628 } 629 630 /** 631 * Calculate the cubic Bézier point with B(t) = (1 - t)<sup>3</sup>P<sub>0</sub> + 3t(1 - t)<sup>2</sup> 632 * P<sub>1</sub> + 3t<sup>2</sup> (1 - t) P<sub>2</sub> + t<sup>3</sup> P<sub>3</sub>. 633 * @param t double; the fraction 634 * @param p0 double; the first point of the curve 635 * @param p1 double; the first control point 636 * @param p2 double; the second control point 637 * @param p3 double; the end point of the curve 638 * @return the cubic bezier value B(t) 639 */ 640 @SuppressWarnings("checkstyle:methodname") 641 private static double B3(final double t, final double p0, final double p1, final double p2, final double p3) 642 { 643 double t2 = t * t; 644 double t3 = t2 * t; 645 double m = (1.0 - t); 646 double m2 = m * m; 647 double m3 = m2 * m; 648 return m3 * p0 + 3.0 * t * m2 * p1 + 3.0 * t2 * m * p2 + t3 * p3; 649 } 650 651 /** 652 * Construct a Bézier curve of degree n. 653 * @param size int; the number of points for the Bézier curve to be constructed 654 * @param points Point3d...; the points of the curve, where the first and last are begin and end point, and the intermediate 655 * ones are control points. There should be at least two points. 656 * @return the Bézier value B(t) of degree n, where n is the number of points in the array 657 * @throws DrawRuntimeException in case the number of points is less than 2 or the Bézier curve could not be 658 * constructed 659 */ 660 public static PolyLine3d bezier(final int size, final Point3d... points) throws DrawRuntimeException 661 { 662 Throw.when(points.length < 2, DrawRuntimeException.class, "Too few points; need at least two"); 663 Throw.when(size < 2, DrawRuntimeException.class, "size too small (must be at least 2)"); 664 Point3dhtml#Point3d">Point3d[] result = new Point3d[size]; 665 double[] px = new double[points.length]; 666 double[] py = new double[points.length]; 667 double[] pz = new double[points.length]; 668 for (int i = 0; i < points.length; i++) 669 { 670 px[i] = points[i].x; 671 py[i] = points[i].y; 672 pz[i] = points[i].z; 673 } 674 for (int n = 0; n < size; n++) 675 { 676 double t = n / (size - 1.0); 677 double x = Bn(t, px); 678 double y = Bn(t, py); 679 double z = Bn(t, pz); 680 result[n] = new Point3d(x, y, z); 681 } 682 return new PolyLine3d(result); 683 } 684 685 /** 686 * Approximate a Bézier curve of degree n using <code>DEFAULT_BEZIER_SIZE</code> points. 687 * @param points Point3d...; the points of the curve, where the first and last are begin and end point, and the intermediate 688 * ones are control points. There should be at least two points. 689 * @return the Bézier value B(t) of degree n, where n is the number of points in the array 690 * @throws DrawRuntimeException in case the number of points is less than 2 or the Bézier curve could not be 691 * constructed 692 */ 693 public static PolyLine3d bezier(final Point3d... points) throws DrawRuntimeException 694 { 695 return bezier(DEFAULT_BEZIER_SIZE, points); 696 } 697 698 /** 699 * Approximate a Bézier curve of degree n with a specified precision. 700 * @param epsilon double; the precision. 701 * @param points Point3d...; the points of the curve, where the first and last are begin and end point, and the intermediate 702 * ones are control points. There should be at least two points. 703 * @return PolyLine3d; an approximation of a cubic Bézier curve between start and end, with the provided control 704 * points 705 * @throws NullPointerException when points contains a null value 706 * @throws DrawRuntimeException in case the number of points is less than 2 or the Bézier curve could not be 707 * constructed 708 */ 709 public static PolyLine3d bezier(final double epsilon, final Point3d... points) 710 throws NullPointerException, DrawRuntimeException 711 { 712 Throw.when(points.length < 2, DrawRuntimeException.class, "Too few points; need at least two"); 713 Throw.when(Double.isNaN(epsilon) || epsilon <= 0, DrawRuntimeException.class, 714 "epsilonPosition must be a positive number"); 715 NavigableMap<Double, Point3d> result = new TreeMap<>(); 716 double[] px = new double[points.length]; 717 double[] py = new double[points.length]; 718 double[] pz = new double[points.length]; 719 for (int i = 0; i < points.length; i++) 720 { 721 Point3d p = points[i]; 722 Throw.whenNull(p, "points contains a null value"); 723 px[i] = p.x; 724 py[i] = p.y; 725 pz[i] = p.z; 726 } 727 int initialSize = points.length - 1; 728 for (int n = 0; n < initialSize; n++) 729 { 730 double t = n / (initialSize - 1.0); 731 double x = Bn(t, px); 732 double y = Bn(t, py); 733 double z = Bn(t, pz); 734 result.put(t, new Point3d(x, y, z)); 735 } 736 // Walk along all point pairs and see if additional points need to be inserted 737 Double prevT = result.firstKey(); 738 Point3d prevPoint = result.get(prevT); 739 Map.Entry<Double, Point3d> entry; 740 while ((entry = result.higherEntry(prevT)) != null) 741 { 742 Double nextT = entry.getKey(); 743 Point3d nextPoint = entry.getValue(); 744 double medianT = (prevT + nextT) / 2; 745 double x = Bn(medianT, px); 746 double y = Bn(medianT, py); 747 double z = Bn(medianT, pz); 748 Point3dl#Point3d">Point3d medianPoint = new Point3d(x, y, z); 749 Point3d projectedPoint = medianPoint.closestPointOnSegment(prevPoint, nextPoint); 750 double errorPosition = medianPoint.distance(projectedPoint); 751 if (errorPosition >= epsilon) 752 { 753 // We need to insert another point 754 result.put(medianT, medianPoint); 755 continue; 756 } 757 if (prevPoint.distance(nextPoint) > epsilon) 758 { 759 // Check for an inflection point by creating additional points at one quarter and three quarters. If these 760 // are on opposite sides of the line from prevPoint to nextPoint; there must be an inflection point. 761 // https://stackoverflow.com/questions/1560492/how-to-tell-whether-a-point-is-to-the-right-or-left-side-of-a-line 762 double quarterT = (prevT + medianT) / 2; 763 double quarterX = Bn(quarterT, px); 764 double quarterY = Bn(quarterT, py); 765 int sign1 = (int) Math.signum((nextPoint.x - prevPoint.x) * (quarterY - prevPoint.y) 766 - (nextPoint.y - prevPoint.y) * (quarterX - prevPoint.x)); 767 double threeQuarterT = (nextT + medianT) / 2; 768 double threeQuarterX = Bn(threeQuarterT, px); 769 double threeQuarterY = Bn(threeQuarterT, py); 770 int sign2 = (int) Math.signum((nextPoint.x - prevPoint.x) * (threeQuarterY - prevPoint.y) 771 - (nextPoint.y - prevPoint.y) * (threeQuarterX - prevPoint.x)); 772 if (sign1 != sign2) 773 { 774 // There is an inflection point 775 System.out.println("Detected inflection point between " + prevPoint + " and " + nextPoint); 776 // Inserting the halfway point should take care of this 777 result.put(medianT, medianPoint); 778 continue; 779 } 780 } 781 // TODO check angles 782 prevT = nextT; 783 prevPoint = nextPoint; 784 } 785 try 786 { 787 return new PolyLine3d(result.values().iterator()); 788 } 789 catch (NullPointerException | DrawException e) 790 { 791 // Cannot happen? Really? 792 e.printStackTrace(); 793 throw new DrawRuntimeException(e); 794 } 795 } 796 797 /** 798 * 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> 799 * P<sub>i</sub>], where C(n, k) is the binomial coefficient defined by n! / ( k! (n-k)! ), ! being the factorial operator. 800 * @param t double; the fraction 801 * @param p double...; the points of the curve, where the first and last are begin and end point, and the intermediate ones 802 * are control points 803 * @return the Bézier value B(t) of degree n, where n is the number of points in the array 804 */ 805 @SuppressWarnings("checkstyle:methodname") 806 private static double Bn(final double t, final double... p) 807 { 808 double b = 0.0; 809 double m = (1.0 - t); 810 int n = p.length - 1; 811 double fn = factorial(n); 812 for (int i = 0; i <= n; i++) 813 { 814 double c = fn / (factorial(i) * (factorial(n - i))); 815 b += c * Math.pow(m, n - i) * Math.pow(t, i) * p[i]; 816 } 817 return b; 818 } 819 820 /** 821 * Calculate factorial(k), which is k * (k-1) * (k-2) * ... * 1. For factorials up to 20, a lookup table is used. 822 * @param k int; the parameter 823 * @return factorial(k) 824 */ 825 private static double factorial(final int k) 826 { 827 if (k < fact.length) 828 { 829 return fact[k]; 830 } 831 double f = 1; 832 for (int i = 2; i <= k; i++) 833 { 834 f = f * i; 835 } 836 return f; 837 } 838 839 }