write task2

.gitignore

@@ -1,4 +1,5 @@
 *.swp
 *.json
 *.bak
-*.dat
+*.dat
+*.log

A/2/collision_cal.py

@@ -0,0 +1,199 @@
+import mpmath as mp
+import json
+import sys
+import matplotlib.pyplot as plt
+from matplotlib.patches import Rectangle
+import numpy as np
+from typing import *
+import threading
+import numba
+import multiprocessing
+
+mp.dps = 15  # 设置精度为15位小数
+
+kSegLength1 = mp.mpf('2.86')
+kSegLength2 = mp.mpf('1.65')
+kAlpha = mp.mpf('0.55') / (2 * mp.pi)
+
+def Theta2C(theta):
+  tmp = mp.sqrt(1 + theta**2)
+  return kAlpha * 0.5 * (theta * tmp - mp.log(-theta + tmp))
+
+def Theta2Dot(theta):
+  return (kAlpha * theta * mp.cos(theta), kAlpha * theta * mp.sin(theta))
+
+init_C=Theta2C(2*mp.pi*16)
+def GenerateFirstNodeTheta(time_point):
+  cur_C = init_C - time_point
+  def f(theta):
+    return Theta2C(theta) - cur_C
+  return mp.findroot(f, 2*mp.pi*16, solver='secant')
+
+def GenerateFollowNodeTheta(cur_node_theta, expected_distance):
+  cur_node_dot = Theta2Dot(cur_node_theta)
+  def f(theta):
+    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)
+
+kPointsConsidered=50
+def CalcMoveList(time_point):
+  first_node_theta = GenerateFirstNodeTheta(time_point)
+  first_node_dot = Theta2Dot(first_node_theta)
+  first_node_C = Theta2C(first_node_theta)
+
+  node_list = [{"theta": first_node_theta, "node": first_node_dot, "C": first_node_C, "v": mp.mpf('1.0')}]
+
+  for i in range(1, kPointsConsidered):
+    expected_distance = kSegLength1 if i == 1 else kSegLength2
+    cur_node_theta = GenerateFollowNodeTheta(node_list[-1]["theta"], expected_distance)
+    cur_node_dot = Theta2Dot(cur_node_theta)
+    cur_node_C = Theta2C(cur_node_theta)
+    node_list.append({"theta": cur_node_theta, "node": cur_node_dot, "C": cur_node_C})
+  for i in range(kPointsConsidered-1):
+    AA = kSegLength1 if i == 0 else kSegLength2
+    theta_i = node_list[i]["theta"]
+    theta_ip1 = node_list[i+1]["theta"]
+    alpha_i = mp.atan(theta_i)
+    alpha_ip1 = mp.atan(theta_ip1)
+    beta_i = mp.acos(((kAlpha*theta_i)**2 + AA**2 - (kAlpha*theta_ip1)**2) / (2*kAlpha*theta_i*AA))
+    gama_i = mp.acos(((kAlpha*theta_ip1)**2 + AA**2 - (kAlpha*theta_i)**2) / (2*kAlpha*theta_ip1*AA))
+    node_list[i+1]["v"] = node_list[i]["v"] * (-mp.cos(alpha_i + beta_i) / mp.cos(alpha_ip1 - gama_i))
+
+  return node_list
+
+
+# 将结果转换为float并保留6位小数
+def mp2float(time_point_list):
+  float_res_list = [
+    {k: round(float(v), 6) if isinstance(v, mp.mpf) else 
+      [round(float(x), 6) for x in v] if isinstance(v, tuple) else v
+    for k, v in node.items()}
+    for node in time_point_list
+  ]
+  return float_res_list
+
+def status2blocks(node_list):
+  res=[]
+  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"]]
+    # 计算并绘制木板（长方形）
+    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
+    
+    # 左下角坐标
+    rect1_x = center_x - rect_length/2 * np.cos(angle) + rect_width/2 * np.sin(angle)
+    rect1_y = center_y - rect_length/2 * np.sin(angle) - rect_width/2 * np.cos(angle)
+
+    # 右下角坐标
+    rect2_x = center_x + rect_length/2 * np.cos(angle) + rect_width/2 * np.sin(angle)
+    rect2_y = center_y + rect_length/2 * np.sin(angle) - rect_width/2 * np.cos(angle)
+
+    # 右上角坐标
+    rect3_x = center_x + rect_length/2 * np.cos(angle) - rect_width/2 * np.sin(angle)
+    rect3_y = center_y + rect_length/2 * np.sin(angle) + rect_width/2 * np.cos(angle)
+
+    # 左上角坐标
+    rect4_x = center_x - rect_length/2 * np.cos(angle) - rect_width/2 * np.sin(angle)
+    rect4_y = center_y - rect_length/2 * np.sin(angle) + rect_width/2 * np.cos(angle)
+
+    res.append(((rect1_x, rect1_y), (rect2_x, rect2_y), (rect3_x, rect3_y), (rect4_x, rect4_y)))
+    
+  return res
+
+@numba.njit
+def CrossProduct(a,b):
+  return a[0]*b[1]-a[1]*b[0]
+@numba.njit
+def PointInBlock(point,block):
+  vec1_alpha=(block[1][0]-block[0][0],block[1][1]-block[0][1])
+  vec1_beta=(point[0]-block[0][0],point[1]-block[0][1])
+  vec2_alpha=(block[2][0]-block[1][0],block[2][1]-block[1][1])
+  vec2_beta=(point[0]-block[1][0],point[1]-block[1][1])
+  vec3_alpha=(block[3][0]-block[2][0],block[3][1]-block[2][1])
+  vec3_beta=(point[0]-block[2][0],point[1]-block[2][1])
+  vec4_alpha=(block[0][0]-block[3][0],block[0][1]-block[3][1])
+  vec4_beta=(point[0]-block[3][0],point[1]-block[3][1])
+  status1=CrossProduct(vec1_alpha,vec1_beta)
+  status2=CrossProduct(vec2_alpha,vec2_beta)
+  status3=CrossProduct(vec3_alpha,vec3_beta)
+  status4=CrossProduct(vec4_alpha,vec4_beta)
+  if status1<0:
+    return -1
+  if status2<0:
+    return -1
+  if status3<0:
+    return -1
+  if status4<0:
+    return -1
+  kEps=1e-10
+  if status1<kEps or status2<kEps or status3<kEps or status4<kEps:
+    return 0
+  return 1
+def CheckCollision(block_list):
+  res = -1
+  for i in range(len(block_list)-1):
+    for j in range(2):
+      point=block_list[i][j]
+      for k in range(i+1,len(block_list)):
+        status=PointInBlock(point,block_list[k])
+        if status>res:
+          res=status
+          if res==1:
+            break
+  return res
+kParallelNum=24
+kBeg=412.4737
+kEnd=412.4739
+kStep=(kEnd-kBeg)/10000
+tasks_list=[i for i in np.arange(kBeg, kEnd, kStep)]
+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)
+res_array={}
+def ProcessEntryPoint(arg):
+  time_point_list, process_id, shared_dict, lock = arg
+  logf=open(f"collision_cal_{process_id}.log","w")
+  print(f"calculating time_point={time_point_list}",file=logf)
+  cnt = 0
+  tmp_res={}
+  for ti in time_point_list:
+    cnt+=1
+    print(f"calculating {cnt}/{len(time_point_list)} time_point={ti}",file=logf)
+    status=CalcMoveList(ti)
+    status=mp2float(status)
+    blocks=status2blocks(status)
+    status=CheckCollision(blocks)
+    tmp_res[ti]=status
+    print(f"res={status}",file=logf)
+  with lock:  # 添加锁保护对共享字典的操作
+    shared_dict.update(tmp_res)
+
+if __name__ == "__main__":
+  manager = multiprocessing.Manager()
+  shared_dict = manager.dict()  # 创建一个共享字典
+  lock = manager.Lock()  # 创建一个共享的锁
+  task_args_list = [(task_list_per_process[i], i, shared_dict, lock) for i in range(kParallelNum)]
+  with multiprocessing.Pool(processes=kParallelNum) as pool:
+    pool.map(ProcessEntryPoint, task_args_list)
+  print(f"\nFinal Results: \n")
+  res_array=sorted(shared_dict.items())
+  for item in res_array:
+    time_point, status = item
+    print(f"time point={time_point}",end=" ")
+    if status==-1:
+      print("NoCollision")
+    elif status==0:
+      print("CollisionOnEdge")
+    else:
+      print("CollisionInBlock")

