csolver.h

/*************************************************************************************
 * MechSys - A C++ library to simulate (Continuum) Mechanical Systems                *
 * Copyright (C) 2005 Dorival de Moraes Pedroso <dorival.pedroso at gmail.com>       *
 * Copyright (C) 2005 Raul Dario Durand Farfan  <raul.durand at gmail.com>           *
 *                                                                                   *
 * This file is part of MechSys.                                                     *
 *                                                                                   *
 * MechSys is free software; you can redistribute it and/or modify it under the      *
 * terms of the GNU General Public License as published by the Free Software         *
 * Foundation; either version 2 of the License, or (at your option) any later        *
 * version.                                                                          *
 *                                                                                   *
 * MechSys is distributed in the hope that it will be useful, but WITHOUT ANY        *
 * WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A   *
 * PARTICULAR PURPOSE. See the GNU General Public License for more details.          *
 *                                                                                   *
 * You should have received a copy of the GNU General Public License along with      *
 * MechSys; if not, write to the Free Software Foundation, Inc., 51 Franklin Street, *
 * Fifth Floor, Boston, MA 02110-1301, USA                                           *
 *************************************************************************************/

#ifndef MECHSYS_FEM_CSOLVER_H
#define MECHSYS_FEM_CSOLVER_H

#ifdef HAVE_CONFIG_H
  #include "config.h"
#else
  #ifndef REAL
    #define REAL double
  #endif
#endif

#include "fem/solver/intsolverdata.h"
#include "fem/data.h"
#include "fem/output.h"
#include "fem/node.h"
#include "fem/ele/element.h"
#include "linalg/vector.h"
#include "linalg/matrix.h"
#include "linalg/lawrap.h"

namespace FEM
{


class CSolver
{
public:
    // Constructor
    CSolver(FEM::InputData const & ID, FEM::Data & data, FEM::Output & output);
    // Destructor
    virtual ~CSolver() {}
    // Methods
    void Solve(int iStage);
protected:
    // Data
    FEM::InputData            const & _idat;
    FEM::Data                       & _data;      // Nodes and elements data
    FEM::Output                     & _output;    // Nodes and elements data
    Array<FEM::Node::DOFVarsStruct*>  _a_udofs;   // Array of ptrs to unknown DOFs.    size = total number of unknown dofs of a current stage
    Array<FEM::Node::DOFVarsStruct*>  _a_pdofs;   // Array of ptrs to prescribed DOFs. size = total number of prescribed dofs of a current stage
    LinAlg::Vector<REAL>              _DF_ext;    // External increment of natural dofs.    size = total number of DOFs
    LinAlg::Vector<REAL>              _DU_ext;    // External increment of essential dofs.  size = total number of DOFs

    // Local
    void _realloc_and_setup_dof_vectors();
    void _assemb_natural_stage_inc();         // Allocate and assemble global (stage) increment of external force vector for the current stage
    void _assemb_essential_stage_inc();       // Allocate and assemble global (stage) increment displacements vector for the current stage
    void _assemb_G(LinAlg::Matrix<REAL> & K, LinAlg::Vector<REAL> & RHS, REAL TimeInc); // Assemble matrix K for the actual state of elements/nodes inside _data

    void _inv_K_times_X(LinAlg::Matrix<REAL> & K           ,  // In/Out: (if DoPreserveK==true => just In)  Out => unknown state (values)
                        bool                   DoPreserveK ,  // In:
                        LinAlg::Vector<REAL> & X           ,  // In/Out:
                        LinAlg::Vector<REAL> & Y           ); // In/Out:   Y <- inv(K)*X   (K*Y=X)
    int _n_tot_dof() const { return _a_udofs.size() + _a_pdofs.size(); }

    void _backup_nodes_and_elements_state();
    void _update_nodes_and_elements_state(LinAlg::Vector<REAL> const & dU, LinAlg::Vector<REAL> & dF_int, REAL TimeInc);
    void _restore_nodes_and_elements_state();

    REAL _norm_essential_vector();
    REAL _norm_natural_vector();

