//
// Algorithme Uzawa pour le probleme des disques en mouvement
//
n=3;
q=[0,1,1/2;0,0,1];
r=[1/5,1/5,1/5]
v=[0.3,-0.4,1.3;0,-0.3,-1.1];
rho=4/(n*(n-1))/2;
rhograd=0.1;    
N=100;
//    
mu = zeros(n,n);
u = zeros(2,n);
D = zeros(n,n); // matrice D_i,j
E = zeros(2,n,n); // gradient de D_i,j
for i=1:n
for j=i+1:n
  D(i,j) = norm(q(:,i)-q(:,j)) - r(i) - r(j);
  E(:,i,j) = (q(:,i)-q(:,j)) / norm(q(:,i)-q(:,j));
end
end
// iteration algorithme Uzawa
// 
// 
u=v;
for k=0:N-1
for i=1:n
for j=i+1:n
    //
   u(:,i) = v(:,i) +mu(i,j)*E(:,i,j);  // resolution exacte en u du pb de minimisation
   u(:,j) = v(:,j) -mu(i,j)*E(:,i,j);  // 
end
end
for i=1:n
for j=i+1:n
   mu(i,j) = max(0,mu(i,j)+rho *(-D(i,j)-E(:,i,j)'*(u(:,i)-u(:,j))))
end
end
end
disp('ideal:');disp(v)
disp('optimal:');disp(u)
//
// tracés
//
t=0:0.01:2*%pi;
//
figure(1)
clf()
subplot(121)
for i=1:n
   plot2d([q(1,i),q(1,i)+v(1,i)],[q(2,i),q(2,i)+v(2,i)],1) 
   plot2d(q(1,i)*ones(t)+r(i)*cos(t),q(2,i)*ones(t)+r(i)*sin(t),4)
   plot2d((q(1,i)+v(1,i))*ones(t)+r(i)*cos(t),(q(2,i)+v(2,i))*ones(t)+r(i)*sin(t),6)
   xtitle('deplacement ideal')
end
subplot(122)
for i=1:n
   plot2d([q(1,i),q(1,i)+u(1,i)],[q(2,i),q(2,i)+u(2,i)],1) 
   plot2d(q(1,i)*ones(t)+r(i)*cos(t),q(2,i)*ones(t)+r(i)*sin(t),4)
   plot2d((q(1,i)+u(1,i))*ones(t)+r(i)*cos(t),(q(2,i)+u(2,i))*ones(t)+r(i)*sin(t),6)
   xtitle('deplacement optimal')
end
