1# -*- coding: utf-8 -*- 2"""fontTools.misc.bezierTools.py -- tools for working with Bezier path segments. 3""" 4 5from fontTools.misc.arrayTools import calcBounds, sectRect, rectArea 6from fontTools.misc.transform import Identity 7import math 8from collections import namedtuple 9 10Intersection = namedtuple("Intersection", ["pt", "t1", "t2"]) 11 12 13__all__ = [ 14 "approximateCubicArcLength", 15 "approximateCubicArcLengthC", 16 "approximateQuadraticArcLength", 17 "approximateQuadraticArcLengthC", 18 "calcCubicArcLength", 19 "calcCubicArcLengthC", 20 "calcQuadraticArcLength", 21 "calcQuadraticArcLengthC", 22 "calcCubicBounds", 23 "calcQuadraticBounds", 24 "splitLine", 25 "splitQuadratic", 26 "splitCubic", 27 "splitQuadraticAtT", 28 "splitCubicAtT", 29 "solveQuadratic", 30 "solveCubic", 31 "quadraticPointAtT", 32 "cubicPointAtT", 33 "linePointAtT", 34 "segmentPointAtT", 35 "lineLineIntersections", 36 "curveLineIntersections", 37 "curveCurveIntersections", 38 "segmentSegmentIntersections", 39] 40 41 42def calcCubicArcLength(pt1, pt2, pt3, pt4, tolerance=0.005): 43 """Calculates the arc length for a cubic Bezier segment. 44 45 Whereas :func:`approximateCubicArcLength` approximates the length, this 46 function calculates it by "measuring", recursively dividing the curve 47 until the divided segments are shorter than ``tolerance``. 48 49 Args: 50 pt1,pt2,pt3,pt4: Control points of the Bezier as 2D tuples. 51 tolerance: Controls the precision of the calcuation. 52 53 Returns: 54 Arc length value. 55 """ 56 return calcCubicArcLengthC( 57 complex(*pt1), complex(*pt2), complex(*pt3), complex(*pt4), tolerance 58 ) 59 60 61def _split_cubic_into_two(p0, p1, p2, p3): 62 mid = (p0 + 3 * (p1 + p2) + p3) * 0.125 63 deriv3 = (p3 + p2 - p1 - p0) * 0.125 64 return ( 65 (p0, (p0 + p1) * 0.5, mid - deriv3, mid), 66 (mid, mid + deriv3, (p2 + p3) * 0.5, p3), 67 ) 68 69 70def _calcCubicArcLengthCRecurse(mult, p0, p1, p2, p3): 71 arch = abs(p0 - p3) 72 box = abs(p0 - p1) + abs(p1 - p2) + abs(p2 - p3) 73 if arch * mult >= box: 74 return (arch + box) * 0.5 75 else: 76 one, two = _split_cubic_into_two(p0, p1, p2, p3) 77 return _calcCubicArcLengthCRecurse(mult, *one) + _calcCubicArcLengthCRecurse( 78 mult, *two 79 ) 80 81 82def calcCubicArcLengthC(pt1, pt2, pt3, pt4, tolerance=0.005): 83 """Calculates the arc length for a cubic Bezier segment. 84 85 Args: 86 pt1,pt2,pt3,pt4: Control points of the Bezier as complex numbers. 87 tolerance: Controls the precision of the calcuation. 88 89 Returns: 90 Arc length value. 91 """ 92 mult = 1.0 + 1.5 * tolerance # The 1.5 is a empirical hack; no math 93 return _calcCubicArcLengthCRecurse(mult, pt1, pt2, pt3, pt4) 94 95 96epsilonDigits = 6 97epsilon = 1e-10 98 99 100def _dot(v1, v2): 101 return (v1 * v2.conjugate()).real 102 103 104def _intSecAtan(x): 105 # In : sympy.integrate(sp.sec(sp.atan(x))) 106 # Out: x*sqrt(x**2 + 1)/2 + asinh(x)/2 107 return x * math.sqrt(x ** 2 + 1) / 2 + math.asinh(x) / 2 108 109 110def calcQuadraticArcLength(pt1, pt2, pt3): 111 """Calculates the arc length for a quadratic Bezier segment. 112 113 Args: 114 pt1: Start point of the Bezier as 2D tuple. 115 pt2: Handle point of the Bezier as 2D tuple. 116 pt3: End point of the Bezier as 2D tuple. 117 118 Returns: 119 Arc length value. 120 121 Example:: 122 123 >>> calcQuadraticArcLength((0, 0), (0, 0), (0, 0)) # empty segment 124 0.0 125 >>> calcQuadraticArcLength((0, 0), (50, 0), (80, 0)) # collinear points 126 80.0 127 >>> calcQuadraticArcLength((0, 0), (0, 50), (0, 80)) # collinear points vertical 128 80.0 129 >>> calcQuadraticArcLength((0, 0), (50, 20), (100, 40)) # collinear points 130 107.70329614269008 131 >>> calcQuadraticArcLength((0, 0), (0, 100), (100, 0)) 132 154.02976155645263 133 >>> calcQuadraticArcLength((0, 0), (0, 50), (100, 0)) 134 120.21581243984076 135 >>> calcQuadraticArcLength((0, 0), (50, -10), (80, 50)) 136 102.53273816445825 137 >>> calcQuadraticArcLength((0, 0), (40, 0), (-40, 0)) # collinear points, control point outside 138 66.66666666666667 139 >>> calcQuadraticArcLength((0, 0), (40, 0), (0, 0)) # collinear points, looping back 140 40.0 141 """ 142 return calcQuadraticArcLengthC(complex(*pt1), complex(*pt2), complex(*pt3)) 143 144 145def calcQuadraticArcLengthC(pt1, pt2, pt3): 146 """Calculates the arc length for a quadratic Bezier segment. 147 148 Args: 149 pt1: Start point of the Bezier as a complex number. 150 pt2: Handle point of the Bezier as a complex number. 151 pt3: End point of the Bezier as a complex number. 152 153 Returns: 154 Arc length value. 155 """ 156 # Analytical solution to the length of a quadratic bezier. 157 # I'll explain how I arrived at this later. 158 d0 = pt2 - pt1 159 d1 = pt3 - pt2 160 d = d1 - d0 161 n = d * 1j 162 scale = abs(n) 163 if scale == 0.0: 164 return abs(pt3 - pt1) 165 origDist = _dot(n, d0) 166 if abs(origDist) < epsilon: 167 if _dot(d0, d1) >= 0: 168 return abs(pt3 - pt1) 169 a, b = abs(d0), abs(d1) 170 return (a * a + b * b) / (a + b) 171 x0 = _dot(d, d0) / origDist 172 x1 = _dot(d, d1) / origDist 173 Len = abs(2 * (_intSecAtan(x1) - _intSecAtan(x0)) * origDist / (scale * (x1 - x0))) 174 return Len 175 176 177def approximateQuadraticArcLength(pt1, pt2, pt3): 178 """Calculates the arc length for a quadratic Bezier segment. 179 180 Uses Gauss-Legendre quadrature for a branch-free approximation. 181 See :func:`calcQuadraticArcLength` for a slower but more accurate result. 182 183 Args: 184 pt1: Start point of the Bezier as 2D tuple. 185 pt2: Handle point of the Bezier as 2D tuple. 186 pt3: End point of the Bezier as 2D tuple. 187 188 Returns: 189 Approximate arc length value. 190 """ 191 return approximateQuadraticArcLengthC(complex(*pt1), complex(*pt2), complex(*pt3)) 192 193 194def approximateQuadraticArcLengthC(pt1, pt2, pt3): 195 """Calculates the arc length for a quadratic Bezier segment. 196 197 Uses Gauss-Legendre quadrature for a branch-free approximation. 198 See :func:`calcQuadraticArcLength` for a slower but more accurate result. 199 200 Args: 201 pt1: Start point of the Bezier as a complex number. 202 pt2: Handle point of the Bezier as a complex number. 203 pt3: End point of the Bezier as a complex number. 204 205 Returns: 206 Approximate arc length value. 207 """ 208 # This, essentially, approximates the length-of-derivative function 209 # to be integrated with the best-matching fifth-degree polynomial 210 # approximation of it. 211 # 212 # https://en.wikipedia.org/wiki/Gaussian_quadrature#Gauss.E2.80.93Legendre_quadrature 213 214 # abs(BezierCurveC[2].diff(t).subs({t:T})) for T in sorted(.5, .5±sqrt(3/5)/2), 215 # weighted 5/18, 8/18, 5/18 respectively. 216 v0 = abs( 217 -0.492943519233745 * pt1 + 0.430331482911935 * pt2 + 0.0626120363218102 * pt3 218 ) 219 v1 = abs(pt3 - pt1) * 0.4444444444444444 220 v2 = abs( 221 -0.0626120363218102 * pt1 - 0.430331482911935 * pt2 + 0.492943519233745 * pt3 222 ) 223 224 return v0 + v1 + v2 225 226 227def calcQuadraticBounds(pt1, pt2, pt3): 228 """Calculates the bounding rectangle for a quadratic Bezier segment. 229 230 Args: 231 pt1: Start point of the Bezier as a 2D tuple. 232 pt2: Handle point of the Bezier as a 2D tuple. 233 pt3: End point of the Bezier as a 2D tuple. 234 235 Returns: 236 A four-item tuple representing the bounding rectangle ``(xMin, yMin, xMax, yMax)``. 237 238 Example:: 239 240 >>> calcQuadraticBounds((0, 0), (50, 100), (100, 0)) 241 (0, 0, 100, 50.0) 242 >>> calcQuadraticBounds((0, 0), (100, 0), (100, 100)) 243 (0.0, 0.0, 100, 100) 244 """ 245 (ax, ay), (bx, by), (cx, cy) = calcQuadraticParameters(pt1, pt2, pt3) 246 ax2 = ax * 2.0 247 ay2 = ay * 2.0 248 roots = [] 249 if ax2 != 0: 250 roots.append(-bx / ax2) 251 if ay2 != 0: 252 roots.append(-by / ay2) 253 points = [ 254 (ax * t * t + bx * t + cx, ay * t * t + by * t + cy) 255 for t in roots 256 if 0 <= t < 1 257 ] + [pt1, pt3] 258 return calcBounds(points) 259 260 261def approximateCubicArcLength(pt1, pt2, pt3, pt4): 262 """Approximates the arc length for a cubic Bezier segment. 263 264 Uses Gauss-Lobatto quadrature with n=5 points to approximate arc length. 265 See :func:`calcCubicArcLength` for a slower but more accurate result. 266 267 Args: 268 pt1,pt2,pt3,pt4: Control points of the Bezier as 2D tuples. 269 270 Returns: 271 Arc length value. 272 273 Example:: 274 275 >>> approximateCubicArcLength((0, 0), (25, 100), (75, 100), (100, 0)) 276 190.04332968932817 277 >>> approximateCubicArcLength((0, 0), (50, 0), (100, 50), (100, 100)) 278 154.8852074945903 279 >>> approximateCubicArcLength((0, 0), (50, 0), (100, 0), (150, 0)) # line; exact result should be 150. 280 149.99999999999991 281 >>> approximateCubicArcLength((0, 0), (50, 0), (100, 0), (-50, 0)) # cusp; exact result should be 150. 282 136.9267662156362 283 >>> approximateCubicArcLength((0, 0), (50, 0), (100, -50), (-50, 0)) # cusp 284 154.80848416537057 285 """ 286 return approximateCubicArcLengthC( 287 complex(*pt1), complex(*pt2), complex(*pt3), complex(*pt4) 288 ) 289 290 291def approximateCubicArcLengthC(pt1, pt2, pt3, pt4): 292 """Approximates the arc length for a cubic Bezier segment. 293 294 Args: 295 pt1,pt2,pt3,pt4: Control points of the Bezier as complex numbers. 296 297 Returns: 298 Arc length value. 299 """ 300 # This, essentially, approximates the length-of-derivative function 301 # to be integrated with the best-matching seventh-degree polynomial 302 # approximation of it. 303 # 304 # https://en.wikipedia.org/wiki/Gaussian_quadrature#Gauss.E2.80.93Lobatto_rules 305 306 # abs(BezierCurveC[3].diff(t).subs({t:T})) for T in sorted(0, .5±(3/7)**.5/2, .5, 1), 307 # weighted 1/20, 49/180, 32/90, 49/180, 1/20 respectively. 308 v0 = abs(pt2 - pt1) * 0.15 309 v1 = abs( 310 -0.558983582205757 * pt1 311 + 0.325650248872424 * pt2 312 + 0.208983582205757 * pt3 313 + 0.024349751127576 * pt4 314 ) 315 v2 = abs(pt4 - pt1 + pt3 - pt2) * 0.26666666666666666 316 v3 = abs( 317 -0.024349751127576 * pt1 318 - 0.208983582205757 * pt2 319 - 0.325650248872424 * pt3 320 + 0.558983582205757 * pt4 321 ) 322 v4 = abs(pt4 - pt3) * 0.15 323 324 return v0 + v1 + v2 + v3 + v4 325 326 327def calcCubicBounds(pt1, pt2, pt3, pt4): 328 """Calculates the bounding rectangle for a quadratic Bezier segment. 329 330 Args: 331 pt1,pt2,pt3,pt4: Control points of the Bezier as 2D tuples. 332 333 Returns: 334 A four-item tuple representing the bounding rectangle ``(xMin, yMin, xMax, yMax)``. 335 336 Example:: 337 338 >>> calcCubicBounds((0, 0), (25, 100), (75, 100), (100, 0)) 339 (0, 0, 100, 75.0) 340 >>> calcCubicBounds((0, 0), (50, 0), (100, 50), (100, 100)) 341 (0.0, 0.0, 100, 100) 342 >>> print("%f %f %f %f" % calcCubicBounds((50, 0), (0, 100), (100, 100), (50, 0))) 343 35.566243 0.000000 64.433757 75.000000 344 """ 345 (ax, ay), (bx, by), (cx, cy), (dx, dy) = calcCubicParameters(pt1, pt2, pt3, pt4) 346 # calc first derivative 347 ax3 = ax * 3.0 348 ay3 = ay * 3.0 349 bx2 = bx * 2.0 350 by2 = by * 2.0 351 xRoots = [t for t in solveQuadratic(ax3, bx2, cx) if 0 <= t < 1] 352 yRoots = [t for t in solveQuadratic(ay3, by2, cy) if 0 <= t < 1] 353 roots = xRoots + yRoots 354 355 points = [ 356 ( 357 ax * t * t * t + bx * t * t + cx * t + dx, 358 ay * t * t * t + by * t * t + cy * t + dy, 359 ) 360 for t in roots 361 ] + [pt1, pt4] 362 return calcBounds(points) 363 364 365def splitLine(pt1, pt2, where, isHorizontal): 366 """Split a line at a given coordinate. 367 368 Args: 369 pt1: Start point of line as 2D tuple. 370 pt2: End point of line as 2D tuple. 371 where: Position at which to split the line. 372 isHorizontal: Direction of the ray splitting the line. If true, 373 ``where`` is interpreted as a Y coordinate; if false, then 374 ``where`` is interpreted as an X coordinate. 375 376 Returns: 377 A list of two line segments (each line segment being two 2D tuples) 378 if the line was successfully split, or a list containing the original 379 line. 380 381 Example:: 382 383 >>> printSegments(splitLine((0, 0), (100, 100), 50, True)) 384 ((0, 0), (50, 50)) 385 ((50, 50), (100, 100)) 386 >>> printSegments(splitLine((0, 0), (100, 100), 100, True)) 387 ((0, 0), (100, 100)) 388 >>> printSegments(splitLine((0, 0), (100, 100), 0, True)) 389 ((0, 0), (0, 0)) 390 ((0, 0), (100, 100)) 391 >>> printSegments(splitLine((0, 0), (100, 100), 0, False)) 392 ((0, 0), (0, 0)) 393 ((0, 0), (100, 100)) 394 >>> printSegments(splitLine((100, 0), (0, 0), 50, False)) 395 ((100, 0), (50, 0)) 396 ((50, 0), (0, 0)) 397 >>> printSegments(splitLine((0, 100), (0, 0), 50, True)) 398 ((0, 100), (0, 50)) 399 ((0, 50), (0, 0)) 400 """ 401 pt1x, pt1y = pt1 402 pt2x, pt2y = pt2 403 404 ax = pt2x - pt1x 405 ay = pt2y - pt1y 406 407 bx = pt1x 408 by = pt1y 409 410 a = (ax, ay)[isHorizontal] 411 412 if a == 0: 413 return [(pt1, pt2)] 414 t = (where - (bx, by)[isHorizontal]) / a 415 if 0 <= t < 1: 416 midPt = ax * t + bx, ay * t + by 417 return [(pt1, midPt), (midPt, pt2)] 418 else: 419 return [(pt1, pt2)] 420 421 422def splitQuadratic(pt1, pt2, pt3, where, isHorizontal): 423 """Split a quadratic Bezier curve at a given coordinate. 424 425 Args: 426 pt1,pt2,pt3: Control points of the Bezier as 2D tuples. 427 where: Position at which to split the curve. 428 isHorizontal: Direction of the ray splitting the curve. If true, 429 ``where`` is interpreted as a Y coordinate; if false, then 430 ``where`` is interpreted as an X coordinate. 431 432 Returns: 433 A list of two curve segments (each curve segment being three 2D tuples) 434 if the curve was successfully split, or a list containing the original 435 curve. 436 437 Example:: 438 439 >>> printSegments(splitQuadratic((0, 0), (50, 100), (100, 0), 150, False)) 440 ((0, 0), (50, 100), (100, 0)) 441 >>> printSegments(splitQuadratic((0, 0), (50, 100), (100, 0), 50, False)) 442 ((0, 0), (25, 50), (50, 50)) 443 ((50, 50), (75, 50), (100, 0)) 444 >>> printSegments(splitQuadratic((0, 0), (50, 100), (100, 0), 25, False)) 445 ((0, 0), (12.5, 25), (25, 37.5)) 446 ((25, 37.5), (62.5, 75), (100, 0)) 447 >>> printSegments(splitQuadratic((0, 0), (50, 100), (100, 0), 25, True)) 448 ((0, 0), (7.32233, 14.6447), (14.6447, 25)) 449 ((14.6447, 25), (50, 75), (85.3553, 25)) 450 ((85.3553, 25), (92.6777, 14.6447), (100, -7.10543e-15)) 451 >>> # XXX I'm not at all sure if the following behavior is desirable: 452 >>> printSegments(splitQuadratic((0, 0), (50, 100), (100, 0), 50, True)) 453 ((0, 0), (25, 50), (50, 50)) 454 ((50, 50), (50, 50), (50, 50)) 455 ((50, 50), (75, 50), (100, 0)) 456 """ 457 a, b, c = calcQuadraticParameters(pt1, pt2, pt3) 458 solutions = solveQuadratic( 459 a[isHorizontal], b[isHorizontal], c[isHorizontal] - where 460 ) 461 solutions = sorted(t for t in solutions if 0 <= t < 1) 462 if not solutions: 463 return [(pt1, pt2, pt3)] 464 return _splitQuadraticAtT(a, b, c, *solutions) 465 466 467def splitCubic(pt1, pt2, pt3, pt4, where, isHorizontal): 468 """Split a cubic Bezier curve at a given coordinate. 469 470 Args: 471 pt1,pt2,pt3,pt4: Control points of the Bezier as 2D tuples. 472 where: Position at which to split the curve. 473 isHorizontal: Direction of the ray splitting the curve. If true, 474 ``where`` is interpreted as a Y coordinate; if false, then 475 ``where`` is interpreted as an X coordinate. 476 477 Returns: 478 A list of two curve segments (each curve segment being four 2D tuples) 479 if the curve was successfully split, or a list containing the original 480 curve. 481 482 Example:: 483 484 >>> printSegments(splitCubic((0, 0), (25, 100), (75, 100), (100, 0), 150, False)) 485 ((0, 0), (25, 100), (75, 100), (100, 0)) 486 >>> printSegments(splitCubic((0, 0), (25, 100), (75, 100), (100, 0), 50, False)) 487 ((0, 0), (12.5, 50), (31.25, 75), (50, 75)) 488 ((50, 75), (68.75, 75), (87.5, 50), (100, 0)) 489 >>> printSegments(splitCubic((0, 0), (25, 100), (75, 100), (100, 0), 25, True)) 490 ((0, 0), (2.29379, 9.17517), (4.79804, 17.5085), (7.47414, 25)) 491 ((7.47414, 25), (31.2886, 91.6667), (68.7114, 91.6667), (92.5259, 25)) 492 ((92.5259, 25), (95.202, 17.5085), (97.7062, 9.17517), (100, 1.77636e-15)) 493 """ 494 a, b, c, d = calcCubicParameters(pt1, pt2, pt3, pt4) 495 solutions = solveCubic( 496 a[isHorizontal], b[isHorizontal], c[isHorizontal], d[isHorizontal] - where 497 ) 498 solutions = sorted(t for t in solutions if 0 <= t < 1) 499 if not solutions: 500 return [(pt1, pt2, pt3, pt4)] 501 return _splitCubicAtT(a, b, c, d, *solutions) 502 503 504def splitQuadraticAtT(pt1, pt2, pt3, *ts): 505 """Split a quadratic Bezier curve at one or more values of t. 506 507 Args: 508 pt1,pt2,pt3: Control points of the Bezier as 2D tuples. 509 *ts: Positions at which to split the curve. 510 511 Returns: 512 A list of curve segments (each curve segment being three 2D tuples). 513 514 Examples:: 515 516 >>> printSegments(splitQuadraticAtT((0, 0), (50, 100), (100, 0), 0.5)) 517 ((0, 0), (25, 50), (50, 50)) 518 ((50, 50), (75, 50), (100, 0)) 519 >>> printSegments(splitQuadraticAtT((0, 0), (50, 100), (100, 0), 0.5, 0.75)) 520 ((0, 0), (25, 50), (50, 50)) 521 ((50, 50), (62.5, 50), (75, 37.5)) 522 ((75, 37.5), (87.5, 25), (100, 0)) 523 """ 524 a, b, c = calcQuadraticParameters(pt1, pt2, pt3) 525 return _splitQuadraticAtT(a, b, c, *ts) 526 527 528def splitCubicAtT(pt1, pt2, pt3, pt4, *ts): 529 """Split a cubic Bezier curve at one or more values of t. 530 531 Args: 532 pt1,pt2,pt3,pt4: Control points of the Bezier as 2D tuples. 533 *ts: Positions at which to split the curve. 534 535 Returns: 536 A list of curve segments (each curve segment being four 2D tuples). 537 538 Examples:: 539 540 >>> printSegments(splitCubicAtT((0, 0), (25, 100), (75, 100), (100, 0), 0.5)) 541 ((0, 0), (12.5, 50), (31.25, 75), (50, 75)) 542 ((50, 75), (68.75, 75), (87.5, 50), (100, 0)) 543 >>> printSegments(splitCubicAtT((0, 0), (25, 100), (75, 100), (100, 0), 0.5, 0.75)) 544 ((0, 0), (12.5, 50), (31.25, 75), (50, 75)) 545 ((50, 75), (59.375, 75), (68.75, 68.75), (77.3438, 56.25)) 546 ((77.3438, 56.25), (85.9375, 43.75), (93.75, 25), (100, 0)) 547 """ 548 a, b, c, d = calcCubicParameters(pt1, pt2, pt3, pt4) 549 return _splitCubicAtT(a, b, c, d, *ts) 550 551 552def _splitQuadraticAtT(a, b, c, *ts): 553 ts = list(ts) 554 segments = [] 555 ts.insert(0, 0.0) 556 ts.append(1.0) 557 ax, ay = a 558 bx, by = b 559 cx, cy = c 560 for i in range(len(ts) - 1): 561 t1 = ts[i] 562 t2 = ts[i + 1] 563 delta = t2 - t1 564 # calc new a, b and c 565 delta_2 = delta * delta 566 a1x = ax * delta_2 567 a1y = ay * delta_2 568 b1x = (2 * ax * t1 + bx) * delta 569 b1y = (2 * ay * t1 + by) * delta 570 t1_2 = t1 * t1 571 c1x = ax * t1_2 + bx * t1 + cx 572 c1y = ay * t1_2 + by * t1 + cy 573 574 pt1, pt2, pt3 = calcQuadraticPoints((a1x, a1y), (b1x, b1y), (c1x, c1y)) 575 segments.append((pt1, pt2, pt3)) 576 return segments 577 578 579def _splitCubicAtT(a, b, c, d, *ts): 580 ts = list(ts) 581 ts.insert(0, 0.0) 582 ts.append(1.0) 583 segments = [] 584 ax, ay = a 585 bx, by = b 586 cx, cy = c 587 dx, dy = d 588 for i in range(len(ts) - 1): 589 t1 = ts[i] 590 t2 = ts[i + 1] 591 delta = t2 - t1 592 593 delta_2 = delta * delta 594 delta_3 = delta * delta_2 595 t1_2 = t1 * t1 596 t1_3 = t1 * t1_2 597 598 # calc new a, b, c and d 599 a1x = ax * delta_3 600 a1y = ay * delta_3 601 b1x = (3 * ax * t1 + bx) * delta_2 602 b1y = (3 * ay * t1 + by) * delta_2 603 c1x = (2 * bx * t1 + cx + 3 * ax * t1_2) * delta 604 c1y = (2 * by * t1 + cy + 3 * ay * t1_2) * delta 605 d1x = ax * t1_3 + bx * t1_2 + cx * t1 + dx 606 d1y = ay * t1_3 + by * t1_2 + cy * t1 + dy 607 pt1, pt2, pt3, pt4 = calcCubicPoints( 608 (a1x, a1y), (b1x, b1y), (c1x, c1y), (d1x, d1y) 609 ) 610 segments.append((pt1, pt2, pt3, pt4)) 611 return segments 612 613 614# 615# Equation solvers. 616# 617 618from math import sqrt, acos, cos, pi 619 620 621def solveQuadratic(a, b, c, sqrt=sqrt): 622 """Solve a quadratic equation. 623 624 Solves *a*x*x + b*x + c = 0* where a, b and c are real. 625 626 Args: 627 a: coefficient of *x²* 628 b: coefficient of *x* 629 c: constant term 630 631 Returns: 632 A list of roots. Note that the returned list is neither guaranteed to 633 be sorted nor to contain unique values! 634 """ 635 if abs(a) < epsilon: 636 if abs(b) < epsilon: 637 # We have a non-equation; therefore, we have no valid solution 638 roots = [] 639 else: 640 # We have a linear equation with 1 root. 641 roots = [-c / b] 642 else: 643 # We have a true quadratic equation. Apply the quadratic formula to find two roots. 644 DD = b * b - 4.0 * a * c 645 if DD >= 0.0: 646 rDD = sqrt(DD) 647 roots = [(-b + rDD) / 2.0 / a, (-b - rDD) / 2.0 / a] 648 else: 649 # complex roots, ignore 650 roots = [] 651 return roots 652 653 654def solveCubic(a, b, c, d): 655 """Solve a cubic equation. 656 657 Solves *a*x*x*x + b*x*x + c*x + d = 0* where a, b, c and d are real. 658 659 Args: 660 a: coefficient of *x³* 661 b: coefficient of *x²* 662 c: coefficient of *x* 663 d: constant term 664 665 Returns: 666 A list of roots. Note that the returned list is neither guaranteed to 667 be sorted nor to contain unique values! 668 669 Examples:: 670 671 >>> solveCubic(1, 1, -6, 0) 672 [-3.0, -0.0, 2.0] 673 >>> solveCubic(-10.0, -9.0, 48.0, -29.0) 674 [-2.9, 1.0, 1.0] 675 >>> solveCubic(-9.875, -9.0, 47.625, -28.75) 676 [-2.911392, 1.0, 1.0] 677 >>> solveCubic(1.0, -4.5, 6.75, -3.375) 678 [1.5, 1.5, 1.5] 679 >>> solveCubic(-12.0, 18.0, -9.0, 1.50023651123) 680 [0.5, 0.5, 0.5] 681 >>> solveCubic( 682 ... 9.0, 0.0, 0.0, -7.62939453125e-05 683 ... ) == [-0.0, -0.0, -0.0] 684 True 685 """ 686 # 687 # adapted from: 688 # CUBIC.C - Solve a cubic polynomial 689 # public domain by Ross Cottrell 690 # found at: http://www.strangecreations.com/library/snippets/Cubic.C 691 # 692 if abs(a) < epsilon: 693 # don't just test for zero; for very small values of 'a' solveCubic() 694 # returns unreliable results, so we fall back to quad. 695 return solveQuadratic(b, c, d) 696 a = float(a) 697 a1 = b / a 698 a2 = c / a 699 a3 = d / a 700 701 Q = (a1 * a1 - 3.0 * a2) / 9.0 702 R = (2.0 * a1 * a1 * a1 - 9.0 * a1 * a2 + 27.0 * a3) / 54.0 703 704 R2 = R * R 705 Q3 = Q * Q * Q 706 R2 = 0 if R2 < epsilon else R2 707 Q3 = 0 if abs(Q3) < epsilon else Q3 708 709 R2_Q3 = R2 - Q3 710 711 if R2 == 0.0 and Q3 == 0.0: 712 x = round(-a1 / 3.0, epsilonDigits) 713 return [x, x, x] 714 elif R2_Q3 <= epsilon * 0.5: 715 # The epsilon * .5 above ensures that Q3 is not zero. 716 theta = acos(max(min(R / sqrt(Q3), 1.0), -1.0)) 717 rQ2 = -2.0 * sqrt(Q) 718 a1_3 = a1 / 3.0 719 x0 = rQ2 * cos(theta / 3.0) - a1_3 720 x1 = rQ2 * cos((theta + 2.0 * pi) / 3.0) - a1_3 721 x2 = rQ2 * cos((theta + 4.0 * pi) / 3.0) - a1_3 722 x0, x1, x2 = sorted([x0, x1, x2]) 723 # Merge roots that are close-enough 724 if x1 - x0 < epsilon and x2 - x1 < epsilon: 725 x0 = x1 = x2 = round((x0 + x1 + x2) / 3.0, epsilonDigits) 726 elif x1 - x0 < epsilon: 727 x0 = x1 = round((x0 + x1) / 2.0, epsilonDigits) 728 x2 = round(x2, epsilonDigits) 729 elif x2 - x1 < epsilon: 730 x0 = round(x0, epsilonDigits) 731 x1 = x2 = round((x1 + x2) / 2.0, epsilonDigits) 732 else: 733 x0 = round(x0, epsilonDigits) 734 x1 = round(x1, epsilonDigits) 735 x2 = round(x2, epsilonDigits) 736 return [x0, x1, x2] 737 else: 738 x = pow(sqrt(R2_Q3) + abs(R), 1 / 3.0) 739 x = x + Q / x 740 if R >= 0.0: 741 x = -x 742 x = round(x - a1 / 3.0, epsilonDigits) 743 return [x] 744 745 746# 747# Conversion routines for points to parameters and vice versa 748# 749 750 751def calcQuadraticParameters(pt1, pt2, pt3): 752 x2, y2 = pt2 753 x3, y3 = pt3 754 cx, cy = pt1 755 bx = (x2 - cx) * 2.0 756 by = (y2 - cy) * 2.0 757 ax = x3 - cx - bx 758 ay = y3 - cy - by 759 return (ax, ay), (bx, by), (cx, cy) 760 761 762def calcCubicParameters(pt1, pt2, pt3, pt4): 763 x2, y2 = pt2 764 x3, y3 = pt3 765 x4, y4 = pt4 766 dx, dy = pt1 767 cx = (x2 - dx) * 3.0 768 cy = (y2 - dy) * 3.0 769 bx = (x3 - x2) * 3.0 - cx 770 by = (y3 - y2) * 3.0 - cy 771 ax = x4 - dx - cx - bx 772 ay = y4 - dy - cy - by 773 return (ax, ay), (bx, by), (cx, cy), (dx, dy) 774 775 776def calcQuadraticPoints(a, b, c): 777 ax, ay = a 778 bx, by = b 779 cx, cy = c 780 x1 = cx 781 y1 = cy 782 x2 = (bx * 0.5) + cx 783 y2 = (by * 0.5) + cy 784 x3 = ax + bx + cx 785 y3 = ay + by + cy 786 return (x1, y1), (x2, y2), (x3, y3) 787 788 789def calcCubicPoints(a, b, c, d): 790 ax, ay = a 791 bx, by = b 792 cx, cy = c 793 dx, dy = d 794 x1 = dx 795 y1 = dy 796 x2 = (cx / 3.0) + dx 797 y2 = (cy / 3.0) + dy 798 x3 = (bx + cx) / 3.0 + x2 799 y3 = (by + cy) / 3.0 + y2 800 x4 = ax + dx + cx + bx 801 y4 = ay + dy + cy + by 802 return (x1, y1), (x2, y2), (x3, y3), (x4, y4) 803 804 805# 806# Point at time 807# 808 809 810def linePointAtT(pt1, pt2, t): 811 """Finds the point at time `t` on a line. 812 813 Args: 814 pt1, pt2: Coordinates of the line as 2D tuples. 815 t: The time along the line. 816 817 Returns: 818 A 2D tuple with the coordinates of the point. 819 """ 820 return ((pt1[0] * (1 - t) + pt2[0] * t), (pt1[1] * (1 - t) + pt2[1] * t)) 821 822 823def quadraticPointAtT(pt1, pt2, pt3, t): 824 """Finds the point at time `t` on a quadratic curve. 825 826 Args: 827 pt1, pt2, pt3: Coordinates of the curve as 2D tuples. 828 t: The time along the curve. 829 830 Returns: 831 A 2D tuple with the coordinates of the point. 832 """ 833 x = (1 - t) * (1 - t) * pt1[0] + 2 * (1 - t) * t * pt2[0] + t * t * pt3[0] 834 y = (1 - t) * (1 - t) * pt1[1] + 2 * (1 - t) * t * pt2[1] + t * t * pt3[1] 835 return (x, y) 836 837 838def cubicPointAtT(pt1, pt2, pt3, pt4, t): 839 """Finds the point at time `t` on a cubic curve. 840 841 Args: 842 pt1, pt2, pt3, pt4: Coordinates of the curve as 2D tuples. 843 t: The time along the curve. 844 845 Returns: 846 A 2D tuple with the coordinates of the point. 847 """ 848 x = ( 849 (1 - t) * (1 - t) * (1 - t) * pt1[0] 850 + 3 * (1 - t) * (1 - t) * t * pt2[0] 851 + 3 * (1 - t) * t * t * pt3[0] 852 + t * t * t * pt4[0] 853 ) 854 y = ( 855 (1 - t) * (1 - t) * (1 - t) * pt1[1] 856 + 3 * (1 - t) * (1 - t) * t * pt2[1] 857 + 3 * (1 - t) * t * t * pt3[1] 858 + t * t * t * pt4[1] 859 ) 860 return (x, y) 861 862 863def segmentPointAtT(seg, t): 864 if len(seg) == 2: 865 return linePointAtT(*seg, t) 866 elif len(seg) == 3: 867 return quadraticPointAtT(*seg, t) 868 elif len(seg) == 4: 869 return cubicPointAtT(*seg, t) 870 raise ValueError("Unknown curve degree") 871 872 873# 874# Intersection finders 875# 876 877 878def _line_t_of_pt(s, e, pt): 879 sx, sy = s 880 ex, ey = e 881 px, py = pt 882 if abs(sx - ex) < epsilon and abs(sy - ey) < epsilon: 883 # Line is a point! 884 return -1 885 # Use the largest 886 if abs(sx - ex) > abs(sy - ey): 887 return (px - sx) / (ex - sx) 888 else: 889 return (py - sy) / (ey - sy) 890 891 892def _both_points_are_on_same_side_of_origin(a, b, origin): 893 xDiff = (a[0] - origin[0]) * (b[0] - origin[0]) 894 yDiff = (a[1] - origin[1]) * (b[1] - origin[1]) 895 return not (xDiff <= 0.0 and yDiff <= 0.0) 896 897 898def lineLineIntersections(s1, e1, s2, e2): 899 """Finds intersections between two line segments. 900 901 Args: 902 s1, e1: Coordinates of the first line as 2D tuples. 903 s2, e2: Coordinates of the second line as 2D tuples. 904 905 Returns: 906 A list of ``Intersection`` objects, each object having ``pt``, ``t1`` 907 and ``t2`` attributes containing the intersection point, time on first 908 segment and time on second segment respectively. 909 910 Examples:: 911 912 >>> a = lineLineIntersections( (310,389), (453, 222), (289, 251), (447, 367)) 913 >>> len(a) 914 1 915 >>> intersection = a[0] 916 >>> intersection.pt 917 (374.44882952482897, 313.73458370177315) 918 >>> (intersection.t1, intersection.t2) 919 (0.45069111555824465, 0.5408153767394238) 920 """ 921 s1x, s1y = s1 922 e1x, e1y = e1 923 s2x, s2y = s2 924 e2x, e2y = e2 925 if ( 926 math.isclose(s2x, e2x) and math.isclose(s1x, e1x) and not math.isclose(s1x, s2x) 927 ): # Parallel vertical 928 return [] 929 if ( 930 math.isclose(s2y, e2y) and math.isclose(s1y, e1y) and not math.isclose(s1y, s2y) 931 ): # Parallel horizontal 932 return [] 933 if math.isclose(s2x, e2x) and math.isclose(s2y, e2y): # Line segment is tiny 934 return [] 935 if math.isclose(s1x, e1x) and math.isclose(s1y, e1y): # Line segment is tiny 936 return [] 937 if math.isclose(e1x, s1x): 938 x = s1x 939 slope34 = (e2y - s2y) / (e2x - s2x) 940 y = slope34 * (x - s2x) + s2y 941 pt = (x, y) 942 return [ 943 Intersection( 944 pt=pt, t1=_line_t_of_pt(s1, e1, pt), t2=_line_t_of_pt(s2, e2, pt) 945 ) 946 ] 947 if math.isclose(s2x, e2x): 948 x = s2x 949 slope12 = (e1y - s1y) / (e1x - s1x) 950 y = slope12 * (x - s1x) + s1y 951 pt = (x, y) 952 return [ 953 Intersection( 954 pt=pt, t1=_line_t_of_pt(s1, e1, pt), t2=_line_t_of_pt(s2, e2, pt) 955 ) 956 ] 957 958 slope12 = (e1y - s1y) / (e1x - s1x) 959 slope34 = (e2y - s2y) / (e2x - s2x) 960 if math.isclose(slope12, slope34): 961 return [] 962 x = (slope12 * s1x - s1y - slope34 * s2x + s2y) / (slope12 - slope34) 963 y = slope12 * (x - s1x) + s1y 964 pt = (x, y) 965 if _both_points_are_on_same_side_of_origin( 966 pt, e1, s1 967 ) and _both_points_are_on_same_side_of_origin(pt, s2, e2): 968 return [ 969 Intersection( 970 pt=pt, t1=_line_t_of_pt(s1, e1, pt), t2=_line_t_of_pt(s2, e2, pt) 971 ) 972 ] 973 return [] 974 975 976def _alignment_transformation(segment): 977 # Returns a transformation which aligns a segment horizontally at the 978 # origin. Apply this transformation to curves and root-find to find 979 # intersections with the segment. 980 start = segment[0] 981 end = segment[-1] 982 angle = math.atan2(end[1] - start[1], end[0] - start[0]) 983 return Identity.rotate(-angle).translate(-start[0], -start[1]) 984 985 986def _curve_line_intersections_t(curve, line): 987 aligned_curve = _alignment_transformation(line).transformPoints(curve) 988 if len(curve) == 3: 989 a, b, c = calcQuadraticParameters(*aligned_curve) 990 intersections = solveQuadratic(a[1], b[1], c[1]) 991 elif len(curve) == 4: 992 a, b, c, d = calcCubicParameters(*aligned_curve) 993 intersections = solveCubic(a[1], b[1], c[1], d[1]) 994 else: 995 raise ValueError("Unknown curve degree") 996 return sorted(i for i in intersections if 0.0 <= i <= 1) 997 998 999def curveLineIntersections(curve, line): 1000 """Finds intersections between a curve and a line. 1001 1002 Args: 1003 curve: List of coordinates of the curve segment as 2D tuples. 1004 line: List of coordinates of the line segment as 2D tuples. 1005 1006 Returns: 1007 A list of ``Intersection`` objects, each object having ``pt``, ``t1`` 1008 and ``t2`` attributes containing the intersection point, time on first 1009 segment and time on second segment respectively. 1010 1011 Examples:: 1012 >>> curve = [ (100, 240), (30, 60), (210, 230), (160, 30) ] 1013 >>> line = [ (25, 260), (230, 20) ] 1014 >>> intersections = curveLineIntersections(curve, line) 1015 >>> len(intersections) 1016 3 1017 >>> intersections[0].pt 1018 (84.9000930760723, 189.87306176459828) 1019 """ 1020 if len(curve) == 3: 1021 pointFinder = quadraticPointAtT 1022 elif len(curve) == 4: 1023 pointFinder = cubicPointAtT 1024 else: 1025 raise ValueError("Unknown curve degree") 1026 intersections = [] 1027 for t in _curve_line_intersections_t(curve, line): 1028 pt = pointFinder(*curve, t) 1029 # Back-project the point onto the line, to avoid problems with 1030 # numerical accuracy in the case of vertical and horizontal lines 1031 line_t = _line_t_of_pt(*line, pt) 1032 pt = linePointAtT(*line, line_t) 1033 intersections.append(Intersection(pt=pt, t1=t, t2=line_t)) 1034 return intersections 1035 1036 1037def _curve_bounds(c): 1038 if len(c) == 3: 1039 return calcQuadraticBounds(*c) 1040 elif len(c) == 4: 1041 return calcCubicBounds(*c) 1042 raise ValueError("Unknown curve degree") 1043 1044 1045def _split_segment_at_t(c, t): 1046 if len(c) == 2: 1047 s, e = c 1048 midpoint = linePointAtT(s, e, t) 1049 return [(s, midpoint), (midpoint, e)] 1050 if len(c) == 3: 1051 return splitQuadraticAtT(*c, t) 1052 elif len(c) == 4: 1053 return splitCubicAtT(*c, t) 1054 raise ValueError("Unknown curve degree") 1055 1056 1057def _curve_curve_intersections_t( 1058 curve1, curve2, precision=1e-3, range1=None, range2=None 1059): 1060 bounds1 = _curve_bounds(curve1) 1061 bounds2 = _curve_bounds(curve2) 1062 1063 if not range1: 1064 range1 = (0.0, 1.0) 1065 if not range2: 1066 range2 = (0.0, 1.0) 1067 1068 # If bounds don't intersect, go home 1069 intersects, _ = sectRect(bounds1, bounds2) 1070 if not intersects: 1071 return [] 1072 1073 def midpoint(r): 1074 return 0.5 * (r[0] + r[1]) 1075 1076 # If they do overlap but they're tiny, approximate 1077 if rectArea(bounds1) < precision and rectArea(bounds2) < precision: 1078 return [(midpoint(range1), midpoint(range2))] 1079 1080 c11, c12 = _split_segment_at_t(curve1, 0.5) 1081 c11_range = (range1[0], midpoint(range1)) 1082 c12_range = (midpoint(range1), range1[1]) 1083 1084 c21, c22 = _split_segment_at_t(curve2, 0.5) 1085 c21_range = (range2[0], midpoint(range2)) 1086 c22_range = (midpoint(range2), range2[1]) 1087 1088 found = [] 1089 found.extend( 1090 _curve_curve_intersections_t( 1091 c11, c21, precision, range1=c11_range, range2=c21_range 1092 ) 1093 ) 1094 found.extend( 1095 _curve_curve_intersections_t( 1096 c12, c21, precision, range1=c12_range, range2=c21_range 1097 ) 1098 ) 1099 found.extend( 1100 _curve_curve_intersections_t( 1101 c11, c22, precision, range1=c11_range, range2=c22_range 1102 ) 1103 ) 1104 found.extend( 1105 _curve_curve_intersections_t( 1106 c12, c22, precision, range1=c12_range, range2=c22_range 1107 ) 1108 ) 1109 1110 unique_key = lambda ts: (int(ts[0] / precision), int(ts[1] / precision)) 1111 seen = set() 1112 unique_values = [] 1113 1114 for ts in found: 1115 key = unique_key(ts) 1116 if key in seen: 1117 continue 1118 seen.add(key) 1119 unique_values.append(ts) 1120 1121 return unique_values 1122 1123 1124def curveCurveIntersections(curve1, curve2): 1125 """Finds intersections between a curve and a curve. 1126 1127 Args: 1128 curve1: List of coordinates of the first curve segment as 2D tuples. 1129 curve2: List of coordinates of the second curve segment as 2D tuples. 1130 1131 Returns: 1132 A list of ``Intersection`` objects, each object having ``pt``, ``t1`` 1133 and ``t2`` attributes containing the intersection point, time on first 1134 segment and time on second segment respectively. 1135 1136 Examples:: 1137 >>> curve1 = [ (10,100), (90,30), (40,140), (220,220) ] 1138 >>> curve2 = [ (5,150), (180,20), (80,250), (210,190) ] 1139 >>> intersections = curveCurveIntersections(curve1, curve2) 1140 >>> len(intersections) 1141 3 1142 >>> intersections[0].pt 1143 (81.7831487395506, 109.88904552375288) 1144 """ 1145 intersection_ts = _curve_curve_intersections_t(curve1, curve2) 1146 return [ 1147 Intersection(pt=segmentPointAtT(curve1, ts[0]), t1=ts[0], t2=ts[1]) 1148 for ts in intersection_ts 1149 ] 1150 1151 1152def segmentSegmentIntersections(seg1, seg2): 1153 """Finds intersections between two segments. 1154 1155 Args: 1156 seg1: List of coordinates of the first segment as 2D tuples. 1157 seg2: List of coordinates of the second segment as 2D tuples. 1158 1159 Returns: 1160 A list of ``Intersection`` objects, each object having ``pt``, ``t1`` 1161 and ``t2`` attributes containing the intersection point, time on first 1162 segment and time on second segment respectively. 1163 1164 Examples:: 1165 >>> curve1 = [ (10,100), (90,30), (40,140), (220,220) ] 1166 >>> curve2 = [ (5,150), (180,20), (80,250), (210,190) ] 1167 >>> intersections = segmentSegmentIntersections(curve1, curve2) 1168 >>> len(intersections) 1169 3 1170 >>> intersections[0].pt 1171 (81.7831487395506, 109.88904552375288) 1172 >>> curve3 = [ (100, 240), (30, 60), (210, 230), (160, 30) ] 1173 >>> line = [ (25, 260), (230, 20) ] 1174 >>> intersections = segmentSegmentIntersections(curve3, line) 1175 >>> len(intersections) 1176 3 1177 >>> intersections[0].pt 1178 (84.9000930760723, 189.87306176459828) 1179 1180 """ 1181 # Arrange by degree 1182 swapped = False 1183 if len(seg2) > len(seg1): 1184 seg2, seg1 = seg1, seg2 1185 swapped = True 1186 if len(seg1) > 2: 1187 if len(seg2) > 2: 1188 intersections = curveCurveIntersections(seg1, seg2) 1189 else: 1190 intersections = curveLineIntersections(seg1, seg2) 1191 elif len(seg1) == 2 and len(seg2) == 2: 1192 intersections = lineLineIntersections(*seg1, *seg2) 1193 else: 1194 raise ValueError("Couldn't work out which intersection function to use") 1195 if not swapped: 1196 return intersections 1197 return [Intersection(pt=i.pt, t1=i.t2, t2=i.t1) for i in intersections] 1198 1199 1200def _segmentrepr(obj): 1201 """ 1202 >>> _segmentrepr([1, [2, 3], [], [[2, [3, 4], [0.1, 2.2]]]]) 1203 '(1, (2, 3), (), ((2, (3, 4), (0.1, 2.2))))' 1204 """ 1205 try: 1206 it = iter(obj) 1207 except TypeError: 1208 return "%g" % obj 1209 else: 1210 return "(%s)" % ", ".join(_segmentrepr(x) for x in it) 1211 1212 1213def printSegments(segments): 1214 """Helper for the doctests, displaying each segment in a list of 1215 segments on a single line as a tuple. 1216 """ 1217 for segment in segments: 1218 print(_segmentrepr(segment)) 1219 1220 1221if __name__ == "__main__": 1222 import sys 1223 import doctest 1224 1225 sys.exit(doctest.testmod().failed) 1226