FEMspectral
% *******************************************************************************
% Solution of finite element equations via Spectral Lanczos Decomposition Method
% In order to obtain stable solutions, the tri-diagonal matrix was determined
% using the complete reorthogonalization Lanczos scheme that relies on repetitive Householder transformations
% as suggested in Golub et al., Matrix Computations, pg. 481 and pg. 208.
% 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;
magF = 0.0;
damp = 0.1;
DeltaT=0.1;
num_step=1500;
DAMPING_COEFF = damp;
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;
mysize = mm;
M=mass*eye(mm);
F=0.0*[1:mm]’;
F(1)=3;
F(2)=1;
F(3)=5;
F(4)=1;
%*********************************************************************
%************ SPECTRAL ANALYSIS using LANCZOS METHOD ******************
%**********************************************************************
%convert your M.ddXt + B.dXt + K.Xt = F to a special form defined as
%(Aprime1 + DAMPING_COEFF.d/dt + dd/dt).Eprime = Fprime
% where Aprime1 = M^(-1/2) * K * M^(-1/2)
% Eprime = M^(1/2)*X
% Fprime = M^(-1/2)*F
Mmod = M;
for i=1:mm
Mmod(i,i) = 1.0/sqrt(abs(M(i,i)));
end
A = Mmod*K*Mmod;
myForcer = Mmod*F;
magF=0;
for j=1:mm
magF = magF + myForcer(j)*myForcer(j);
end
magF = sqrt(magF);
m=4; %number of selected orthogonal vectors, max(m) = mm-1
myQQ(1:mm,1)=myForcer;
myQQ(:,1)=myQQ(:,1)/sqrt(myQQ(:,1)’*myQQ(:,1));
[a1,b1,c1]=hholder2(myQQ(:,1),1);
alphaV(1) = myQQ(:,1)’*A*myQQ(:,1);
allH1 = b1;
allH2 = b1;
E=eye(mm);
flops(0)
for k=1:(m-1)
if k == 1
r(1:mysize,k)=(A-alphaV(k)*E)*myQQ(:,k);
else
r(1:mysize,k)=(A-alphaV(k)*E)*myQQ(:,k) – betaV(k-1)*myQQ(:,k-1);
end
betaV(k) = sqrt(r(:,k)’*r(:,k));
w = allH1 * r(:,k)
[a,b,c]=hholder2(w,k+1);
tesa(1:mysize,k) = a;
test(1:mysize,k) = b*w;
%allH1 = b*allH1; % computationally slow
%faster solution (page 212 of Golub))
allH1(k+1:mysize,:) = (eye(mysize-k)-c*a(k+1:mysize)*a(k+1:mysize)’)*allH1(k+1:mysize,:);
allH2 = allH2*b;
myQQ(1:mysize,k+1) = allH2*E(:,k+1);
myQQ(1:mysize,k+1) = r(:,k)/betaV(k);
alphaV(k+1) = myQQ(:,k+1)’*A*myQQ(:,k+1);
end
flops
nM = m;
% set-up the H matrix, z matrix for tqli
for i=1:nM
for j=1:nM
H(i,j) = 0.0;
end
end
for i=1:nM
H(i,i) = alphaV(i);
end
for i=2:nM
H(i,i-1) = betaV(i-1);
H(i-1,i) = betaV(i-1);
end
Aprime = H;
[v,d]=eig(Aprime);
diagd=diag(d);
temp=eye(length(diagd));
for i=1:length(diagd)
bterm(i)=(diagd(i)-(DAMPING_COEFF^2.0)/4.0);
if(bterm(i) > 0.0)
soltype(i)=1;
else
soltype(i)=2;
end
end
e1=0.0*[1:nM]’;
e1(1)=1.0;
%%%%%%%%%%%%%%%%%%%%%%
%%%%%% num solution
%%%%%%%%%%%%%%%%%%%%%%%%
for j=1:num_step
t=DeltaT*j;
time(j)=DeltaT*j;
%%%%%%%%%%%%%%%%%%%%%%%%%
for i=1:length(diagd)
if (soltype(i)==1)
tmp1=exp(-DAMPING_COEFF*t/2.0)*cos(sqrt(bterm(i))*t);
tmp2=(DAMPING_COEFF/(2*sqrt(bterm(i))))*exp(-DAMPING_COEFF*t/2.0)*sin(sqrt(bterm(i))*t);
temp(i,i)=1.0/diagd(i) – (1.0/diagd(i))*(tmp1+tmp2);
else
tmp1=exp(-DAMPING_COEFF*t/2.0)*cosh(sqrt(abs(bterm(i)))*t);
tmp2=(DAMPING_COEFF/(2*sqrt(abs(bterm(i)))))*exp(-DAMPING_COEFF*t/2.0)*sinh(sqrt(abs(bterm(i)))*t);
temp(i,i)=1.0/diagd(i) – (1.0/diagd(i))*(tmp1+tmp2);
end
end
sol=magF*myQQ*v*temp*v’*e1;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
totsol=sol;
sol1 = Mmod*totsol;
pos1(j)=sol1(1);
pos2(j)=sol1(2);
end
figure(8)
plot(time,pos1);
figure(9);
plot(time,pos2);