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