working-beam-mechanism-simple-draw-trace.py

from sympy import pi, cos, sin, sqrt, atan, atan2, N, oo
import matplotlib.pyplot as plt
from pprint import pprint


def getSum(arr, n):
    pos = [0, 0]
    for r, rho in arr[:n]:
        if rho == oo:
            continue
        pos[0] += r * cos(rho)
        pos[1] += r * sin(rho)
    return pos


def getRange(arr):
    leng = sum([r for r, rho in arr])
    return -leng / 2, leng / 2


def vectTopolar(v):
    r = sqrt(v[0]**2 + v[1]**2)
    rho = atan2(v[1], v[0])
    return r, -rho


def solve(want):
    k1 = 2 * want[2][0] * (want[0][0] * cos(want[0][1]) +
                           want[1][0] * cos(want[1][1]))
    k2 = 2 * want[2][0] * (want[0][0] * sin(want[0][1]) +
                           want[1][0] * sin(want[1][1]))
    k3 = (want[0][0]**2 + want[1][0]**2 + want[2][0]**2 - want[3][0]**2) + \
        2 * want[0][0] * want[1][0] * (
        cos(want[0][1]) * cos(want[1][1]) +
        sin(want[0][1]) * sin(want[1][1]))
    A = -k1 + k3
    B = 2 * k2
    C = k1 + k3
    ans = [(-B - sqrt(B**2 - 4 * A * C)) / (2 * A),
           (-B + sqrt(B**2 - 4 * A * C)) / (2 * A)]
    return map(lambda a: 2 * atan(a), ans)


def listN(v):
    return N(v[0]), N(v[1])


fig, ax = plt.subplots()
allxy = []
for angle in range(0, 361, 15):
    # input
    want = [(56.4, 210 * pi / 180), (25.4, angle), (52.3, oo), (59.2, oo)]
    want[1] = (want[1][0], want[1][1] * pi / 180)
    Plen = 77.7
    Pang = 31 * pi / 180

    # set
    # plt.xlim(getRange(want))
    # plt.ylim(getRange(want))

    # output
    ans = list(solve(want))
    # for a in ans:
    a = ans[0]
    want[2] = want[2][0], N(a)
    start_pos = getSum(want, 2)
    want_pos = (start_pos[0] + Plen * cos(want[2][1] + Pang),
                start_pos[1] + Plen * sin(want[2][1] + Pang))
    print(listN(want_pos))
    allxy.append(want_pos)

plt.plot(*list(zip(*allxy)), '-o')
fig.show()
input()