Python--基于离散点的等距线（equidistant line）计算

 

此为毕业设计需要而写

本人毕业设计需要利用差分原理，控制两个电机，达到完成上位机给定的轨迹。

 

基本思路：

一.通过离散点分别计算出中垂线上与远线等距的两个点，计算可得两个值，分别是（+-d）值

二.选取两个值中的一个

准则1：

（1）计算出第一个双值时，随便选取一个值A。

（2）计算出第二个双值时，分别用A与两值做连线，即计算两个直线方程。

（3）两个直线方程将与原直线分别有一个交点，计算出交点。

（4）计算该两个交点与原直线上的recent_dot的距离。

（5）选择距离大的点的值。

准则2：

（1）计算A点与双值之间的距离

（2）选取距离小的

准则1，准则2相与，可以完成筛选。

Python代码如下：

'''
    拟合出第二条线
'''
import math
import sys
import sympy
i = 0
j = 0
cc=[0,0]
output_1=0.0
output_2=0.0
line1_x = 0
line1_y = 0
line2_x = 0
line2_y = 0
x=0
b10 = 0
b11 = 0
b20 = 0
b21 = 0
d1_x = 0
d1_y = 0
d2_x = 0
d2_y = 0
d21_x = 0
d21_y = 0
c1_x = 0
c1_y = 0
c2_x = 0
c2_y = 0
c3_x = 0
c3_y = 0
p_x = 0
p_y = 0
q_x = 0
q_y = 0

with open(r'C:\Users\Administrator\Desktop\chart track\data\line1_data.txt','r') as data,open(r'C:\Users\Administrator\Desktop\chart track\data\line2_Data.txt','a') as output_data:
    for line in data.readlines():
        line = eval(line)
        if(i==0):                           #获得出c1坐标值
            line1_x = line[0]
            line1_y = line[-1]
            c1_x = line1_x
            c1_y = line1_y
        if(i>0):
            if(i==1):                       #得到c2值
                line2_x = line[0]
                line2_y = line[-1] 
                c2_x = line2_x
                c2_y = line2_y
            if(i>1):                       #得到c3值
                line2_x = line[0]
                line2_y = line[-1] 
                c3_x = line2_x

                c3_y = line2_y




                                    #计算直线Ax+By+C=0中的参数
            A = line2_y-line1_y
            B = line1_x-line2_x
            C = line2_x*line1_y-line1_x*line2_y
            
            x = sympy.Symbol('x')
            y = sympy.Symbol('y')
            if(A==0):
                
                b10=(line2_x+line1_x)/2+0.111541
                b11=line1_y+30+0.115145814
                b20=(line2_x+line1_x)/2+0.111541
                b21=line1_y-30+0.115145814




            else:
                a=sympy.solve([B/A*x+(line2_y+line1_y)/2-B*(line2_x+line1_x)/(2*A)-y,(x-(line2_x+line1_x)/2)**2+(y-(line2_y+line1_y)/2)**2-900],[x,y])
                b1=a[0]
                b10=b1[0]
                b11=b1[1]
                b2=a[1]
                b20=b2[0]
                b21=b2[1]

                

                print(a)
            if(i==1):
                cc[0]=b10
                cc[1]=b11
                d1_x = b10
                d1_y = b11
            if(i>1):

                d2_x = b10+0.10101101
                d2_y = b11+0.11100101
                d21_x = b20+0.1101010
                d21_y = b21+0.1110111
                x1 = sympy.Symbol('x1')
                y1 = sympy.Symbol('y1')
                p = sympy.solve([(x1-c1_x)*(c3_y-c1_y)-(c3_x-c1_x)*(y1-c1_y),(x1-d1_x)*(d2_y-d1_y)-(d2_x-d1_x)*(y1-d1_y)],[x1,y1])

                q = sympy.solve([(x1-c1_x)*(c3_y-c1_y)-(c3_x-c1_x)*(y1-c1_y),(x1-d1_x)*(d21_y-d1_y)-(d21_x-d1_x)*(y1-d1_y)],[x1,y1])

                p_x = p[x1]
                p_y = p[y1]
                q_x = q[x1]
                q_y = q[y1]



                if((d1_x-d2_x)**2+(d1_y-d2_y)**2<=(d1_x-d21_x)**2+(d1_y-d21_y)**2 and (p_x-c1_x)**2+(p_x-c1_y)**2>=(q_x-c1_x)**2+(q_y-c1_y)**2):
                    
                    cc[0]=b10
                    cc[1]=b11
                    

                    output_data.write(str(cc))
                    output_data.write('\n')
                    d1_x = b10
                    d1_y = b11
                else:
                    
                    cc[0]=b20
                    cc[1]=b21

                    output_data.write(str(cc))
                    output_data.write('\n')
                    d1_x = b20
                    d1_y = b21

                c1_x = c2_x
                c1_y = c2_y
                c2_x = c3_x
                c2_y = c3_y 
        line1_x = line2_x
        line1_y = line2_y

            
            
        i = i+1
        print("成功计算出",i,"个坐标值")