simple fillet example with numpy

fillet_demo.py

from matplotlib import pyplot as plt
import numpy as np
from shapely.geometry import Polygon

from ipdb import set_trace as db

def main():
    fillet_demo()


def fillet_demo():

    # create simple polyline with one acute angle
    rad = 3
    angle_d = 25
    angle_r = 25 * np.pi/180
    xy = np.array([
        [rad, 0],
        [0, 0],
        [rad*np.cos(angle_r), rad*np.sin(angle_r)],
    ])

    R = 0.25/2  # fillet radius
    filleted, part1, part2, circle_center = fillet_single_corner(xy, R)

    # part1 and part2 don't need to be explicitly found, because they
    # can be pulled out of the filleted point list
    _part1 = filleted[[0, 1],:]
    _part2 = filleted[[-2, -1], :]

    xy_eroded = np.vstack(Polygon(xy).buffer(-R, join_style=1).buffer(R, join_style=1).exterior.coords.xy).T

    fig = plt.figure()
    plt.plot(xy[:,0], xy[:,1], 'r-', label='original')
    c = np.exp(np.linspace(0, 2j*np.pi, 64))
    #plt.plot(circle_center[0] + R*c.real, circle_center[1] + R*c.imag, 'b-', label='fillet circle')
    plt.plot(part1[:,0], part1[:,1], 'g-', label='trimmed')
    plt.plot(part2[:,0], part2[:,1], 'g-', label='trimmed')
    #plt.plot(filleted[:,0], filleted[:,1], 'c-', label='manual fillet')
    #plt.plot(xy_eroded[:,0], xy_eroded[:,1], 'k--', label="shapely buffer")

    plt.axis('equal')
    plt.legend()
    plt.show()


def fillet_single_corner(xy, r):
    # xy is a 3x2 array, a length-3 list of 2d points
    # r is the fillet radius

    # basic geometry
    v1 = xy[0,:] - xy[1,:]
    v2 = xy[2,:] - xy[1,:]
    v1_norm = v1 / np.linalg.norm(v1)
    v2_norm = v2 / np.linalg.norm(v2)
    angle = np.arccos(np.dot(v1_norm, v2_norm))
    bisector = (v1 + v2) / 2 - xy[1,:]
    bisector_norm = bisector / np.linalg.norm(bisector)

    # trigonometry of the fillet
    distance_along_bisector = r / np.sin(angle/2)
    distance_along_line = r / np.tan(angle/2)

    # explicitly define the trimmed line segments
    part1 = np.array([
        xy[0,:],
        xy[1,:] + v1_norm * distance_along_line,
    ])
    part2 = np.array([
        xy[1,:] + v2_norm * distance_along_line,
        xy[2,:],
    ])

    # create the fillet curve
    circle_center = xy[1,:] + distance_along_bisector * bisector_norm
    va1 = part1[1,:] - circle_center
    va2 = part2[0,:] - circle_center
    a1 = np.arctan2(va1[1], va1[0])
    a2 = np.arctan2(va2[1], va2[0])

    # NOTE this angle vector is not fully general
    a_vec = np.arange(a1, a2-2*np.pi, np.sign(a1-a2) * np.pi/32)
    fillet = np.vstack((circle_center[0] + r*np.cos(a_vec), circle_center[1] + r*np.sin(a_vec))).T
    filleted = np.vstack((xy[0,:], fillet, xy[2,:]))
    return filleted, part1, part2, circle_center


main()