Finds closest point on a bezier path using Newton–Raphson method and visualizes it in drawbot app

bezier-path-find-closest-point.py

import math

def bezier_curve(p0, p1, p2, p3, t):
    x = (1-t)**3 * p0[0] + 3*(1-t)**2 * t * p1[0] + 3*(1-t) * t**2 * p2[0] + t**3 * p3[0]
    y = (1-t)**3 * p0[1] + 3*(1-t)**2 * t * p1[1] + 3*(1-t) * t**2 * p2[1] + t**3 * p3[1]
    return (x, y)

def bezier_curve_derivative(p0, p1, p2, p3, t):
    x = 3*(1-t)**2 * (p1[0] - p0[0]) + 6*(1-t) * t * (p2[0] - p1[0]) + 3*t**2 * (p3[0] - p2[0])
    y = 3*(1-t)**2 * (p1[1] - p0[1]) + 6*(1-t) * t * (p2[1] - p1[1]) + 3*t**2 * (p3[1] - p2[1])
    return (x, y)

def align_to_x(p0, p1, p2, p3):
    """
    moves p0 to (0, 0) and after rotate the points so y of p3 becomes 0, making the curve align to x axis.
    """
    # convert points to complex numbers
    p0 = complex(*p0)
    p1 = complex(*p1)
    p2 = complex(*p2)
    p3 = complex(*p3)

    # move p0 to (0, 0)
    p1 -= p0
    p2 -= p0
    p3 -= p0
    angle = p3.imag / p3.real

    # rotate using complex number multiplication
    rotation = complex(1, -angle) / (1 + angle**2)**0.5
    p1 *= rotation
    p2 *= rotation
    p3 *= rotation
    return ((0, 0), (p1.real, p1.imag), (p2.real, p2.imag), (p3.real, p3.imag))

def calculate_inflection_points(p0, p1, p2, p3):
    # moving the curve to origin (0, 0) to remove p0 from equation
    _p0, _p1, _p2, _p3 = align_to_x(p0, p1, p2, p3)
    x2, y2 = _p1
    x3, y3 = _p2
    x4, _ = _p3

    # coefficients
    a = x3 * y2
    b = x4 * y2
    c = x2 * y3
    d = x4 * y3

    # coefficients of quadratic equation
    x = -3*a + 2*b + 3*c - d
    y = 3*a - b - 3*c
    z = c - a

    # the discriminator is negative or if x is zero
    if y**2 - 4*x*z < 0 or x == 0:
        return []

    # the roots of the quadratic equation
    t1 = (-y + (y**2 - 4*x*z)**0.5) / (2*x)
    t2 = (-y - (y**2 - 4*x*z)**0.5) / (2*x)

    # remove roots that are not in the Bézier interval [0,1]
    inflection_points = [t for t in [t1, t2] if 0 <= t <= 1]
    return inflection_points

def vector(p1, p2):
    return p2[0]-p1[0], p2[1]-p1[1]

def length(v):
    return math.hypot(*v)

def fill_inbetween(input_list, num_steps=5):
    output_list = [input_list[0]]
    for start, end in zip(input_list, input_list[1:]):
        step_size = (end - start) / num_steps
        output_list += [start + i * step_size for i in range(1, num_steps + 1)]
    return output_list

# average 0.23 ms per call on 2,3 GHz 8-Core Intel Core i9
def closest_point_on_bezier(p0, p1, p2, p3, target):
    t_intervals = [0]
    t_intervals.extend(calculate_inflection_points(p0, p1, p2, p3))
    t_intervals.append(1)

    t_intervals = fill_inbetween(t_intervals)
    t_gaps = [t_intervals[i] - t_intervals[i-1] for i in range(1, len(t_intervals))]
    LUT = []
    for i, t in enumerate(t_intervals):
        point = bezier_curve(p0, p1, p2, p3, t)
        v = vector(target, point)
        dist = length(v)
        LUT.append({'t': t, 'd': dist, 'i': i})

    T_D = {} # t to distance

    def refine_t(p0, p1, p2, p3, t0, t1, target):
        while t1 - t0 > 1e-3:
            t_m = (t0 + t1) / 2
            p = bezier_curve(p0, p1, p2, p3, t_m)
            d_m = bezier_curve_derivative(p0, p1, p2, p3, t_m)
            distance_vector = vector(target, p)
            dot_product = d_m[0] * distance_vector[0] + d_m[1] * distance_vector[1]
            if abs(dot_product) < 1e-2:
                break 
            elif dot_product < 0:
                t0 = t_m
            else:
                t1 = t_m
        T_D[t_m] = length(distance_vector)

    bestCandids = list(sorted(LUT, key=lambda p: p['d']))[:4]
    for p in bestCandids:
        i = p['i']
        t_m = t_intervals[i]
        try:
            t_g = t_gaps[i]/2
        except IndexError:
            t_g = t_gaps[-1]/2
        t0 = max(t_m - t_g, 0)
        t1 = min(t_m + t_g, 1)
        refine_t(p0, p1, p2, p3, t0, t1, target)
    min_d = min(T_D.values())
    return {t for t, d in T_D.items() if d - min_d < 1}

size(500, 500)
fill(1)
rect(0, 0, 500, 500)
stroke(0)
fill(None)

p0 = (40, 360)
p1 = (380, 470)
p2 = (30, -110)
p3 = (218, 380)
p = (192, 296)
oval(p[0]-2, p[1]-2, 4, 4)

# Draw the Bezier curve
newPath()
moveTo(p0)
curveTo(p1, p2, p3)
drawPath()
stroke(0, 1, 0)
for t in closest_point_on_bezier(p0, p1, p2, p3, p):
    p = bezier_curve(p0, p1, p2, p3, t)
    oval(p[0]-2, p[1]-2, 4, 4)

Sample render

Really nice!

❤️