write visualization

A/2/position_watcher.py

@@ -1,6 +1,9 @@
 import mpmath as mp
 import json
 import sys
+import matplotlib.pyplot as plt
+from matplotlib.patches import Rectangle
+import numpy as np
 
 if __name__ != "__main__":
   sys.exit()
@@ -60,7 +63,7 @@ def CalcMoveList(time_point):
 
 
 time_point= float(input())
-print(f"calculating time_point={time_point}")
+print(f"calculating time_point={time_point}",file=sys.stderr)
 time_point_list = CalcMoveList(time_point)
 
 # 将结果转换为float并保留6位小数
@@ -72,3 +75,55 @@ float_res_list = [
 ]
 # print(float_res_list)
 json.dump(float_res_list, sys.stdout, indent=4)
+
+def visualize_spiral(node_list):
+  plt.figure(figsize=(12, 12))
+  
+  # 绘制灰色螺旋线
+  theta = np.linspace(0, float(node_list[-1]["theta"]), 1000)
+  x = [float(Theta2Dot(t)[0]) for t in theta]
+  y = [float(Theta2Dot(t)[1]) for t in theta]
+  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')
+  plt.title(f"Spiral visualization at time={time_point}")
+  plt.show()
+
+# 在计算完节点列表后调用可视化函数
+visualize_spiral(float_res_list)

A/2/visualize.py

@@ -0,0 +1,48 @@
+import matplotlib.pyplot as plt
+from matplotlib.patches import Rectangle
+import mpmath as mp
+import json
+import sys
+
+def visualize_spiral(node_list):
+    plt.figure(figsize=(12, 12))
+    
+    # 绘制灰色螺旋线
+    theta = np.linspace(0, node_list[0]["theta"], 1000)
+    x = [Theta2Dot(t)[0] for t in theta]
+    y = [Theta2Dot(t)[1] for t in theta]
+    plt.plot(x, y, color='gray', linewidth=0.5)
+
+    # 绘制节点和连接线
+    for i in range(len(node_list) - 1):
+        x1, y1 = node_list[i]["node"]
+        x2, y2 = 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.275 * 2
+        rect_width = 0.3
+        
+        rect_x = x1 - 0.275 * np.cos(angle)
+        rect_y = y1 - 0.275 * np.sin(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)
+
+    plt.axis('equal')
+    plt.title(f"Spiral visualization at time={time_point}")
+    plt.show()
+
+content=json.load(sys.stdin)
+visualize_spiral(content)