diff --git a/A/4/kinematics_check.py b/A/4/kinematics_check.py new file mode 100644 index 0000000..27a03bd --- /dev/null +++ b/A/4/kinematics_check.py @@ -0,0 +1,2 @@ +import loong +loong.kPointsConsidered = 2 # Just Check the first 2 blocks to see whether it will be stuck \ No newline at end of file diff --git a/A/4/loong.py b/A/4/loong.py new file mode 100644 index 0000000..fe313d8 --- /dev/null +++ b/A/4/loong.py @@ -0,0 +1,54 @@ +import mpmath as mp +import numba as nb + +mp.dps = 25 # 设置精度为25位小数 + +class Orbit: + def __init__(self): + self.kAlpha = mp.mpf('1.7') / (2 * mp.pi) + def InitIdx(self) -> mp.mpf: + raise NotImplementedError + def InitC(self) -> mp.mpf: + raise NotImplementedError + def Idx2C(self, idx) -> mp.mpf: + raise NotImplementedError + def Idx2Cartesian(self, idx) -> [mp.mpf, mp.mpf]: + raise NotImplementedError + def C2Idx(self, C:[mp.mpf, mp.mpf]) -> mp.mpf: + raise NotImplementedError + def GenerateNextPointIdx(self, cur_point_idx:mp.mpf, expected_distance:mp.mpf)->mp.mpf: + raise NotImplementedError + +class Loong: + def __init__(self, orbit:Orbit, total_points:int, speed:mp.mpf, delta_idx:mp.mpf): + self.orbit = orbit + self.total_points = total_points + self.speed = speed + self.delta_idx = delta_idx + self.kSegLength1 = mp.mpf('2.86') + self.kSegLength2 = mp.mpf('1.65') + def CalcStatusListByIdx(self, cur_idx:mp.mpf): + first_node_idx=cur_idx + first_node_C=self.orbit.Idx2C(first_node_idx) + first_node_dot = self.orbit.Idx2Cartesian(first_node_idx) + virtual_first_node_idx = first_node_idx + self.delta_idx + virtual_first_node_C = self.orbit.Idx2C(virtual_first_node_idx) + delta_T = (virtual_first_node_C - first_node_C) / self.speed + node_list = [{"idx": first_node_idx, "node": first_node_dot, "C": first_node_C, "v": mp.mpf('1.0')}] + for i in range(1, self.total_points): + expected_distance = self.kSegLength1 if i == 1 else self.kSegLength2 + cur_node_idx = self.orbit.GenerateNextPointIdx(node_list[-1]["idx"], expected_distance) + cur_node_dot = self.orbit.Idx2Cartesian(cur_node_idx) + cur_node_C = self.orbit.Idx2C(cur_node_idx) + + virtual_cur_node_idx = self.orbit.GenerateNextPointIdx(virtual_first_node_idx, expected_distance) + virtual_cur_node_C = self.orbit.Idx2C(virtual_cur_node_idx) + v = (virtual_cur_node_C - cur_node_C) / delta_T + + node_list.append({"idx": cur_node_idx, "node": cur_node_dot, "C": cur_node_C, "v": v}) + virtual_first_node_idx = virtual_cur_node_idx + return node_list + def CalcStatusListByTime(self, time_point:mp.mpf): + first_node_C = self.orbit.InitC() - time_point * self.speed + first_node_idx = self.orbit.C2Idx(first_node_C) + return self.CalcStatusListByIdx(first_node_idx) \ No newline at end of file diff --git a/A/4/simulator.py b/A/4/simulator.py new file mode 100644 index 0000000..508a89d --- /dev/null +++ b/A/4/simulator.py @@ -0,0 +1,40 @@ +from loong import * +import json +class BestOrbit(Orbit): + def __init__(self): + self.kAlpha = mp.mpf('1.7') / (2 * mp.pi) + def InitIdx(self): + return mp.mpf('0.0') + def InitC(self): + return mp.mpf('0.0') + def Idx2C(self, idx): + return idx / self.kAlpha + def Idx2Cartesian(self, idx): + return mp.matrix([mp.cos(idx), mp.sin(idx)]) + def C2Idx(self, C): + return C * self.kAlpha + def GenerateNextPointIdx(self, cur_point_idx, expected_distance): + return cur_point_idx + expected_distance + +if __name__ == "__main__": + orbit=BestOrbit() + loong=Loong(orbit, 224, mp.mpf('2.0'), mp.mpf('1e-8')) + res_list=[] + for ti in range(-100,101): + print(f"calculating time_point={ti}") + res_list.append(loong.CalcStatusListByTime(mp.mpf(ti))) + # 转换成内置浮点数并保留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) \ No newline at end of file diff --git a/A/4/testplot.py b/A/4/testplot.py index f595a5f..cd297d4 100644 --- a/A/4/testplot.py +++ b/A/4/testplot.py @@ -74,7 +74,7 @@ theta_grid = np.arctan2(Y, X) # 仅绘制半径不超过kPlotingRadius的点 valid_points = r_grid <= kPlotingRadius -ax.scatter(theta_grid[valid_points], r_grid[valid_points], color='red', s=10) # 红色小点 +ax.scatter(theta_grid[valid_points], r_grid[valid_points], color='grey', s=10) # 灰色小点 plt.title("The Moving Path")