Add x rotation to rotate point function

Question

I have got this function:

def rotatePoint(p1, p2, a):
    if a == 0: return p2
    x1, y1, z1 = p1
    x2, y2, z2 = p2

    d12 = math.sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1) + (z2 - z1) * (z2 - z1))

    aP1P2 = math.atan2(z2 - z1, x2 - x1)  # In radians, not degrees
    aP1P3 = aP1P2 - math.radians(a)

    x3 = x1 + d12 * math.cos(aP1P3)
    y3 = y2# same as P1 an P2
    z3 = z1 + d12 * math.sin(aP1P3)
    
    p3 = (x3, y3, z3)
    
    return p3
    

It can rotate a point around a nother on the y-plane. E.g.:

p1 = (0,0,0) 
p2 = (0,0,20) 
a = 45 
                
print(rotatePoint(p1,p2,a))

#Outputs:(14.142135623730951, 0, 14.14213562373095) 

How can I change it, that it can also rotate around the x-plane?

EDIT: I looked into a rotation matrix, but I couldn't figure it out. This is what I got, I know it's wrong. What is wrong with it:

def getRotationMatrix(a1, a2, a3):
    
    sin = math.sin
    cos = math.cos
    
    xrot = cos(a2) * cos(a3)  *  cos(a1)*sin(a3)+sin(a1)*sin(a2)*cos(a3)    *  sin(a1)*sin(a3) - cos(a1)*sin(a2)*cos(a3)
    yrot = -cos(a2)*sin(a3)   *  cos(a1)*cos(a3) - sin(a1)*sin(a2)*sin(a3)  *  sin(a1)*cos(a3) + cos(a1)*sin(a2)*sin(a3)
    zrot = sin(a2)            *  -sin(a1)*cos(a2)                           *  cos(a1)*cos(a2)
    
    return [xrot, yrot, zrot]

Answer 1

I am following transformation details as presented in -

https://en.wikipedia.org/wiki/Davenport_chained_rotations https://en.wikipedia.org/wiki/Rotation_matrix

def rotatePoint(p1, p2, a):
    '''
    https://en.wikipedia.org/wiki/Davenport_chained_rotations
    https://en.wikipedia.org/wiki/Rotation_matrix

    '''
    # rotation in degrees
    # convert to radians
    alpha,beta,gamma=[math.radians(x) for x in a]
    print(f'Rotation angles = {alpha},{beta},{gamma}')
    # math functions alias
    cos=math.cos
    sin=math.sin

    Rx_alpha=np.array([[1.0        ,   0.0     ,    0.0    ],
                       [0.0        , cos(alpha),-sin(alpha)],
                       [0.0        , sin(alpha),cos(alpha) ]])
    
    Ry_beta=np.array([[ cos(beta),   0.0,   sin(beta)],
                      [       0.0,   1.0,   0.0      ],
                      [-sin(beta),   0.0,   cos(beta)]])

    Rz_gamma=np.array([[cos(gamma) ,-sin(gamma),0.0],
                       [sin(gamma) , cos(gamma),0.0],
                       [0.0        , 0.0       ,1.0]])

    R=np.matmul(np.matmul(Rz_gamma,Ry_beta),Rx_alpha)

    print(f'Rotation matrix = {R}')
    
    # calculate the vector about point p1,
    
    point_vector=np.array(p2)-np.array(p1)
    point_vector_magnitude=np.linalg.norm(point_vector)
    # normalized
    point_vector=point_vector/point_vector_magnitude


    print(f'point_vector={point_vector}')
    print(f'point_vector_magnitude={point_vector_magnitude}')

    rotated_vector=np.matmul(R,point_vector)*point_vector_magnitude

    # calculate the transformed point p3
    p3=np.array(p1)+rotated_vector
    print(f'New Rotate point {p3}')

Checks:

rotatePoint([0,0,0],[0,0,20],[0,0,45])

Output:

Rotation angles = 0.0,0.0,0.7853981633974483
Rotation matrix = [[ 0.70710678 -0.70710678  0.        ]
 [ 0.70710678  0.70710678  0.        ]
 [ 0.          0.          1.        ]]
point_vector=[0. 0. 1.]
point_vector_magnitude=20.0
unit_rotated_vector =[0. 0. 1.]
New Rotate point [ 0.  0. 20.]

rotatePoint([0,0,0],[0,0,20],[45,0,0])

Output:

Rotation angles = 0.7853981633974483,0.0,0.0
Rotation matrix = [[ 1.          0.          0.        ]
 [ 0.          0.70710678 -0.70710678]
 [ 0.          0.70710678  0.70710678]]
point_vector=[0. 0. 1.]
point_vector_magnitude=20.0
unit_rotated_vector =[ 0.         -0.70710678  0.70710678]
New Rotate point [  0.         -14.14213562  14.14213562]

Answer 2

You are going correct in my opinion. Only one problem. The xrot, yrot and zrot are lists with three components each. So remove the multiplication sign and replace with comma and put them in a list. Below is a naive implementation. The order of rotation is important first x-axis, then y-axis and then z-axis. In your case it is ok since you wish to rotate about a single axis. You can use the function below by setting other two angles to zero. Please double check the results of this method, by testing on multiple cases, before using it further. If there is an error please let me know. I tested for two simple cases only.

import math
def TranslateToOriginAndReturnUndoVector(x1,y1,z1):
    return (x1,y1,z1)

def getRotationMatrix(a1, a2, a3, x1, y1, z1, x2, y2, z2):

    ## Step 1 translate to origin
    x_translated, y_translated, z_translated = TranslateToOriginAndReturnUndoVector(x1,y1,z1)

    ## Step 2 Calculate Rotation Matrix and new x,y,z w.r.t Step 1 values
    sin = math.sin
    cos = math.cos
    
    xrot = [cos(a2) * cos(a3)  , cos(a1)*sin(a3)+sin(a1)*sin(a2)*cos(a3)   , sin(a1)*sin(a3) - cos(a1)*sin(a2)*cos(a3)]
    yrot = [-cos(a2)*sin(a3)   ,  cos(a1)*cos(a3) - sin(a1)*sin(a2)*sin(a3)  ,  sin(a1)*cos(a3) + cos(a1)*sin(a2)*sin(a3)]
    zrot = [sin(a2)            ,  -sin(a1)*cos(a2)                           ,  cos(a1)*cos(a2)]
    x_new = 0
    y_new = 0
    z_new = 0
    for i in range(3):
        x_new += xrot[i]*(x2 - x_translated)
        y_new += yrot[i]*(y2 - y_translated)
        z_new += zrot[i]*(z2 - z_translated)
    x_new, y_new, z_new = round(x_new, 3), round(y_new,3), round(z_new,3)
    ## Step 3, Undo Step 1
    x_new, y_new, z_new = x_new + x_translated, y_new + y_translated, z_new + z_translated

    ## Return the new values
    return (x_new, y_new, z_new)

print(getRotationMatrix(0,math.pi/4,0,0,0,0, 20,20, 20))
print(getRotationMatrix(0,math.pi/4,0,10,10,10, 20,20, 20))

Gives the following output.

(0.0, 20.0, 28.284)
(10.0, 20.0, 24.142)