#include <lcp.h>
Public Member Functions | |
double | CheckPivotResult (const fVec &q_new, std::vector< int > &w2a, std::vector< int > &w2g) |
LCP (const fMat &_NN, const fVec &_r) | |
Constructor. | |
const fMat & | Mref () |
int | NumErrors () |
int | NumLoops () |
const fVec & | Qref () |
int | Solve (fVec &g, fVec &a) |
Solve using iterative method. | |
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. | |
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 | 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. | |
~LCP () | |
Static Public Member Functions | |
static void | Pivot (int idx1, int idx2, const fMat &M, const fVec &q, fMat &M_new, fVec &q_new) |
static void | Pivot (std::vector< int > &idx1, std::vector< int > &idx2, const fMat &M, const fVec &q, fMat &M_new, fVec &q_new) |
static void | Pivot (std::vector< int > &w2a, std::vector< int > &w2g, std::vector< int > &z2a, std::vector< int > &z2g, const fMat &M, const fVec &q, fMat &M_new, fVec &q_new) |
static void | Pivot (const fMat &M, const fVec &q, std::vector< int > &w2a, std::vector< int > &w2g, std::vector< int > &z2a, std::vector< int > &z2g, int new_jr, fMat &m_jr, fVec &q_new) |
static void | Pivot (const fMat &M, const fVec &q, const fMat &oldMinv, std::vector< int > &old_w2a, std::vector< int > &old_w2g, std::vector< int > &old_z2a, std::vector< int > &old_z2g, int ys2a, int ys2g, int yr2a, int yr2g, std::vector< int > &w2a, std::vector< int > &w2g, std::vector< int > &z2a, std::vector< int > &z2g, int jr, fMat &newMinv, fMat &m_jr, fVec &q_new) |
Protected Member Functions | |
int | check_q (const fVec &q, double max_error) |
Static Protected Member Functions | |
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) |
Protected Attributes | |
fMat | M |
fMat | N |
int | n_vars |
fVec | q |
fVec | r |
Solves a linear complementarity problem (LCP): find g and a that satisfy N*g + r = a, a>=0, g>=0, a^T*g = 0.
int LCP::check_q | ( | const fVec & | q, |
double | max_error | ||
) | [inline, protected] |
double LCP::CheckPivotResult | ( | const fVec & | q_new, |
std::vector< int > & | w2a, | ||
std::vector< int > & | w2g | ||
) |
Definition at line 2009 of file lcp_pivot.cpp.
int LCP::NumErrors | ( | ) |
Definition at line 558 of file lcp_pivot.cpp.
int LCP::NumLoops | ( | ) |
Definition at line 555 of file lcp_pivot.cpp.
void LCP::Pivot | ( | int | idx1, |
int | idx2, | ||
const fMat & | M, | ||
const fVec & | q, | ||
fMat & | M_new, | ||
fVec & | q_new | ||
) | [static] |
Definition at line 1321 of file lcp_pivot.cpp.
void LCP::Pivot | ( | std::vector< int > & | idx1, |
std::vector< int > & | idx2, | ||
const fMat & | M, | ||
const fVec & | q, | ||
fMat & | M_new, | ||
fVec & | q_new | ||
) | [static] |
Definition at line 1196 of file lcp_pivot.cpp.
void LCP::Pivot | ( | std::vector< int > & | w2a, |
std::vector< int > & | w2g, | ||
std::vector< int > & | z2a, | ||
std::vector< int > & | z2g, | ||
const fMat & | M, | ||
const fVec & | q, | ||
fMat & | M_new, | ||
fVec & | q_new | ||
) | [static] |
Definition at line 1063 of file lcp_pivot.cpp.
void LCP::Pivot | ( | const fMat & | M, |
const fVec & | q, | ||
std::vector< int > & | w2a, | ||
std::vector< int > & | w2g, | ||
std::vector< int > & | z2a, | ||
std::vector< int > & | z2g, | ||
int | new_jr, | ||
fMat & | m_jr, | ||
fVec & | q_new | ||
) | [static] |
Definition at line 1866 of file lcp_pivot.cpp.
void LCP::Pivot | ( | const fMat & | M, |
const fVec & | q, | ||
const fMat & | oldMinv, | ||
std::vector< int > & | old_w2a, | ||
std::vector< int > & | old_w2g, | ||
std::vector< int > & | old_z2a, | ||
std::vector< int > & | old_z2g, | ||
int | ys2a, | ||
int | ys2g, | ||
int | yr2a, | ||
int | yr2g, | ||
std::vector< int > & | w2a, | ||
std::vector< int > & | w2g, | ||
std::vector< int > & | z2a, | ||
std::vector< int > & | z2g, | ||
int | jr, | ||
fMat & | newMinv, | ||
fMat & | m_jr, | ||
fVec & | q_new | ||
) | [static] |
Definition at line 1481 of file lcp_pivot.cpp.
void LCP::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 | ||
) | [static, protected] |
Definition at line 1433 of file lcp_pivot.cpp.
int LCP::Solve | ( | fVec & | g, |
fVec & | a | ||
) |
int LCP::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.
Solve using iterative method.
[out] | g | the solution (non-basic variables) |
[out] | a | the solution (basic variables) |
[in] | g_init | the initial value for g |
[in] | _max_error | maximum permissible error |
[in] | _max_iteration | maximum number of iterations |
[in] | _speed | convergence speed |
[out] | n_iteration | actual number of iterations |
int LCP::SolvePivot | ( | fVec & | g, |
fVec & | a, | ||
double | _max_error, | ||
int | _max_iteration, | ||
int * | n_iteration, | ||
std::vector< int > & | _g2w | ||
) |
Solve using Lemke's method.
Solve using Lemke's method.
[out] | g | the solution (non-basic variables) |
[out] | a | the solution (basic variables) |
[in] | _max_error | maximum permissible error |
[in] | _max_iteration | maximum number of iteration |
[out] | n_iteration | actual number of iterations |
[out] | _g2w | resulting pivot |
Definition at line 698 of file lcp_pivot.cpp.
int LCP::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.
Solve by pivot using path planning algorithm.
[out] | g | the solution (non-basic variables) |
[out] | a | the solution (basic variables) |
[in] | _max_error | maximum permissible error |
[in] | _max_iteration | maximum number of iteration |
[out] | n_iteration | actual number of iterations |
[out] | _g2w | resulting pivot |
Definition at line 562 of file lcp_pivot.cpp.
int LCP::n_vars [protected] |