130 lines
4.4 KiB
Python
130 lines
4.4 KiB
Python
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) |