def calcApproxAngularVelocity(t1,t2):
q1 = Quat.from_xyzw(t1['rotation']).normalized()
q2 = Quat.from_xyzw(t2['rotation']).normalized()
dt = t2['t'] - t1['t']
assert dt>0
w1 = np.array(t1['angular_velocity'])
w2 = np.array(t2['angular_velocity'])
t_interp = .5
q_interp = (q1*t_interp+q2*(1-t_interp)).normalized()
w_interp = w1*t_interp+w2*(1-t_interp)
w_approx = (q1-q2)*q_interp.inverse() * ((2.0)/dt)
w_approx = np.array(w_approx.q[1:])
return (w_approx, w_interp, w_approx-w_interp)
from Quat import Quat import numpy as np q1 = Quat(34,12,-67,.23) q2 = Quat(.123,.756,5999,.567) assert ((q1*q2 - (q2.c()*q1.c()).c()).norm_sq() < 1e-10) q3 = q1.normalized() assert (np.linalg.norm ((q3.rotm()*np.matrix([1, 1, 0]).T).T - q3.rotVec([1, 1, 0])) < 1e-10)
