Codes presented in this article is useful for you want to create a quatrotate function (found in MATLAB) by scratch.
In brief, quatrotate perform a rotation to a vector V (x, y, z) by using the quaternion Q (q0, q1,q2, q3). Don't forget to verify the results as presented in section 3 of this article (end part).
Quatrotate works by creating Direct Cosine Matrices (DCM) as shown in figure below, and afterwards multiplies the DCM and transposed vector to produce rotated V vector.
1. MATLAB IMPLEMENTATION
function Rotated_Vector = rotateVectorByQ(Q,V)
qn = Q;
% create DCM (Direct Cosine Matrices)
dcm = zeros(3,3,size(qn,1));
dcm(1,1,:) = qn(:,1).^2 + qn(:,2).^2 - qn(:,3).^2 - qn(:,4).^2;
dcm(1,2,:) = 2.*(qn(:,2).*qn(:,3) + qn(:,1).*qn(:,4));
dcm(1,3,:) = 2.*(qn(:,2).*qn(:,4) - qn(:,1).*qn(:,3));
dcm(2,1,:) = 2.*(qn(:,2).*qn(:,3) - qn(:,1).*qn(:,4));
dcm(2,2,:) = qn(:,1).^2 - qn(:,2).^2 + qn(:,3).^2 - qn(:,4).^2;
dcm(2,3,:) = 2.*(qn(:,3).*qn(:,4) + qn(:,1).*qn(:,2));
dcm(3,1,:) = 2.*(qn(:,2).*qn(:,4) + qn(:,1).*qn(:,3));
dcm(3,2,:) = 2.*(qn(:,3).*qn(:,4) - qn(:,1).*qn(:,2));
dcm(3,3,:) = qn(:,1).^2 - qn(:,2).^2 - qn(:,3).^2 + qn(:,4).^2;
Rotated_Vector = (dcm*V')';
2. PYTHON IMPLEMENTATION
def rotateVectorByQ(Q, V):
# Equivalen to quatrotate MATLAB
# 1x4 Q
# 1x3 V
q0 = Q[0]
q1 = Q[1]
q2 = Q[2]
q3 = Q[3]
q11 = (q0*q0)+(q1*q1)-(q2*q2)-(q3*q3)
q12 = 2*((q1*q2)+(q0*q3))
q13 = 2*((q1*q3)-(q0*q2))
q21 = 2*((q1*q2)-q0*q3)
q22 = (q0*q0)-(q1*q1)+(q2*q2)-(q3*q3)
q23 = 2*((q2*q3)+(q0*q1))
q31 = 2*((q1*q3)+(q0*q2))
q32 = 2*((q2*q3)-(q0*q1))
q33 = (q0*q0)-(q1*q1)-(q2*q2)+(q3*q3)
DCM = np.array([[q11, q12, q13],
[q21, q22, q23],
[q31, q32, q33]]);
V = np.matmul(DCM,V.transpose())
return V