Quatrotate Function from Scratch in Matlab and Python

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 !