from loong import * 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 BetterOrbit(Orbit): def __init__(self): self.kAlpha = mp.mpf("1.7") / (2 * mp.pi) def f(x): r=(1/3)*self.kAlpha*mp.sqrt(1+x**2) phi=mp.atan(x) L=mp.mpf("2.86") return (r+3*r*mp.cos(mp.pi-2*phi)-L)**2+(3*r*mp.sin(mp.pi-2*phi))**2-L**2 self.kCriticalTheta = mp.findroot(f, 15, solver='secant') print(f"CriticalTheta={self.kCriticalTheta}", file=sys.stderr) self.r = (1 / 3) * self.kAlpha * mp.sqrt(1 + self.kCriticalTheta**2) self.point_A_cartesian = ( self.kAlpha * self.kCriticalTheta * mp.cos(self.kCriticalTheta), self.kAlpha * self.kCriticalTheta * mp.sin(self.kCriticalTheta), ) 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) 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 self.edge_k = self.kAlpha * mp.sqrt(1 + self.kCriticalTheta * self.kCriticalTheta) self.n = -1 for i in range(3, 20, 2): self.a = (self.arclength - 2 * self.edge_k * self.kCriticalTheta) / (2 * (1 - i) * self.kCriticalTheta**i) self.b = (i * self.arclength - 2 * self.edge_k * self.kCriticalTheta) / (2 * (i - 1) * self.kCriticalTheta) if self.a > 0 and self.b > 0: self.n = i break print(f"arclength={self.arclength}", file=sys.stderr) print(f"edge_k={self.edge_k}", file=sys.stderr) print(f"a={self.a}", file=sys.stderr) print(f"b={self.b}", file=sys.stderr) print(f"n={self.n}", file=sys.stderr) print(f"now k={self.n*self.a*self.kCriticalTheta**(self.n-1)+self.b}", file=sys.stderr) if self.n == -1: raise Exception("n must be set") self.edge_raw_C = self.kAlpha * 0.5 * ( self.kCriticalTheta * mp.sqrt(1 + self.kCriticalTheta * self.kCriticalTheta) - mp.log(-self.kCriticalTheta + mp.sqrt(1 + self.kCriticalTheta * self.kCriticalTheta))) def InitIdx(self): return mp.mpf("0.0") def InitC(self): return mp.mpf("0.0") def Idx2C(self, idx): # this function must be monotonically increasing if idx >= 0: theta = idx + self.kCriticalTheta tmp = mp.sqrt(1 + theta * theta) return self.kAlpha * 0.5 * (theta * tmp - mp.log(-theta + tmp)) - self.edge_raw_C elif idx >= -2 * self.kCriticalTheta: x = idx + self.kCriticalTheta y = (self.a * (x**self.n) + self.b * x) - 0.5 * self.arclength return y else: theta = -idx - self.kCriticalTheta tmp = mp.sqrt(1 + theta * theta) return -self.kAlpha * 0.5 * (theta * tmp - mp.log(-theta + tmp)) + self.edge_raw_C - self.arclength def Idx2Cartesian(self, 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): def f(idx): return self.Idx2C(idx) - C return mp.findroot(f, (-100*2*mp.pi,100*2*mp.pi), solver='bisect') 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(-12 * 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) if __name__ == "__main__": orbit = BetterOrbit() loong = Loong(orbit, 224, mp.mpf("1.0"), mp.mpf("1e-8")) res_list = [] for ti in np.arange(10, 20, 0.025): print(f"calculating time_point={ti}", file=sys.stderr) res_list.append(loong.CalcStatusListByTime(mp.mpf(ti), res_list[-1] if res_list else None)) # 转换成内置浮点数并保留6位 float_res_list = [[{ "idx": round(float(node["idx"]), 6), "node": [ round(float(node["node"][0]), 6), round(float(node["node"][1]), 6), ], "C": round(float(node["C"]), 6), "v": round(float(node["v"]), 6), } 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=25, loop=0)