Cubic Bezier tracks with constant speed animation, using Tkinter

tk_bezier_track.py

#!/usr/bin/env python3

''' Create a closed track from cubic Bezier curves, and animate a circle
    following the track at constant speed.

    https://en.wikipedia.org/wiki/B%C3%A9zier_curve#Cubic_B%C3%A9zier_curves
    
    https://gist.github.com/PM2Ring/d6a19f5062b39467ac669a4fb4715779
    
    Press the Add button to add black control dots to the Canvas. The first
    control dot is actually dark purple to make it easier to find the start
    of the track. For a closed track you should add pairs of dots. White
    midpoint dots are added between every 2nd pair of control dots. Black
    dots can be dragged to new positions, white dots can be dragged along
    the segment that contains them. Remember to toggle the Add button off
    before attempting to drag dots. ;)

    Written by PM 2Ring 2018.09.21
'''

import tkinter as tk
from collections import deque
from math import sqrt

def integrate(func, xlo, xhi, err_max):
    ''' Adaptive integration using Simpson's Rule.
        Recurses internally using a deque.
    '''
    h = (xhi - xlo ) / 4.0
    xt = (xlo, (xhi + xlo ) / 2.0, xhi)
    yt = tuple(func(x) for x in xt)
    stack = deque([(xt, yt, h, err_max)])
    sigma = err = 0
    #count = 0
    while stack:
        xt, yt, h, err_max = stack.popleft()
        x1, x3, x5 = xt
        y1, y3, y5 = yt
        while True:
            x2, x4 = x1 + h, x5 - h
            y2, y4 = func(x2), func(x4)
            s1 = y1 + y5
            s2 = 4.0 * (y2 + y4)

            # Get error from the absolute difference between
            # the 3-point & 5-point approximations
            err1 = abs((s1 - s2 + 6.0 * y3) * h / 45.0)
            if err1 < err_max:
                sigma += (s1 + s2 + 2.0 * y3) * h / 3.0
                err += err1
                #count += 1
                break

            h *= 0.5
            err_max *= 0.5

            # Make the left points 1, 2, 3 the new 1, 3, 5
            # and put the right points on the stack
            stack.append([(x3, x4, x5), (y3, y4, y5), h, err_max])
            x3, x5 = x2, x3
            y3, y5 = y2, y3
    return sigma, err #, count

def bezier(p, x0, y0, x1, y1, x2, y2, x3, y3):
    ''' Find the x,y coords of the point with parameter p
        on the cubic Bezier curve with the given control points
    '''
    q = 1 - p
    t0 = q * q * q
    t1 = q * q * p
    t2 = q * p * p
    t3 = p * p * p
    x = t0 * x0 + 3 * t1 * x1 + 3 * t2 * x2 + t3 * x3
    y = t0 * y0 + 3 * t1 * y1 + 3 * t2 * y2 + t3 * y3
    return x, y

def flat(x0, y0, x1, y1, tol):
    return abs(x0*y1 - x1*y0) < tol * abs(x0 * x1 + y0 * y1)

def bezier_points(x0, y0, x1, y1, x2, y2, x3, y3, tol=0.001):
    ''' Draw a cubic Bezier cirve by recursive subdivision

        The curve is subdivided until each of the 4 sections is 
        sufficiently flat, determined by the angle between them.
        tol is the tolerance expressed as the tangent of the angle
    '''
    if (flat(x1-x0, y1-y0, x2-x0, y2-y0, tol) and
        flat(x2-x1, y2-y1, x3-x1, y3-y1, tol)):
        return [x0, y0, x3, y3]

    x01, y01 = (x0 + x1) / 2., (y0 + y1) / 2.
    x12, y12 = (x1 + x2) / 2., (y1 + y2) / 2.
    x23, y23 = (x2 + x3) / 2., (y2 + y3) / 2.
    xa, ya = (x01 + x12) / 2., (y01 + y12) / 2.
    xb, yb = (x12 + x23) / 2., (y12 + y23) / 2.
    xc, yc = (xa + xb) / 2., (ya + yb) / 2.

    # Double the tolerance angle
    tol = 2. / (1. / tol - tol)
    return (bezier_points(x0, y0, x01, y01, xa, ya, xc, yc, tol)[:-2] + 
        bezier_points(xc, yc, xb, yb, x23, y23, x3, y3, tol))

class Dot:
    ''' Movable Canvas circles '''
    dots = {}
    def __init__(self, canvas, x, y, color, rad=1, tags=""):
        self.x, self.y = x, y
        self.canvas = canvas
        self.weight = 0
        self.id = canvas.create_oval(x-rad, y-rad, x+rad, y+rad,
            outline=color, fill=color, tags=tags)
        Dot.dots[self.id] = self

    def update(self, x, y):
        dx, dy = x - self.x, y - self.y
        self.x, self.y = x, y
        self.canvas.move(self.id, dx, dy)

    def __repr__(self):
        return f'Dot({self.id}, {self.x}, {self.y})'

    def delete(self):
        self.canvas.delete(self.id)
        del Dot.dots[self.id]

