通过 3D 向量拟合线并找到与平面的交点

Posted 2023-03-27

【中文标题】通过 3D 向量拟合线并找到与平面的交点

【英文标题】：Fit line through 3D-vector and find intersection with plane

【发布时间】：2022-01-20 09:04:12

【问题描述】：

我试图在第一步中找到一种通过已知向量拟合直线的方法。 第二步应该是找到这条线和所示平面之间的交点。 我要拟合一条线的向量用黑色椭圆标记，并在代码中命名为vector_3。

目标是在蓝色矢量路径围绕 x 或 y 轴旋转时调整 vector_3 的长度，以获得始终在平面处结束的新矢量。

这是代码（简化为现在相关的内容）和结果图：

import numpy as np
import matplotlib.pyplot as plt

# Import 6D-Pose 
translation_6D_pose = np.array([100, 0, 0, 1])
rotation_6D_pose = np.array([0, 0, 0])

# Definition of vectors
origin = np.array([0, 0, 0])
vector_1 = np.array([0, 100, 10, 1])
vector_2 = np.array([-100, 0, 50, 1])
vector_3 = np.array([0, 100, 0, 1])
vector_4 = np.array([0, 0, 60, 1])

X, Y, Z = origin
U, V, W, _ = translation_6D_pose

A, B, C, _ = vector_1
D, E, F, _ = vector_2

G, H, I, _ = vector_3
J, K, L, _ = vector_4

# Plot figure:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
ax.set_xlim([-50, 200])
ax.set_ylim([-50, 200])
ax.set_zlim([-50, 200])

# Make plane:
xx, yy = np.meshgrid(range(-100, 100), range(0, 200))
z = xx*0 + 60
ax.plot_surface(xx, yy, z, alpha=0.2)

# Plot vectors:
ax.quiver(X, Y, Z, U, V, W)
ax.quiver(U, V, W, A, B, C)
ax.quiver(U+A, V+B, W+C, D, E, F)
ax.quiver(X, Y, Z, G, H, I, color="r")
ax.quiver(G, H, I, J, K, L, color="r")

plt.show()

【参考方案1】：

要通过矢量绘制一条线，您只需创建与矢量对齐的点并绘制它们。要找到交点，您可以使用 sympy 中的 intersection 函数。请注意，在您的情况下，您也可以直接计算它。你的飞机只是z=60，所以计算很简单。

请参阅下面的代码，了解我使用 sympy 以使答案更通用的示例：

import numpy as np
import matplotlib.pyplot as plt
from sympy import Point3D, Line3D, Plane


# Import 6D-Pose 
translation_6D_pose = np.array([100, 0, 0, 1])
rotation_6D_pose = np.array([0, 0, 0])

# Definition of vectors
origin = np.array([0, 0, 0])
vector_1 = np.array([0, 100, 10, 1])
vector_2 = np.array([-100, 0, 50, 1])
vector_3 = np.array([0, 100, 0, 1])
vector_4 = np.array([0, 0, 60, 1])

X, Y, Z = origin
U, V, W, _ = translation_6D_pose

A, B, C, _ = vector_1
D, E, F, _ = vector_2

G, H, I, _ = vector_3
J, K, L, _ = vector_4

# Plot figure:
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.set_xlabel("X")
ax.set_ylabel("Y")
ax.set_zlabel("Z")
ax.set_xlim([-50, 200])
ax.set_ylim([-50, 200])
ax.set_zlim([-50, 200])

# Make plane:
xx, yy = np.meshgrid(range(-100, 100), range(0, 200))
z = xx*0 + 60
ax.plot_surface(xx, yy, z, alpha=0.2)

# Plot vectors:
ax.quiver(X, Y, Z, U, V, W)
ax.quiver(U, V, W, A, B, C)
ax.quiver(U+A, V+B, W+C, D, E, F)
x=[U+A+k*D for k in range(10)]
y=[V+B+k*E for k in range(10)]
z=[W+C+k*F for k in range(10)]
ax.quiver(X, Y, Z, G, H, I, color="r")
ax.quiver(G, H, I, J, K, L, color="r")

#Computing intersection
plane = Plane(Point3D(0, 0, 60),(50,0,60),(0,50,60))
line = Line3D(Point3D(U+A, V+B, W+C), Point3D(U+A+10*D, V+B+10*E, W+C+10*F))
inter=plane.intersection(line)

#PLotting intersection
ax.scatter(float(inter[0][0]),float(inter[0][1]),float(inter[0][2]),color='tab:green',marker='x',s=100,label='intersection')

#PLotting line
ax.plot(x,y,z,label='line')


plt.legend()

plt.show()

输出给出：

【讨论】：

这正是我所需要的。非常感谢！

不客气。如果您对结果感到满意，您可以对答案进行投票并将问题标记为已回答，方法是勾选我的答案旁边的绿色复选标记。谢谢！