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