FourBarPlot.py

from sympy import pi, cos, sin, sqrt, atan, atan2, N, oo
import matplotlib.pyplot as plt
import random
import time
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()
# for angle in range(0, 360, 15):
if True:
    """
    angle = 30
    want = [(152.4, pi), (50.8, angle), (177.8,), (228.6,)]
    angle = 85
    want = [(177.8, pi), (228.6, angle), (76.2,), (203.2,)]
    angle = 45
    want = [(76.2, pi), (254.0, angle), (152.4,), (203.2,)]
    angle = 25
    want = [(203.2, pi), (127.0, angle), (177.8,), (152.4,)]
    """
    angle = 75
    want = [(203.2, pi), (127.0, angle), (203.2, oo), (152.4, oo)]

    # set axix
    want[1] = (want[1][0], want[1][1] * pi / 180)
    plt.xlim(getRange(want))
    plt.ylim(getRange(want))

    # solve
    ans = solve(want)

    # output
    for a in ans:
        want[2] = want[2][0], a
        want[3] = vectTopolar(getSum(want, 3))
        pprint(list(map(lambda a: (N(a[0]), N(a[1] / pi * 180)), want)))
        pprint([listN(getSum(want, i)) for i, w in enumerate(want)])

        color = random.choice('bgr')
        for i, w in enumerate(want):
            plt.plot(*zip(getSum(want, i - 1), getSum(want, i)),
                     color + '-', lw=2)
    fig.show()
    input()
    fig.clear()