FEMmodal
% *******************************************************************************
% Solution of finite element equations using Modal Analysis Approach
% % 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;
[n,n]=size(M);
[n,m]=size(F);
gf=inv(M)*K;
[V,D]=eig(gf);
[lamda,k]=sort(diag(D));
V=V(:,k);
Factor=diag(V’*M*V);
Vnorm1=V*inv(sqrt(diag(Factor)));
nreduced=4;
Vnorm=Vnorm1(:,1:nreduced);
omega=diag(sqrt(Vnorm’*K*Vnorm));
Fnorm=Vnorm’*F;
%—————————–
% comp modal damp matrix from proportinal damp mat
%———————-
Modamp = Vnorm’*(damp*M)*Vnorm;
zeta=diag((1/2)*Modamp*inv(diag(omega)));
eta0=Vnorm’*M*q0;
deta0=Vnorm’*M*dq0;
kmat=omega.*omega;
c=diag(Modamp);
size=length(kmat);
DAMPING_COEFF = damp;
DeltaT=0.1;
num_step =1500;
%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;
kmod = kmat+a0;
theta = 1.0;
myForceplusDeltaT = Vnorm’*(F);
myForcer = 0.0*myForceplusDeltaT;
Xt = 0.0*myForceplusDeltaT;
XtminusDeltaT = Xt;
XtplusDeltaT = Xt;
dXtplusDeltaT = Xt;
dXt = Xt;
%ddXt = Vnorm’*F;
ddXt = Xt;
ddXtplusDeltaT = 0.0*F;
for j=1:num_step
time(j)=DeltaT*j;
ForceMod = myForcer + theta*(myForceplusDeltaT – myForcer) + (a1*Xt+a2*dXt+a3*ddXt);
Solution = ForceMod./kmod;
ddXtplusDeltaT = a4*Solution + a5*Xt + a6*dXt + a7*ddXt;
dXtplusDeltaT = dXt + a8*(ddXt+ddXtplusDeltaT);
XtplusDeltaT = Solution;
myForceplusDeltaT = ddXtplusDeltaT + DAMPING_COEFF*dXtplusDeltaT + kmat.*XtplusDeltaT;
XtminusDeltaT = Xt;
Xt = XtplusDeltaT;
dXt = dXtplusDeltaT;
ddXt = ddXtplusDeltaT;
myForcer = myForceplusDeltaT;
XXt=Vnorm*Xt;
FFt=Vnorm*myForceplusDeltaT;
pos1(j)=XXt(1);
pos2(j)=XXt(2);
end
figure(1);
plot(time,pos1);
figure(2);
plot(time,pos2);