disp(' ');
disp('  mdof_modal_enforced_displacement_ri.m  ver 1.2  February 9, 2012 ');
disp('  by Tom Irvine ');
%
disp(' ');
disp('  Response of a multi-degree-of-freedom system to enforced ');
disp('  displacement at selected point mass nodes via a ramp invariant ');
disp('  digital recursive filtering relationship. ');
disp(' ');
%
close all hidden;
%
clear Mdd;  % driven
clear Mdf;
clear Mfd;
clear Mff;  % free
%
clear Mdw;
clear Mwd;
clear Mww;
%
clear Kdw;
clear Kwd;
clear Kww;
%
clear MP;
clear MT;
%
clear Kdd;  
clear Kdf;
clear Kfd;
clear Kff;
%
clear invKff;
%
%
clear KP;
clear KT;
%
clear n;
clear na;
clear nv;
clear nd;
%
clear m;
clear k;
%
clear TT;
clear T1;
clear T2;
%
clear I;
clear Idd;
clear Iff;
%
clear damp;
clear ea;
clear f;
clear ngw;
%
clear ttend;
clear ttstart;
clear length;
%
clear inv_KH;
clear inv_KH_diag;
%
clear dT;
clear vT;
clear accT;
%
clear ZdT;
clear ZvT;
clear ZaccT;
%
clear dvelox;
clear dvelox;
clear ddisp;
%
clear forig;
%
disp(' ');
disp(' Enter the units system ');
disp(' 1=English  2=metric ');
iu=input(' ');
%
disp(' Assume symmetric mass and stiffness matrices. ');
%
mass_scale=1;
%
if(iu==1)
     disp(' Select input mass unit ');
     disp('  1=lbm  2=lbf sec^2/in  ');
     imu=input(' ');
     if(imu==1)
         mass_scale=386;
     end
else
    disp(' mass unit = kg ');
end
%
if(iu==1)
    disp(' stiffness unit = lbf/in ');
else
    disp(' stiffness unit = N/m ');
end
%
disp(' ');
disp(' Select file input method ');
disp('   1=file preloaded into Matlab ');
disp('   2=Excel file ');
file_choice = input('');
%
disp(' ');
disp(' Mass Matrix ');
%
if(file_choice==1)
        m = input(' Enter the matrix name:  ');
end
if(file_choice==2)
        [filename, pathname] = uigetfile('*.*');
        xfile = fullfile(pathname, filename);
%
        m = xlsread(xfile);
%
end
%
m=m/mass_scale;
%
mass=m;
num=max(size(m));
%
disp(' ');
disp(' Stiffness Matrix ');
%
if(file_choice==1)
        k = input(' Enter the matrix name:  ');
end
if(file_choice==2)
        [filename, pathname] = uigetfile('*.*');
        xfile = fullfile(pathname, filename);
%
        k = xlsread(xfile);
%
end
stiff=k;
%
disp(' Input Matrices ');
mass
stiff
%
[fn,omega,ModeShapes,MST]=Generalized_Eigen(stiff,mass,1);
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
disp(' Select modal damping input method ');
disp('  1=uniform damping for all modes ');
disp('  2=damping vector ');
idm=input(' ');
%
disp(' ');
if(idm==1)
   disp(' Enter damping ratio ');
   dampr=input(' ');
   damp(1:num)=dampr;
else
   disp(' Modal Damping Vector ');
%
   if(file_choice==1)
        damp = input(' Enter the vector:  ');
   end
   if(file_choice==2)
        [filename, pathname] = uigetfile('*.*');
        xfile = fullfile(pathname, filename);
%        
        damp = xlsread(xfile);
%         
   end
end
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
sz=size(mass);
ndof=(sz(1));
num=ndof;
%
out1=sprintf('\n number of dofs =%d \n',num);
disp(out1);
%
disp(' ');
disp(' Enter the duration (sec) ');
dur=input(' ');
disp(' ');
sug=20*(max(omega)/(2*pi));
%
disp(' Enter the sample rate (samples/sec)');
out1=sprintf(' (suggest > %8.4g ) ',sug);
disp(out1);
sr=input(' ');
dt=1/sr;
nt=1+round(dur/dt);
%
t =  linspace(0,dur,nt);
nt=length(t);
t=t';
%
[FFI,disp_dof,nff,nem,nfree,enforced_dof,free_dof,ngw]=...
    ODE_displacement_input(iu,num,nt,t,file_choice);
%
forig=zeros(nt,nem);
%
k=1;
for i=1:num
    nfi=disp_dof(i);
    if(nfi>0)
        forig(:,k)=FFI(:,nfi);
        k=k+1;
    end    
end
%
clear FFI;
FFI=forig;
%
clear FFI;
FFI=forig;
%
n=nt;
ddt=12.*dt;
for i=1:nem
    clear y
    y=FFI(:,i);
	dvelox(1,i)=( -y(3)+4.*y(2)-3.*y(1) )/(2.*dt);
	dvelox(2,i)=( -y(4)+4.*y(3)-3.*y(2) )/(2.*dt);
