All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Pages
lemke_algorithm.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 NUMERICS_LEMKE_ALGORITHM_H_
21 #define NUMERICS_LEMKE_ALGORITHM_H_
22 
23 #include <fmatvec/fmatvec.h>
24 #include <fmatvec/atom.h>
25 
26 namespace MBSim {
27 
28  class LemkeAlgorithm : virtual public fmatvec::Atom {
29  public:
30  LemkeAlgorithm(const bool & DEBUGLEVEL_ = 0) :
31  M(), q(), steps(0), DEBUGLEVEL(DEBUGLEVEL_), info(-1) {
32  }
33 
34  LemkeAlgorithm(const fmatvec::SqrMat & M_, const fmatvec::Vec & q_, const bool & DEBUGLEVEL_ = 0) :
35  M(M_), q(q_), steps(0), DEBUGLEVEL(DEBUGLEVEL_), info(-1) {
36  assert(M_.rows() == q.size());
37  assert(M_.cols() == q.size());
38  }
39 
40  LemkeAlgorithm(const fmatvec::SymMat & M_, const fmatvec::Vec & q_, const bool & DEBUGLEVEL_ = 0) :
41  DEBUGLEVEL(DEBUGLEVEL_) {
42  setSystem(M_, q_);
43  }
44 
45  /* GETTER / SETTER */
49  int getInfo() {
50  return info;
51  }
52 
56  int getSteps(void) {
57  return steps;
58  }
59 
63  void setSystem(const fmatvec::SqrMat & M_, const fmatvec::Vec & q_) {
64  assert(M_.rows() == q_.size());
65  assert(M_.cols() == q_.size());
66  M.resize() = M_;
67  q.resize() = q_;
68  }
69 
73  void setSystem(const fmatvec::SymMat & M_, const fmatvec::Vec & q_) {
74  M = fmatvec::SqrMat(M_.size(), fmatvec::NONINIT);
75  for (int i = 0; i < M.size(); i++)
76  for (int j = 0; j < M.size(); j++)
77  M(i, j) = M_(i, j);
78  q = q_;
79  }
80  /***************************************************/
81 
85  fmatvec::Vec solve(unsigned int maxloops = 0);
86 
87  virtual ~LemkeAlgorithm() {
88  }
89 
90  protected:
91  int findLexicographicMinimum(const fmatvec::Mat &A, const int & pivotColIndex);
92  bool LexicographicPositive(const fmatvec::Vec & v);
93  void GaussJordanEliminationStep(fmatvec::Mat &A, int pivotRowIndex, int pivotColumnIndex, const std::vector<size_t> & basis);
94  bool greaterZero(const fmatvec::Vec & vector);
95  bool validBasis(const std::vector<size_t> & basis);
96 
98  fmatvec::Vec q;
99 
103  unsigned int steps;
104 
109 
116  int info;
117  };
118 
119 } /* namespace MBSim */
120 #endif /* NUMERICS_LEMKE_ALGORITHM_H_ */
bool DEBUGLEVEL
define level of debug output
Definition: lemke_algorithm.h:108
void setSystem(const fmatvec::SymMat &M_, const fmatvec::Vec &q_)
set system with Matrix M and vector q
Definition: lemke_algorithm.h:73
Definition: lemke_algorithm.h:28
void setSystem(const fmatvec::SqrMat &M_, const fmatvec::Vec &q_)
set system with Matrix M and vector q
Definition: lemke_algorithm.h:63
fmatvec::Vec solve(unsigned int maxloops=0)
solve algorithm adapted from : Fast Implementation of Lemke’s Algorithm for Rigid Body Contact Simula...
Definition: lemke_algorithm.cc:34
SquareMatrix< Ref, double > SqrMat
int getSteps(void)
get the number of steps until the solution was found
Definition: lemke_algorithm.h:56
int getInfo()
return info of solution process
Definition: lemke_algorithm.h:49
unsigned int steps
number of steps until the Lemke algorithm found a solution
Definition: lemke_algorithm.h:103
int info
did the algorithm find a solution
Definition: lemke_algorithm.h:116

Impressum / Disclaimer / Datenschutz Generated by doxygen 1.8.5 Valid HTML