From ad195eca762c580ad06cbea6ed89837bcf8c4cf3 Mon Sep 17 00:00:00 2001 From: ZhuangYumin Date: Sat, 7 Sep 2024 14:30:20 +0800 Subject: [PATCH] Orbit OK --- A/4/loong.py | 2 +- A/4/simulator.py | 146 ++++++++++++++++++++++++++++++++++------------- 2 files changed, 108 insertions(+), 40 deletions(-) diff --git a/A/4/loong.py b/A/4/loong.py index fe313d8..8ec53db 100644 --- a/A/4/loong.py +++ b/A/4/loong.py @@ -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): diff --git a/A/4/simulator.py b/A/4/simulator.py index 12ff5d0..862fba0 100644 --- a/A/4/simulator.py +++ b/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)