146 lines
4.6 KiB
Python
146 lines
4.6 KiB
Python
# trajectory_3d.py
|
|
|
|
import numpy as np
|
|
|
|
|
|
def circle_trajectory_3d(center=(0, 0, 50), radius=30, axis='xy', num_points=200):
|
|
"""
|
|
3D 圆形轨迹（可指定平面）
|
|
参数：
|
|
center: 圆心 (x, y, z)
|
|
radius: 半径
|
|
axis: 'xy', 'yz', 'xz' 平面
|
|
num_points: 点数
|
|
返回：
|
|
x_list, y_list, z_list
|
|
"""
|
|
angles = np.linspace(0, 2 * np.pi, num_points)
|
|
cx, cy, cz = center
|
|
|
|
if axis == 'xy':
|
|
x_list = cx + radius * np.cos(angles)
|
|
y_list = cy + radius * np.sin(angles)
|
|
z_list = cz + np.zeros(num_points)
|
|
elif axis == 'yz':
|
|
x_list = cx + np.zeros(num_points)
|
|
y_list = cy + radius * np.cos(angles)
|
|
z_list = cz + radius * np.sin(angles)
|
|
elif axis == 'xz':
|
|
x_list = cx + radius * np.cos(angles)
|
|
y_list = cy + np.zeros(num_points)
|
|
z_list = cz + radius * np.sin(angles)
|
|
else:
|
|
raise ValueError("axis must be 'xy', 'yz', or 'xz'")
|
|
|
|
return x_list, y_list, z_list
|
|
|
|
|
|
def line_trajectory_3d(start=(40, 0, 0), end=(120, 0, 60), num_points=100):
|
|
""" 3D 直线轨迹 """
|
|
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])
|
|
z_list = start[2] + t * (end[2] - start[2])
|
|
return x_list, y_list, z_list
|
|
|
|
|
|
def line_trajectory_3d_uniform_speed(start=(40, 0, 0), end=(120, 60, 60), vx=0.1, vy=0.1, vz=0.05, num_points=20):
|
|
""" 匀速 3D 斜线轨迹，按速度分量生成 """
|
|
speed = np.sqrt(vx ** 2 + vy ** 2 + vz ** 2)
|
|
if speed == 0:
|
|
raise ValueError("速度不能为零")
|
|
|
|
# 计算距离和总时间
|
|
dx = end[0] - start[0]
|
|
dy = end[1] - start[1]
|
|
dz = end[2] - start[2]
|
|
distance = np.sqrt(dx ** 2 + dy ** 2 + dz ** 2)
|
|
total_time = distance / speed
|
|
|
|
t = np.linspace(0, total_time, num_points)
|
|
x_list = start[0] + vx * t
|
|
y_list = start[1] + vy * t
|
|
z_list = start[2] + vz * t
|
|
return x_list, y_list, z_list
|
|
|
|
|
|
def ellipse_trajectory_3d(center=(0, 0, 50), rx=50, ry=25, axis='xy', num_points=200):
|
|
""" 3D 椭圆轨迹 """
|
|
angles = np.linspace(0, 2 * np.pi, num_points)
|
|
cx, cy, cz = center
|
|
|
|
if axis == 'xy':
|
|
x_list = cx + rx * np.cos(angles)
|
|
y_list = cy + ry * np.sin(angles)
|
|
z_list = cz + np.zeros(num_points)
|
|
elif axis == 'yz':
|
|
x_list = cx + np.zeros(num_points)
|
|
y_list = cy + rx * np.cos(angles)
|
|
z_list = cz + ry * np.sin(angles)
|
|
elif axis == 'xz':
|
|
x_list = cx + rx * np.cos(angles)
|
|
y_list = cy + np.zeros(num_points)
|
|
z_list = cz + ry * np.sin(angles)
|
|
else:
|
|
raise ValueError("axis must be 'xy', 'yz', or 'xz'")
|
|
|
|
return x_list, y_list, z_list
|
|
|
|
|
|
def helix_trajectory(center=(0, 0, 0), radius=30, height=100, turns=2, num_points=200):
|
|
""" 螺旋轨迹（沿 Z 轴上升） """
|
|
t = np.linspace(0, turns * 2 * np.pi, num_points)
|
|
z_list = center[2] + height * t / (turns * 2 * np.pi)
|
|
x_list = center[0] + radius * np.cos(t)
|
|
y_list = center[1] + radius * np.sin(t)
|
|
return x_list, y_list, z_list
|
|
|
|
|
|
def square_trajectory_3d(side=60, center=(80, 0, 50), axis='xy', num_points=80):
|
|
""" 3D 正方形轨迹（指定平面） """
|
|
x_list, y_list, z_list = [], [], []
|
|
cx, cy, cz = center
|
|
half = side / 2
|
|
|
|
if axis == 'xy':
|
|
corners = [
|
|
(cx - half, cy - half, cz),
|
|
(cx + half, cy - half, cz),
|
|
(cx + half, cy + half, cz),
|
|
(cx - half, cy + half, cz),
|
|
(cx - half, cy - half, cz)
|
|
]
|
|
elif axis == 'yz':
|
|
corners = [
|
|
(cx, cy - half, cz - half),
|
|
(cx, cy + half, cz - half),
|
|
(cx, cy + half, cz + half),
|
|
(cx, cy - half, cz + half),
|
|
(cx, cy - half, cz - half)
|
|
]
|
|
elif axis == 'xz':
|
|
corners = [
|
|
(cx - half, cy, cz - half),
|
|
(cx + half, cy, cz - half),
|
|
(cx + half, cy, cz + half),
|
|
(cx - half, cy, cz + half),
|
|
(cx - half, cy, cz - half)
|
|
]
|
|
else:
|
|
raise ValueError("axis must be 'xy', 'yz', or 'xz'")
|
|
|
|
points_per_side = num_points // 4
|
|
for i in range(4):
|
|
x_start, y_start, z_start = corners[i]
|
|
x_end, y_end, z_end = corners[i + 1]
|
|
t = np.linspace(0, 1, points_per_side)
|
|
x_list.extend(x_start + t * (x_end - x_start))
|
|
y_list.extend(y_start + t * (y_end - y_start))
|
|
z_list.extend(z_start + t * (z_end - z_start))
|
|
|
|
return np.array(x_list), np.array(y_list), np.array(z_list)
|
|
|
|
|
|
def custom_trajectory_3d(custom_x, custom_y, custom_z):
|
|
""" 自定义 3D 轨迹 """
|
|
return np.array(custom_x), np.array(custom_y), np.array(custom_z) |