2022国赛B题，第一二问完美解决，第三问思路是：用FY2调整3,3调整4以此类推，最后用9调整2然后循环。
在9调整2的时候报错
Traceback (most recent call last):
File "C:\Users\IDK\Desktop\CUMCM2022\B3_v2.py", line 198, in <module>
FY2 = adjust(F2,FY9,FY2)
File "C:\Users\IDK\Desktop\CUMCM2022\B3_v2.py", line 183, in adjust
kid = [par * angle(k1, F1, F0) + (1 - par) * angle(k3, F1, F0), par * angle(k1, F0, F2) + (1 - par) * angle(k3, F0, k2)]
File "C:\Users\IDK\Desktop\CUMCM2022\B3_v2.py", line 58, in angle
return round(math.acos(abs(n) / d), acc)
ZeroDivisionError: float division by zero
以下是完整代码
import math
import cmath
import random
import numpy as np
from itertools import combinations
acc = 10 # 计算结果保留五位小数
closest = 5 # 最多向心偏差五米
farthest = 20 # 最多离心偏差20米
dis_step = 0.1 # 距离步长
angle_max = 0.018 # 最大偏差角度（1°多一点）
ang_step = 0.1 # 角度步长
F0 = [0, 0, 'F0']
F1 = [100, 0, 'F1']
F2 = [100, 2 / 9 * math.pi, 'F2']
F3 = [100, 4 / 9 * math.pi, 'F3']
F4 = [100, 6 / 9 * math.pi, 'F4']
F5 = [100, 8 / 9 * math.pi, 'F5']
F6 = [100, 10 / 9 * math.pi, 'F6']
F7 = [100, 12 / 9 * math.pi, 'F7']
F8 = [100, 14 / 9 * math.pi, 'F8']
F9 = [100, 16 / 9 * math.pi, 'F9']
FY0 = [0, 0]
FY1 = [100, 0]
FY2 = [98, 40.10 / 180 * math.pi]
FY3 = [112, 80.21 / 180 * math.pi]
FY4 = [105, 119.75/ 180 * math.pi]
FY5 = [98, 159.86/ 180 * math.pi]
FY6 = [112, 199.96/ 180 * math.pi]
FY7 = [105, 240.07/ 180 * math.pi]
FY8 = [98, 280.17/ 180 * math.pi]
FY9 = [112, 320.28/ 180 * math.pi]
def angle_to_radian(p):
return [p[0], p[1] * math.pi / 180]
def polar_to_cartesian(p):
x = p[0] * math.cos(p[1])
y = p[0] * math.sin(p[1])
return [x, y]
def cartesian_to_polar(p):
cn = complex(p[0], p[1])
return cmath.polar(cn)
def angle(ver, tra1, tra2): # 角度计算函数
a = distance_polar(tra1, tra2)
b = distance_polar(ver, tra2)
c = distance_polar(ver, tra1)
n = math.pow(b, 2) + math.pow(c, 2) - math.pow(a, 2)
d = 2 * b * c
return round(math.acos(abs(n) / d), acc)
def distance_artesian(p1, p2):
x = p1[0] - p2[0]
y = p1[1] - p2[1]
return math.pow(math.pow(x, 2) + math.pow(y, 2), 1 / 2)
def distance_polar(p1, p2): # 距离计算函数
x = p1[0] * math.cos(p1[1]) - math.cos(p2[1]) * p2[0]
y = p1[0] * math.sin(p1[1]) - math.sin(p2[1]) * p2[0]
return math.pow(math.pow(x, 2) + math.pow(y, 2), 1 / 2)
def var(li): # 发射器组合生成器 li表示无人机列表
k = list(combinations(li, 2))
return k
def direction(ver, k): # 信号接收器获取到的信号 ver代表接收器，k表示发射器组合
res = []
for i in k:
res.append(angle(ver, i[0], i[1]))
res.sort()
return res
def tra(pre, key, li): # 信号发射器列表 顺序生成器
transmitter = []
for i in combinations([x for x in li if x not in pre], key):
transmitter.append(pre + list(i))
return transmitter
def get_circle_r(p0, p1, r):
if p1[0] == p0[0]:
y0 = y1 = (p0[1] + p1[1]) / 2
delta_y = (y0 - p0[1]) ** 2
delta_x = math.sqrt(r ** 2 - delta_y)
x0 = p1[0] - delta_x
x1 = p1[0] + delta_x
else:
C1 = (p1[0] ** 2 + p1[1] ** 2 - p0[0] ** 2 - p0[1] ** 2) / 2 / (p1[0] - p0[0])
C2 = (p1[1] - p0[1]) / (p1[0] - p0[0])
A = 1 + C2 ** 2
B = 2 * (p0[0] - C1) * C2 - 2 * p0[1]
C = (p0[0] - C1) ** 2 + p0[1] ** 2 - r ** 2
y0 = (-B + math.sqrt(B * B - 4 * A * C)) / 2 / A
y1 = (-B - math.sqrt(B * B - 4 * A * C)) / 2 / A
x0 = C1 - C2 * y0
x1 = C1 - C2 * y1
return [x0, y0], [x1, y1]
def get_circle_p(p1, p2, p3):
x, y, z = p1[0] + p1[1] * 1j, p2[0] + p2[1] * 1j, p3[0] + p3[1] * 1j
w = z - x
w /= y - x
c = (x - y) * (w - abs(w) ** 2) / 2j / w.imag - x
return [-c.real, -c.imag]
def insec(p1, r1, p2, r2):
x = p1[0]
y = p1[1]
R = r1
a = p2[0]
b = p2[1]
S = r2
d = math.sqrt((abs(a - x)) ** 2 + (abs(b - y)) ** 2)
if d > (R + S) or d < (abs(R - S)):
# print("Two circles have no intersection")
return None, None
elif d == 0:
# print("Two circles have same center!")
return None, None
else:
A = (R ** 2 - S ** 2 + d ** 2) / (2 * d)
h = math.sqrt(R ** 2 - A ** 2)
x2 = x + A * (a - x) / d
y2 = y + A * (b - y) / d
x3 = round(x2 - h * (b - y) / d, 2)
y3 = round(y2 + h * (a - x) / d, 2)
x4 = round(x2 + h * (b - y) / d, 2)
y4 = round(y2 - h * (a - x) / d, 2)
c1 = [x3, y3]
c2 = [x4, y4]
return c1, c2
def location(ver, ang, tra0, tra1, tra2):
ver = polar_to_cartesian(ver)
tra0 = polar_to_cartesian(tra0)
tra1 = polar_to_cartesian(tra1)
tra2 = polar_to_cartesian(tra2)
res = []
r1 = abs(distance_artesian(tra1, tra0) / (2 * math.sin(ang[0])))
r2 = abs(distance_artesian(tra2, tra0) / (2 * math.sin(ang[1])))
o1_list = get_circle_r(tra1, tra0, r1)
o1 = get_circle_p(tra1, tra0, ver)
if distance_artesian(o1_list[0], o1) - r1 > distance_artesian(o1_list[1], o1) - r1:
o1 = o1_list[1]
else:
o1 = o1_list[0]
o2_list = get_circle_r(tra2, tra0, r2)
o2 = get_circle_p(tra2, tra0, ver)
if distance_artesian(o2_list[0], o2) - r2 > distance_artesian(o2_list[1], o2) - r2:
o2 = o2_list[1]
else:
o2 = o2_list[0]
p1, p2 = insec(o1, r1, o2, r2)
if p1 is not None and p1 != [0.0, 0.0]:
res.append(p1)
if p2 is not None and p2 != [0.0, 0.0]:
res.append(p2)
result = res[0]
for i in res:
if distance_artesian(result, ver) > distance_artesian(i, ver):
result = i
return result
def adjust(k1, k2, k3):
par = random.uniform(0.05,0.1)
kid = [par * angle(k1, F1, F0) + (1 - par) * angle(k3, F1, F0), par * angle(k1, F0, F2) + (1 - par) * angle(k3, F0, k2)]
print(kid)
print(location(k1, kid, F0, F1, k2))
print(polar_to_cartesian(k1), '理想位置')
print(polar_to_cartesian(k3), '实际位置')
return location(k1, kid, F0, F1, k2)
FY3 = adjust(F3,FY2,FY3)
FY4 = adjust(F4,FY3,FY4)
FY5 = adjust(F5,FY4,FY5)
FY6 = adjust(F6,FY5,FY6)
FY7 = adjust(F7,FY6,FY7)
FY8 = adjust(F8,FY7,FY8)
FY9 = adjust(F9,FY8,FY9)
FY2 = adjust(F2,FY9,FY2)
FY2 = adjust(F2,FY9,FY2)
