disp(' ');
disp(' transfer_find_force.m   ver 1.0   March 27, 2012 ');
disp(' by Tom Irvine ');
%
disp(' ');
disp(' This program calculates a transfer function for two selected   ');
disp(' degrees-of-freedom in a system based on the mode shapes, ');
disp(' natural frequencies, and damping ratios. It also calculates');
disp(' the applied force for a single given displacement, velocity,');
disp(' or acceleration. ');
disp(' ');
%
close all;
%
clear freq;
clear fnv;
clear dampv;
clear QE;
clear H;
clear HM;
clear HP;
clear max;
clear PHA;
clear PPP;
%
clear force;
clear resp;
clear length;
clear s;
%
fig_num=1;
%
tpi=2*pi;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
disp(' ');
disp(' Select input method ');
disp('  1=mass & stiffness matrices ');
disp('  2=natural frequencies and mass-normalized eigenvectors ');
%
imethod=input(' ');
%
disp(' ');
disp(' Select response metric ');
disp('  1=displacement ');
disp('  2=velocity ');
disp('  3=acceleration ');
%
iam=input(' ');
%
if(imethod==1)
    disp(' ');
        mass = input(' Enter the mass matrix name:  ');
        mass
    disp(' ');
    disp(' Divide mass by 386? ');
    disp(' 1=yes 2=no ');
    idm=input(' ');
    if(idm==1)
        mass=mass/386;
        mass
    end
    m=mass;
    disp(' ');
    stiffness = input(' Enter the stiffness matrix name:  ');
    stiffness
%
    [ModeShapes,Eigenvalues]=eig(stiffness,mass);
%
    omega = sqrt(Eigenvalues);
    nf=max(size(mass));
%
    for(i=1:nf)
        fnv(i)=omega(i,i)/tpi;
    end
%
    disp(' ');
    disp('  Natural Frequencies (Hz) ');
    for i=1:nf
       out1=sprintf('  %8.4g ',fnv(i));
       disp(out1);
    end
%
    MST=ModeShapes';
    temp=zeros(nf,nf);
    temp=m*ModeShapes;
    QTMQ=MST*temp;
%   
    clear scale;
    for i=1:nf
        scale(i)=1./sqrt(QTMQ(i,i));
        ModeShapes(:,i) = ModeShapes(:,i)*scale(i);    
    end
%
    disp(' ');
    disp('  Modes Shapes (column format)');
    QE=ModeShapes
else
    disp(' ');
    disp(' Enter the name of the mass-normalized eigenvector matrix ');
    QE=input(' ');
    num=max(size(QE));
%
    disp(' ');
    disp(' Enter the natural frequency (Hz) vector or matrix ');
    clear fff;
    fff=input(' ');
    sz=size(fff);
    if(sz(1)==sz(2))
        for i=1:sz(1)
            fnv(i)=fff(i,i);
        end
    else
        fnv=fff;
    end
end
%
disp(' ');
disp(' Select damping input method ');
disp(' 1=constant for all modes   2=vector ');
dm=input(' ');
if(dm==1)
   disp(' ');
   disp(' Enter the modal damping ratio ');
   mdr=input(' ');
   dampv=mdr*ones(max(size(fnv)),1);
else
   disp(' ');
   disp(' Enter the modal damping ratio vector ');
   dampv=input(' ');
end
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
disp(' ');
disp(' Select acceleration input method:  1=time history  2=FFT ');
jfm=input(' ');
%
if(jfm==1)
    disp(' ');
    disp(' The response time history must have two columns: time(sec) amplitude ');
    disp(' ');
    disp(' The time step must be constant. ');
    disp(' ');
%
    if(iam==1)
        disp(' Enter displacement time history filename: time(sec) disp(in)');
    end
    if(iam==2)
        disp(' Enter velocity time history filename: time(sec) vel(in/sec)');
    end      
    if(iam==3)
        disp(' Enter acceleration time history filename: time(sec) accel(G)');
    end      
%    
    resp_TH=input(' ');
    s=length(resp_TH(:,1));
    N=2^(floor(log(s)/log(2)));
    resp=fft(resp_TH(:,2),N);
    resp=resp/N;
%
    dt=(resp_TH(N)-resp_TH(1))/(N-1);
%
    df=1./(N*dt);
    out4 = sprintf(' df  = %8.4g Hz  ',df);
    disp(out4)
%
    freq=linspace(0,df*(N-1),N);
%
else
    disp(' ');
    disp(' The response FFT must have two columns: freq(Hz) complex amplitude ');
    disp(' ');
    disp(' The frequency step must be constant. ');
    disp(' ');
