mbsim 4.0.0 MBSim Kernel

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() :
31 M(), q() {
32 }
33
34 LemkeAlgorithm(const fmatvec::SqrMat & M_, const fmatvec::Vec & q_) :
35 M(M_), q(q_), steps(0), 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_) {
41 setSystem(M_, q_);
42 }
43
44 /* GETTER / SETTER */
48 int getInfo() {
49 return info;
50 }
51
55 int getSteps() {
56 return steps;
57 }
58
62 void setSystem(const fmatvec::SqrMat & M_, const fmatvec::Vec & q_) {
63 assert(M_.rows() == q_.size());
64 assert(M_.cols() == q_.size());
65 M <<= M_;
66 q <<= q_;
67 }
68
72 void setSystem(const fmatvec::SymMat & M_, const fmatvec::Vec & q_) {
73 M.resize(M_.size(), fmatvec::NONINIT);
74 for (int i = 0; i < M.size(); i++)
75 for (int j = 0; j < M.size(); j++)
76 M(i, j) = M_(i, j);
77 q <<= q_;
78 }
79 /***************************************************/
80
84 fmatvec::Vec solve(unsigned int maxloops = 0);
85
86 ~LemkeAlgorithm() override = default;
87
88 protected:
89 int findLexicographicMinimum(const fmatvec::Mat &A, const int & pivotColIndex);
90 bool LexicographicPositive(const fmatvec::Vec & v);
91 void GaussJordanEliminationStep(fmatvec::Mat &A, int pivotRowIndex, int pivotColumnIndex, const std::vector<size_t> & basis);
92 bool greaterZero(const fmatvec::Vec & vector);
93 bool validBasis(const std::vector<size_t> & basis);
94
95 fmatvec::SqrMat M;
96 fmatvec::Vec q;
97
101 unsigned int steps{0};
102
109 int info{-1};
110 };
111
112 } /* namespace MBSim */
113 #endif /* NUMERICS_LEMKE_ALGORITHM_H_ */
MBSim::LemkeAlgorithm::setSystem
void setSystem(const fmatvec::SymMat &M_, const fmatvec::Vec &q_)
set system with Matrix M and vector q
Definition: lemke_algorithm.h:72
MBSim::LemkeAlgorithm
Definition: lemke_algorithm.h:28
MBSim::LemkeAlgorithm::getSteps
int getSteps()
get the number of steps until the solution was found
Definition: lemke_algorithm.h:55
MBSim::LemkeAlgorithm::setSystem
void setSystem(const fmatvec::SqrMat &M_, const fmatvec::Vec &q_)
set system with Matrix M and vector q
Definition: lemke_algorithm.h:62
MBSim::LemkeAlgorithm::solve
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
MBSim::LemkeAlgorithm::getInfo
int getInfo()
return info of solution process
Definition: lemke_algorithm.h:48
MBSim::LemkeAlgorithm::steps
unsigned int steps
number of steps until the Lemke algorithm found a solution
Definition: lemke_algorithm.h:101
MBSim
namespace MBSim
Definition: bilateral_constraint.cc:30
MBSim::LemkeAlgorithm::info
int info
did the algorithm find a solution
Definition: lemke_algorithm.h:109