# Quatrotate Function from Scratch in Matlab and Python

0

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
``````

3. VERIFICATION
Q = 1x4 vector
V = 1x3 vector

The above codes have been tested using values as follow :

Q = [0.784896612167358, -0.362114369869232, 0.475989699363708, 0.162001579999924]
V = [0.212127346533177, 0.151271140560544, 0.965463117310188]

Resulted rotated_V should be :

Rotated_V = [-0.743479512404512, -0.423328449157434, 0.517717749096532]

ENJOY !

Disclaimer : Content provided on this page is for general informational purposes only. We make no representation or warranty of any kind, express or implied, regarding the accuracy, adequacy, validity, reliability, availability or completeness of any information.