EqSolver.cpp

Go to the documentation of this file.

// Include files

#include <stdlib.h>
#include <stdio.h>
// from Gaudi
#include "GaudiKernel/MsgStream.h"
//from GSL
#include "gsl/gsl_vector.h"
#include "gsl/gsl_multimin.h"
#include "gsl/gsl_math.h"
#include "gsl/gsl_matrix.h"
#include "gsl/gsl_linalg.h"
#include "gsl/gsl_blas.h"
#include "gsl/gsl_errno.h"

// local
#include "EqSolver.h"

// Declaration of the Tool Factory
DECLARE_COMPONENT(EqSolver)


// ============================================================================
// ============================================================================
EqSolver::EqSolverMisc::EqSolverMisc
( const EqSolver::Equations& funcs ,
  EqSolver::Arg&             arg   )
  : m_argum ( arg    )
  , m_eqs   ( &funcs )
{
  const size_t N = funcs.size () ;
  for( size_t i = 0 ; i < N ; ++i ) {
      Equations last;
      for( size_t j = 0 ; j < N ; ++j ) {
          Genfun::GENFUNCTION fij = funcs[i]->partial(j);
          last.push_back( fij.clone() ) ;
      }
      m_jac.push_back( last );
  }
}
// ============================================================================

// ============================================================================
EqSolver::EqSolverMisc::~EqSolverMisc()
{
  while( !m_jac.empty() )
    {
      Equations& last = m_jac.back() ;
      while( !last.empty() ) {
          delete last.back() ;
          last.pop_back () ;
      }
      m_jac.pop_back();
    }
}
// ============================================================================

namespace
{
  using namespace Genfun;

  int fun_gsl  ( const gsl_vector* v      ,
                 void*             params ,
                 gsl_vector*       f      )
  {
    EqSolver::EqSolverMisc* local =
      static_cast <EqSolver::EqSolverMisc*> (params);
    const EqSolver::Equations& eqs = *(local->equations()) ;
    Argument&                  arg = local->argument()  ;

    for ( unsigned int i = 0; i < v->size; ++i) {
        arg[i] = gsl_vector_get (v, i);
    }

    for ( unsigned int i = 0; i < v->size; ++i) {
        gsl_vector_set(f, i, (*eqs[i])(arg));
    }
    return GSL_SUCCESS;
  }


  int dfun_gsl  ( const gsl_vector* v      ,
                  void*             params ,
                  gsl_matrix*       df     )
  {
    EqSolver::EqSolverMisc* local =
      static_cast <EqSolver::EqSolverMisc*> (params);
    const EqSolver::Jacobi&     jac = local -> jacobi()    ;
    Argument&                   arg = local -> argument()  ;

    for ( unsigned int i = 0; i < v->size; ++i) {
        arg[i] = gsl_vector_get (v, i);
    }


    for( unsigned int i = 0 ; i < v->size ; ++i ) {
       for (unsigned int j = 0; j < v->size; ++j) {
            Genfun::GENFUNCTION  f = *(jac[i][j]) ;
            gsl_matrix_set      ( df , i , j , f ( arg ) ) ;
       }
    }
    return GSL_SUCCESS;
  }

  int fdfun_gsl ( const gsl_vector* v ,
                  void*       params  ,
                  gsl_vector* f       ,
                  gsl_matrix* df      )
  {
    EqSolver::EqSolverMisc* local =
      static_cast <EqSolver::EqSolverMisc*> (params);
    const EqSolver::Equations&  eqs = *(local->equations()) ;
    const EqSolver::Jacobi&     jac = local->jacobi()    ;
    Argument&                   arg = local->argument()  ;

    for ( unsigned int i = 0; i < v->size; ++i) {
        arg[i] = gsl_vector_get (v, i);
    }

    for( unsigned int i = 0 ; i < v->size ; ++i ) {
        gsl_vector_set(f, i, (*eqs[i])(arg ) ) ;
        for (unsigned int j = 0; j < v->size; ++j) {
            Genfun::GENFUNCTION  f1 = *(jac[i][j]) ;
            gsl_matrix_set      ( df , i , j , f1(arg) ) ;
        }
    }
    return GSL_SUCCESS;
  }
}

