write task2
This commit is contained in:
3
.gitignore
vendored
3
.gitignore
vendored
@@ -1,4 +1,5 @@
|
||||
*.swp
|
||||
*.json
|
||||
*.bak
|
||||
*.dat
|
||||
*.dat
|
||||
*.log
|
||||
199
A/2/collision_cal.py
Normal file
199
A/2/collision_cal.py
Normal file
@@ -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")
|
||||
@@ -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"]
|
||||
|
||||
@@ -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)
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user