Python - 两圆相交求交点坐标

Python - 两圆相交求交点坐标
Max.Bai
2016-05-16

Python - 两圆相交求交点坐标

三轴机械臂求坐标问题，其实转化为平面问题就是两圆相交求交点问题，交点算出来就可以用反三角函数算出各个机械臂的夹角。

已知圆1， 半径R， 坐标（x, y)

圆2， 半径S， 坐标（a, b)

求两圆交点x3, y3   x4, y4。

算法一脚本：

# -*- coding: utf-8 -*-
import math

def sq(x):
    return float(x * x)


# target point on table
tx = float(10)
ty = float(10)

# hight of table
h0 = float(5)

# length of arm
R = float(10)
S = float(8)

# arm point
x = float(0)
y = float(5)

# target point for arm
a = float(math.sqrt(sq(tx)+ sq(ty)))
b = h0

print "arm target:", a, b

d = math.sqrt(sq(math.fabs(a-x)) + sq(math.fabs(b-y)))
print "desitens:", d

if d > (R+S) or d < (math.fabs(R-S)):
    print "This point can't be rached!"
    #return -1
    exit
    
if d == 0 and R==S :
    print "Can't rach arm point!"
    #return -2
    exit

A = (sq(R) - sq(S) + sq(d)) / (2 * d)
h = math.sqrt(sq(R) - sq(A))

x2 = x + A * (a-x)/d
y2 = y + A * (b-y)/d

#print x2, y2
x3 = x2 - h * ( b - y ) / d
y3 = y2 + h * ( a - x ) / d

x4 = x2 + h * (b - y) / d
y4 = y2 - h * (a - x) / d

print "arm middle point:"
print x3, y3
print x4, y4

已知圆1， 半径L1， 坐标（0, L3)

圆2， 半径L2， 坐标（x, y)

求圆1交点与圆心连线和Y轴的夹角a1，

圆2交点与圆心连线和X轴的夹角a2。

算法二：

# -*- coding: utf-8 -*-
import math

L1 = float(10)
L2 = float(10)
L3 = float(5)

x = float(14.1421356237)
y = float(5)


def sq(x):
    return x * x

A = float(-2 * x * L1)
B = float(2*(y-L3)*L1)
C = float(sq(L2) - sq(L1) - sq(x) -sq(y-L3))

a1 = float(2*math.atan((B-math.sqrt(sq(B)+sq(A)-sq(C)))/(A+C)))

A = float(2 * (y - L3) * L2)
B = float(2 * x * L2)
C = float(sq(L2) + sq(x) + sq(L3-y) -sq(L1))

a2 = float(2* math.atan((B-math.sqrt(sq(B)+sq(A)-sq(C)))/(A+C)))

print a1, a2
print a1*180/math.pi, a2*180/math.pi