//=============================================================================
StatusCode EqSolver::solver (const Equations&  funcs ,
                             Arg&              arg   ) const
//=============================================================================
{
  using namespace Genfun;

  gsl_vector_view vect = gsl_vector_view_array ( &arg[0] ,
                                                 arg.dimension() );
  EqSolverMisc local (funcs, arg);

  const gsl_multiroot_fdfsolver_type *T = m_type;
  gsl_multiroot_fdfsolver *s = nullptr;
  int    status = 0 ;
  size_t iter   = 0 ;

  gsl_multiroot_function_fdf function;

  function.f = &fun_gsl;
  function.df = &dfun_gsl;
  function.fdf = &fdfun_gsl;
  function.n = vect.vector.size;
  function.params = &local;

  s = gsl_multiroot_fdfsolver_alloc(T, vect.vector.size);
  gsl_multiroot_fdfsolver_set (s, &function, &vect.vector);

  for (iter = 0; iter < m_max_iter; ++iter) {
      status = gsl_multiroot_fdfsolver_iterate (s);
      if (status) {
          return Error
            ("Error from gsl_gsl_multiroot_fdfsolver_iterate '"
             + std::string(gsl_strerror(status)) + "'") ;
      }

      status = gsl_multiroot_test_residual (s->f,
                                            m_norm_residual);

      if ( status != GSL_CONTINUE ) { break; }
  }

  for (unsigned int i = 0; i < vect.vector.size; ++i) {
      gsl_vector_set (&vect.vector, i, gsl_vector_get (s->x, i));
  }

  if (status == GSL_SUCCESS) {
      debug()
          << "We stopped in the method on the " << iter
          << " iteration (we have maximum "     << m_max_iter
          << " iterations)"                     << endmsg;
  } else if (status == GSL_CONTINUE && iter <= m_max_iter ) {
      return Error ( "Method finished with '"
                     + std::string(gsl_strerror(status))
                     +  "' error" );
  } else {
      return Error ( "Method finished with '" +
                     std::string(gsl_strerror(status))
                     +  "' error" );
  }

  gsl_multiroot_fdfsolver_free (s);

  if (status) {
      return Error ( "Method finished with '"
                     + std::string(gsl_strerror(status))
                     +  "' error" );
  }

  return GSL_SUCCESS;
}

//=============================================================================
StatusCode  EqSolver::initialize()
{
  StatusCode sc = GaudiTool::initialize() ;
  if( sc.isFailure() ) {
      return Error ("Could not initiliaze base class GaudiTool", sc);
  }

  /* The algorithm for multiional root-finding
     (solving nonlinear systems with n equations in n unknowns)*/

  if(       "fdfsolver_hybridsj" == m_algType ) {
      m_type = gsl_multiroot_fdfsolver_hybridsj  ;
      debug()
          << "Root finding algorithm to be used: "
          << "'gsl_multiroot_fdfsolver_hybridsj'"
          << endmsg;
  } else if ( "fdfsolver_hybridj" == m_algType ) {
      m_type = gsl_multiroot_fdfsolver_hybridj   ;
      debug()
          << "Root finding algorithm to be used: "
          << "'gsl_multiroot_fdfsolver_hybridj'"
          << endmsg;
  } else if ( "fdfsolver_newton" == m_algType ) {
      m_type = gsl_multiroot_fdfsolver_newton    ;
      debug()
          << "Root findind algorithm to be used: "
          << "'gsl_multiroot_fdfsolver_newton'"
          << endmsg;
  } else if ( "fdfsolver_gnewton" == m_algType ) {
      m_type = gsl_multiroot_fdfsolver_gnewton   ;
      debug()
          << "Root findind algorithm to be used: "
          << "'gsl_multiroot_fdfsolver_gnewton'"
          << endmsg;
  } else {
      return Error(" Unknown algorithm type '"
                   + std::string(m_algType) + "'");
  }

  return StatusCode::SUCCESS;
}
//=============================================================================