    // To be overriden by derived methods
    virtual void _do_solve_for_an_increment(int                  const   increment,     // In
                                            LinAlg::Vector<REAL> const & DF_ext   ,     // In
                                            LinAlg::Vector<REAL> const & DU_ext   ,     // In
                                            REAL                 const   dTime    ,     // In
                                            LinAlg::Matrix<REAL>       & G        ,     // (no needed outside)
                                            LinAlg::Vector<REAL>       & hKUn     ,     // (no needed outside)
                                            LinAlg::Vector<REAL>       & dF_int   ,     // (no needed outside)
                                            LinAlg::Vector<REAL>       & Rinternal,     // (no needed outside)
                                            IntSolverData              & ISD      ) =0; // Out

private:
    LinAlg::Vector<REAL> _U_bkp; // A place to store a temporary state of displacements (backup)
    LinAlg::Vector<REAL> _F_bkp; // A place to store a temporary state of internal forces (backup)
    // Local functions
    void _do_scatter(LinAlg::Matrix<REAL> const & K,   // {{{
                     LinAlg::Vector<REAL> const & X,
                     LinAlg::Vector<REAL> const & Y,
                     LinAlg::Matrix<REAL>       & K11, // Unknown_size x Unknown_size
                     LinAlg::Matrix<REAL>       & K12, // Unknown_size x Prescribed_size
                     LinAlg::Matrix<REAL>       & K21, // Prescribed_size x Unknown_size
                     LinAlg::Matrix<REAL>       & K22, // Prescribed_size x Prescribed_size
                     LinAlg::Vector<REAL>       & Y2,  // Prescribed_size
                     LinAlg::Vector<REAL>       & X1); // Unknown_size }}}

    void _do_gather(LinAlg::Vector<REAL> const & Y1, // Unknown_size {{{
                    LinAlg::Vector<REAL> const & Y2, // Prescribed_size
                    LinAlg::Vector<REAL> const & X1, // Unknown_size
                    LinAlg::Vector<REAL> const & X2, // Prescribed_size
                    LinAlg::Vector<REAL>       & Y ,
                    LinAlg::Vector<REAL>       & X); // }}}

