python smooth interpolation of points using cubic bezier, computed from catmull_rom

catmull_rom_to_bezier.py

"""
python reimplementation of http://schepers.cc/getting-to-the-point (source http://schepers.cc/svg/path/catmullrom2bezier.js)
Pretty much the coolest thing ever
"""
from collections import namedtuple
import numpy as np

Point = namedtuple('Point', ('x', 'y'))

def catmull_rom_to_bezier(points):
    """
    points: List[Point]
    yields `len(points) - 1` Cubic bezier segments of the form (start, control0, control1, end)
    control points are in absolute coordinates
    """
    # depending on your usage, you can treat 2 points like so to leave them unchanged
    # if len(points) == 2:
    #     yield (points[0], points[0], points[1], points[1])
    #     return

    N = len(points) - 1
    for i in range(N):
        if i == 0:
            p = (points[i], points[i], points[i + 1], points[i + 2])
        elif i == N - 1:
            p = (points[i - 1], points[i], points[i + 1], points[i + 1])
        else:
            p = (points[i - 1], points[i], points[i + 1], points[i + 2])

        # Catmull-Rom to Cubic Bezier conversion matrix
        #    0       1       0       0
        #  -1/6      1      1/6      0
        #    0      1/6      1     -1/6
        #    0       0       1       0

        yield (
            p[1],
            Point(((-p[0].x + 6*p[1].x + p[2].x) / 6), ((-p[0].y + 6*p[1].y + p[2].y) / 6)),
            Point((( p[1].x + 6*p[2].x - p[3].x) / 6),  ((p[1].y + 6*p[2].y - p[3].y) / 6)),
            p[2],
            )

conversion_matrix = np.array([
    [0,       6,       0,       0],
    [-1,      6,       1,       0],
    [0,       1,       6,      -1],
    [0,       0,       6,       0],
    ])

def catmull_rom_to_bezier_vectorized(points):
    """
    Same as above but uses numpy.
    points: np.array of shape (N, 2)
    returns: np.array of shape (N - 1, 4, 2)
    """
    if len(points) == 2:
        return [(points[0], points[0], points[1], points[1])]
    A = np.empty((len(points) - 1, 4, 2))
    for i in range(1, len(points) - 2):
        A[i, :] = (points[i - 1], points[i], points[i + 1], points[i + 2])
    A[0, :] = (points[0], points[0], points[1], points[2])
    A[-1, :] = (points[-3], points[-2], points[-1], points[-1])

    return np.matmul(conversion_matrix, A) / 6

if __name__ == '__main__':
    fmt_point = lambda p: '{:.2f} {:.2f}'.format(*p)
    fmt_bezier = lambda b: ' '.join(map(fmt_point, b))

    points = [Point(0, 0), Point(0, 1), Point(1, 1), Point(2, 2)]
    out = list(catmull_rom_to_bezier(points))
    print('\n'.join(map(fmt_bezier, out)))

    print()

    pointsv = np.array(points)
    outv = catmull_rom_to_bezier_vectorized(points)
    print('\n'.join(map(fmt_bezier, outv)))

    assert '\n'.join(map(fmt_bezier, out)) == '\n'.join(map(fmt_bezier, outv))