ready to publish code

.gitignore

@@ -6,3 +6,4 @@
 *.gif
 __pycache__/
 .idea/
+/setup.cfg

A/1/mp_cal.py

@@ -1,7 +1,7 @@
 import mpmath as mp
 import json
 
-mp.dps = 15  # 设置精度为15位小数
+mp.dps = 50  # 设置精度为15位小数
 
 kSegLength1 = mp.mpf('2.86')
 kSegLength2 = mp.mpf('1.65')

A/1/mp_cal_it.py

@@ -1,7 +1,7 @@
 import mpmath as mp
 import json
 
-mp.dps = 15  # 设置精度为15位小数
+mp.dps = 50  # 设置精度为50位小数
 
 kSegLength1 = mp.mpf('2.86')
 kSegLength2 = mp.mpf('1.65')

A/2/collision_cal.py

@@ -9,7 +9,7 @@ import threading
 import numba
 import multiprocessing
 
-mp.dps = 15  # 设置精度为15位小数
+mp.dps = 50  # 设置精度为50位小数
 
 kSegLength1 = mp.mpf('2.86')
 kSegLength2 = mp.mpf('1.65')
@@ -35,7 +35,7 @@ def GenerateFollowNodeTheta(cur_node_theta, expected_distance):
     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',tol=1e-20)
+  return mp.findroot(f, cur_node_theta + 0.1, solver='secant')
 
 kPointsConsidered=50
 def CalcMoveList(time_point):

A/2/position_watcher.py

@@ -8,7 +8,7 @@ import numpy as np
 if __name__ != "__main__":
   sys.exit()
 
-mp.dps = 15  # 设置精度为15位小数
+mp.dps = 50  # 设置精度为15位小数
 
 kSegLength1 = mp.mpf('2.86')
 kSegLength2 = mp.mpf('1.65')

A/3/dragon.py

@@ -11,7 +11,7 @@ import multiprocessing
 import io
 from PIL import Image
 
-mp.dps = 15  # 设置精度为15位小数
+mp.dps = 50  # 设置精度为50位小数
 
 kSegLength1 = mp.mpf('2.86')
 kSegLength2 = mp.mpf('1.65')
@@ -37,7 +37,7 @@ class Dragon:
       test_node_dot = self.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',tol=1e-20)
+    return mp.findroot(f, cur_node_theta + 0.1, solver='secant')
 
 
   def CalcMoveList(self, delta_theta=0):

A/3/full_valid_test.py

@@ -1,15 +1,15 @@
 from dragon import *
-kPitchToTest=0.450338
+kPitchToTest=mp.mpf("0.45033740")
 kDeltaThetaBeg=0
 kDeltaThetaEnd=2*2*3.1415926
-kTotalSteps=100000
+kTotalSteps=10000
 kStepDeltaTheta=(kDeltaThetaEnd-kDeltaThetaBeg)/kTotalSteps
 kParallelNum=24
 tasks_list=[i for i in np.arange(kDeltaThetaBeg, kDeltaThetaEnd, kStepDeltaTheta)]
 task_list_per_process=[tasks_list[i::kParallelNum] for i in range(kParallelNum)]
 print(f"len(task_list_per_thread)={len(task_list_per_process)}",file=sys.stderr)
 def ProcessEntryPoint(arg):
-  dragen = Dragon(mp.mpf(kPitchToTest)/(2*mp.pi))
+  dragen = Dragon(kPitchToTest/(2*mp.pi))
   delta_theta_list, process_id = arg
   logf=open(f"sufficiency_test_{process_id}.log","w")
   print(f"calculating delta_theta_list={delta_theta_list} with process_id={process_id}",file=logf)
@@ -29,7 +29,7 @@ if __name__ == "__main__":
   else:
     print("OK")
     # Now generate an gif for human to check
-    dragen = Dragon(mp.mpf(kPitchToTest)/(2*mp.pi))
+    dragen = Dragon(kPitchToTest/(2*mp.pi))
     kTotalFrames=100
     kStepDeltaTheta=(kDeltaThetaEnd-kDeltaThetaBeg)/kTotalFrames
     frame_list=[]

A/3/low_bound_cal.py

@@ -2,7 +2,7 @@ from dragon import *
 
 kBegPitch = 0.3
 kEndPitch = 0.55