    void _mount_into_Global(LinAlg::Matrix<REAL> & Global, LinAlg::Matrix<REAL> & LocalMatrix, Array<size_t> & Map1, Array<size_t> & Map2) const;
    void _mount_into_RHS   (LinAlg::Vector<REAL> &    RHS, LinAlg::Vector<REAL> & LocalVector, Array<size_t> & Map) const;
}; // class CSolver




inline CSolver::CSolver(FEM::InputData const & ID, FEM::Data & data, FEM::Output & output) // {{{
    : _idat   (ID),
      _data   (data),
      _output (output)
{
    for (int i=0; i<ID.nSTAGES; ++i)
    {
        if (ID.dTIME[i]<0) throw new Fatal(_("CSolver::CSolver: < SOLVER DTIME > (%f) is invalid"), ID.dTIME[i]);
    }
} // }}}

inline void CSolver::Solve(int iStage) // {{{
{
    // Solve: (C - alfa*h*K ) * dU  = dF - h*K*U
    // Solve:              G  * dU  = dF - RHS
    
    // - Realloc DOF vectors: _a_pdofs and _a_pdofs
    // - Give an equation ID number to all DOFs inside all nodes
    _realloc_and_setup_dof_vectors(); // EqID. OBS.: _n_tot_dof() will work only after this call

    // Allocate auxiliar vectors for backup displacements and internal forces state
    _U_bkp.Resize(_n_tot_dof());
    _F_bkp.Resize(_n_tot_dof());

    // Allocate and assemble global (stage) increment of external force vector for the current stage
    _assemb_natural_stage_inc(); // DF_ext

    // Allocate and assemble global (stage) increment displacements vector for the current stage
    _assemb_essential_stage_inc(); // DU_ext

    // ------------------------------------------------------------------------------------ do solve ---

    // Total number of DOF == number of equations
    int n_tot_dof = _n_tot_dof();

    // Number of divisions for each increment
    int num_div = _idat.nDIV[iStage];

    // Scale increments according to a step controller
    REAL   lam = 1.0/num_div;                // Step controller
    REAL dTime = _idat.dTIME[iStage]*lam;    // Time increment
    LinAlg::Scal(lam, _DF_ext);              // DF_ext <- lam*_DF_ext
    LinAlg::Scal(lam, _DU_ext);              // DU_ext <- lam*_DU_ext

    // Allocate G stifness (dense) matrix
    LinAlg::Matrix<REAL> G(n_tot_dof, n_tot_dof);

    // Allocate RHS vector 
    LinAlg::Vector<REAL> hKUn(n_tot_dof);
    
    // Allocate a vector of residual (internal of a particular solver)
    LinAlg::Vector<REAL> dF_int   (n_tot_dof);
    LinAlg::Vector<REAL> Rinternal(n_tot_dof);

    // Output initial state
    if (iStage==0) _output.OutputIncrement(iStage, -1);

    // Loop over increments (0<increment<NumDiv)
    for (int increment=0; increment<num_div; ++increment)
    {
        // Internal solver data
        IntSolverData isd;
    
        // Solve for increment
        _do_solve_for_an_increment(increment, _DF_ext, _DU_ext, dTime, G, hKUn, dF_int, Rinternal, isd);

        // Output results for increment
        _output.OutputIncrement(iStage, increment, isd);
    }

    // Output end of Stage
    _output.OutputStage(iStage);
    // ----------------------------------------------------------------------------------- end solve ---
} // }}}

inline void CSolver::_realloc_and_setup_dof_vectors() // {{{
{
    // ID of equation
    int eq_id=0;

    // Deallocate vectors
    _a_udofs.resize(0);
    _a_pdofs.resize(0);

    // Loop along all nodes
    for (size_t i=0; i<_data.Nodes.size(); ++i)
    {
        // Declare a reference to DOFVars array inside Node [i]
        Array<FEM::Node::DOFVarsStruct> & dofs = _data.Nodes[i].DOFVars();

        // Loop along DOFs inside Node [i]
        for (size_t j=0; j<dofs.size(); ++j)
        {
            // Fill DOF vectors
            if (dofs[j].IsEssenPresc) _a_pdofs.push_back(&dofs[j]); // prescribed
            else                      _a_udofs.push_back(&dofs[j]); // unknown
            
            // Assing an equation ID to DOF [j]
            dofs[j].EqID = eq_id;
            eq_id++;
        }
    }
} // }}}

inline void CSolver::_assemb_natural_stage_inc() // Allocate and assemble global (stage) increment of external force vector for the current stage {{{
{
    // Total number of DOF == number of equations
    int n_tot_dof = _n_tot_dof();

    // Resize vector DF_ext
    _DF_ext.Resize(n_tot_dof);

    // Loop along Unknown_size
    for (size_t iu=0; iu<_a_udofs.size(); ++iu)
        _DF_ext(_a_udofs[iu]->EqID) = _a_udofs[iu]->NaturalBry;

    // Loop along Prescribed_size
    for (size_t ip=0; ip<_a_pdofs.size(); ++ip)
        _DF_ext(_a_pdofs[ip]->EqID) = _a_pdofs[ip]->NaturalBry;
} // }}}

inline void CSolver::_assemb_essential_stage_inc() // Allocate and assemble global (stage) increment displacements vector for the current stage {{{
{
    // Total number of DOF == number of equations
    int n_tot_dof = _n_tot_dof();

    // Resize vector DU_ext
    _DU_ext.Resize(n_tot_dof);

    // Loop along Unknown_size
    for (size_t iu=0; iu<_a_udofs.size(); ++iu)
        _DU_ext(_a_udofs[iu]->EqID) = _a_udofs[iu]->EssentialBry;

    // Loop along Prescribed_size
    for (size_t ip=0; ip<_a_pdofs.size(); ++ip)
        _DU_ext(_a_pdofs[ip]->EqID) = _a_pdofs[ip]->EssentialBry;
} // }}}

inline void CSolver::_assemb_G(LinAlg::Matrix<REAL> & G, LinAlg::Vector<REAL> & RHS, REAL dT) // {{{
{
      G.SetValues(0.0);    // Clear G (global) stifness matrix
    RHS.SetValues(0.0);  // Clear RHS (right hand side vautoinc "modified euler"ector)
    REAL alfa = 1.0;
    
    // Variables to be used during mounting
    LinAlg::Matrix<REAL> M;        // Local matrix to be mounted
    LinAlg::Vector<REAL> V;        // Local vector to be mounted
    Array<size_t>        rows_map; // Map for row positions of components
    Array<size_t>        cols_map; // Map for col positions of components
    Array<size_t>        vect_map; // Map for col positions of components
    
    // Loop along elements
    for (size_t i_ele=0; i_ele<_data.Elements.size(); ++i_ele)
    {
        // Get a poniter to an element
        FEM::Element const * const elem = _data.Elements[i_ele];
        if (!elem->IsActive()) continue;

        // Assemble zero order matrices
        size_t n_order0 = elem->nOrder0Matrices();
        for (size_t i=0; i<n_order0; i++)
        {
            elem->Order0Matrix(  i, M, rows_map, cols_map);
            elem->RHSVector   (  i, V, vect_map);
            V =      dT*M*V;
            _mount_into_RHS   (RHS, V, vect_map);
            M = alfa*dT*M;
            _mount_into_Global(  G, M, rows_map, cols_map);
        }

        // Assemble first order matrices
        size_t n_order1 = elem->nOrder1Matrices();
        for (size_t i=0; i<n_order1; i++)
        {
            elem->Order1Matrix(i, M, rows_map, cols_map);
            _mount_into_Global(G, M, rows_map, cols_map);
        }
    }
} // }}}

inline void CSolver::_mount_into_Global(LinAlg::Matrix<REAL> & Global, LinAlg::Matrix<REAL> & LocalMatrix, Array<size_t> & Map1, Array<size_t> & Map2) const // {{{
{
    // Assemble local (LocalMatrix) DOFs into global (Global) matrix
    // Loop along LocalMatrix (local) matrix LocalMatrix.rows and LocalMatrix.cols
    for (int i_row=0; i_row<LocalMatrix.Rows(); ++i_row)
    for (int j_col=0; j_col<LocalMatrix.Cols(); ++j_col)
        Global(Map1[i_row], Map2[j_col]) += LocalMatrix(i_row, j_col);
} // }}}

inline void CSolver::_mount_into_RHS(LinAlg::Vector<REAL> & RHS, LinAlg::Vector<REAL> & LocalVector, Array<size_t> & Map) const // {{{
{
    // Assemble local (LocalVector) DOFs into RHS matrix
    for (int i=0; i<LocalVector.Size(); ++i)
        RHS(Map[i]) += LocalVector(i);
} // }}}

inline void CSolver::_inv_K_times_X(LinAlg::Matrix<REAL> & K           , // In/Out: (if DoPreserveK==true => just In)  Out => unknown state (values) {{{
                                   bool                   DoPreserveK , // In:
                                   LinAlg::Vector<REAL> & X           , // In/Out:
                                   LinAlg::Vector<REAL> & Y           ) // In/Out:
{
    // Modify K for essential boundary conditions
    //if (_do_split)
    //{
        /*   _               _  
         *  |  [K11] | [K12]  | / {Y1}=? \   / {X1}   \
         *  |  - - - - - - -  | | - - -  | = | - - -  |
         *  |_ [K21] | [K22] _| \ {Y2}   /   \ {X2}=? /
         *
         *  {Y2} and {X1} = prescribed
         *
         *  K11*Y1 + K12*Y2 = X1
         *  K21*Y1 + K22*Y2 = X2
         *
         *  Y1 = inv(K11)*(X1 - K12*Y2)
         *  X2 = K21*Y1 + K22*Y2
         */

        // Allocate temporary matrices and vectors
        LinAlg::Matrix<REAL> K11,K12;
        LinAlg::Matrix<REAL> K21,K22;
        LinAlg::Vector<REAL> Y2;
        LinAlg::Vector<REAL> X1;

        // Scatter global K, X and Y values into smaller groups
        _do_scatter(K,X,Y, K11,K12,K21,K22, Y2, X1);

        // Solve for {Y1} and {X2}
        LinAlg::Vector<REAL> TMP(X1);                // TMP <- X1
        LinAlg::Gemv(-1.0,K12,Y2,1.0,TMP);           // TMP <- -1.0*K12*Y2 + 1.0*X1
    // int ptr_udofs_eq[us]; 
    // int ptr_pdofs_eq[ps]; 

        // Call a linear solver for: TMP <- inv(K11)*TMP (K11 is lost)
        if (_idat.linSOLVER==InputData::lsLAPACK)
        {
#ifdef USE_LAPACK
            LinAlg::Gesv(K11, TMP); // TMP <- inv(K11)*TMP (K11 is lost)
#else
            throw new Fatal(_("CSolver::_inv_K_times_X: LAPACK is not available (USE_LAPACK = false)"));
#endif
        }
        else if (_idat.linSOLVER==InputData::lsUMFPACK)
        {
            throw new Fatal(_("CSolver::_inv_K_times_X: UMFPACK is not working yet"));
        }
        else if (_idat.linSOLVER==InputData::lsSUPERLU)
        {
            throw new Fatal(_("CSolver::_inv_K_times_X: SUPERLU is not working yet"));
        }
        else throw new Fatal(_("CSolver::_inv_K_times_X: linSOLVER is invalid"));

        // LinAlg::Gesv2(K11, TMP);                      // TMP <- inv(K11)*TMP (K11 is lost) 
        LinAlg::Vector<REAL> & Y1 = TMP;             //  Y1 <- TMP (reference)
        LinAlg::Vector<REAL>   X2(Y2.Size());        // allocate X2
        LinAlg::Gemvpmv(1.0,K21,Y1, 1.0,K22,Y2, X2); //  X2 <- 1*K21*Y1 + 1*K22*Y2

        // Gather Y1, Y2, X1 and X2 into Y and X
        _do_gather(Y1,Y2, X1,X2, Y,X);
    //}
    //else if (_do_penalty)
    //{
    // Copy X to Res; neccessary for Gesv
    //   LinAlg::Copy(X, Y); // Y <- X
    //   K(ipresc,jpresc) <- Vpres*BIGNUM
    //   Gesv(K,Y) Y <- inv(K)*Y
    //}
} // }}}

void CopyPartialMatrix(LinAlg::Matrix<REAL> const & Source, LinAlg::Vector<int> const & ValidCols, LinAlg::Vector<int> const & ValidRows, LinAlg::Matrix<REAL> & Target) // {{{
{
    int m = ValidRows.Size();
    int n = ValidCols.Size();
    int s = Source.Rows();
    assert(Target.Rows()==m);
    assert(Target.Cols()==n);
    assert(Source.Cols()==s);
    REAL const * ptrSource = Source.GetPtr();
    REAL * ptrTarget = Target.GetPtr();
    int  const * ptrVR     = ValidRows.GetPtr();
    int  const * ptrVC     = ValidCols.GetPtr();
    //  
    for (int j =0; j<n; j++) 
        for (int i =0; i<m; i++)
            ptrTarget[i+m*j] = ptrSource[ ptrVR[i] + s*ptrVC[j]];

} // }}}

inline void CSolver::_do_scatter(LinAlg::Matrix<REAL> const & K,   // {{{
                                LinAlg::Vector<REAL> const & X,
                                LinAlg::Vector<REAL> const & Y,
                                LinAlg::Matrix<REAL>       & K11, // Unknown_size x Unknown_size
                                LinAlg::Matrix<REAL>       & K12, // Unknown_size x Prescribed_size
                                LinAlg::Matrix<REAL>       & K21, // Prescribed_size x Unknown_size
                                LinAlg::Matrix<REAL>       & K22, // Prescribed_size x Prescribed_size
                                LinAlg::Vector<REAL>       & Y2,  // Prescribed_size
                                LinAlg::Vector<REAL>       & X1)  // Unknown_size
{
    // Unknown_size = us
    // Prescribed_size = ps
    int us = _a_udofs.size(); // unknown
    int ps = _a_pdofs.size(); // prescribed
    
    LinAlg::Vector<int> _v_udofs_eq(us);
    LinAlg::Vector<int> _v_pdofs_eq(ps);

    for (int iu=0; iu<us; ++iu) _v_udofs_eq(iu) = _a_udofs[iu]->EqID;
    for (int ip=0; ip<ps; ++ip) _v_pdofs_eq(ip) = _a_pdofs[ip]->EqID;

    // K11: 
    K11.Resize(us,us);
    CopyPartialMatrix(K, _v_udofs_eq, _v_udofs_eq, K11);

    // K12:
    K12.Resize(us,ps);
    CopyPartialMatrix(K, _v_pdofs_eq, _v_udofs_eq, K12);

    // X1
    X1.Resize(us);
    for (int iu=0; iu<us; ++iu)
        X1(iu) = X(_a_udofs[iu]->EqID);

    // K21:
    K21.Resize(ps,us);
    CopyPartialMatrix(K, _v_udofs_eq, _v_pdofs_eq, K21);

    // K22:
    K22.Resize(ps,ps);
    CopyPartialMatrix(K, _v_pdofs_eq, _v_pdofs_eq, K22);

    // Y2
    Y2.Resize(ps);
    for (int ip=0; ip<ps; ++ip)
        Y2(ip) = Y(_a_pdofs[ip]->EqID);
} // }}}

inline void CSolver::_do_gather(LinAlg::Vector<REAL> const & Y1, // Unknown_size {{{
                               LinAlg::Vector<REAL> const & Y2, // Prescribed_size
                               LinAlg::Vector<REAL> const & X1, // Unknown_size
                               LinAlg::Vector<REAL> const & X2, // Prescribed_size
                               LinAlg::Vector<REAL>       & Y ,
                               LinAlg::Vector<REAL>       & X) 
{
    // Unknown_size = us
    // Prescribed_size = ps
    int us = _a_udofs.size(); // unknown
    int ps = _a_pdofs.size(); // prescribed

    // Loop along Unknown_size
    for (int iu=0; iu<us; ++iu)
    {
        // Y1
        Y(_a_udofs[iu]->EqID) = Y1(iu);

        // X1
        X(_a_udofs[iu]->EqID) = X1(iu);
    }

    // Loop along Prescribed_size
    for (int ip=0; ip<ps; ++ip)
    {
        // Y2
        Y(_a_pdofs[ip]->EqID) = Y2(ip);

        // X2
        X(_a_pdofs[ip]->EqID) = X2(ip);
    }
} // }}}

inline void CSolver::_backup_nodes_and_elements_state() // {{{
{
    // ### Backup all nodes ############################################################################
    
    // Loop along Unknown_size
    for (size_t iu=0; iu<_a_udofs.size(); ++iu)
    {
         _U_bkp(_a_udofs[iu]->EqID) = _a_udofs[iu]->EssentialVal;
         _F_bkp(_a_udofs[iu]->EqID) = _a_udofs[iu]->NaturalVal;
    }

    // Loop along Prescribed_size
    for (size_t ip=0; ip<_a_pdofs.size(); ++ip)
    {   
         _U_bkp(_a_pdofs[ip]->EqID) = _a_pdofs[ip]->EssentialVal;
         _F_bkp(_a_pdofs[ip]->EqID) = _a_pdofs[ip]->NaturalVal;
    }

    // ### Backup all elements #########################################################################
    
    // Loop along elements
    for (size_t i=0; i<_data.Elements.size(); ++i)
        _data.Elements[i]->BackupState();

    // #################################################################################################
} // }}}

inline void CSolver::_update_nodes_and_elements_state(LinAlg::Vector<REAL> const & dU, LinAlg::Vector<REAL> & dF_int, REAL TimeInc) // {{{
{
    // ### Update all nodes #############################################################################
    
    // Loop along Unknown_size
    for (size_t iu=0; iu<_a_udofs.size(); ++iu)
        _a_udofs[iu]->EssentialVal += dU(_a_udofs[iu]->EqID);

    // Loop along Prescribed_size
    for (size_t ip=0; ip<_a_pdofs.size(); ++ip)
        _a_pdofs[ip]->EssentialVal += dU(_a_pdofs[ip]->EqID);

    // ### Update all elements ##########################################################################
    
    // --- Setup dU values for all nodes and Zeroes _IncNatural values ----------------------------------
    
    // Loop along Unknown_size
    for (size_t iu=0; iu<_a_udofs.size(); ++iu)
    {
        _a_udofs[iu]->_IncEssenVal = dU(_a_udofs[iu]->EqID);
        _a_udofs[iu]->_IncNaturVal = 0.0; // Need to be set zero because will be output
    }

    // Loop along Prescribed_size
    for (size_t ip=0; ip<_a_pdofs.size(); ++ip)
    {
        _a_pdofs[ip]->_IncEssenVal = dU(_a_pdofs[ip]->EqID);
        _a_pdofs[ip]->_IncNaturVal = 0.0; // Need to be set zero because will be output
    }

    // --- Update all elements --------------------------------------------------------------------------
    
    // Loop along elements
    for (size_t i=0; i<_data.Elements.size(); ++i)
    {
        if (_data.Elements[i]->IsActive())
            _data.Elements[i]->UpdateState(TimeInc); // Read nodal values _IncEssential and write to _IncNatural
    }
    
    // --- Read dF values (just calculated) from all nodes ----------------------------------------------
    
    // Loop along Unknown_size
    for (size_t iu=0; iu<_a_udofs.size(); ++iu)
        dF_int(_a_udofs[iu]->EqID) = _a_udofs[iu]->_IncNaturVal;

    // Loop along Prescribed_size
    for (size_t ip=0; ip<_a_pdofs.size(); ++ip)
        dF_int(_a_pdofs[ip]->EqID) = _a_pdofs[ip]->_IncNaturVal;

    // ##################################################################################################
} // }}}

inline void CSolver::_restore_nodes_and_elements_state() // {{{
{
    // ### Restore all nodes ###########################################################################
    
    // Loop along Unknown_size
    for (size_t iu=0; iu<_a_udofs.size(); ++iu)
    {
        _a_udofs[iu]->EssentialVal = _U_bkp(_a_udofs[iu]->EqID);
          _a_udofs[iu]->NaturalVal = _F_bkp(_a_udofs[iu]->EqID);
    }

    // Loop along Prescribed_size
    for (size_t ip=0; ip<_a_pdofs.size(); ++ip)
    {
        _a_pdofs[ip]->EssentialVal = _U_bkp(_a_pdofs[ip]->EqID);
          _a_pdofs[ip]->NaturalVal = _F_bkp(_a_pdofs[ip]->EqID);
    }

    // ### Restore all elements ########################################################################
    
    // Loop along elements
    for (size_t i=0; i<_data.Elements.size(); ++i)
        _data.Elements[i]->RestoreState();

    // #################################################################################################
} // }}}

inline REAL CSolver::_norm_essential_vector() // {{{
{
    REAL tmp=0.0;
    // Loop along Unknown_size
    for (size_t iu=0; iu<_a_udofs.size(); ++iu)
        tmp += pow(_a_udofs[iu]->EssentialVal,2.0);

    // Loop along Prescribed_size
    for (size_t ip=0; ip<_a_pdofs.size(); ++ip)
        tmp += pow(_a_pdofs[ip]->EssentialVal,2.0);

    return sqrt(tmp);
} // }}}

inline REAL CSolver::_norm_natural_vector() // {{{
{
    REAL tmp=0.0;
    // Loop along Unknown_size
    for (size_t iu=0; iu<_a_udofs.size(); ++iu)
        tmp += pow(_a_udofs[iu]->NaturalVal,2.0);

    // Loop along Prescribed_size
    for (size_t ip=0; ip<_a_pdofs.size(); ++ip)
        tmp += pow(_a_pdofs[ip]->NaturalVal,2.0);

    return sqrt(tmp);
} // }}}



// Define a pointer to a function that makes (allocate) a new solver
typedef CSolver * (*CSolverMakerPtr)(Array<String> const & CSolverCtes, FEM::InputData const & ID, FEM::Data & data, FEM::Output & output);

// Typdef of the array map that contains all the pointers to the functions that makes solvers
typedef std::map<String, CSolverMakerPtr, std::less<String> > CSolverFactory_t;

// Instantiate the array map that contains all the pointers to the functions that makes solvers
CSolverFactory_t CSolverFactory;

// Allocate a new solver according to a string giving the name of the solver
CSolver * AllocCSolver(String const & CSolverName, Array<String> const & CSolverCtes, FEM::InputData const & ID, FEM::Data & data, FEM::Output & output) // {{{
{
    CSolverMakerPtr ptr=NULL;
    ptr = CSolverFactory[CSolverName];
    if (ptr==NULL)
        throw new Fatal(_("FEM::AllocCSolver: There is no < %s > solver implemented in this library"), CSolverName.c_str());
    return (*ptr)(CSolverCtes, ID, data, output);
} // }}}

}; // namespace FEM

#endif // MECHSYS_FEM_CSOLVER_H

// vim:fdm=marker

---