Rodrigues formula

import numpy as np
from math import degrees

# define the cross product matrix
def cross_product_matrix(v):
    return np.array([[0,-v[2],v[1]],
                     [v[2],0,-v[0]],
                     [-v[1],v[0],0]])

def print_ndarray(s:str, v:np.ndarray):
    with np.printoptions(precision=3, suppress=True):
        print(f"{s}{v}")

def print_ndarray2D(s:str, v:np.ndarray):
    with np.printoptions(precision=3, suppress=True):
        print(f"{s}")
        print(f"{v}")

def print_format_float(s:str, v:np.ndarray):
    x = np.format_float_positional(v, unique=False, precision=2)
    print(f"{s}{x}")
    
def normalize_array(x:np.ndarray) -> np.ndarray:
    return x/np.linalg.norm(x)

def axis_between_vectors(v:np.ndarray,w:np.ndarray) -> np.ndarray:
    return normalize_array(np.cross(v, w))

def angle_between_vectors(v:np.ndarray,w:np.ndarray) -> np.float:
    return np.arccos(normalize_array(v)@normalize_array(w))
    
def rodriguez_rotation(v:np.ndarray, theta:np.float, k:np.ndarray) -> np.ndarray:
    '''
    Rodrigues rotation formula
        rotate a vector 𝑣⃗
        by some angle 𝜃
        about an arbitrary axis 𝑘⃗ :

    𝑣⃗ rot=𝑣⃗ cos(𝜃)+(𝑘⃗ ×𝑣⃗ )sin(𝜃)+𝑘⃗ (𝑘⃗ ⋅𝑣⃗ )(1−cos𝜃)
    '''
    return v*np.cos(theta) + (np.cross(k,v)*np.sin(theta)) + k*(np.dot(k,v))*(1.0-np.cos(theta))

# display Rodriguez formula function's help
help(rodriguez_rotation)

## input parameters

# inital vector 
v0 = normalize_array(np.array([1,1,1]))
print_ndarray('v init = ',v0)

# desired final position
vr_final = np.array([1,0,0])
print_ndarray('v final = ',vr_final)

## calculations

# rotation angle
angle_rot = angle_between_vectors(v0,vr_final)
print_format_float('rotation angle=',degrees(angle_rot))

k = np.cross(v0, vr_final) / np.linalg.norm(np.cross(v0, vr_final))
K = np.array([[0, -k[2], k[1]],[k[2], 0, -k[0]],[-k[1], k[0], 0]])
print(K)
R = np.eye(3) + np.sin(angle_rot)*K + (1-np.cos(angle_rot))*np.linalg.matrix_power(K,2)

vr_according_to_matrix_form = np.matmul(R,v0)
print_ndarray("vr_according_to_matrix_form =",vr_according_to_matrix_form)

# rotation axis
k_ax = axis_between_vectors(v0,vr_final)
print_ndarray('rotation ax =',k_ax)

cp = cross_product_matrix(k_ax)
print_ndarray2D('cross product matrix =',cp)

v_rotated = cp@k_ax
print_ndarray2D('v_rotated=',v_rotated)

vr_according_to_vector_form = rodriguez_rotation(v0, angle_rot, k_ax)
print_ndarray('vr_according_to_vector_form=',vr_according_to_vector_form)