% this simple matlab script allows to play with contact forces
% it takes the example shown in the book (chapter 7) :
% an ellipse falling on a wall
%
% you can choose to have a flat (like in the book) or corrugated wall
% you can choose the type of contact forces (repulsion with or without
% cohesion) and the amount of damping.

% computational box (xmin,-xmin)^2 
% and numerical parameters

pi=3.1415926;
spi=sqrt(pi);
pi2=pi^2;

xmin=-1;
ymin=-1;
% number of grid points in each direction and gird size
nx=200;
ny=200;
dx=-2*xmin/nx;
dt=0.01;
% width of level set regularization
eps=dx/2.;
% damping coefficient
coef_damping=0.5/dt;
%time-step, final simulation time, and time increment for visualization
final_time=13.;
tvisu=1;


% parameteres of initial ellipse 
ratio=2.;
angle=pi/6;
time=0.;
b1=0.5;
a1=ratio*b1;
rc1=0.125;
xc=0.;
yc=0.3;
gr=1.;

a2=a1^2;
b2=b1^2;
rc2=rc1^2;

%parameter for wall y=delta*sin(pi*lambda*x)
lambda=4;
delta=0.1;
delta2=delta^2;
lambda2=lambda^2;
zmin = -0.1;
zmax = +0.1;
zinc =0.02;
zlevs = zmin:zinc:zmax;

% initial level set
for i=1:nx;
for j=1:ny;
yg(i,j)=ymin+(j-1)*dx;
xg(i,j)=xmin+(i-1)*dx;
x=xg(i,j)-xc;
y=yg(i,j)-yc;
xx=+x*cos(angle)+y*sin(angle);
yy=-x*sin(angle)+y*cos(angle);
r1=(xx)^2/a2+(yy)^2/b2-rc2;
phig(i,j)=-r1;
omg(i,j)=0.;
vxg(i,j)=0.;
vyg(i,j)=0.;
end;
end;

z=0;
istep=0;
kstep=0;
% time-stepping

while (time <= final_time)

time=time+dt
kstep=kstep+1;

% intialisation of rigid body translation velocity and rotation
vxh=0.;
vyh=0.;
omh=0.;
tot=0.;
tot2=0.;

%--- main loop : computation of contact forces
%--           - and update of velocity and rotation


for i=1:nx;
for j=1:ny;
xx=(xmin+(i-1)*dx);
yy=(ymin+(j-1)*dx);
xxx=pi*xx;
yyy=pi*yy;
%normal to the wall
norm=1+(pi2*delta2*lambda2)*((cos(lambda*xxx))^2);
alpha=1/sqrt(norm);
beta=pi*delta*lambda*cos(lambda*xxx)/sqrt(norm);
% distance to wall
dd=yy-delta*sin(lambda*xxx);
dd=dd/sqrt(norm);
%level set for the wall (only for plot)
psig(i,j)=dd;
% body charateristic function and level-set mollifyer
if (dd > 0)
arg0=phig(i,j)/(dx);
arg0=min(1,max(1+arg0,0));
arg=exp(-phig(i,j)^2/eps)/(spi*eps);
%	damping term
term2=-exp(-dd/(eps));
%       repulsion without cohesive forces :
term1=exp(-dd/(eps))/(eps);
%       repulsion with cohesive forces :
%term1=exp(-dd/(eps))*(3.8-dd/eps)/eps;
% resulting contact force 
fx=arg*(-beta*term1+coef_damping*term2*vxg(i,j));
fy=arg*(alpha*term1+coef_damping*term2*vyg(i,j));
% body velocity update
        vxg(i,j)=vxg(i,j)+dt*fx;
        vyg(i,j)=vyg(i,j)+dt*fy-gr*dt;
% update of rigid body translational velocities and rotation
        vxh=vxh+arg0*vxg(i,j);
        vyh=vyh+arg0*vyg(i,j);
        rx=xx-xc;
        ry=yy-yc;
        omh=omh-vxg(i,j)*(ry)*arg0;
        omh=omh+vyg(i,j)*(rx)*arg0;
        tot=tot+arg0;
        tot2=tot2+(rx^2+ry^2)*arg0;
end;
        end;
        end;
% volume normalization
        vxh=vxh/tot;
        vyh=vyh/tot
        omh=omh/tot2;
%--- end of main loop for translation/rotation update

%  push center of body and compute new orientation of body
        yc=yc+dt*vyh
        xc=xc+dt*vxh;
        angle=angle+dt*omh;

% update rigid body velocity everywhere
        for i=1:nx;
        xx=xmin+(i-1)*dx-xc;
        for j=1:ny;
        yy=ymin+(j-1)*dx-yc;
        vxg(i,j)=vxh-omh*yy;
        vyg(i,j)=vyh+omh*xx;
        end;
        end;

% update of level set function for the new ellipse

        for i=1:nx;
        x=xmin+((i)-1.)*dx-xc;
        for j=1:ny;
        y=ymin+((j)-1.)*dx-yc;
        xx=+x*cos(angle)+y*sin(angle);
        yy=-x*sin(angle)+y*cos(angle);
        r1=(xx)^2/a2+(yy)^2/b2-rc2;
        phig(i,j)=-r1;
        end;
        end;

% plot of zero contour of psi for the ellipse and of the wall
b=mod(kstep,2);
if (b == 1) 
z=0;
contour(xg,yg,phig,[z z],'k','LineWidth',2);
hold on;
x = -1:1./100:1;
y = 1.5*dx+delta*sin(pi*lambda*x);
hold on;
plot(x,y,'LineWidth',4);
set(gca,'DataAspectRatio',[1 1 1]);
hold on;
%make of movie
istep=istep+1;
movieVector(istep)=getframe;
        close;
end;
% end of time-stepping
end;

final_v=vyh
final_y=yc

myWriter=VideoWriter('movie4');
myWriter.FrameRate=20;
open(myWriter);
writeVideo(myWriter,movieVector);
close(myWriter);

clear all
