99 lines
3.1 KiB
Python
99 lines
3.1 KiB
Python
|
# trajectory.py
|
|||
|
|
|
|||
|
|
import numpy as np
|
|||
|
|
|
|||
|
|
def circle_trajectory(center=(80, 0), radius=40, num_points=200):
|
|||
|
|
""" 圆形轨迹 """
|
|||
|
|
angles = np.linspace(0, 2 * np.pi, num_points)
|
|||
|
|
x_list = center[0] + radius * np.cos(angles)
|
|||
|
|
y_list = center[1] + radius * np.sin(angles)
|
|||
|
|
return x_list, y_list
|
|||
|
|
|
|||
|
|
def line_trajectory(start=(40, 0), end=(120, 0), num_points=100):
|
|||
|
|
""" 直线轨迹 """
|
|||
|
|
t = np.linspace(0, 1, num_points)
|
|||
|
|
x_list = start[0] + t * (end[0] - start[0])
|
|||
|
|
y_list = start[1] + t * (end[1] - start[1])
|
|||
|
|
return x_list, y_list
|
|||
|
|
|
|||
|
|
|
|||
|
|
def line_trajectory_fix(start=(40, 0), end=(120, 100), vx=0.1, vy=0.1, num_points=20):
|
|||
|
|
"""
|
|||
|
|
生成带速度分量的匀速斜线轨迹
|
|||
|
|
参数：
|
|||
|
|
start: 起始点 (x, y)
|
|||
|
|
end: 终点 (x, y) —— 仅用于估算运行时间（可选）
|
|||
|
|
vx: x方向速度（单位/秒）
|
|||
|
|
vy: y方向速度（单位/秒）
|
|||
|
|
num_points: 生成的轨迹点数
|
|||
|
|
返回：
|
|||
|
|
x_list, y_list: 轨迹坐标数组
|
|||
|
|
"""
|
|||
|
|
# 速度大小
|
|||
|
|
speed = np.sqrt(vx**2 + vy**2)
|
|||
|
|
if speed == 0:
|
|||
|
|
raise ValueError("速度不能为零")
|
|||
|
|
|
|||
|
|
# 估算从 start 到 end 的距离（用于估算总时间）
|
|||
|
|
if end is not None:
|
|||
|
|
dx = end[0] - start[0]
|
|||
|
|
dy = end[1] - start[1]
|
|||
|
|
distance = np.sqrt(dx**2 + dy**2)
|
|||
|
|
total_time = distance / speed # 理论到达时间
|
|||
|
|
print(total_time)
|
|||
|
|
else:
|
|||
|
|
total_time = 10.0 # 默认运行10秒
|
|||
|
|
|
|||
|
|
# 时间序列：从 0 到 total_time，均匀分布 num_points 个点
|
|||
|
|
t = np.linspace(0, total_time, num_points)
|
|||
|
|
|
|||
|
|
# 位置 = 起点 + 速度 × 时间
|
|||
|
|
x_list = start[0] + vx * t
|
|||
|
|
y_list = start[1] + vy * t
|
|||
|
|
|
|||
|
|
return x_list, y_list
|
|||
|
|
|
|||
|
|
def ellipse_trajectory(center=(80, 0), rx=50, ry=25, num_points=200):
|
|||
|
|
""" 椭圆轨迹 """
|
|||
|
|
angles = np.linspace(0, 2 * np.pi, num_points)
|
|||
|
|
x_list = center[0] + rx * np.cos(angles)
|
|||
|
|
y_list = center[1] + ry * np.sin(angles)
|
|||
|
|
return x_list, y_list
|
|||
|
|
|
|||
|
|
def square_trajectory(side=60, num_points=60):
|
|||
|
|
""" 正方形轨迹 """
|
|||
|
|
x_list, y_list = [], []
|
|||
|
|
for i in range(num_points):
|
|||
|
|
t = i / num_points
|
|||
|
|
if t < 0.25:
|
|||
|
|
x = 80 + 60 * t * 4
|
|||
|
|
y = 0
|
|||
|
|
elif t < 0.5:
|
|||
|
|
x = 140
|
|||
|
|
y = 0 + 60 * (t - 0.25) * 4
|
|||
|
|
elif t < 0.75:
|
|||
|
|
x = 140 - 60 * (t - 0.5) * 4
|
|||
|
|
y = 60
|
|||
|
|
else:
|
|||
|
|
x = 80
|
|||
|
|
y = 60 - 60 * (t - 0.75) * 4
|
|||
|
|
x_list.append(x)
|
|||
|
|
y_list.append(y)
|
|||
|
|
return x_list, y_list
|
|||
|
|
|
|||
|
|
def triangle_trajectory(base_length=100, height=80, num_points=60):
|
|||
|
|
""" 三角形轨迹 """
|
|||
|
|
x_list, y_list = [], []
|
|||
|
|
points = [(80, 0), (130, 80), (30, 80), (80, 0)]
|
|||
|
|
for i in range(num_points):
|
|||
|
|
idx = int(i / num_points * 3)
|
|||
|
|
t = (i % (num_points // 3)) / (num_points // 3)
|
|||
|
|
x = points[idx][0] + t * (points[idx+1][0] - points[idx][0])
|
|||
|
|
y = points[idx][1] + t * (points[idx+1][1] - points[idx][1])
|
|||
|
|
x_list.append(x)
|
|||
|
|
y_list.append(y)
|
|||
|
|
return x_list, y_list
|
|||
|
|
|
|||
|
|
def custom_trajectory(custom_x, custom_y):
|
|||
|
|
""" 自定义轨迹，输入两个列表即可 """
|
|||
|
|
return custom_x, custom_y
|