A/2/position_watcher.py

@@ -36,6 +36,7 @@ def GenerateFollowNodeTheta(cur_node_theta, expected_distance):
     return actual_distance - expected_distance
   return mp.findroot(f, cur_node_theta + 0.1, solver='secant')
 
+kPotingPoints=50
 def CalcMoveList(time_point):
   first_node_theta = GenerateFirstNodeTheta(time_point)
   first_node_dot = Theta2Dot(first_node_theta)
@@ -43,13 +44,13 @@ def CalcMoveList(time_point):
 
   node_list = [{"theta": first_node_theta, "node": first_node_dot, "C": first_node_C, "v": mp.mpf('1.0')}]
 
-  for i in range(1, 224):
+  for i in range(1, kPotingPoints):
     expected_distance = kSegLength1 if i == 1 else kSegLength2
     cur_node_theta = GenerateFollowNodeTheta(node_list[-1]["theta"], expected_distance)
     cur_node_dot = Theta2Dot(cur_node_theta)
     cur_node_C = Theta2C(cur_node_theta)
     node_list.append({"theta": cur_node_theta, "node": cur_node_dot, "C": cur_node_C})
-  for i in range(223):
+  for i in range(kPotingPoints-1):
     AA = kSegLength1 if i == 0 else kSegLength2
     theta_i = node_list[i]["theta"]
     theta_ip1 = node_list[i+1]["theta"]

