00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #ifndef NUMERICSLCPREFORMULATEDFUNCTION_H_
00021 #define NUMERICSLCPREFORMULATEDFUNCTION_H_
00022
00023 #include <mbsim/utils/function.h>
00024
00025 #include <mbsim/numerics/functions/newton_method_jacobian_functions.h>
00026
00027 namespace MBSim {
00028
00029 class LCPReformulationFunction : public Function1<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
00045
00046 virtual ~LCPReformulationFunction();
00047
00048
00053 virtual fmatvec::Vec operator()(const fmatvec::Vec &q, const void * = NULL) = 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
00128 fmatvec::Vec operator()(const fmatvec::Vec &q, const void * = NULL);
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
00159 fmatvec::Vec operator()(const fmatvec::Vec &q, const void * = NULL);
00160
00161 };
00162
00163 class LinearComplementarityJacobianFunction : public NewtonJacobianFunction {
00164 public:
00168 LinearComplementarityJacobianFunction();
00169
00170
00171
00172
00173 virtual ~LinearComplementarityJacobianFunction();
00174
00175 virtual fmatvec::SqrMat operator ()(const fmatvec::Vec & x, const void* = NULL);
00176
00177 virtual void setFunction(Function1<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_