class GUI:
    def __init__(self):
        root = tk.Tk()
        width = root.winfo_screenwidth() * 4 // 5
        height = root.winfo_screenheight() * 4 // 5
        root.geometry(f'{width}x{height}')
        root.title('Bezier Track')
        root.rowconfigure(0, weight=1)
        root.columnconfigure(0, weight=1)

        self.canvas = canvas = tk.Canvas(root)
        canvas.grid(row=0, column=0, sticky='nsew')

        # The Bezier control points, and groups for each curve
        self.dots, self.middots, self.groups = [], [], []

        # Various Canvas-related items: the current Dot being dragged, the
        # lines comprising the convex hull, the lines of the Bezier curve
        # and the animated spot.
        self.drag_item = self.hull = self.curve = self.spot = None

        # Add bindings for clicking & dragging any object with the "dot" tag
        canvas.tag_bind("dot", "<ButtonPress-1>", self.on_dot_press)
        canvas.tag_bind("dot", "<B1-Motion>", self.on_dot_motion)
        canvas.tag_bind("dot", "<ButtonRelease-1>", self.on_dot_release)

        canvas.tag_bind("mdot", "<ButtonPress-1>", self.on_mdot_press)
        canvas.tag_bind("mdot", "<B1-Motion>", self.on_mdot_motion)
        canvas.tag_bind("mdot", "<ButtonRelease-1>", self.on_dot_release)

        canvas.bind("<ButtonPress-1>", self.add_dot)
        canvas.bind("<ButtonRelease-1>", lambda evt: self.draw_hull())

        frame = tk.Frame(root, bd=1, relief=tk.SUNKEN)
        frame.grid(row=1, column=0, sticky='nsew')

        # Button to add new Dots to the canvas
        self.add_btn = tk.Button(frame, text="Add", relief="sunken", 
            command=self.add_dot_cb)
        self.add_btn.pack(side=tk.LEFT)
        self.add_btn.state = True

        tk.Button(frame, text="Delete", command=self.delete_cb).pack(side=tk.LEFT)
        tk.Button(frame, text="Clear", command=self.clear_cb).pack(side=tk.LEFT)

        self.anim_btn = tk.Button(frame, text="Animate", command=self.animate)
        self.anim_btn.pack(side=tk.LEFT)
        self.anim_btn.state = False

        root.mainloop()

    def add_dot_cb(self):
        self.add_btn.state ^= 1
        self.add_btn["relief"] = "sunken" if self.add_btn.state else "raised"

    def add_dot(self, event):
        if not self.add_btn.state:
            return
        x, y = self.canvas.canvasx(event.x), self.canvas.canvasy(event.y) 
        color = "black" if self.dots else "#70a"
        self.dots.append(Dot(self.canvas, x, y, color, rad=4, tags="dot"))

    def clear_cb(self):
        for dot in self.dots + self.middots:
            dot.delete()
        self.dots.clear()
        self.middots.clear()

        if self.hull:
            self.canvas.delete(self.hull)
            self.hull = None
        if self.curve:
            self.canvas.delete(self.curve)
            self.curve = None
        if not self.add_btn.state:
            self.add_dot_cb()

    def delete_cb(self):
        self.dots.pop().delete()
        if len(self.dots) % 2:
            self.middots.pop().delete()
        self.draw_hull()

    def on_dot_press(self, event):
        ''' Beginning drag of a dot '''
        # Record the item being dragged
        x, y = self.canvas.canvasx(event.x), self.canvas.canvasy(event.y)
        self.drag_item = Dot.dots[self.canvas.find_closest(x, y)[0]]

    def on_dot_motion(self, event):
        ''' Handle dragging of a dot '''
        if self.drag_item is None:
            return
        # Get the mouse location
        x, y = self.canvas.canvasx(event.x), self.canvas.canvasy(event.y) 
        # Move the Dot to the new location
        self.drag_item.update(x, y)
        self.draw_hull()

    def on_mdot_press(self, event):
        ''' Beginning drag of an mdot '''
        # Record the item being dragged
        x, y = self.canvas.canvasx(event.x), self.canvas.canvasy(event.y)
        self.drag_item = Dot.dots[self.canvas.find_closest(x, y)[0]]

        # Find the control points of the segment containing this mdot
        idx = self.middots.index(self.drag_item) * 2
        d0, d1 = self.dots[idx:idx+2]
        # Create a function to find the weight of the closest point
        # on the segment to (x, y)
        x0, y0, x1, y1 = d0.x, d0.y, d1.x, d1.y
        dx, dy = x1 - x0, y1 - y0
        d2 = dx ** 2 + dy ** 2
        def get_weight(x, y):
            # Use the dot product to find the weight of the closest
            # point between (x0, y0) and (x1, y1) to (x, y)
            w = (dx * (x - x0) + dy * (y - y0)) / d2
            # Bound it to inside the segment
            return min(max(0.001, w), 0.999)
        self.get_weight = get_weight

    def on_mdot_motion(self, event):
        ''' Handle dragging of an mdot object '''
        if self.drag_item is None:
            return
        # Get the mouse location
        x, y = self.canvas.canvasx(event.x), self.canvas.canvasy(event.y)
        # Set the weight of the closest point on the bounding segment
        self.drag_item.weight = self.get_weight(x, y)
        self.draw_hull()

    def on_dot_release(self, event):
        ''' End drag of an object '''
        # Reset the drag information
        self.drag_item = None

    def draw_hull(self):
        ''' Draw the convex hull of the Bezier curve '''
        if len(self.dots) < 2:
            return

        # Create a flat list of the control point coords
        coords = []
        for dot in self.dots:
            coords.extend((dot.x, dot.y))
        if len(coords) % 4 == 0:
            coords.extend(coords[:2])

        if self.hull:
            self.canvas.coords(self.hull, coords)
        else:
            self.hull = self.canvas.create_line(coords, fill="grey", dash=[4, 4])

        self.draw_curve()

    def draw_curve(self):
        if len(self.dots) < 4:
            return

        # Place the midpoints, which are actually the start
        # start and end points of each Bezier section
        middots = self.middots
        dot_queue = deque(middots)

        coords = []
        for i, dot in enumerate(self.dots):
            x, y = dot.x, dot.y
            if i % 2:
                px, py = coords[-2:]
                if dot_queue:
                    mdot = dot_queue.popleft()
                    w0, w1 = mdot.weight, 1 - mdot.weight
                    mx, my = px*w1 + x*w0, py*w1 + y*w0
                    mdot.update(mx, my)
                else:
                    w0 = w1 = 0.5
                    mx, my = px*w1 + x*w0, py*w1 + y*w0
                    mdot = Dot(self.canvas, mx, my, "white", tags="mdot", rad=4)
                    mdot.weight = w0
                    middots.append(mdot)
                coords.extend((mx, my))
            coords.extend((x, y))

        coords.extend(coords[:4])

        # Draw the Bezier curves
        points = []
        self.groups.clear()
        maxi = (len(coords) - 2) // 6 * 6 + 2
        for i in range(2, maxi, 6):
            group = coords[i:i+8]
            if len(group) < 8: 
                break
            points.extend(bezier_points(*group)[:-2])
            self.groups.append(group)
        points.extend(coords[2:4])

        if self.curve:
            self.canvas.coords(self.curve, points)
        else:
            self.curve = self.canvas.create_line(points, fill="blue")

    def anim_spot(self, gen):
        ''' Callback loop for the animated Dot '''
        try:
            self.spot.update(*next(gen))
        except (AttributeError, StopIteration):
            return
        self.canvas.after(10, lambda: self.anim_spot(gen))

    def animate(self):
        ''' Animate a Dot moving along the Bezier curve at constant speed '''
        state = self.anim_btn.state = self.anim_btn.state ^ 1
        self.anim_btn["relief"] = "sunken" if state else "raised"
        if self.spot or not state:
            return

        start = self.middots[0]
        self.spot = Dot(self.canvas, start.x, start.y, "red", rad=4, tags="spot")
        self.anim_spot(self.do_spot())

    def do_spot(self):
        while True:
            for group in self.groups:
                yield from self.anim_group(group)
            if not self.anim_btn.state:
                self.spot = self.spot.delete()
                return

    def anim_group(self, group):
        ''' A generator that calculates the animated Dot coordinates '''
        x0, y0, x1, y1, x2, y2, x3, y3 = group
        u1, v1 = x1 - x0, y1 - y0
        u2, v2 = x2 - x1, y2 - y1
        u3, v3 = x3 - x2, y3 - y2

        def bezier_diff(p):
            ''' The arclength differential of the Bezier curve '''
            q = 1 - p
            t1 = q * q
            t2 = q * p
            t3 = p * p
            dx = 3 * t1 * u1 + 6 * t2 * u2 + 3 * t3 * u3
            dy = 3 * t1 * v1 + 6 * t2 * v2 + 3 * t3 * v3
            return sqrt(dx*dx + dy*dy)

        steplen = 2.0
        old_p = p = 0.0
        #vlo, vhi = 10 * steplen, 0.0
        while self.anim_btn.state and p < 1.0:
            # Estimate next p
            p = old_p + steplen / bezier_diff(old_p)
            # Refine it using Newton's Method
            for j in range(9):
                # Get the distance along the curve from old_p to p
                a, err = integrate(bezier_diff, old_p, p, 1e-5)
                delta = steplen - a
                p += delta / bezier_diff(p)
                if abs(delta) < 0.01:
                    break

            # Measure the actual step length
            #a, err = integrate(bezier_diff, old_p, p, 1e-6)
            #vlo, vhi = min(vlo, a), max(vhi, a)

            yield bezier(p, x0, y0, x1, y1, x2, y2, x3, y3)
            old_p = p
        #print('vlo', vlo, 'vhi', vhi) 

if __name__ == "__main__":
    GUI()

White dots can now be dragged along the segment that contains them.