foot_design.py

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


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])


def calMiny(l, num):
    wanty = list(sorted([i[1] for i in l]))[:num]
    miny = min(wanty)
    gap = 5
    return all([i <= miny + gap for i in wanty])


fig, ax = plt.subplots()

# find length
"""
for fourth in range(10, 101, 10):
    for third in range(max(abs(40 - fourth), abs(60 - fourth), 10) + 1,
                       40 + fourth - 1, 5):
        allxy = [[], [], []]
        for angle in range(0, 361, 15):
            # input
            want = [(50, pi), (10, angle), (third, oo), (fourth, oo)]
            want[1] = (want[1][0], want[1][1] * pi / 180)

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

        print(third)
        print(fourth)
        ok = calMiny(allxy[0], 360 // 15 // 2)
        print(ok)
        if ok:
            fig.clf()
            plt.plot(*list(zip(*allxy[0])), '-o')
            plt.plot(*list(zip(*allxy[1])), '-o')
            plt.plot(*list(zip(*allxy[2])), '-o')
            fig.show()
            input()
"""

# find foot position
third = 65
fourth = 40
want_ans = []
for angle in range(0, 361, 15):
    want = [(50, pi), (20, angle), (third, oo), (fourth, oo)]
    want[1] = (want[1][0], want[1][1] * pi / 180)
    # output
    ans = list(solve(want))
    a = ans[0]
    want[2] = want[2][0], N(a)
    want_ans.append(want)

# foot
"""
for want in want_ans:
    allxy = []
    for Plen,Pang in [(65,20),(43,50)]:
        start_pos = getSum(want, 2)
        pang = -Pang * pi / 180
        want_pos = (start_pos[0] + Plen * cos(want[2][1] + pang),
                    start_pos[1] + Plen * sin(want[2][1] + pang))
        want_pos = listN(want_pos)
        # print(want_pos)
        allxy.append(want_pos)
    plt.plot(*list(zip(*allxy)), '-o')
"""

# foot's angle and pos
angs = []
ys = []
for want in want_ans:
    allxy = []
    for Plen, Pang in [(65, 20), (43, 50)]:
        start_pos = getSum(want, 2)
        pang = -Pang * pi / 180
        want_pos = (start_pos[0] + Plen * cos(want[2][1] + pang),
                    start_pos[1] + Plen * sin(want[2][1] + pang))
        want_pos = listN(want_pos)
        # print(want_pos)
        allxy.append(want_pos)
    angs.append(math.atan((allxy[0][1] - allxy[1][1]) /
                          (allxy[0][0] - allxy[1][0])) / pi * 180)
    ys.append((allxy[0][1] + allxy[1][1]) / 2)

fig.clf()
plt.plot(angs, '-o')
plt.plot(ys, '-o')

fig.show()
input()