Fivebar Mechanism

import numpy as np
from kingdon import Algebra
import ipywidgets

alg = Algebra(2, 0, 1)

def point(x, y): return alg.vector(e0=1, e1=x, e2=y).dual()
def plane(a, b, c): return alg.vector(e1=a, e2=b, e0=c)

def exp_np(x):
    xsq = (x|x).grade(0)
    if not xsq:
        return 1 + x
    lsqrt = (-xsq.e)**0.5
    return x * np.sinc(lsqrt / np.pi) + np.cos(lsqrt)

def translator(line, dist):
    e0 = line.algebra.blades['e0']
    return 1 + 0.5 * dist * (e0 * line.normalized() * e0.dual())

def elbow(point1, point2, r1, r2, alternative=False):
    """
    Given two points and two radii, give the intersection point of the
    circles centered on those points with those radii.
    """
    u = point1 & point2
    v = u | point1
    usq_inv = 1 / (u|u).e
    s = 0.5 * (usq_inv * (r1**2 - r2**2) + 1)
    t = ((usq_inv * r1**2) - s**2)**0.5
    point2perp = (1 - point1) / 2**0.5 >> point2
    if alternative:
        return point1 + (point2-point1) * s - (point2perp-point1) *  t
    return point1 + (point2-point1) * s + (point2perp-point1) * t

def fivebar(t, A, B, ac, bd, cd, de, theta_D, pol_A, full_output=False, exp=exp_np):
    # Create C by translating A up, and then rotating it by the desired angle.
    tr_ac = translator(A & B, ac)
    # tr_ac = exp(0.5 * ac * alg.blades['e01'])
    if isinstance(pol_A, (tuple, list)):
        theta_At = sum(t**i * coeff for i, coeff in enumerate(reversed(pol_A)))
    else:
        theta_At = pol_A
    M_A = exp(- 0.5 * A * theta_At)
    C = (M_A * tr_ac) >> A
    # Create D as the elbow of C and B.
    D = elbow(B, C, bd, cd)
    # Create E by translating D up, and then rotating it by the desired angle.
    tr_de = translator(C & D, de)
    # tr_de = exp(0.5 * de * alg.blades['e01'])
    M_D = exp(- 0.5 * theta_D * D)
    E = (M_D * tr_de) >> D
    if full_output:
        return C, D, E
    return E

from timeit import default_timer
from functools import partial

DistSlider = partial(ipywidgets.FloatSlider, min=-10, max=10, step=0.01, readout=True, readout_format='.2f')

ac_widget = DistSlider(value=4.329777002067039)
bd_widget = DistSlider(value=-6.418389614876762)

usecase = [point(1.5, 3.0), point(5.0, 5.0), point(8.5, 2.0)]
best = [
    7.522375791897677,
    -1.2297104295438015,
    1.4883454465783046,
    9.65978858247454,
    4.329777002067039,
    -6.418389614876762,
    12.91788027878568,
    10.223852704675853,
    -3.393426372984221,
    0.9098149166712446,
    -3.763919738437536,
    -5.266926540202572
]

Ax, Ay, Bx, By, ac, bd, cd, de, theta_D, tA1, tA2, tA3 = best
# Anchor points
A = point(Ax, Ay);
B = point(Bx, By);
endpoints = []

def graph_func():
    t = default_timer() / 1000
    ac = ac_widget.value
    bd = bd_widget.value
    C, D, E = fivebar(t, A, B, ac, bd, cd, de, theta_D, [1, 0, 0], full_output=True)
    endpoints.append(E)
    if len(endpoints) > 50:
        endpoints.pop(0)

    return [
        # usecase
        *usecase,
        # Anchor points
        0xff0000, A, 'A', B, 'B',
        0x000000, [A, C], [B, D], [C, D], [D, E],
        # Moving points
        0x00ff00,
        C, 'C', D, 'D',
        # end point and previous endpoints
        0xff00ff, *endpoints,
        0x0000ff,
        E, 'E',
    ]

graph = alg.graph(
    graph_func,
    scale=0.1, animate=True,
)
# graph
ipywidgets.VBox([ac_widget, bd_widget, graph])