A/2/visualize.py

@@ -1,48 +0,0 @@
-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)

environment.yml

@@ -7,11 +7,14 @@ dependencies:
   - _libgcc_mutex=0.1=main
   - _openmp_mutex=5.1=1_gnu
   - blas=1.0=mkl
+  - brotli=1.0.9=h5eee18b_8
+  - brotli-bin=1.0.9=h5eee18b_8
   - brotli-python=1.0.9=py312h6a678d5_8
   - bzip2=1.0.8=h5eee18b_6
   - ca-certificates=2024.7.2=h06a4308_0
-  - certifi=2024.7.4=py312h06a4308_0
+  - certifi=2024.8.30=py312h06a4308_0
   - charset-normalizer=3.3.2=pyhd3eb1b0_0
+  - contourpy=1.2.0=py312hdb19cb5_0
   - cuda-cudart=12.1.105=0
   - cuda-cupti=12.1.105=0
   - cuda-libraries=12.1.0=0
@@ -20,29 +23,49 @@ dependencies:
   - cuda-opencl=12.6.68=0
   - cuda-runtime=12.1.0=0
   - cuda-version=12.6=3
+  - cycler=0.11.0=pyhd3eb1b0_0
+  - cyrus-sasl=2.1.28=h52b45da_1
+  - dbus=1.13.18=hb2f20db_0
   - expat=2.6.2=h6a678d5_0
   - ffmpeg=4.3=hf484d3e_0
   - filelock=3.13.1=py312h06a4308_0
+  - fontconfig=2.14.1=h55d465d_3
+  - fonttools=4.51.0=py312h5eee18b_0
   - freetype=2.12.1=h4a9f257_0
+  - glib=2.78.4=h6a678d5_0
+  - glib-tools=2.78.4=h6a678d5_0
   - gmp=6.2.1=h295c915_3
   - gnutls=3.6.15=he1e5248_0
+  - gst-plugins-base=1.14.1=h6a678d5_1
+  - gstreamer=1.14.1=h5eee18b_1
+  - icu=73.1=h6a678d5_0
   - idna=3.7=py312h06a4308_0
   - intel-openmp=2023.1.0=hdb19cb5_46306
   - jinja2=3.1.4=py312h06a4308_0
   - jpeg=9e=h5eee18b_3
+  - kiwisolver=1.4.4=py312h6a678d5_0
+  - krb5=1.20.1=h143b758_1
   - lame=3.100=h7b6447c_0
   - lcms2=2.12=h3be6417_0
   - ld_impl_linux-64=2.38=h1181459_1
   - lerc=3.0=h295c915_0
+  - libbrotlicommon=1.0.9=h5eee18b_8
+  - libbrotlidec=1.0.9=h5eee18b_8
+  - libbrotlienc=1.0.9=h5eee18b_8
+  - libclang=14.0.6=default_hc6dbbc7_1
+  - libclang13=14.0.6=default_he11475f_1
   - libcublas=12.1.0.26=0
   - libcufft=11.0.2.4=0
   - libcufile=1.11.1.6=0
+  - libcups=2.4.2=h2d74bed_1
   - libcurand=10.3.7.68=0
   - libcusolver=11.4.4.55=0
   - libcusparse=12.0.2.55=0
   - libdeflate=1.17=h5eee18b_1
