function [Wmax,Hmax,J,J_plus,MSC_S,PSI,D,K]= Funkce_G(R,W,H,G) %G=nx3
Wmax=max(abs(G(:,1)));
Hmax=max(abs(G(:,2)));
J=-1/R*[    -1  1     W+H;
             1  1   -(W+H);
            -1  1   -(W+H);
             1  1     W+H ;];
J_plus=-R/4*[-1    1  -1    1;
            1       1       1       1
           1/(W+H)      -1/(W+H)  -1/(W+H)  1/(W+H)];
PSI=atan(G(:,2)./G(:,1));
for i=1:size(G,1)
    if G(i,1)>0 && G(i,2)>0
        PSI(i)=PSI(i);
    elseif G(i,1)<0 && G(i,2)>0
        PSI(i)=PSI(i)+pi;
    elseif G(i,1)<0 && G(i,2)<0
        PSI(i)=PSI(i)+pi;
    elseif G(i,1)>0 && G(i,2)<0
        PSI(i)=PSI(i)+2*pi;
    elseif G(i,1)==0 && G(i,2)==0
        PSI(i)=0;
    end
        
end
D=sqrt(G(:,1).^2+G(:,2).^2);
K=zeros(size(G,1)*3,3);
MSC_S=zeros(size(G,1)*3,3);
for i=1:size(G,1)
K(1+(3*(i-1)):i*3,:)=[ cos(G(i,3))  sin(G(i,3)) D(i)*sin(G(i,3)-PSI(i)); 
    -sin(G(i,3))  cos(G(i,3)) D(i)*cos(G(i,3)-PSI(i));
    0           0           1               ];
MSC_S(1+(3*(i-1)):i*3,:)=[ cos(G(i,3))  -sin(G(i,3)) 0; 
    sin(G(i,3))  cos(G(i,3)) 0;
    0           0           1               ];
end