from sympy import Point, Circle, Line, var
import matplotlib.pyplot as plt
var('t')
c1 = Circle(Point(0, 0), 2)
c2 = Circle(Point(4, 4), 3)
l1 = Line(c1.center, c2.center)
p1 = l1.arbitrary_point(t).subs({t: -c1.radius / (c2.radius - c1.radius)})
p2 = l1.arbitrary_point(t).subs({t: c1.radius / (c1.radius + c2.radius)})
t1 = c1.tangent_lines(p1)
t2 = c1.tangent_lines(p2)
ta = t1 + t2
fig = plt.gcf()
ax = fig.gca()
ax.set_xlim((-10, 10))
ax.set_ylim((-10, 10))
ax.set_aspect(1)
cp1 = plt.Circle((c1.center.x, c1.center.y), c1.radius, fill = False)
cp2 = plt.Circle((c2.center.x, c2.center.y), c2.radius, fill = False)
tp = [0 for i in range(4)]
for i in range(4):
start = ta[i].arbitrary_point(t).subs({t:-10})
end = ta[i].arbitrary_point(t).subs({t:10})
tp[i] = plt.Line2D([start.x, end.x], [start.y, end.y], lw = 2)
ax.add_artist(cp1)
ax.add_artist(cp2)
# Krävs Sympy 1.0 for att fungera # 1.1.1 Kraschade from sympy import Point,Line,Circle,intersection,Triangle,N import time p1 = Point(0,8) p0 = Point(0,2) X = Line(Point(0,0),Point(1,0)) c0 = Circle(p0,2) l0 = c0.tangent_lines(p1)[0] p2 = intersection(l0, X)[0] l2 = Line(p2, p0) def findCircle(circle): p3 = intersection(circle, l2)[0] # Kraschar här. l3 = l2.perpendicular_line(p3) p5 = intersection(l3, l0)[0] p6 = intersection(l3, X)[0] t1 = Triangle(p5, p2, p6) return t1.incircle circle = c0 for i in range(5): start = time.time() circle = findCircle(circle) r = circle.radius print() print(r) print(i,time.time()-start, N(r))
target = Point(10, 8)
R0 = 3
phi0 = np.pi / 4
phi1 = np.arctan2(8, 10)
center = []
np.arctan2()
if phi0 > phi1:
center = Point(UAV.x - R0 * np.sin(phi0),
UAV.y + R0 * np.cos(phi0)) #逆时针起始圆圆心
else:
center = Point(Point.x + R0 * np.sin(phi0),
Point.y - R0 * np.cos(phi0)) #顺时针起始圆圆心
c1 = Circle(center, R0)
t1 = c1.tangent_lines(target)
for tangent_line in t1:
p1 = np.array([tangent_line.p1.x.evalf(),
tangent_line.p1.y.evalf()]) # 目标点
p2 = np.array([tangent_line.p2.x.evalf(), tangent_line.p2.y.evalf()]) # 切点
tan = (p1 - p2)[1] / (p1 - p2)[0]
angle = phi0 - math.atan(tan) #
lens = math.sqrt(sum((p2 - UAV_p)**2))
theta1 = 2 * math.asin(lens / 2 / R0)
juge = theta1 == angle
def Arrival_time(UAV, target, R0):
# 单架无人机到达目标点所需时间
# 这里的UAV和target是无人机和目标对象
v = UAV.v # 飞机速度
phi0 = UAV.phi # 转化为弧度，[0,2pi]
UAV_p = Point(UAV.site)
target_p = Point(target[0:2])
# 以上为所有已知信息
# 1. 求两个圆心，判断出采用哪一个圆
# 2. 求切线
# 3. 确定用那一段弧长
# 1.求两个圆心，判断出采用哪一个圆
c1 = Point(UAV_p.x + R0 * np.sin(phi0), UAV_p.y - R0 * np.cos(phi0))
c2 = Point(UAV_p.x - R0 * np.sin(phi0), UAV_p.y + R0 * np.cos(phi0))
len1 = c1.distance(target_p)
len2 = c2.distance(target_p)
center = c1
if len2 > len1:
center = c2
# 2. 求切线
circle = Circle(center, R0)
tangent_lines = circle.tangent_lines(target_p)
tangent_line1 = tangent_lines[0] # 注意这里的切线方向是从target-> 切点
tangent_line1 = Line(tangent_line1.p2, tangent_line1.p1) # 改为从切点->target
tangent_point1 = tangent_line1.p1 # 切点1
y = float((target_p.y - tangent_point1.y).evalf())
x = float((target_p.x - tangent_point1.x).evalf())
tangent_angle1 = np.arctan2(y, x) # arctan2(y,x) 向量(x,y)的角度[-pi,pi]
tangent_line2 = tangent_lines[1]
tangent_line2 = Line(tangent_line2.p2, tangent_line2.p1) # 改为从切点->target
tangent_point2 = tangent_line2.p1 # 切点２
y = float((target_p.y - tangent_point2.y).evalf())
x = float((target_p.x - tangent_point2.x).evalf())
tangent_angle2 = np.arctan2(y, x) # arctan2(y,x) 向量(x,y)的角度[-pi,pi]
# 3. 确定用哪一段弧长
# a. 确定用顺时针还是逆时针
vec1 = [UAV_p.x - center.x, UAV_p.y - center.y]
vec2 = [np.cos(phi0), np.sin(phi0)]
direction = np.sign(vec1[0] * vec2[1] - vec1[1] * vec2[0]) # 1 表示逆时针 -1 表示顺时针
# b. 判断是哪一个切点，哪一段弧
sin1 = float(tangent_point1.distance(UAV_p).evalf()) / (2 * R0)
angle1 = 2 * np.arcsin(sin1) # 无人机位置与切点之间的弧度[0,pi] 小弧
sin2 = float(tangent_point2.distance(UAV_p).evalf()) / (2 * R0)
angle2 = 2 * np.arcsin(sin2)
tangent_point = []
hudu = 0
# 判断式的意思 角度要在误差范围内相隔2kpi，使用modf(abs 把值控制在0-1之内，误差范围内靠近1，靠近0 都ok 用于0.5之间的距离来判断
if abs(modf(abs(direction * angle1 + phi0 - tangent_angle1) / (2 * np.pi))[0] - 0.5) > 0.5 - deviation:
tangent_point = tangent_point1
hudu = angle1
# modf 返回浮点数的小数部分和整数部分modf(1.23) return [0.23,1]
elif abs(modf(abs(direction * (2 * np.pi - angle1) + phi0 - tangent_angle1) / (2 * np.pi))[
0] - 0.5) > 0.5 - deviation:
tangent_point = tangent_point1
hudu = 2 * np.pi - angle1
elif abs(modf(abs(direction * angle2 + phi0 - tangent_angle2) / (2 * np.pi))[0] - 0.5) > 0.5 - deviation:
tangent_point = tangent_point2
hudu = angle2
elif abs(modf(abs(direction * (2 * np.pi - angle2) + phi0 - tangent_angle2) / (2 * np.pi))[
0] - 0.5) > 0.5 - deviation:
tangent_point = tangent_point2
hudu = 2 * np.pi - angle2
path_length = R0 * hudu + float(tangent_point.distance(target_p).evalf())
return path_length / v
# 2. 求切线
# 3. 确定用那一段弧长
# 1.求两个圆心，判断出采用哪一个圆
c1 = Point(UAV_p.x + R0 * np.sin(phi0), UAV_p.y - R0 * np.cos(phi0))
c2 = Point(UAV_p.x - R0 * np.sin(phi0), UAV_p.y + R0 * np.cos(phi0))
len1 = c1.distance(target_p)
len2 = c2.distance(target_p)
center = c1
if len2 > len1:
center = c2
# 2. 求切线
circle = Circle(center, R0)
tangent_lines = circle.tangent_lines(target_p)
tangent_line1 = tangent_lines[0] # 注意这里的切线方向是从target-> 切点
tangent_line1 = Line(tangent_line1.p2, tangent_line1.p1) # 改为从切点->target
tangent_point1 = tangent_line1.p1 # 切点1
y = float((target_p.y - tangent_point1.y).evalf())
x = float((target_p.x - tangent_point1.x).evalf())
tangent_angle1 = np.arctan2(y, x) # arctan2(y,x) 向量(x,y)的角度[-pi,pi]
tangent_line2 = tangent_lines[1]
tangent_line2 = Line(tangent_line2.p2, tangent_line2.p1) # 改为从切点->target
tangent_point2 = tangent_line2.p1 # 切点２
y = float((target_p.y - tangent_point2.y).evalf())
x = float((target_p.x - tangent_point2.x).evalf())
tangent_angle2 = np.arctan2(y, x) # arctan2(y,x) 向量(x,y)的角度[-pi,pi]
from sympy import Point,Line,Circle,intersection,Triangle,N
from svg import Svg
C = Point(0,8)
D = Point(0,2)
xaxis = Line(Point(0,0),Point(1,0))
CircleD = Circle(D,2)
tangentE = CircleD.tangent_lines(C)[0]
E = intersection(tangentE,CircleD)[0]
A = intersection(tangentE, xaxis)[0]
CircleD = Circle(D,2)
svg = Svg()
svg.append(C,"C")
#svg.append(D)
svg.append(CircleD,"CircleD")
svg.append(tangentE,"tangE")
svg.append(E,"E")
svg.append(A,"A")
def find_circle(circle,A,C,D,i):
AD = Line(A,D)
svg.append(AD,"AD",i)
K = intersection(circle, AD)[0]
svg.append(K,"K",i)
tangentK = Line(A,D).perpendicular_line(K)
svg.append(tangentK,"tangK",i)
P1 = intersection(tangentK, Line(A,C))[0]
svg.append(P1,"P1",i)
P2 = intersection(tangentK, xaxis)[0]
svg.append(P2,"P2",i)