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)