FrictionProblem.hpp

/*
 * This file is part of So-bogus, a C++ sparse block matrix library and
 * Second Order Cone solver.
 *
 * Copyright 2013 Gilles Daviet <gdaviet@gmail.com>
 *
 * So-bogus 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.

 * So-bogus 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 So-bogus.  If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef BOGUS_FRICTION_PROBLEM_HPP
#define BOGUS_FRICTION_PROBLEM_HPP

#include "../Core/Block.hpp"
#include "../Core/BlockSolvers.fwd.hpp"

#include "../Extra/SecondOrder.fwd.hpp"

#include "../Core/Utils/Signal.hpp"

namespace bogus
{


template< unsigned Dimension >
struct PrimalFrictionProblem
{
    // Primal Data

    typedef SparseBlockMatrix< Eigen::MatrixXd > MType ;
    MType M ;
    bogus::SparseBlockMatrix< Eigen::Matrix< double, Dimension, Dimension > > E ;

    typedef Eigen::Matrix< double, Dimension, Eigen::Dynamic > HBlock ;
    typedef SparseBlockMatrix< HBlock, UNCOMPRESSED > HType ;
    HType H;

    const double *f ;
    const double *w ;
    const double *mu ;

    // Cached data

    typedef SparseBlockMatrix< LU< Eigen::MatrixBase< Eigen::MatrixXd > > > MInvType ;
    MInvType MInv ;

    void computeMInv () ;


    // Primal-dual solve functions

    typedef ADMM   < HType >    ADMMType ;
    typedef DualAMA< HType > DualAMAType ;
    typedef ProductGaussSeidel< HType, MInvType, true > ProductGaussSeidelType ;


    double solveWith( ProductGaussSeidelType &pgs, double * r, const bool staticProblem = false ) const ;

    double solveWith( ADMMType    &admm, double lambda, double* v, double * r ) const ;

    double solveWith( DualAMAType &dama, double* v, double * r, const bool staticProblem = false ) const ;
} ;


template< unsigned Dimension >
struct DualFrictionProblem
{
    typedef SparseBlockMatrix< Eigen::Matrix< double, Dimension, Dimension, Eigen::RowMajor >,
                               SYMMETRIC > WType ;

    typedef GaussSeidel< WType > GaussSeidelType ;
    typedef ProjectedGradient< WType > ProjectedGradientType ;

    typedef SOCLaw< Dimension, double, true  > CoulombLawType   ;
    typedef SOCLaw< Dimension, double, false > SOCLawType   ;

    typedef Signal< unsigned, double > SignalType ;

    WType W ;

    Eigen::VectorXd b ;

    Eigen::VectorXd mu ;


    void computeFrom( const PrimalFrictionProblem< Dimension >& primal ) ;


    double solveWith( GaussSeidelType &gs, double * r, const bool staticProblem = false ) const ;
    double solveWith( ProjectedGradientType &pg, double * r ) const ;


    double evalWith( const GaussSeidelType &gs, const double * r, const bool staticProblem = false ) const ;
    double evalWith( const ProjectedGradientType &gs, const double * r ) const ;


    double solveCadoux( GaussSeidelType &gs, double * r, const unsigned fpIterations,
           const SignalType* callback = BOGUS_NULL_PTR(const SignalType) ) const ;
    double solveCadoux( ProjectedGradientType &pg, double * r, const unsigned fpIterations,
           const SignalType* callback = BOGUS_NULL_PTR(const SignalType) ) const ;

    double solveCadouxVel( GaussSeidelType &gs, double * u, const unsigned fpIterations,
           const SignalType* callback = BOGUS_NULL_PTR(const SignalType) ) const ;
    double solveCadouxVel( ProjectedGradientType &pg, double * u, const unsigned fpIterations,
           const SignalType* callback = BOGUS_NULL_PTR(const SignalType) ) const ;



    void applyPermutation( const std::vector< std::size_t > &permutation ) ;
    void undoPermutation() ;
    bool permuted() const { return !m_permutation.empty() ; }

    const std::vector< std::size_t > &permutation() const { return m_permutation ; }
    const std::vector< std::size_t > &invPermutation() const { return m_invPermutation ; }
private:

    // Current permutation of contact indices
    std::vector< std::size_t > m_permutation ;
    std::vector< std::size_t > m_invPermutation ;
} ;

} //namespace bogus

#endif