Untitled

import numpy as np
import matplotlib.pyplot as plt
from shapely.geometry import Polygon
from shapely.ops import cascaded_union

def cart2pol(x, y):
    rho = np.sqrt(x**2 + y**2)
    phi = np.arctan2(y, x)
    return(rho, phi)


def pol2cart(rho, phi):
    x = rho * np.cos(phi)
    y = rho * np.sin(phi)
    return(x, y)


def rotate(x, y, angle, center = None):
    if center is not None:
        raise("Not implemented yet.")
    
    rr, tt = cart2pol(x,y)
    x1, y1 = pol2cart(rr, tt + angle)

    return x1, y1


def derivative(x, y, rotor = None):
    if rotor is not None:
        raise("Not implemented yet.")
    
    DXDY = []
    for i in range(1, np.size(x)-1):

        DXP=x[i+1]-x[i]
        DXM=x[i]-x[i-1]
        DYP=y[i+1]-y[i]+1.E-30
        DYM=y[i]-y[i-1]+1.E-30
        ELP=1. #maybe it should be 2
        ELM=1. #maybe it should be 2
        #ELP=np.sqrt(DXP**2+DXP**2)
        #ELM=np.sqrt(DYM**2+DYM**2)
        DXX=(DXP*ELM+DXM*ELP)/(ELP+ELM)
        DYY=(DYP*ELM+DYM*ELP)/(ELP+ELM)
        DXDY.append(DXX/(DYY+1.E-30))

    return DXDY


def rack2Inner(rack_x, rack_y, dxdy_rack, r1w):
    FI = (rack_y - dxdy_rack*(r1w-rack_x))/r1w

    x_inner = rack_x*np.cos(FI) - (rack_y - r1w*FI)*np.sin(FI)
    y_inner = rack_x*np.sin(FI) + (rack_y - r1w*FI)*np.cos(FI)

    return x_inner, y_inner


def rack2Outer(rack_x, rack_y, dxdy_rack, r1w, r2w):
    E = r2w - r1w # r2w - r1w
    XX = E + rack_x # E + rack_x
    FI = (rack_y + dxdy_rack * (r2w-XX)) / r2w

    x_outer = (XX*np.cos(FI) - (rack_y - r2w*FI) * np.sin(FI))
    y_outer = (XX*np.sin(FI) + (rack_y - r2w*FI) * np.cos(FI))

    return x_outer, y_outer


def transformPoints(x, y, phi1, phi2, E, flag = None):

    if flag is not None:
        raise("Not implemented yet.")
    
    M21 = [[np.cos((phi2-phi1)), -np.sin((phi2-phi1)), 0, E*np.cos(phi2)], 
                [np.sin((phi2-phi1)), np.cos((phi2-phi1)), 0, E*np.sin(phi2)], 
                [0, 0, 1, 0], 
                [0, 0, 0, 1]]
    M21 = np.matrix(M21)

    tmp = np.array(M21 * np.matrix([x, y, np.zeros(np.size(x)), np.ones(np.size(x))]))

    return tmp[0], tmp[1]


def envelopePolygon(x1, y1, N1, N2, E):
    m21 = N2 / N1
    phi1 = np.linspace(0, 2*np.pi, 100)
    polygons = []
    for PHI_1 in phi1:
        PHI_2 = (1./m21) * PHI_1

        tmpx, tmpy = transformPoints(x1, y1, PHI_1, PHI_2, E)
        plt.plot(tmpx, tmpy)
        pxy = np.transpose([tmpx, tmpy])
        polygons.append(Polygon(pxy))
    u = cascaded_union(polygons)
    xr2, yr2 = u.exterior.xy
    plt.show()

    return np.array(xr2), np.array(yr2)


def genProfileFromLobe(x_lobe, y_lobe, N):

    r, t = cart2pol(x_lobe, y_lobe)
    x_full = np.array([])
    y_full = np.array([])

    angle = 2*np.pi / N
    plt.figure()
    for i in range(N):
        x, y = pol2cart(r, t - i*angle)
        x_full = np.concatenate((x_full, x))
        y_full = np.concatenate((y_full, y))

    return x_full, y_full


def generatePureCycloidRack(r1w, N1):
    theta = np.linspace(0, 2*np.pi, 200)
    r = r1w / (2* N1)
    x = r*theta - np.sin(theta)*r
    x = np.append(x, x[0:]+x[-1])

    x = x - 0.7
    y = r - r*np.cos(theta)
    y = np.append(y, -y[0:])

    rr, tr = cart2pol(x, y)
    x, y = pol2cart(rr, tr - np.pi/2)
    x = x + r1w 

    xc1 = r1w * np.cos(theta)
    yc1 = r1w * np.sin(theta)

    plt.plot(x,y)
    plt.plot(xc1, yc1)
    plt.show()
    return x, y

def generateSineRack(r1w, N1):
    tooth_width = r1w * (2*np.pi/N1)
    theta = np.linspace(0, 2*np.pi, 50)
    A = 0.2
    phi = 0
    y = A*np.sin(theta+phi)
    x = (r1w/N1) * (theta+phi) - tooth_width/2
    rr, tr = cart2pol(x, y)
    x, y = pol2cart(rr, tr - np.pi/2)
    x = x + r1w 

    xc1 = r1w * np.cos(theta)
    yc1 = r1w * np.sin(theta)

    plt.plot(x,y)
    plt.plot(xc1, yc1)
    plt.show()
    return x, y 


def main():
    N1 = 10; N2 = N1 + 1
    m21 = N2 / N1
    r1w = 2; r2w = m21 * r1w 
    E = r2w - r1w 
    c1 = [0, 0]
    c2 = [-E, 0]
    
    theta_circle = np.linspace(0, 2*np.pi, 200)
    xc1 = np.cos(theta_circle) * r1w; xc2 = np.cos(theta_circle) * r2w + c2[0]
    yc1 = np.sin(theta_circle) * r1w; yc2 = np.sin(theta_circle) * r2w + c2[1]

    rack_x, rack_y = generatePureCycloidRack(r1w, N1)
    rack_x, rack_y = generateSineRack(r1w, N1)

    xr1, yr1 = rack2Inner(rack_x[1:-1], rack_y[1:-1], derivative(rack_x, rack_y), r1w)

    ######
    tmpx, tmpy = rack2Outer(rack_x[1:-1], rack_y[1:-1], derivative(rack_x, rack_y), r1w, r2w)
    tmpxx, tmpyy = genProfileFromLobe(tmpx, tmpy, N2)
  
    plt.plot(rack_x, rack_y, 'g')
    plt.plot(xc1, yc1, 'r--')
    plt.plot(xc2, yc2, 'k--')
    plt.plot(xr1, yr1, 'r')
    plt.plot(tmpx, tmpy, 'k')
    plt.show()

    ########
    xr1, yr1 = genProfileFromLobe(xr1, yr1, N1)
    xr2, yr2 = envelopePolygon(xr1, yr1, N1, N2, E)

    
    plt.plot(xr1, yr1, 'r')
    plt.plot(xr2-E, yr2, 'k')
    plt.plot(xc1, yc1, 'r--')
    plt.plot(xc2, yc2, 'k--')
    plt.plot(tmpxx-E, tmpyy, 'c--')
    plt.axis('equal')
    plt.show()


    
    print("Success")


if __name__ == '__main__':
    main()