fea_to_statespace( modes , frequencies , damping , inputScaling , outputScaling )

function [ ssObjectPVA , ssObjectP , A , B , C , D , C1 , D1 , C2 ] = fea_to_statespace( modes , frequencies , damping , inputScaling , outputScaling )
%FEA_TO_STATESPACE Data structures for using FEA results in state-space formalism
%
%  [ ssObject , A , B , C , D , C1 , D1 , C2 ] = fea_to_statespace( modes , frequencies , damping , inputScaling , outputScaling )
% ssObject implements the state-space system defined by the A, B, C, D matrices:
% dx/dt = A*x + B*v
% y     = C*x + D*v
%
% where v=Fu; that is, the matrix F selects to which nodes the input u
% corresponds. The output y in this formulation is a column vector of length
% 3*length(find(outputScaling)) consisting of all the positions, then all the
% velocities, and then all the accelerations of the nodes given by find(
% outputScaling ):
%    
%    y = [ p1 ; p2 ; p3 ; ...
%          v1 ; v2 ; v3 ; ...
%          a1 ; a2 ; a3 ; ...
%          ]
    
% The state variables x correspond to the mode variables eta: x=[ eta ;
% d(eta)/dt]. 

% Copyright 2006, The MathWorks, Inc.
    
    
    doNew = true;
    
% Get sizes
%
    
    [ numNodes               , numModes               ] = size( modes );
    [ inputScaling_numNodes  , numInputs              ] = size( inputScaling );
    [ numOutputs             , outputScaling_numNodes ] = size( outputScaling );
    
    [ frequencies_numModes ] = prod( size( frequencies ) );
    [ damping_numModes     ] = prod( size( damping ) );
 
    
    %
    % Assert size consistency
    %
    if ...
            ( numNodes ~= inputScaling_numNodes  ) || ...
            ( numNodes ~= outputScaling_numNodes ) || ...
            ( numModes ~= frequencies_numModes   ) || ...
            ( numModes ~= damping_numModes       ) 
        
        error( 'Inconsistent number of modes or nodes. Check matrices.' );
    end
    
    
    %
    % Form matrices
    %
    
    Omega = diag( frequencies.^2 );
    Beta  = diag( frequencies .*  damping );
    Gamma  = diag( frequencies .*  damping );
    Sigma = modes' * inputScaling ;
    Theta = outputScaling * modes ;

    A = [ zeros( [ numModes , numModes ] )     eye( [ numModes , numModes ] )
          -Omega                               -Beta                           ];
    
    B = [ zeros( [ numModes , numInputs ] ) 
          Sigma                              ];
    
    Cp = [ Theta       zeros( [ numOutputs , numModes ] )   ];
    
    Dp = zeros( [ numOutputs , numInputs ] ) ;
    ssObjectP = ss( A , B ,Cp ,Dp );
    

    % new
    zeroOutputModes  = zeros( [ numOutputs numModes ] );
    zeroOutputInputs = zeros( [ numOutputs numInputs] );

    Cpva = [ Theta            zeroOutputModes
           zeroOutputModes  Theta
           -Theta*Omega     -Theta*Gamma           ];
    Dpva = [ zeroOutputInputs
          zeroOutputInputs
          Theta * Sigma ];
    ssObjectPVA = ss( A , B , Cpva , Dpva ); % zeroing D
    
    if doNew
        C = -Cpva; D= Dpva; %%% zeroing this to avoid double counting
    else
        C=Cp;D=Dp;
    end

 %{
    
    C1 = [ eyeModes   zeroModes
           zeroModes  eyeModes
           zeroModes  zeroModes ];
    
    zeroModeInputs = zeros( [ numModes numInputs] );
    eyeInputs      = eye( numInputs );
    D1 = [ zeroModeInputs
           zeroModeInputs
           eyeInputs ];
    
    C2 = Theta * ...
         [ eyeModes   zeroModes   zeroModes
           zeroModes  eyeModes    zeroModes
           -Omega     -Gamma      Sigma ];
    
 %}
    
%    ssObject = ss( A , B , C , D );