Solving for a rotation angle analytically given a height constraint

I have 2 3D points that I want to rotate about the Y-axis:

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

And, I have 2 other 3D points which will be used as reference to extract a constraint for my rotation angle.

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

I want to rotate the points A and B about the Y-axis such that the height of the line segment AB (z1-z2) will be the same as the height of the line segment CD (z'1-z'2).

(Edit: Based on the comments, it seems the figure below was not clear. The figure below represents the projection of the 3D points on the XZ plane. It is intended to show the projection of the points A and B which will be rotated about the Y-axis such that the height (h= z1-z2) of the line segment AB will be equal to the height of the line segment CD (h'=z'1-z'2))

And, the line segment AB will always be long enough so that a solution always exists.

To do this, I first wrote down the equation that rotates the points A and B along the Y-axis and set them equal to the points C and D. (I will be only interested in the last row of the equation to conserve the height constraint):

Using the last row, I can extract the following system of equations:

Then, to solve this system of equations and find the angle theta, I subtracted the second equation from the first equation and got the following:

In the case where (z'1-z'2)=0, the equation can be solved easily and the angle can be calculated as follows:

However, I am facing difficulties for the case where (z'1-z'2) is non-zero.

I tried to solve for the case where (z'1-z'2) is non-zero by dividing the first equation by the second equation:

Then, by multiplying both sides of the equation by the denominator, I was able to isolate the angle theta on one side and calculate the arctan2 to find theta as follows:

I understand that by using this method, I am assuming that z'2 is non-zero in order to have a solution. And, I also understand that I could have divided equation 2 by equation 1 in the system of equations and I would have got z'1 in the denominator which I would have assumed to be non-zero as well.

So, I developed a python function that calculates theta using the equations above and returns the rotation matrix which will then be used to rotate A and B along the Y-axis:

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

Then, I tested the function for the case where the height (z'1-z'2) is non-zero :

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}")

However, I am getting the following output when I execute the code:

current_height= 105.0
desired_height= 100
transformed_height= 102.95657644356697

As can be seen above, the height of the transformed points is not equal to the desired height.

What am I doing wrong? Is there an analytical solution to my problem that can be applied in the case where (z'1-z'2) is non-zero?

The INTENDED angle is arcsin(h/L), where h is the intended height (difference in z) and L is the length of the segment projected to the xz plane. The CURRENT angle in that plane is atan2( z2-z1,x2-x1). So just rotate (about the y axis) by INTENDED - CURRENT angle.

But be careful of two things.

Firstly, arcsin may give the wrong quadrant if you are not careful.

Secondly, the formulae that you (and I) are using find angles counterclockwise from x toward z as drawn in your diagram (as is usually done for an x-y plot, since the positive z axis will be OUT of your drawing). However, a positive rotation about the y axis will be CLOCKWISE (since the positive y axis is INTO your drawing for a right-handed coordinate system). This is taken account of in the code below by simply reversing the angular change.

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])}" )

Output:

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