基于高度约束解析求解Y轴旋转角的问题排查与方案咨询

现有两个3D点A=(x₁, y₁, z₁)、B=(x₂, y₂, z₂)，需绕Y轴旋转，要求旋转后线段AB的高度（z₁-z₂）与参考线段CD（C=(x'₁, y'₁, z'₁)、D=(x'₂, y'₂, z'₂)）的高度（z'₁-z'₂）一致。

已推导Y轴旋转矩阵方程并提取高度约束对应的方程组，在(z'₁-z'₂)=0的场景下已成功实现解析求解，但在(z'₁-z'₂)非零的场景中，编写的Python函数测试未达预期：旋转后AB的高度与目标高度存在偏差。

旋转角计算函数 #

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

测试代码 #

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

疑问 #

1. 现有实现存在什么问题？
2. 针对(z'₁-z'₂)非零的情况是否存在通用解析解？

内容的提问来源于stack exchange，提问作者Elie