MBSim

Project Page
lcp_reformulation_functions.h
00001 /* Copyright (C) 2004-2012 MBSim Development Team
00002  *
00003  * This library is free software; you can redistribute it and/or
00004  * modify it under the terms of the GNU Lesser General Public
00005  * License as published by the Free Software Foundation; either
00006  * version 2.1 of the License, or (at your option) any later version.
00007  *
00008  * This library is distributed in the hope that it will be useful,
00009  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00010  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00011  * Lesser General Public License for more details.
00012  *
00013  * You should have received a copy of the GNU Lesser General Public
00014  * License along with this library; if not, write to the Free Software
00015  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
00016  *
00017  * Contact: martin.o.foerg@googlemail.com
00018  */
00019 
00020 #ifndef NUMERICSLCPREFORMULATEDFUNCTION_H_
00021 #define NUMERICSLCPREFORMULATEDFUNCTION_H_
00022 
00023 #include <fmatvec/function.h>
00024 
00025 #include <mbsim/numerics/functions/newton_method_jacobian_functions.h>
00026 
00027 namespace MBSim {
00028 
00029   class LCPReformulationFunction : public fmatvec::Function<fmatvec::Vec(fmatvec::Vec)> {
00030     public:
00034       LCPReformulationFunction(const double &r_ = 10) :
00035           r(r_), DEBUGLEVEL(0) {
00036       }
00037 
00041       LCPReformulationFunction(const fmatvec::Vec &q_, const fmatvec::SqrMat &M_, const double &r_ = 10, const unsigned int & DEBUGLEVEL_ = 0);
00042 
00043       /*
00044        * \brief destructor
00045        */
00046       virtual ~LCPReformulationFunction();
00047 
00048       /* INHERITED INTERFACE */
00053       virtual fmatvec::Vec operator()(const fmatvec::Vec &q) = 0;
00054       /***************************************************/
00055 
00057       fmatvec::Vec getq(void) {
00058         return q;
00059       }
00060       void setSystem(const fmatvec::SqrMat & M_, const fmatvec::Vec & q_) {
00061         M.resize() = M_;
00062         q.resize() = q_;
00063         assert(M.rows() == q.rows());
00064         NumberOfContacts = q.rows();
00065       }
00066       fmatvec::SqrMat getM(void) {
00067         return M;
00068       }
00069       double getr(void) {
00070         return r;
00071       }
00072       void setr(const double & r_) {
00073         r = r_;
00074       }
00075       /*****************/
00076 
00077     protected:
00081       int NumberOfContacts;
00082 
00086       fmatvec::Vec q;
00087 
00091       fmatvec::SqrMat M;
00092 
00096       double r;
00097 
00101       unsigned int DEBUGLEVEL;
00102   };
00103 
00104   class LCPNewtonReformulationFunction : public LCPReformulationFunction {
00105     public:
00109       LCPNewtonReformulationFunction() :
00110           LCPReformulationFunction() {
00111       }
00112 
00120       LCPNewtonReformulationFunction(const fmatvec::Vec &q_, const fmatvec::SqrMat &M_, const double &r_ = 10, const unsigned int & DEBUGLEVEL_ = 0);
00121 
00125       virtual ~LCPNewtonReformulationFunction();
00126 
00127       /* INHERITED INTERFACE */
00128       fmatvec::Vec operator()(const fmatvec::Vec &q);
00129       /***************************************************/
00130 
00131     protected:
00132 
00133   };
00134 
00135   class LCPFixpointReformulationFunction : public LCPReformulationFunction {
00136     public:
00140       LCPFixpointReformulationFunction() :
00141           LCPReformulationFunction() {
00142       }
00143 
00151       LCPFixpointReformulationFunction(const fmatvec::Vec &q_, const fmatvec::SqrMat &M_, const double &r_ = 10, const unsigned int & DEBUGLEVEL_ = 0);
00152 
00156       virtual ~LCPFixpointReformulationFunction();
00157 
00158       /* INHERITED INTERFACE */
00159       fmatvec::Vec operator()(const fmatvec::Vec &q);
00160       /***************************************************/
00161   };
00162 
00163   class LinearComplementarityJacobianFunction : public NewtonJacobianFunction {
00164     public:
00168       LinearComplementarityJacobianFunction();
00169 
00170       /*
00171        * \brief destructor
00172        */
00173       virtual ~LinearComplementarityJacobianFunction();
00174 
00175       virtual fmatvec::SqrMat operator ()(const fmatvec::Vec & x);
00176 
00177       virtual void setFunction(Function<fmatvec::Vec(fmatvec::Vec)> *function_);
00178 
00179       void updateJacobian(const fmatvec::Vec & x);
00180 
00181     protected:
00185       fmatvec::SqrMat J;
00186 
00187   };
00188 }
00189 #endif //NUMERICSLCPREFORMULATEDFUNCTION_H_