MBSim

Project Page
lcp_reformulation_functions.h
1 /* Copyright (C) 2004-2012 MBSim Development Team
2  *
3  * This library is free software; you can redistribute it and/or
4  * modify it under the terms of the GNU Lesser General Public
5  * License as published by the Free Software Foundation; either
6  * version 2.1 of the License, or (at your option) any later version.
7  *
8  * This library is distributed in the hope that it will be useful,
9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11  * Lesser General Public License for more details.
12  *
13  * You should have received a copy of the GNU Lesser General Public
14  * License along with this library; if not, write to the Free Software
15  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
16  *
17  * Contact: martin.o.foerg@googlemail.com
18  */
19 
20 #ifndef NUMERICSLCPREFORMULATEDFUNCTION_H_
21 #define NUMERICSLCPREFORMULATEDFUNCTION_H_
22 
23 #include <mbsim/utils/function.h>
24 
25 #include <mbsim/numerics/functions/newton_method_jacobian_functions.h>
26 
27 namespace MBSim {
28 
29  class LCPReformulationFunction : public Function1<fmatvec::Vec, fmatvec::Vec> {
30  public:
34  LCPReformulationFunction(const double &r_ = 10) :
35  r(r_), DEBUGLEVEL(0) {
36  }
37 
41  LCPReformulationFunction(const fmatvec::Vec &q_, const fmatvec::SqrMat &M_, const double &r_ = 10, const unsigned int & DEBUGLEVEL_ = 0);
42 
43  /*
44  * \brief destructor
45  */
46  virtual ~LCPReformulationFunction();
47 
48  /* INHERITED INTERFACE */
53  virtual fmatvec::Vec operator()(const fmatvec::Vec &q, const void * = NULL) = 0;
54  /***************************************************/
55 
57  fmatvec::Vec getq(void) {
58  return q;
59  }
60  void setSystem(const fmatvec::SqrMat & M_, const fmatvec::Vec & q_) {
61  M = M_;
62  q = q_;
63  assert(M.rows() == q.rows());
64  NumberOfContacts = q.rows();
65  }
66  fmatvec::SqrMat getM(void) {
67  return M;
68  }
69  double getr(void) {
70  return r;
71  }
72  void setr(const double & r_) {
73  r = r_;
74  }
75  /*****************/
76 
77  protected:
82 
86  fmatvec::Vec q;
87 
91  fmatvec::SqrMat M;
92 
96  double r;
97 
101  unsigned int DEBUGLEVEL;
102  };
103 
105  public:
111  }
112 
120  LCPNewtonReformulationFunction(const fmatvec::Vec &q_, const fmatvec::SqrMat &M_, const double &r_ = 10, const unsigned int & DEBUGLEVEL_ = 0);
121 
126 
127  /* INHERITED INTERFACE */
128  fmatvec::Vec operator()(const fmatvec::Vec &q, const void * = NULL);
129  /***************************************************/
130 
131  protected:
132 
133  };
134 
136  public:
142  }
143 
151  LCPFixpointReformulationFunction(const fmatvec::Vec &q_, const fmatvec::SqrMat &M_, const double &r_ = 10, const unsigned int & DEBUGLEVEL_ = 0);
152 
157 
158  /* INHERITED INTERFACE */
159  fmatvec::Vec operator()(const fmatvec::Vec &q, const void * = NULL);
160  /***************************************************/
161  };
162 
164  public:
169 
170  /*
171  * \brief destructor
172  */
174 
175  virtual fmatvec::SqrMat operator ()(const fmatvec::Vec & x, const void* = NULL);
176 
177  virtual void setFunction(Function1<fmatvec::Vec, fmatvec::Vec> * function_);
178 
179  void updateJacobian(const fmatvec::Vec & x);
180 
181  protected:
185  fmatvec::SqrMat J;
186 
187  };
188 }
189 #endif //NUMERICSLCPREFORMULATEDFUNCTION_H_