%  	
    dvelox(3:(n-2),i)=(-y(5:n)+8*y(4:(n-1))-8*y(2:(n-3))+y(1:(n-4)))/ddt;
%
    dvelox(n-1,i)=( +y(n-1)-y(n-3) )/(2.*dt);
    dvelox(n,i)  =( +y(n-1)-y(n-2) )/dt;
end
%
for i=1:nem
    clear y
    y=dvelox(:,i); 
	daccel(1,i)=( -y(3)+4.*y(2)-3.*y(1) )/(2.*dt);
	daccel(2,i)=( -y(4)+4.*y(3)-3.*y(2) )/(2.*dt);
%  	
    daccel(3:(n-2),i)=(-y(5:n)+8*y(4:(n-1))-8*y(2:(n-3))+y(1:(n-4)))/ddt;
%
    daccel(n-1,i)=( +y(n-1)-y(n-3) )/(2.*dt);
    daccel(n,i)  =( +y(n-1)-y(n-2) )/dt;    
end
%
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  Partition Mass & Stiffness Matrices
%
%
[fn,omega,ModeShapes,MST,Mwd,Kwd,Mww,Kww,TT,T1,T2,KT,MT]=...
    partition_matrices(mass,stiff,num,nfree,nem,free_dof,enforced_dof,'disp');
%
%
omegan=omega;
%
clear r;
clear part;
clear ModalMass;
%
r=ones(nfree,1);
%
part = MST*Mww*r;
%
disp(' Participation Factors  ');
part
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  Modal Transient
%
clear omegad;
for i=1:nfree
    omegad(i)=omegan(i)*sqrt(1-damp(i)^2);
end
%
clear Q;
Q=ModeShapes;
QT=Q';
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  displacement
%
%  d
%  v
%  acc
%
%
%%
%%
t=linspace(0,dur,nt);
%
%%
nFA=zeros(nt,nfree);
%
for i=1:nt
%    
        nFA(i,:)=-QT*Kwd*FFI(i,:);  
%
end 
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%  Calculate Filter Coefficients
%
disp(' ');
disp(' Calculate Filter Coefficients ');
%
[a1,a2,df1,df2,df3,vf1,vf2,vf3,af1,af2,af3]=ramp_invariant_filter_coefficients(nfree,omegan,damp,dt);
%
%  Numerical Engine
%
disp(' ')
disp(' Calculating response...');
%
clear d;
clear v;
clear acc;
%
d=zeros(nt,nfree);
v=zeros(nt,nfree);
acc=zeros(nt,nfree);
%
nx=zeros(nt,nfree);
nv=zeros(nt,nfree);
na=zeros(nt,nfree);
%
for j=1:nfree
%
%  displacement
%
    d_forward=[   df1(j),  df2(j), df3(j) ];
    d_back   =[     1, -a1(j), -a2(j) ];
    d_resp=filter(d_forward,d_back,nFA(:,j));
%    
%  velocity
%
    v_forward=[   vf1(j),  vf2(j), vf3(j) ];
    v_back   =[     1, -a1(j), -a2(j) ];
    v_resp=filter(v_forward,v_back,nFA(:,j));
%    
%  acceleration
%   
    a_forward=[   af1(j),  af2(j), af3(j) ];
    a_back   =[     1, -a1(j), -a2(j) ]; 
    a_resp=filter(a_forward,a_back,nFA(:,j));
%
    nx(:,j)=d_resp;  % displacement
    nv(:,j)=v_resp;  % velocity
    na(:,j)=a_resp;  % acceleration  
%
end
%
%
for i=1:nt
    d(i,:)=(Q*(nx(i,:))')';
    v(i,:)=(Q*(nv(i,:))')';
    acc(i,:)=(Q*(na(i,:))')';    
end
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
clear d_save;
clear v_save;
clear a_save;
d_save=d;
v_save=v;
a_save=acc;
%
%    Transformation back to xd xf
%
%%    dT=TT*d';
%%    vT=TT*v';
%%    accT=TT*acc';
%
dT=(T1*FFI' + T2*d')';
%
vT=(T1*dvelox' + T2*v')';
%
accT=(T1*daccel' + T2*acc')';
%
%  Put in original order
%
ZdT=[ forig  dT ];
ZvT=[ dvelox  vT ];
ZaccT=[ daccel  accT ];
%
clear d;   
clear v;
clear acc;
d=zeros(nt,num);
v=zeros(nt,num);
acc=zeros(nt,num);
%
for i=1:num
   for j=1:nt
        d(j,ngw(i))=  (ZdT(j,i));        
        v(j,ngw(i))=  (ZvT(j,i));     
      acc(j,ngw(i))=(ZaccT(j,i));
   end
end
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
mdof_plot(t,d,v,acc,num,iu)
%