%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% CORIOLIS MATRIX

%Coriolis and centrifual terms
b11 = B(1,1)
b12 = B(1,2)
b21 = B(2,1)
b22 = B(2,2)
%cijk = simplify(0.5*(diff(bij,qk) + diff(bik,qj)-diff(bjk,qi)))
c111 = simplify(0.5*(diff(b11,q1) + diff(b11,q1)-diff(b11,q1)))
c112 = simplify(0.5*(diff(b11,q2) + diff(b12,q1)-diff(b12,q1)))
c121 = c112
c122 = simplify(0.5*(diff(b12,q2) + diff(b12,q2)-diff(b22,q1)))
c211 = simplify(0.5*(diff(b21,q1) + diff(b21,q1)-diff(b11,q2)))
c212 = simplify(0.5*(diff(b21,q2) + diff(b22,q1)-diff(b12,q2)))
c221 = c212
c222 = simplify(0.5*(diff(b22,q2) + diff(b22,q2)-diff(b22,q2)))

c11 = c111*qdot1+c112*qdot2
c12 = c121*qdot1+c122*qdot2
c21 = c211*qdot1+c212*qdot2
c22 = c221*qdot1+c222*qdot2

C = [c11 c12; c21 c22]