%
    if(iam==1)
        disp(' Enter displacement FFT filename: freq(Hz) complex(in)');
    end
    if(iam==2)
        disp(' Enter velocity FFT filename:  freq(Hz) complex(in/sec)');
    end      
    if(iam==3)
        disp(' Enter acceleration FFT filename: freq(Hz) complex(G) ');
    end
    resp=input(' ');
%
    clear num;
    clear N;
    num=max(size(resp));
    iflag=0;
%
    for i=1:18
        if( 2^i == num )
                N=num;
                iflag=1;
                break;
        end
    end
%
    if( iflag==0)
            N=2^( floor(log(num)/log(2)) ); 
    end   
%
    freq=resp(:,1);
    clear temp;
    temp=resp(1:N,2);
    clear resp;
    resp=temp;
%
%  Force symmetry
%
    N2M1=(N/2)-1;
%
    for i=1:N2M1 
        resp(N-i+1) =  complex(real(resp(i+1)),-imag(resp(i+1)));
    end
%
    N_half=N/2;
%
end
%
out1=sprintf('\n N=%d \n',N);
disp(out1);
N_fft=N;
%
df=(freq(N)-freq(1))/(N-1);
%
out1=sprintf(' df=%8.4g \n',df);
disp(out1);
%
disp(' ');
disp(' Enter minimum plot frequency (Hz) '); 
fmin=input(' ');
disp(' ');
disp(' Enter maximum plot frequency (Hz) '); 
fmax=input(' ');
%
if(fmax<=fmin)
    fmax=100*fmin;
end
%
clear omega;
%
omega=2*pi*freq;
omega2=omega.*omega;
%
j=sqrt(-1);
%
clear omn;
omn=tpi*fnv;
omn2=omn.*omn;
H=zeros(N_half,1);
%freq=resp(:,1);
force=zeros(N,1);
%
while(1)
disp(' ');
i=input(' Enter response dof ');
k=input(' Enter force input dof ');
%
i_resp=i;
k_resp=k;
%
small_num=1.0e-20;
%
for s=1:N_half % frequency loop
%
    for r=1:nf
        rho=freq(s)/fnv(r);
        den=1-rho^2+j*2*dampv(r)*rho;
%                
        term=(QE(i,r)*QE(k,r)/den)/omn2(r);
%        
        if(iam==2)
            term=term*omega(s);                   
        end
        if(iam==3)
            term=term*omega2(s);                    
        end 
        H(s)=H(s)+term;
    end
%
    if( abs(H(s))>small_num )
        force(s)=resp(s)/H(s);
    else
        force(s)=small_num;
    end    
%
end
%
for i=1:N2M1 
    force(N-i+1) =  complex(real(force(i+1)),-imag(force(i+1)));
end
%
%
HM=abs(H);
HP=-atan2(imag(H),real(H))*180/pi;
%
PHF=-atan2(imag(force),real(force))*180/pi;
%
disp(' ');
disp(' Enter units: ');
if(iam==1)
    disp(' 1= inches, lbf ');
    disp(' 2= meters, N ');
end
if(iam==2)
    disp(' 1= inches/sec, lbf ');
    disp(' 2= meters/sec, N ');
end
if(iam==3)
    disp(' 1= G, lbf ');
    disp(' 2= meters/sec^2, N ');
end
iu=input(' ');
%
if(iam==3 && iu==1)
   HM=HM/386;
end
%
ijk=0;
%%% for(i=1:num)
%%%     for(k=1:num)
      ijk=ijk+1;
% 
      PHA=HP;
      PPP=HM;     
%
      for ia=1:N_half
%
        while(PHA(ia)<0)
            PHA(ia)=PHA(ia)+360.;
        end
%
      end
%
      ymax=max(PPP);
      ymin=min(PPP);
%
      ymax=10^(ceil(0.1+log10(ymax)));
%
      ymin=10^(floor(log10(ymin)));
%
      if(ymin<ymax*0.0001)
          ymin=ymax*0.0001;
      end
%
      sz=size(freq);
      if(sz(2)>sz(1))
          freq=freq';
      end
%
      sz=size(PPP);
      if(sz(2)>sz(1))
          PPP=PPP';
      end 
%
      nsz=max(size(PPP));
      fp=freq(1:nsz);
%
      figure(fig_num);
      fig_num=fig_num+1;
      subplot(3,1,1);
      plot(fp,PHA);
      out1=sprintf('Transfer Function Phase H%d%d',i_resp,k_resp);
      title(out1);     
      grid on;
      ylabel('Phase(deg)');  
