Аналитическое определение угла поворота с учетом ограничения по высоте

У меня есть две 3D-точки, которые я хочу повернуть вокруг оси Y:

• А=(х1, у1, z1)
• В=(х2, у2, z2)

И у меня есть еще две трехмерные точки, которые будут использоваться в качестве ссылки для определения ограничения для моего угла поворота.

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

Я хочу повернуть точки A и B вокруг оси Y так, чтобы высота отрезка AB (z1-z2) была такой же, как высота отрезка CD (z'1-z'2).

(Редактировать: судя по комментариям, рисунок ниже не совсем понятен. Рисунок ниже представляет собой проекцию 3D-точек на плоскость XZ. Он предназначен для отображения проекции точек A и B, которые будут повернуты вокруг ось Y так, чтобы высота (h= z1-z2) отрезка AB была равна высоте отрезка CD (h'=z'1-z'2))

И отрезок AB всегда будет достаточно длинным, чтобы решение всегда существовало.

Для этого я сначала записал уравнение, которое вращает точки A и B вдоль оси Y, и приравнял их к точкам C и D. (Меня будет интересовать только последняя строка уравнения для сохранения высоты ограничение):

Используя последнюю строку, я могу извлечь следующую систему уравнений:

Затем, чтобы решить эту систему уравнений и найти угол тета, я вычел второе уравнение из первого уравнения и получил следующее:

В случае, когда (z'1-z'2)=0, уравнение можно легко решить и вычислить угол следующим образом:

Однако я сталкиваюсь с трудностями в случае, когда (z'1-z'2) не равно нулю.

Я попытался решить случай, когда (z'1-z'2) не равно нулю, разделив первое уравнение на второе уравнение:

Затем, умножив обе части уравнения на знаменатель, я смог выделить угол тета с одной стороны и вычислить арктанс2, чтобы найти тета следующим образом:

Я понимаю, что, используя этот метод, я предполагаю, что z'2 не равно нулю, чтобы иметь решение. И я также понимаю, что я мог бы разделить уравнение 2 на уравнение 1 в системе уравнений и получить z'1 в знаменателе, который, как я предполагал, также был бы ненулевым.

Итак, я разработал функцию Python, которая вычисляет тету, используя приведенные выше уравнения, и возвращает матрицу вращения, которая затем будет использоваться для поворота A и B вдоль оси Y:

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) не равно нулю?