-kTotalSteps = 10000
+kTotalSteps = 250000
 kStepPitch = (kEndPitch - kBegPitch) / kTotalSteps
 kParallelNum=24
 tasks_list = [kBegPitch + kStepPitch * i for i in range(kTotalSteps)]
@@ -19,6 +19,8 @@ def ProcessEntryPoint(arg):
     status = CheckCollision(status2blocks(dragen.CalcMoveList()))
     tmp_res[pitch]=status
     print(f"res={status}",file=logf)
+    if status == -1:
+      break
   with lock:  # 添加锁保护对共享字典的操作
     shared_dict.update(tmp_res)
 

A/3/sufficiency_test.py

@@ -1,7 +1,7 @@
 from dragon import *
-kBegPitch = 0.4503
-kEndPitch = 0.4504
-kTotalSteps = 100
+kBegPitch = 0.45033
+kEndPitch = 0.45034
+kTotalSteps = 10
 kStepPitch = (kEndPitch - kBegPitch) / kTotalSteps
 kParallelNum=24
 tasks_list = [kBegPitch + kStepPitch * i for i in range(kTotalSteps)]
@@ -9,7 +9,7 @@ task_list_per_process=[tasks_list[i::kParallelNum] for i in range(kParallelNum)]
 
 kDeltaThetaBeg=0
 kDeltaThetaEnd=5*2*3.1415926
-kStepDeltaTheta=(kDeltaThetaEnd-kDeltaThetaBeg)/1000
+kStepDeltaTheta=(kDeltaThetaEnd-kDeltaThetaBeg)/10000
 print(f"len(task_list_per_thread)={len(task_list_per_process)}",file=sys.stderr)
 def ProcessEntryPoint(arg):
   pitch_list, process_id, shared_dict, lock = arg

A/4/A4_to_csv.py

@@ -0,0 +1,19 @@
+import json
+with open("A4_res.json", "r") as file:
+  content=json.load(file)
+fout1=open("tmp1.dat","w")
+for node_point in range(224):
+  for time_point in range(201):
+    v=content[time_point][node_point]["v"]
+    print(v,'\t',file=fout1,sep="",end="")
+  print('\n',file=fout1,end="",sep="")
+fout2=open("tmp2.dat","w")
+for node_point in range(224):
+  for time_point in range(201):
+    x=content[time_point][node_point]["node"][0]
+    print(x,'\t',file=fout2,sep="",end="")
+  print('\n',file=fout2,end="",sep="")
+  for time_point in range(201):
+    y=content[time_point][node_point]["node"][1]
+    print(y,'\t',file=fout2,sep="",end="")
+  print('\n',file=fout2,end="",sep="")

A/4/kinematics_check.py

@@ -1,2 +1,207 @@
-import loong
-loong.kPointsConsidered = 2 # Just Check the first 2 blocks to see whether it will be stuck
+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)

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):
@@ -16,7 +16,7 @@ class Orbit:
     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:
+  def GenerateNextPointIdx(self, cur_point_idx:mp.mpf, expected_distance:mp.mpf, guess=None)->mp.mpf:
     raise NotImplementedError
 
 class Loong:
@@ -27,28 +27,28 @@ class Loong:
     self.delta_idx = delta_idx
     self.kSegLength1 = mp.mpf('2.86')
     self.kSegLength2 = mp.mpf('1.65')
-  def CalcStatusListByIdx(self, cur_idx:mp.mpf):
+  def CalcStatusListByIdx(self, cur_idx:mp.mpf, last_time_status=None):
     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')}]
+    node_list = [{"idx": first_node_idx, "node": first_node_dot, "C": first_node_C, "v": self.speed}]
     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_idx = self.orbit.GenerateNextPointIdx(node_list[-1]["idx"], expected_distance, guess=last_time_status[i]["idx"] if last_time_status else None)
       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_idx = self.orbit.GenerateNextPointIdx(virtual_first_node_idx, expected_distance, guess=last_time_status[i]["idx"] if last_time_status else None)
       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):
+  def CalcStatusListByTime(self, time_point:mp.mpf, last_time_status=None):
     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)
+    return self.CalcStatusListByIdx(first_node_idx, last_time_status)

A/4/seek_max.py

