diff --git a/A/1/A1_to_csv.py b/A/1/A1_to_csv.py index 496de55..33a64c1 100644 --- a/A/1/A1_to_csv.py +++ b/A/1/A1_to_csv.py @@ -2,13 +2,13 @@ import json with open("A1_res.json", "r") as file: content=json.load(file) fout1=open("tmp1.dat","w") -for node_point in range(225): +for node_point in range(224): for time_point in range(301): v=content[time_point][node_point]["v"] print(v,'\t',file=fout1,sep="",end="") print('\n',file=fout1,end="",sep="") fout2=open("tmp2.dat","w") -for node_point in range(225): +for node_point in range(224): for time_point in range(301): x=content[time_point][node_point]["node"][0] print(x,'\t',file=fout2,sep="",end="") diff --git a/A/1/mp_cal.py b/A/1/mp_cal.py index 384b097..94a9c09 100644 --- a/A/1/mp_cal.py +++ b/A/1/mp_cal.py @@ -41,7 +41,7 @@ def CalcMoveList(time_point, last_theta): node_list = [{"theta": first_node_theta, "node": first_node_dot, "C": first_node_C, "v": mp.mpf('1.0')}] - for i in range(1, 225): + for i in range(1, 224): expected_distance = kSegLength1 if i == 1 else kSegLength2 cur_node_theta = GenerateFollowNodeTheta(node_list[-1]["theta"], expected_distance) cur_node_dot = Theta2Dot(cur_node_theta) diff --git a/A/1/mp_cal_it.py b/A/1/mp_cal_it.py new file mode 100644 index 0000000..e11ed49 --- /dev/null +++ b/A/1/mp_cal_it.py @@ -0,0 +1,77 @@ +import mpmath as mp +import json + +mp.dps = 15 # 设置精度为15位小数 + +kSegLength1 = mp.mpf('2.86') +kSegLength2 = mp.mpf('1.65') +kAlpha = mp.mpf('0.55') / (2 * mp.pi) + +def Theta2C(theta): + tmp = mp.sqrt(1 + theta**2) + return kAlpha * 0.5 * (theta * tmp - mp.log(-theta + tmp)) + +def Theta2Dot(theta): + return (kAlpha * theta * mp.cos(theta), kAlpha * theta * mp.sin(theta)) + +init_C=Theta2C(2*mp.pi*16) +def GenerateFirstNodeTheta(time_point, last_theta): + cur_C = init_C - time_point + def f(theta): + return Theta2C(theta) - cur_C + return mp.findroot(f, last_theta, solver='secant') + +def GenerateFollowNodeTheta(cur_node_theta, expected_distance): + cur_node_dot = Theta2Dot(cur_node_theta) + def f(theta): + test_node_dot = Theta2Dot(theta) + actual_distance = mp.sqrt((cur_node_dot[0]-test_node_dot[0])**2 + (cur_node_dot[1]-test_node_dot[1])**2) + return actual_distance - expected_distance + return mp.findroot(f, cur_node_theta + 0.1, solver='secant') + +def CalcMoveList(time_point, last_theta): + first_node_theta = GenerateFirstNodeTheta(time_point, last_theta) + first_node_dot = Theta2Dot(first_node_theta) + first_node_C = Theta2C(first_node_theta) + + node_list = [{"theta": first_node_theta, "node": first_node_dot, "C": first_node_C, "v": mp.mpf('1.0')}] + + for i in range(1, 224): + expected_distance = kSegLength1 if i == 1 else kSegLength2 + cur_node_theta = GenerateFollowNodeTheta(node_list[-1]["theta"], expected_distance) + cur_node_dot = Theta2Dot(cur_node_theta) + cur_node_C = Theta2C(cur_node_theta) + node_list.append({"theta": cur_node_theta, "node": cur_node_dot, "C": cur_node_C}) + for i in range(223): + AA = kSegLength1 if i == 0 else kSegLength2 + theta_i = node_list[i]["theta"] + theta_ip1 = node_list[i+1]["theta"] + alpha_i = mp.atan(theta_i) + alpha_ip1 = mp.atan(theta_ip1) + beta_i = mp.acos(((kAlpha*theta_i)**2 + AA**2 - (kAlpha*theta_ip1)**2) / (2*kAlpha*theta_i*AA)) + gama_i = mp.acos(((kAlpha*theta_ip1)**2 + AA**2 - (kAlpha*theta_i)**2) / (2*kAlpha*theta_ip1*AA)) + node_list[i+1]["v"] = node_list[i]["v"] * (-mp.cos(alpha_i + beta_i) / mp.cos(alpha_ip1 - gama_i)) + + return node_list, first_node_theta + +res_list = [] +last_theta = 2 * mp.pi * 16 + +for time_point in range(301): + print(f"calculating time_point={time_point}") + move_list, last_theta = CalcMoveList(time_point, last_theta) + res_list.append(move_list) + +# 将结果转换为float并保留6位小数 +float_res_list = [ + [ + {k: round(float(v), 6) if isinstance(v, mp.mpf) else + [round(float(x), 6) for x in v] if isinstance(v, tuple) else v + for k, v in node.items()} + for node in time_point + ] + for time_point in res_list +] + +with open("A1_res.json", "w") as file: + json.dump(float_res_list, file, indent=4) \ No newline at end of file