load scamod.mat;

r11=sym('-ro1;0;0;1');
r22=sym('-ro2;0;0;1');

P1=symmul(A1,r11);
P1=symmul(g,P1);
P1=symmul('-m1',P1);

P2=symmul(A12,r22);
P2=symmul(g,P2);
P2=symmul('-m2',P2);

P=symadd(P1,P2);

J1=sym(J1,1,4,'-m1*ro1');
J1=sym(J1,4,1,'-m1*ro1');

J2=sym(J2,1,4,'-m2*ro2');
J2=sym(J2,4,1,'-m2*ro2');

poma=symmul(U11,J1);
poma=symmul(poma,transpose(U11));
pomb=0;
for ii=1:4,
  pomb=symadd(pomb,sym(poma,ii,ii));
end
poma=symmul(pomb,'t1d^2');
poma=simple(symmul('1/2',poma));

K=poma;

poma=symmul(U21,J2);
poma=symmul(poma,transpose(U21));
pomb=0;
for ii=1:4,
  pomb=symadd(pomb,sym(poma,ii,ii));
end
poma=symmul(pomb,'t1d^2');
poma=simple(symmul('1/2',poma));

K=symadd(K,poma);

poma=symmul(U21,J2);
poma=symmul(poma,transpose(U22));
pomb=0;
for ii=1:4,
  pomb=symadd(pomb,sym(poma,ii,ii));
end
poma=symmul(pomb,'t1d*t2d');
poma=simple(symmul('1/2',poma));

K=symadd(K,poma);

poma=symmul(U22,J2);
poma=symmul(poma,transpose(U21));
pomb=0;
for ii=1:4,
  pomb=symadd(pomb,sym(poma,ii,ii));
end
poma=symmul(pomb,'t1d*t2d');
poma=simple(symmul('1/2',poma));

K=symadd(K,poma);

poma=symmul(U22,J2);
poma=symmul(poma,transpose(U22));
pomb=0;
for ii=1:4,
  pomb=symadd(pomb,sym(poma,ii,ii));
end
poma=symmul(pomb,'t2d^2');
poma=simple(symmul('1/2',poma));

K=symadd(K,poma);

L=simple(symsub(K,P));

