Orbit OK
This commit is contained in:
@@ -1,7 +1,7 @@
|
||||
import mpmath as mp
|
||||
import numba as nb
|
||||
|
||||
mp.dps = 25 # 设置精度为25位小数
|
||||
mp.dps = 50 # 设置精度为50位小数
|
||||
|
||||
class Orbit:
|
||||
def __init__(self):
|
||||
|
||||
146
A/4/simulator.py
146
A/4/simulator.py
@@ -3,6 +3,10 @@ import json
|
||||
import numpy as np
|
||||
import sys
|
||||
import matplotlib.pyplot as plt
|
||||
import io
|
||||
from PIL import Image
|
||||
from matplotlib.patches import Rectangle
|
||||
import multiprocessing
|
||||
|
||||
|
||||
class GoodOrbit(Orbit):
|
||||
@@ -18,10 +22,14 @@ class GoodOrbit(Orbit):
|
||||
self.point_B_cartesian = (-self.kAlpha * self.kCriticalTheta * mp.cos(self.kCriticalTheta),
|
||||
-self.kAlpha * self.kCriticalTheta * mp.sin(self.kCriticalTheta))
|
||||
self.kPhi = mp.atan(self.kCriticalTheta)
|
||||
print(f"Phi={self.kPhi}", file=sys.stderr)
|
||||
dx, dy = self.point_A_cartesian[0] - self.point_B_cartesian[0], self.point_A_cartesian[1] - self.point_B_cartesian[1]
|
||||
self.angle = mp.atan2(dy, dx)
|
||||
self.point_C1_cartesian = (self.point_A_cartesian[0] - 2 * self.r * dx, self.point_A_cartesian[1] - 2 * self.r * dy)
|
||||
self.point_C2_cartesian = (self.point_B_cartesian[0] + 1 * self.r * dx, self.point_B_cartesian[1] + 1 * self.r * dy)
|
||||
print(f"angle={self.angle}", file=sys.stderr)
|
||||
self.point_C1_cartesian = (self.point_A_cartesian[0] + 2 * self.r * mp.cos(self.angle + 0.5 * mp.pi + self.kPhi),
|
||||
self.point_A_cartesian[1] + 2 * self.r * mp.sin(self.angle + 0.5 * mp.pi + self.kPhi))
|
||||
self.point_C2_cartesian = (self.point_B_cartesian[0] + 1 * self.r * mp.cos(self.angle - 0.5 * mp.pi + self.kPhi),
|
||||
self.point_B_cartesian[1] + 1 * self.r * mp.sin(self.angle - 0.5 * mp.pi + self.kPhi))
|
||||
self.radius_of_C1 = 2 * self.r
|
||||
self.radius_of_C2 = 1 * self.r
|
||||
self.arclength = 6 * self.r * self.kPhi
|
||||
@@ -66,7 +74,31 @@ class GoodOrbit(Orbit):
|
||||
return -self.kAlpha * 0.5 * (theta * tmp - mp.log(-theta + tmp)) + self.edge_raw_C - self.arclength
|
||||
|
||||
def Idx2Cartesian(self, idx):
|
||||
return mp.matrix([mp.cos(idx), mp.sin(idx)])
|
||||
if idx >= 0:
|
||||
theta = idx + self.kCriticalTheta
|
||||
return [self.kAlpha * theta * mp.cos(theta), self.kAlpha * theta * mp.sin(theta)]
|
||||
elif idx >= -2 * self.kCriticalTheta:
|
||||
c = self.Idx2C(idx) + self.arclength
|
||||
# if c < 0 or c > self.arclength:
|
||||
# raise Exception(f"idx={idx}, c={c}")
|
||||
if c <= self.arclength / 3:
|
||||
# In C2
|
||||
delta_angle = c / self.radius_of_C2
|
||||
actual_angle = self.angle + 0.5 * mp.pi + self.kPhi - delta_angle
|
||||
return [
|
||||
self.point_C2_cartesian[0] + self.radius_of_C2 * mp.cos(actual_angle),
|
||||
self.point_C2_cartesian[1] + self.radius_of_C2 * mp.sin(actual_angle)
|
||||
]
|
||||
else:
|
||||
delta_angle = (c - self.arclength / 3) / self.radius_of_C1
|
||||
actual_angle = self.angle - 0.5 * mp.pi - self.kPhi + delta_angle
|
||||
return [
|
||||
self.point_C1_cartesian[0] + self.radius_of_C1 * mp.cos(actual_angle),
|
||||
self.point_C1_cartesian[1] + self.radius_of_C1 * mp.sin(actual_angle)
|
||||
]
|
||||
else:
|
||||
theta = -idx - self.kCriticalTheta
|
||||
return [-self.kAlpha * theta * mp.cos(theta), -self.kAlpha * theta * mp.sin(theta)]
|
||||
|
||||
def C2Idx(self, C):
|
||||
|
||||
@@ -75,48 +107,82 @@ class GoodOrbit(Orbit):
|
||||
|
||||
return mp.findroot(f, 0, solver='secant')
|
||||
|
||||
def GenerateNextPointIdx(self, cur_point_idx, expected_distance):
|
||||
return cur_point_idx + expected_distance
|
||||
def GenerateNextPointIdx(self, cur_point_idx, expected_distance, guess=None):
|
||||
if guess is None:
|
||||
cur_point_C = self.Idx2C(cur_point_idx)
|
||||
guess = self.C2Idx(cur_point_C + expected_distance)
|
||||
cur_point_dot = self.Idx2Cartesian(cur_point_idx)
|
||||
|
||||
def f(idx):
|
||||
test_point_dot = self.Idx2Cartesian(idx)
|
||||
return mp.sqrt((cur_point_dot[0] - test_point_dot[0])**2 +
|
||||
(cur_point_dot[1] - test_point_dot[1])**2) - expected_distance
|
||||
|
||||
return mp.findroot(f, guess, solver='secant')
|
||||
|
||||
def GenerateImg(self, node_list):
|
||||
fig = plt.figure(figsize=(12, 12))
|
||||
|
||||
# 绘制轨道线
|
||||
idx_list = np.linspace(-8 * 2 * np.pi, 8 * 2 * np.pi, 10000)
|
||||
x = [float(self.Idx2Cartesian(t)[0]) for t in idx_list]
|
||||
y = [float(self.Idx2Cartesian(t)[1]) for t in idx_list]
|
||||
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')
|
||||
|
||||
# 创建一个 BytesIO 对象来存储图像
|
||||
buf = io.BytesIO()
|
||||
plt.savefig(buf, format='png')
|
||||
buf.seek(0)
|
||||
|
||||
# 清除当前图形，释放内存
|
||||
plt.close(fig)
|
||||
|
||||
# 返回图像对象
|
||||
return Image.open(buf)
|
||||
|
||||
|
||||
orbit = GoodOrbit()
|
||||
|
||||
|
||||
def f(x):
|
||||
return float(orbit.Idx2C(x))
|
||||
|
||||
|
||||
# 获取 kCriticalTheta
|
||||
kCriticalTheta = float(orbit.kCriticalTheta)
|
||||
print(f"kCriticalTheta={kCriticalTheta}")
|
||||
|
||||
# 定义范围 [-kCriticalTheta-1, kCriticalTheta +1]
|
||||
start = -2.5 * kCriticalTheta - 1
|
||||
end = 0.5 * kCriticalTheta + 1
|
||||
|
||||
# 生成范围内的点
|
||||
x_vals = np.linspace(start, end, 1000)
|
||||
|
||||
# 计算 f(x) 的值
|
||||
y_vals = [f(mp.mpf(x)) for x in x_vals]
|
||||
|
||||
# 绘制图像
|
||||
plt.figure(figsize=(10, 6))
|
||||
plt.plot(x_vals, y_vals, label='Idx2C(x)')
|
||||
plt.title('Idx2C function plot')
|
||||
plt.xlabel('Index (x)')
|
||||
plt.ylabel('C Value')
|
||||
plt.grid(True)
|
||||
plt.axhline(0, color='black', linewidth=0.5)
|
||||
plt.axvline(0, color='black', linewidth=0.5)
|
||||
plt.legend()
|
||||
plt.show()
|
||||
sys.exit()
|
||||
if __name__ == "__main__":
|
||||
orbit = GoodOrbit()
|
||||
loong = Loong(orbit, 224, mp.mpf("2.0"), mp.mpf("1e-8"))
|
||||
res_list = []
|
||||
for ti in range(-100, 101):
|
||||
for ti in range(-10, 2):
|
||||
print(f"calculating time_point={ti}")
|
||||
res_list.append(loong.CalcStatusListByTime(mp.mpf(ti)))
|
||||
# 转换成内置浮点数并保留6位
|
||||
@@ -131,3 +197,5 @@ if __name__ == "__main__":
|
||||
} for node in res] for res in res_list]
|
||||
with open("A4_res.json", "w") as file:
|
||||
json.dump(float_res_list, file, indent=4)
|
||||
img_list = [orbit.GenerateImg(res) for res in res_list]
|
||||
img_list[0].save("A4.gif", save_all=True, append_images=img_list[1:], duration=1000, loop=0)
|
||||
|
||||
Reference in New Issue
Block a user