+  - libedit=3.1.20230828=h5eee18b_0
   - libffi=3.4.4=h6a678d5_1
   - libgcc-ng=11.2.0=h1234567_1
+  - libglib=2.78.4=hdc74915_0
   - libgomp=11.2.0=h1234567_1
   - libiconv=1.16=h5eee18b_3
   - libidn2=2.3.4=h5eee18b_0
@@ -52,21 +75,28 @@ dependencies:
   - libnvjitlink=12.1.105=0
   - libnvjpeg=12.1.1.14=0
   - libpng=1.6.39=h5eee18b_0
+  - libpq=12.17=hdbd6064_0
   - libstdcxx-ng=11.2.0=h1234567_1
   - libtasn1=4.19.0=h5eee18b_0
   - libtiff=4.5.1=h6a678d5_0
   - libunistring=0.9.10=h27cfd23_0
   - libuuid=1.41.5=h5eee18b_0
   - libwebp-base=1.3.2=h5eee18b_0
+  - libxcb=1.15=h7f8727e_0
+  - libxkbcommon=1.0.1=h097e994_2
+  - libxml2=2.13.1=hfdd30dd_2
   - llvm-openmp=14.0.6=h9e868ea_0
   - llvmlite=0.43.0=py312h6a678d5_0
   - lz4-c=1.9.4=h6a678d5_1
   - markupsafe=2.1.3=py312h5eee18b_0
+  - matplotlib=3.9.2=py312h06a4308_0
+  - matplotlib-base=3.9.2=py312h66fe004_0
   - mkl=2023.1.0=h213fc3f_46344
   - mkl-service=2.4.0=py312h5eee18b_1
   - mkl_fft=1.3.10=py312h5eee18b_0
   - mkl_random=1.2.7=py312h526ad5a_0
   - mpmath=1.3.0=py312h06a4308_0
+  - mysql=5.7.24=h721c034_2
   - ncurses=6.4=h6a678d5_0
   - nettle=3.7.3=hbbd107a_1
   - networkx=3.2.1=py312h06a4308_0
@@ -75,18 +105,28 @@ dependencies:
   - numpy-base=1.26.4=py312h0da6c21_0
   - openh264=2.1.1=h4ff587b_0
   - openjpeg=2.5.2=he7f1fd0_0
-  - openssl=3.0.14=h5eee18b_0
+  - openssl=3.0.15=h5eee18b_0
+  - packaging=24.1=py312h06a4308_0
+  - pcre2=10.42=hebb0a14_1
   - pillow=10.4.0=py312h5eee18b_0
   - pip=24.2=py312h06a4308_0
+  - ply=3.11=py312h06a4308_1
+  - pyparsing=3.1.2=py312h06a4308_0
+  - pyqt=5.15.10=py312h6a678d5_0
+  - pyqt5-sip=12.13.0=py312h5eee18b_0
   - pysocks=1.7.1=py312h06a4308_0
   - python=3.12.4=h5148396_1
+  - python-dateutil=2.9.0post0=py312h06a4308_2
   - pytorch=2.4.1=py3.12_cuda12.1_cudnn9.1.0_0
   - pytorch-cuda=12.1=ha16c6d3_5
   - pytorch-mutex=1.0=cuda
   - pyyaml=6.0.1=py312h5eee18b_0
+  - qt-main=5.15.2=h53bd1ea_10
   - readline=8.2=h5eee18b_0
   - requests=2.32.3=py312h06a4308_0
   - setuptools=72.1.0=py312h06a4308_0
+  - sip=6.7.12=py312h6a678d5_0
+  - six=1.16.0=pyhd3eb1b0_1
   - sqlite=3.45.3=h5eee18b_0
   - sympy=1.13.2=py312h06a4308_0
   - tbb=2021.8.0=hdb19cb5_0
@@ -94,8 +134,10 @@ dependencies:
   - torchaudio=2.4.1=py312_cu121
   - torchtriton=3.0.0=py312
   - torchvision=0.19.1=py312_cu121
+  - tornado=6.4.1=py312h5eee18b_0
   - typing_extensions=4.11.0=py312h06a4308_0
   - tzdata=2024a=h04d1e81_0
+  - unicodedata2=15.1.0=py312h5eee18b_0
   - urllib3=2.2.2=py312h06a4308_0
   - wheel=0.43.0=py312h06a4308_0
   - xz=5.4.6=h5eee18b_1