@@ -0,0 +1,52 @@
+from simulator import *
+
+kTiBeg = -100
+kTiEnd = 100
+kSampleNum = 1000
+kStep = (kTiEnd - kTiBeg) / kSampleNum
+kParallelNum=24
+tasks_list = [i for i in np.arange(kTiBeg, kTiEnd, kStep)]
+for i in np.arange(10, 20, 1/1000):
+  tasks_list.append(i)
+for i in np.arange(12, 14.2, 1/10000):
+  tasks_list.append(i)
+for i in np.arange(13.085,13.095,1/1000000):
+  tasks_list.append(i)
+task_list_per_process=[tasks_list[i::kParallelNum] for i in range(kParallelNum)]
+
+print(f"len(task_list_per_thread)={len(task_list_per_process)}",file=sys.stderr)
+def ProcessEntryPoint(arg):
+  ti_list, process_id = arg
+  orbit = GoodOrbit()
+  loong = Loong(orbit, 224, mp.mpf("1.0"), mp.mpf("1e-8"))
+  max_speed_found=mp.mpf("0.0")
+  max_speed_time=0
+  last_status = None
+  last_ti = ti_list[0]
+  for ti in ti_list:
+    print(f"calculating time_point={ti}",file=sys.stderr)
+    try:
+      res = loong.CalcStatusListByTime(mp.mpf(ti), last_status if (last_status and abs(ti-last_ti)<=0.1) else None)
+      for node in res:
+        if node["v"] > max_speed_found:
+          max_speed_found = node["v"]
+          max_speed_time = ti
+      last_status = res
+      last_ti = ti
+    except ValueError as e:
+      print(f"Error: {e}",file=sys.stdout)
+  return max_speed_found, max_speed_time
+
+if __name__ == "__main__":
+  manager = multiprocessing.Manager()
+  task_args_list = [(task_list_per_process[i], i) for i in range(kParallelNum)]
+  with multiprocessing.Pool(processes=kParallelNum) as pool:
+    res_list=pool.map(ProcessEntryPoint, task_args_list)
+  max_speed_found=mp.mpf("0.0")
+  max_speed_time=0
+  for res in res_list:
+    if res[0]>max_speed_found:
+      max_speed_found=res[0]
+      max_speed_time=res[1]
+  valid_head_speed = mp.mpf("1.0") * (mp.mpf("2.0")/max_speed_found)
+  print(f"max_speed_found={max_speed_found} at {max_speed_time}, valid_head_speed={valid_head_speed}")

A/4/simulator.py

@@ -1,40 +1,207 @@
 from loong import *
 import json
-class BestOrbit(Orbit):
+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):
+
   def __init__(self):
-    self.kAlpha = mp.mpf('1.7') / (2 * mp.pi)
+    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')
+    return mp.mpf("0.0")
+
   def InitC(self):
-    return mp.mpf('0.0')
-  def Idx2C(self, idx):
-    return idx / self.kAlpha
+    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):
-    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):
-    return C * self.kAlpha
-  def GenerateNextPointIdx(self, cur_point_idx, expected_distance):
-    return cur_point_idx + expected_distance
+
+    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=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}")
+  orbit = GoodOrbit()
+  loong = Loong(orbit, 224, mp.mpf("1.0"), mp.mpf("1e-8"))
+  res_list = []
+  for ti in range(-100, 101):
+    print(f"calculating time_point={ti}", file=sys.stderr)
     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
-  ]
+  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=100, loop=0)

A/4/testplot.py

@@ -1,14 +1,20 @@
 import matplotlib.pyplot as plt
 import numpy as np
+import mpmath as mp
 
 kPitch = 1.7
 kAlpha = kPitch / (2 * np.pi)
 kCriticalRadius = 4.5
 
-theta_max = (kCriticalRadius) / kAlpha + 2*2*np.pi
+theta_max = (kCriticalRadius) / kAlpha + 2 * 2 * np.pi
 kPlotingRadius = theta_max * kAlpha
 
-kCriticalTheta =  2.86 / ((2/3)*kAlpha)
+def f(x):
+  r=(1/3)*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
+kCriticalTheta = float(mp.findroot(f, 15, solver='secant'))
 # 生成角度数组
 theta = np.linspace(kCriticalTheta, theta_max, 1000)
 
@@ -34,36 +40,40 @@ circle_theta = np.linspace(0, 2 * np.pi, 1000)
 circle_r = np.full_like(circle_theta, kCriticalRadius)
 ax.plot(circle_theta, circle_r, linestyle='--')
 
