# Quatrotate Function from Scratch in Matlab and Python

0

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

3. VERIFICATION
Please noted that :
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 !