25 #define ACTIVATE_SAME_STATE // activate same state check during planning 68 int SolveEx(
fVec& g,
fVec& a,
const fVec& g_init,
double _max_error = 1e-8,
int _max_iteration = 100,
double _speed = 0.5,
int* n_iteration = 0);
80 int SolvePivot(
fVec& g,
fVec& a,
double _max_error,
int _max_iteration,
int* n_iteration, std::vector<int>& _g2w);
92 int SolvePivot2(
fVec& g,
fVec& a,
double _max_error,
int _max_iteration,
int* n_iteration, std::vector<int>& _g2w);
94 static void Pivot(
int idx1,
int idx2,
97 static void Pivot(std::vector<int>& idx1, std::vector<int>& idx2,
100 static void Pivot(std::vector<int>& w2a, std::vector<int>& w2g,
101 std::vector<int>& z2a, std::vector<int>& z2g,
105 std::vector<int>& w2a, std::vector<int>& w2g,
106 std::vector<int>& z2a, std::vector<int>& z2g,
111 std::vector<int>& old_w2a, std::vector<int>& old_w2g,
112 std::vector<int>& old_z2a, std::vector<int>& old_z2g,
113 int ys2a,
int ys2g,
int yr2a,
int yr2g,
114 std::vector<int>& w2a, std::vector<int>& w2g,
115 std::vector<int>& z2a, std::vector<int>& z2g,
132 static void pivot_body(
const fMat& M12,
const fMat& M1,
const fMat& M2,
const fMat& M12bar,
const fVec& q1,
const fVec& q1bar,
fMat& Md12,
fMat& Md1,
fMat& Md2,
fMat& Md12bar,
fVec& qd1,
fVec& qd1bar);
137 if(
q(
i) < -max_error)
double CheckPivotResult(const fVec &q_new, std::vector< int > &w2a, std::vector< int > &w2g)
static void Pivot(int idx1, int idx2, const fMat &M, const fVec &q, fMat &M_new, fVec &q_new)
int col() const
Returns the number of columns.
int check_q(const fVec &q, double max_error)
static void pivot_body(const fMat &M12, const fMat &M1, const fMat &M2, const fMat &M12bar, const fVec &q1, const fVec &q1bar, fMat &Md12, fMat &Md1, fMat &Md2, fMat &Md12bar, fVec &qd1, fVec &qd1bar)
int SolveEx(fVec &g, fVec &a, const fVec &g_init, double _max_error=1e-8, int _max_iteration=100, double _speed=0.5, int *n_iteration=0)
Solve using iterative method.
Generic matrix/vector classes.
LCP(const fMat &_NN, const fVec &_r)
Constructor.
int SolvePivot(fVec &g, fVec &a, double _max_error, int _max_iteration, int *n_iteration, std::vector< int > &_g2w)
Solve using Lemke's method.
int Solve(fVec &g, fVec &a)
Solve using iterative method.
int row() const
Returns the number of rows.
int SolvePivot2(fVec &g, fVec &a, double _max_error, int _max_iteration, int *n_iteration, std::vector< int > &_g2w)
Solve by pivot using path planning algorithm.
int size() const
Size of the vector (same as row()).
Matrix of generic size. The elements are stored in a one-dimensional array in row-major order...