在给定高度约束的情况下分析求解旋转角度

Question

我有 2 个 3D 点，我想绕 Y 轴旋转：

• A=(x1,y1,z1)
• B=(x2,y2,z2)

而且，我还有另外 2 个 3D 点，它们将用作提取旋转角度约束的参考。

• C=(x'1, y'1, z'1)
• D=(x'2, y'2, z'2)

我想绕Y轴旋转点A和B，使得线段AB（z1-z2）的高度与线段CD（z'1-z'2）的高度相同.

（编辑：根据评论，下图似乎不清楚。下图表示3D点在XZ平面上的投影。它的目的是显示将被旋转的点A和B的投影绕 Y 轴，使得线段 AB 的高度 (h= z1-z2) 等于线段 CD 的高度 (h'=z'1-z'2))

而且，线段 AB 总是足够长，因此解总是存在。

为此，我首先写下沿 Y 轴旋转点 A 和 B 的方程，并将它们设置为等于点 C 和 D。（我只对方程的最后一行感兴趣，以保存高度限制）：

使用最后一行，我可以提取以下方程组：

然后，为了求解这个方程组并找到角度 theta，我从第一个方程中减去第二个方程，得到以下结果：

在(z'1-z'2)=0的情况下，方程很容易解，角度可以计算如下：

但是，对于 (z'1-z'2) 非零的情况，我面临困难。

我尝试通过将第一个方程除以第二个方程来求解 (z'1-z'2) 非零的情况：

然后，通过将方程两边乘以分母，我能够分离一侧的角度 theta 并计算 arctan2 来找到 theta，如下所示：

我明白，通过使用这种方法，我假设 z'2 不为零才能得到解决方案。而且，我还了解到，我可以在方程组中将方程 2 除以方程 1，并且我会在分母中得到 z'1，我也假设它不为零。

因此，我开发了一个 python 函数，它使用上面的方程计算 theta 并返回旋转矩阵，然后使用该矩阵沿 Y 轴旋转 A 和 B：

def get_rot_y(current_points, reference_points):
    (x1, y1, z1) = current_points[:, 0]
    (x2, y2, z2) = current_points[:, 1]
    (xp1, yp1, zp1) = reference_points[:, 0]
    (xp2, yp2, zp2) = reference_points[:, 1]

    if ((zp1-zp2) == 0):
        th = np.arctan2((z2-z1), (x2-x1))
    else:
        th = np.arctan2((-z1 + (zp1*z2 / zp2)), (-x1 + (zp1*x2 / zp2)))
    rot_y = np.array([
        [np.cos(th), 0, np.sin(th)],
        [0, 1, 0],
        [-np.sin(th), 0, np.cos(th)],
    ])
    return rot_y

然后，我测试了高度 (z'1-z'2) 不为零的情况下的函数：

A = np.array([1450, 0.5, -1545])
B = np.array([6000, 0.7, -1650])

C = np.array([1500, 0, -1500])
D = np.array([5600, 0, -1600])

current_height = A[2] - B[2]
desired_height = C[2] - D[2]

current_points = np.array([A, B]).T
reference_points = np.array([C, D]).T

rot_y = get_rot_y(current_points=current_points, reference_points=reference_points)
transfomed_points = rot_y @ current_points

transformed_height = transfomed_points[2,0] - transfomed_points[2,1]
print(f"current_height= {current_height}")
print(f"desired_height= {desired_height}")
print(f"transformed_height= {transformed_height}")

但是，当我执行代码时，我得到以下输出：

current_height= 105.0
desired_height= 100
transformed_height= 102.95657644356697

从上面可以看出，变换后的点的高度不等于所需的高度。

我做错了什么？我的问题是否有一个解析解可以应用于 (z'1-z'2) 不为零的情况？

Answer 1

预期角度为 arcsin(h/L)，其中 h 是预期高度（z 差），L 是投影到 xz 平面的线段长度。该平面中的当前角度为 atan2( z2-z1,x2-x1)。因此，只需按预期 - 当前角度旋转（绕 y 轴）即可。

但是要注意两件事。

首先，如果你不小心，反正弦可能会给出错误的象限。

其次，您（和我）使用的公式可以找到图表中绘制的从 x 到 z 的逆时针角度（通常对 x-y 图执行此操作，因为正 z 轴将超出您的绘图）。但是，绕 y 轴的正旋转将是顺时针旋转（因为正 y 轴位于右手坐标系的绘图中）。下面的代码通过简单地反转角度变化来考虑这一点。

import numpy as np

def rot_y( theta ):
    '''Right-handed rotation about the y axis'''
    return np.array( [ [  np.cos( theta ), 0, np.sin( theta ) ],
                       [      0          , 1,     0           ],
                       [ -np.sin( theta ), 0, np.cos( theta ) ] ] )


A = np.array( [ 1450, 0.5, -1545 ] )
B = np.array( [ 6000, 0.7, -1650 ] )

C = np.array( [ 1500,   0, -1500 ] )
D = np.array( [ 5600,   0, -1600 ] )

# Angle COUNTER-CLOCKWISE from X-AXIS (from x toward z)
current_angle = np.arctan2( B[2]-A[2], B[0]-A[0] )
desired_angle = np.arcsin ( ( D[2]-C[2] ) / np.sqrt( (B[0]-A[0])**2 + (B[2]-A[2])**2 ) )
if D[0]-C[0] < 0: desired_angle = np.pi - desired_angle       # Get correct quadrant for arcsin
theta = desired_angle - current_angle

# Note that a positive rotation about the y axis will go CLOCKWISE (i.e. from z toward x) ...
# ... so we need to reverse it
theta = -theta
R = rot_y( theta )

# Rotate about its centre
centre = ( A + B ) / 2
AP = centre + R @ ( A - centre ).T
BP = centre + R @ ( B - centre ).T

print( f"current_height = {(B[2]-A[2])}")
print( f"desired_height = {(D[2]-C[2])}")
print( f"transformed_height = {(BP[2]-AP[2])}" )

输出：

current_height = -105.0
desired_height = -100
transformed_height = -100.0