slerp_quat.py

import numpy as np



def quat_len(q):
    return np.sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3])

def quat_norm(q):
    d = quat_len(q)
    if (d==0): return np.array([0,0,0,0]) 
    return np.array([q[0]/d, q[1]/d, q[2]/d, q[3]/d])
    
def quat_conj(q):
    return np.array([q[0], -q[1],-q[2],-q[3]])

def quat_inv(q):
    l = quat_len (q)
    return np.array([q[0]/l, -q[1]/l,-q[2]/l,-q[3]/l])

def quat_mul(q_a, q_b):
    [s_a, x_a, y_a, z_a] = q_a
    [s_b, x_b, y_b, z_b] = q_b
    q = [0,0,0,0]
    q[0] = s_a*s_b - x_a*x_b - y_a*y_b - z_a*z_b
    q[1] = s_a*x_b + x_a*s_b + y_a*z_b - z_a*y_b
    q[2] = s_a*y_b - x_a*z_b + y_a*s_b + z_a*x_b
    q[3] = s_a*z_b + x_a*y_b - y_a*x_b + z_a*s_b
    return np.array(q)

def quat_log_map(q):
    phi = np.arccos(q[0])
    sin_theta = np.sin(phi)
    u = [q[1]/sin_theta,q[2]/sin_theta,q[3]/sin_theta]
    return np.array([0,phi*u[0],phi*u[1],phi*u[2]])

def quat_exp_map(q):
    phi = quat_len(q)
    sin_phi = np.sin(phi)
    u = quat_norm(q)
    return quat_norm(np.array([np.cos(phi), sin_phi*u[1],sin_phi*u[2],sin_phi*u[3]]))

def quat_slerp (q0,q1, t):
    from_q0_to_q1 = quat_mul(q1, quat_conj(q0))
    tan_from_q0_to_q1 = quat_log_map(from_q0_to_q1)
    temp = quat_exp_map(t*tan_from_q0_to_q1)
    return quat_mul(temp, q0)

         
theta = np.pi/4    
axis = [0,0,1]
q0 = [np.cos(theta), np.sin(theta)*axis[0], np.sin(theta)*axis[1], np.sin(theta)*axis[2]]

theta = np.pi/4 + np.pi/4    
axis = [0,0,1]
q1 = [np.cos(theta), np.sin(theta)*axis[0], np.sin(theta)*axis[1], np.sin(theta)*axis[2]]

slerp_t = 0

slerped = quat_slerp(q0,q1,slerp_t)