-point_A_cartesian = (kAlpha*kCriticalTheta*np.cos(kCriticalTheta),kAlpha*kCriticalTheta*np.sin(kCriticalTheta))
-point_B_cartesian = (-kAlpha*kCriticalTheta*np.cos(kCriticalTheta),-kAlpha*kCriticalTheta*np.sin(kCriticalTheta))
+point_A_cartesian = (kAlpha * kCriticalTheta * np.cos(kCriticalTheta), kAlpha * kCriticalTheta * np.sin(kCriticalTheta))
+point_B_cartesian = (-kAlpha * kCriticalTheta * np.cos(kCriticalTheta),
+                     -kAlpha * kCriticalTheta * np.sin(kCriticalTheta))
 kPhi = np.arctan(kCriticalTheta)
-r = (1/3) * kAlpha * np.sqrt(1 + kCriticalTheta**2)
+r = (1 / 3) * kAlpha * np.sqrt(1 + kCriticalTheta**2)
 dx, dy = point_A_cartesian[0] - point_B_cartesian[0], point_A_cartesian[1] - point_B_cartesian[1]
 angle = np.arctan2(dy, dx)
-dx, dy = np.cos(angle - (0.5*np.pi-kPhi)), np.sin(angle - (0.5*np.pi-kPhi))
-point_C1_cartesian = (point_A_cartesian[0] - 2*r*dx, point_A_cartesian[1] - 2*r*dy)
-point_C2_cartesian = (point_B_cartesian[0] + 1*r*dx, point_B_cartesian[1] + 1*r*dy)
-radius_of_C1 = 2*r
-radius_of_C2 = 1*r
+dx, dy = np.cos(angle - (0.5 * np.pi - kPhi)), np.sin(angle - (0.5 * np.pi - kPhi))
+point_C1_cartesian = (point_A_cartesian[0] - 2 * r * dx, point_A_cartesian[1] - 2 * r * dy)
+point_C2_cartesian = (point_B_cartesian[0] + 1 * r * dx, point_B_cartesian[1] + 1 * r * dy)
+radius_of_C1 = 2 * r
+radius_of_C2 = 1 * r
+
 
 # 定义用于绘制圆的函数
 def draw_circle(ax, center, radius, num_points, beg_angle, span_angle):
-    t = np.linspace(beg_angle, beg_angle+span_angle, num_points)
+  t = np.linspace(beg_angle, beg_angle + span_angle, num_points)
   x = center[0] + radius * np.cos(t)
   y = center[1] + radius * np.sin(t)
   r, theta = np.sqrt(x**2 + y**2), np.arctan2(y, x)
   ax.plot(theta, r)
 
+
 # 绘制圆C1
-draw_circle(ax, point_C1_cartesian, radius_of_C1, 100, angle+0.5*np.pi-kPhi-np.pi, 2*kPhi)
+draw_circle(ax, point_C1_cartesian, radius_of_C1, 100, angle + 0.5 * np.pi - kPhi - np.pi, 2 * kPhi)
 
 # 绘制圆C2
-draw_circle(ax, point_C2_cartesian, radius_of_C2, 100, angle+0.5*np.pi-kPhi, 2*kPhi)
+draw_circle(ax, point_C2_cartesian, radius_of_C2, 100, angle + 0.5 * np.pi - kPhi, 2 * kPhi)
 
 print(f"Total length={6*r*kPhi}")
+print(f"kCriticalTheta={kCriticalTheta}")
 
-x_ticks = np.arange(-int(kPlotingRadius)-1, int(kPlotingRadius)+1, 1)
-y_ticks = np.arange(-int(kPlotingRadius)-1, int(kPlotingRadius)+1, 1)
+x_ticks = np.arange(-int(kPlotingRadius) - 1, int(kPlotingRadius) + 1, 1)
+y_ticks = np.arange(-int(kPlotingRadius) - 1, int(kPlotingRadius) + 1, 1)
 X, Y = np.meshgrid(x_ticks, y_ticks)
 X = X.flatten()
 Y = Y.flatten()
@@ -76,7 +86,5 @@ theta_grid = np.arctan2(Y, X)
 valid_points = r_grid <= kPlotingRadius
 ax.scatter(theta_grid[valid_points], r_grid[valid_points], color='grey', s=10)  # 灰色小点
 
-plt.title("The Moving Path")
-
 # 显示图像
 plt.show()