FEMexact
% *********************************************************
% 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);