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


