commit 9528906db8fbaa68892d636d12f87ade4948433d Author: ZhuangYumin Date: Thu Sep 5 20:54:18 2024 +0800 half finish A1 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c1f7946 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +*.swp +*.json +*.bak \ No newline at end of file diff --git a/A/1/A1_to_csv.py b/A/1/A1_to_csv.py new file mode 100644 index 0000000..7267c5a --- /dev/null +++ b/A/1/A1_to_csv.py @@ -0,0 +1,5 @@ +import json +with open("A1_res.json", "r") as file: + content=json.load(file) +for lst in content: + print(lst[0]) \ No newline at end of file diff --git a/A/1/cal.py b/A/1/cal.py new file mode 100644 index 0000000..134d85b --- /dev/null +++ b/A/1/cal.py @@ -0,0 +1,103 @@ +import sys +import json +import numba +import math + +kSegLength1=2.86 +kSegLength2=1.65 +kAlpha=0.55/(2*math.pi) +if __name__ !="__main__": + sys.exit() + +@numba.njit +def Theta2C(theta): + tmp=math.sqrt(1+theta*theta) + return kAlpha*0.5*(theta*tmp-math.log(-theta+tmp)) + +@numba.njit +def Theta2Dot(theta): + return (kAlpha*theta*math.cos(theta),kAlpha*theta*math.sin(theta)) +res_list=[] +init_C=Theta2C(2*math.pi*16) +print(f"init_C={init_C}") +GenerateFirstNodeTheta_last_res_store=2*math.pi*16 +@numba.njit +def GenerateFirstNodeTheta(time_point,GenerateFirstNodeTheta_last_res): + cur_C=init_C-1*time_point + R=GenerateFirstNodeTheta_last_res + L=GenerateFirstNodeTheta_last_res-10.0 + if L<0: + L=0 + M=(L+R)/2 + kEps=1e-10 + while True: + test_C=Theta2C(M) + # print(L,M,R,test_C-cur_C) + if -kEps < test_C-cur_C cur_C: + R=M + M=(L+R)/2 + GenerateFirstNodeTheta_last_res=M + return M + +@numba.njit +def GenerateFollowNodeTheta(cur_node_theta,expected_distance): + cur_node_dot= Theta2Dot(cur_node_theta) + step=1.0 + res_node_theta=cur_node_theta + kEps=1e-10 + # print(f"expected_distance={expected_distance}") + while True: + test_node_theta=res_node_theta+step + test_node_dot=Theta2Dot(test_node_theta) + actual_distance=math.sqrt((cur_node_dot[0]-test_node_dot[0])**2 + (cur_node_dot[1]-test_node_dot[1])**2) + # print(f"actual_distance={actual_distance}, expected_distance={expected_distance}, step={step}") + if actual_distance < expected_distance-kEps: + res_node_theta+=step + elif actual_distance < expected_distance+kEps: + break + else: + pass + step*=0.5 + return res_node_theta + +def CalcMoveList(time_point): + global GenerateFirstNodeTheta_last_res_store + first_node_theta=GenerateFirstNodeTheta(time_point,GenerateFirstNodeTheta_last_res_store) + GenerateFirstNodeTheta_last_res_store=first_node_theta + first_node_dot=Theta2Dot(first_node_theta) + first_node_C=Theta2C(first_node_theta) + # print(f"\t{node_list}") + delta_theta=1e-4 + virtual_first_node_theta=first_node_theta+delta_theta + virtual_first_node_C=Theta2C(virtual_first_node_theta) + virtual_lst_node_theta=virtual_first_node_theta + delta_T=virtual_first_node_C-first_node_C + node_list=[{"delta_theta":delta_theta,"delta_T":delta_T},{"theta":first_node_theta,"node":first_node_dot,"C":first_node_C,"v":1.0}] + for i in range(1,225): + if i==1: + expected_distance=kSegLength1 + else: + expected_distance=kSegLength2 + # print(f"i={i}, expected_distance={expected_distance}") + cur_node_theta=GenerateFollowNodeTheta(node_list[-1]["theta"],expected_distance) + cur_node_dot= Theta2Dot(cur_node_theta) + cur_node_C=Theta2C(cur_node_theta) + virtural_cur_node_theta=GenerateFollowNodeTheta(virtual_lst_node_theta,expected_distance) + virtural_cur_node_C=Theta2C(virtural_cur_node_theta) + v=(virtural_cur_node_C-cur_node_C)/delta_T + node_list.append({"theta":cur_node_theta,"node":cur_node_dot,"C":cur_node_C,"v":v}) + virtual_lst_node_theta=virtural_cur_node_theta + # print(f"\t{node_list[-1]}") + return node_list + +for time_point in range(0,301): + print(f"calculating time_point={time_point}") + res_list.append(CalcMoveList(time_point)) + +with open("A1_res.json","w") as file: + json.dump(res_list, file, indent=4) \ No newline at end of file diff --git a/A/1/mp_cal.py b/A/1/mp_cal.py new file mode 100644 index 0000000..384b097 --- /dev/null +++ b/A/1/mp_cal.py @@ -0,0 +1,79 @@ +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) + + delta_theta = mp.mpf('1e-8') + virtual_first_node_theta = first_node_theta + delta_theta + virtual_first_node_C = Theta2C(virtual_first_node_theta) + delta_T = virtual_first_node_C - first_node_C + + 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): + 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) + + virtual_cur_node_theta = GenerateFollowNodeTheta(virtual_first_node_theta, expected_distance) + virtual_cur_node_C = Theta2C(virtual_cur_node_theta) + v = (virtual_cur_node_C - cur_node_C) / delta_T + + node_list.append({"theta": cur_node_theta, "node": cur_node_dot, "C": cur_node_C, "v": v}) + virtual_first_node_theta = virtual_cur_node_theta + + 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