%
%
      if(iam==1)
        axis([fmin,fmax,-180,0])
        set(gca,'MinorGridLineStyle','none','GridLineStyle',':','XScale','log','YScale','lin','ytick',[-180,-90,0]);    
      end
      if(iam==2)
        axis([fmin,fmax,-90,90])
        set(gca,'MinorGridLineStyle','none','GridLineStyle',':','XScale','log','YScale','lin','ytick',[-90,0,90]);    
      end
      if(iam==3)
        axis([fmin,fmax,0,180])
        set(gca,'MinorGridLineStyle','none','GridLineStyle',':','XScale','log','YScale','lin','ytick',[0,90,180]);    
      end
%
      subplot(3,1,[2 3]);
      plot(fp,PPP);
      grid on;
      xlabel('Frequency(Hz)');
%      
      if(iam==1)
        if(iu==1)
            ylabel('Disp/Force (in/lbf)');
            out1=sprintf('\n maximum = %8.4g (in/lbf) \n',max(PPP));  
        else
            ylabel('Disp/Force (m/N) ');
            out1=sprintf('\n maximum = %8.4g  (m/N) \n',max(PPP));
        end      
      end
%
      if(iam==2)
        if(iu==1)
            ylabel('Vel/Force (in/sec/lbf)');
            out1=sprintf('\n maximum = %8.4g (in/sec/lbf) \n',max(PPP));        
        else
            ylabel('Vel/Force (m/sec/N) ');
            out1=sprintf('\n maximum = %8.4g (m/sec/N) \n',max(PPP));      
        end
      end
%
      if(iam==3)
        if(iu==1)
            ylabel('Accel/Force (G/lbf)');
            out1=sprintf('\n maximum = %8.4g (G/lbf) \n',max(PPP));   
        else
            ylabel('Accel/Force (m/sec^2/N) '); 
            out1=sprintf('\n maximum = %8.4g (m/sec^2/N) \n',max(PPP));        
        end
      end
%
      disp(out1);  
%
%%      out1=sprintf('Transfer Function Magnitude H%d%d',i,k);
%%      title(out1);
      axis([fmin,fmax,ymin,ymax]);
      set(gca,'MinorGridLineStyle',':','GridLineStyle',':','XScale','log','YScale','log');  
%
%
%
      figure(fig_num);
      fig_num=fig_num+1;
%      
      ymax=max(abs(force(1:max(size(fp)))));
      ymin=min(abs(force(1:max(size(fp)))));
%      
      ymax=10^(ceil(0.1+log10(ymax)));
      ymin=10^(floor(log10(ymin)));
%
      if(ymin<ymax*0.0001)
          ymin=ymax*0.0001;
      end
%
      subplot(3,1,1);
      PHF_plot=PHF(1:max(size(fp)));
      plot(fp,PHF_plot);
      out1=sprintf('Force at %d',k);
      title(out1);          
      grid on;
      ylabel('Phase(deg)'); 
      axis([fmin,fmax,-180,180])
      set(gca,'MinorGridLineStyle','none','GridLineStyle',':','XScale','log','YScale','lin','ytick',[-180,0,180]);          
%
      subplot(3,1,[2 3]);
      plot(fp,abs(force(1:max(size(fp)))));         
      grid on;      
      xlabel('Frequency(Hz)');
      ylabel('Force(lbf)');
      axis([fmin,fmax,ymin,ymax]);     
      set(gca,'MinorGridLineStyle',':','GridLineStyle',':','XScale','log','YScale','log');       
%
%
      for i=1:N2M1 
          force(N-i+1) =  complex(real(force(i+1)),-imag(force(i+1)));
      end
%
      clear force_FFT;
      force_FFT=[freq force];     
%
%
%%%     end
%%% end
%       
%
%% disp(' ');
%% disp(' Perform calculation for another pair ? ');
%% disp(' 1=yes  2=no ');
%% inc=input(' ');
%%    if(inc==2)
%%        break;
%%    end
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
    disp(' ');
    disp(' Calculate force time history?  1=yes  2=no');
    ifth=input(' ');
%
    if(ifth==1)
%
        clear ccc;
        clear num;
        clear TT;
%
        N=N_fft;
%
        Y = ifft(force,N,'symmetric');  % resume here
%
        dt=1./max(freq(1:N));
%
        TT = linspace(0.,N*dt,N);
        TT=TT';
%
        figure(fig_num);
        fig_num=fig_num+1;
        YR=real(Y)*N;
%
        if(iam==3 && iu==1)
            YR=YR*386;
        end
%
        plot(TT,YR)
        xlabel(' Time (sec)');
        ylabel(' Amplitude (lbf)'); 
    end
%
    break;
%    
end
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%