% *********************************************************
% Direct numerical solution of finite element eqs. using Newark numerical integration scheme
% Cagatay Basdogan, JPL-California Institute of Technology
% 
% Ref: Basdogan, C., 2001, "Real-time Simulation of Dynamically 
% Deformable Finite Element Models Using Modal Analysis and Spectral 
% Lanczos Decomposition Methods", Proceedings of the Medicine Meets 
% Virtual Reality (MMVR'2001) Conference, Jan 24-27, Irvine, CA.
% *********************************************************
clear all;


K(1,1)=39.0;
K(1,2)=-9.0;
K(1,3)=21.0;
K(1,4)=-11.0;
K(2,1)=-9.0;
K(2,2)=39.0;
K(2,3)=-11.0;
K(2,4)=21.0;
K(3,1)=21.0;
K(3,2)=-11.0;
K(3,3)=39.0;
K(3,4)=-9.0;
K(4,1)=-11.0;
K(4,2)=21.0;
K(4,3)=-9.0;
K(4,4)=39.0;

mass = 50.0;
%mass = mass/100.0;


mm=4;
M=mass*eye(mm);
F=0.0*[1:mm]';
q0=0.0*[1:mm]';
dq0=0.0*[1:mm]';

F(1)=3;
F(2)=1;
F(3)=5;
F(4)=1;


damp=0.1;
DAMPING_COEFF = damp;
DeltaT=0.1;
num_step =1500;


myForceplusDeltaT = F;
myForcer = 0.0*F;
Xt = 0.0*[1:mm]';
XtminusDeltaT = Xt;
XtplusDeltaT = Xt;
dXtplusDeltaT = Xt;
dXt = Xt;

ddXt = inv(M)*F;
ddXtplusDeltaT = 0.0*F;


%newark

a0 = (4.0+2.0*DAMPING_COEFF*DeltaT)/(DeltaT*DeltaT);
a1 = 4.0/(DeltaT*DeltaT)+(2.0*DAMPING_COEFF)/(DeltaT);
a2 = 4.0/(DeltaT)+DAMPING_COEFF;
a3 = 1.0;
a4 = 4.0/(DeltaT*DeltaT);
a5 = -a4;
a6 = a5*(DeltaT);
a7 = -1.0;
a8 = DeltaT/2.0;
a9 = (DeltaT*DeltaT)/4.0;
a10 = a9;

kmat = K;
kmod = kmat+a0*M;
invkmod = inv(kmod);
theta = 1.0;


for j=1:num_step
	time(j)=DeltaT*j;
	ForceMod = myForcer + theta*(myForceplusDeltaT - myForcer) + M*(a1*Xt+a2*dXt+a3*ddXt);
	%Solution = ForceMod./kmod;
	Solution = invkmod*ForceMod;

	ddXtplusDeltaT = a4*Solution + a5*Xt + a6*dXt + a7*ddXt;
	dXtplusDeltaT = dXt + a8*(ddXt+ddXtplusDeltaT);
		XtplusDeltaT = Solution;
	%XtplusDeltaT = Xt + DeltaT*dXt + a9*ddXt + a10*ddXtplusDeltaT;
	myForceplusDeltaT = M*ddXtplusDeltaT + DAMPING_COEFF*M*dXtplusDeltaT + kmat*XtplusDeltaT;
	XtminusDeltaT = Xt;
	Xt = XtplusDeltaT;
	dXt = dXtplusDeltaT;
	ddXt = ddXtplusDeltaT;
	myForcer = myForceplusDeltaT;
	XXt=Xt;
	pos1(j)=XXt(1);
	pos2(j)=XXt(2);

end

figure(10);
plot(time,pos1);
figure(11);
plot(time,pos2);
