import mpmath as mp import json import sys import matplotlib.pyplot as plt from matplotlib.patches import Rectangle import numpy as np if __name__ != "__main__": sys.exit() mp.dps = 50 # 设置精度为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): cur_C = init_C - time_point def f(theta): return Theta2C(theta) - cur_C return mp.findroot(f, 2*mp.pi*16, 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') kPotingPoints=50 def CalcMoveList(time_point): first_node_theta = GenerateFirstNodeTheta(time_point) 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, kPotingPoints): 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(kPotingPoints-1): 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 time_point= float(input()) print(f"calculating time_point={time_point}",file=sys.stderr) time_point_list = CalcMoveList(time_point) # 将结果转换为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_list ] # print(float_res_list) json.dump(float_res_list, sys.stdout, indent=4) def visualize_spiral(node_list): plt.figure(figsize=(12, 12)) # 绘制灰色螺旋线 theta = np.linspace(0, float(node_list[-1]["theta"]), 1000) x = [float(Theta2Dot(t)[0]) for t in theta] y = [float(Theta2Dot(t)[1]) for t in theta] plt.plot(x, y, color='gray', linewidth=0.5) # 绘制节点、连接线和木板 for i in range(len(node_list) - 1): x1, y1 = [float(coord) for coord in node_list[i]["node"]] x2, y2 = [float(coord) for coord in node_list[i+1]["node"]] # 绘制红色节点 plt.plot(x1, y1, 'ro', markersize=3) # 绘制蓝色连接线 plt.plot([x1, x2], [y1, y2], 'b-', linewidth=0.5) # 计算并绘制木板(长方形) dx = x2 - x1 dy = y2 - y1 length = np.sqrt(dx**2 + dy**2) angle = np.arctan2(dy, dx) rect_length = length + 0.55 # 总长度加上两端各延伸的0.275m rect_width = 0.3 # 计算长方形的中心点 center_x = (x1 + x2) / 2 center_y = (y1 + y2) / 2 # 计算长方形的左下角坐标 rect_x = center_x - rect_length/2 * np.cos(angle) + rect_width/2 * np.sin(angle) rect_y = center_y - rect_length/2 * np.sin(angle) - rect_width/2 * np.cos(angle) rect = Rectangle((rect_x, rect_y), rect_length, rect_width, angle=angle*180/np.pi, fill=False, edgecolor='g') plt.gca().add_patch(rect) # 绘制最后一个节点 x, y = [float(coord) for coord in node_list[-1]["node"]] plt.plot(x, y, 'ro', markersize=3) plt.axis('equal') plt.title(f"Spiral visualization at time={time_point}") plt.show() # 在计算完节点列表后调用可视化函数 visualize_spiral(float_res_list)