lcp_reformulation_functions.h

/* Copyright (C) 2004-2012 MBSim Development Team
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library 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
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
 *
 * Contact: martin.o.foerg@googlemail.com
 */
 
#ifndef NUMERICSLCPREFORMULATEDFUNCTION_H_
#define NUMERICSLCPREFORMULATEDFUNCTION_H_
 
#include <mbsim/functions/function.h>
 
#include <mbsim/numerics/functions/newton_method_jacobian_functions.h>
 
namespace MBSim {
 
  class LCPReformulationFunction : public Function<fmatvec::Vec(fmatvec::Vec)> {
    public:
      LCPReformulationFunction(const double &r_ = 10) :
          r(r_) {
      }
 
      LCPReformulationFunction(const fmatvec::Vec &q_, const fmatvec::SqrMat &M_, const double &r_ = 10);
 
      /*
       * \brief destructor
       */
      ~LCPReformulationFunction() override;
 
      /* INHERITED INTERFACE */
      fmatvec::Vec operator()(const fmatvec::Vec &q) override = 0;
      /***************************************************/
 
      fmatvec::Vec getq() {
        return q;
      }
      void setSystem(const fmatvec::SqrMat & M_, const fmatvec::Vec & q_) {
        M <<= M_;
        q <<= q_;
        assert(M.rows() == q.rows());
        NumberOfContacts = q.rows();
      }
      fmatvec::SqrMat getM() {
        return M;
      }
      double getr() {
        return r;
      }
      void setr(const double & r_) {
        r = r_;
      }
      /*****************/
 
    protected:
      int NumberOfContacts;
 
      fmatvec::Vec q;
 
      fmatvec::SqrMat M;
 
      double r;
  };
 
  class LCPNewtonReformulationFunction : public LCPReformulationFunction {
    public:
      LCPNewtonReformulationFunction() :
          LCPReformulationFunction() {
      }
 
      LCPNewtonReformulationFunction(const fmatvec::Vec &q_, const fmatvec::SqrMat &M_, const double &r_ = 10);
 
      ~LCPNewtonReformulationFunction() override;
 
      /* INHERITED INTERFACE */
      fmatvec::Vec operator()(const fmatvec::Vec &q) override;
      /***************************************************/
 
    protected:
 
  };
 
  class LCPFixpointReformulationFunction : public LCPReformulationFunction {
    public:
      LCPFixpointReformulationFunction() :
          LCPReformulationFunction() {
      }
 
      LCPFixpointReformulationFunction(const fmatvec::Vec &q_, const fmatvec::SqrMat &M_, const double &r_ = 10);
 
      ~LCPFixpointReformulationFunction() override;
 
      /* INHERITED INTERFACE */
      fmatvec::Vec operator()(const fmatvec::Vec &q) override;
      /***************************************************/
  };
 
  class LinearComplementarityJacobianFunction : public NewtonJacobianFunction {
    public:
      LinearComplementarityJacobianFunction();
 
      /*
       * \brief destructor
       */
      ~LinearComplementarityJacobianFunction() override;
 
      fmatvec::SqrMat operator ()(const fmatvec::Vec & x) override;
 
      void setFunction(Function<fmatvec::Vec(fmatvec::Vec)> *function_) override;
 
      void updateJacobian(const fmatvec::Vec & x);
 
    protected:
      fmatvec::SqrMat J;
 
  };
}
#endif //NUMERICSLCPREFORMULATEDFUNCTION_H_