bool LCPSolver::Solve(const MatrixXd& _A, const VectorXd& _b, VectorXd& _x, double mu, int numDir, bool bUseODESolver)
{
if (!bUseODESolver)
{
int err = Lemke(_A, _b, _x);
return (err == 0);
}
else
{
assert(numDir >= 4);
MatrixXd AODE;
VectorXd bODE;
transferToODEFormulation(_A, _b, AODE, bODE, numDir);
double* A, *b, *x, *w, *lo, *hi;
int n = AODE.rows();
int nContacts = n / 3;
int nSkip = dPAD(n);
A = new double[n * nSkip];
b = new double[n];
x = new double[n];
w = new double[n];
lo = new double[n];
hi = new double[n];
int* findex = new int[n];
memset(A, 0, n * nSkip * sizeof(double));
for (int i = 0; i < n; ++i)
{
for (int j = 0; j < n; ++j)
{
A[i * nSkip + j] = AODE(i, j);
}
}
for (int i = 0; i < n; ++i)
{
b[i] = -bODE[i];
x[i] = w[i] = lo[i] = 0;
hi[i] = dInfinity;
}
for (int i = 0; i < nContacts; ++i)
{
findex[i] = -1;
findex[nContacts + i * 2 + 0] = i;
findex[nContacts + i * 2 + 1] = i;
lo[nContacts + i * 2 + 0] = -mu;
lo[nContacts + i * 2 + 1] = -mu;
hi[nContacts + i * 2 + 0] = mu;
hi[nContacts + i * 2 + 1] = mu;
}
// dClearUpperTriangle (A,n);
dSolveLCP (n,A,x,b,w,0,lo,hi,findex);
VectorXd xODE = VectorXd::Zero(n);
for (int i = 0; i < n; ++i)
{
xODE[i] = x[i];
}
transferSolFromODEFormulation(xODE, _x, numDir);
// checkIfSolution(reducedA, reducedb, _x);
delete[] A;
delete[] b;
delete[] x;
delete[] w;
delete[] lo;
delete[] hi;
delete[] findex;
return 1;
}
}
bool ODELCPSolver::Solve(const Eigen::MatrixXd& _A,
const Eigen::VectorXd& _b,
Eigen::VectorXd* _x,
int _numContacts,
double _mu,
int _numDir,
bool _bUseODESolver) {
if (!_bUseODESolver) {
int err = Lemke(_A, _b, _x);
return (err == 0);
} else {
assert(_numDir >= 4);
DART_UNUSED(_numDir);
double* A, *b, *x, *w, *lo, *hi;
int n = _A.rows();
int nSkip = dPAD(n);
A = new double[n * nSkip];
b = new double[n];
x = new double[n];
w = new double[n];
lo = new double[n];
hi = new double[n];
int* findex = new int[n];
memset(A, 0, n * nSkip * sizeof(double));
for (int i = 0; i < n; ++i) {
for (int j = 0; j < n; ++j) {
A[i * nSkip + j] = _A(i, j);
}
}
for (int i = 0; i < n; ++i) {
b[i] = -_b[i];
x[i] = w[i] = lo[i] = 0;
hi[i] = dInfinity;
findex[i] = -1;
}
for (int i = 0; i < _numContacts; ++i) {
findex[_numContacts + i * 2 + 0] = i;
findex[_numContacts + i * 2 + 1] = i;
lo[_numContacts + i * 2 + 0] = -_mu;
lo[_numContacts + i * 2 + 1] = -_mu;
hi[_numContacts + i * 2 + 0] = _mu;
hi[_numContacts + i * 2 + 1] = _mu;
}
// dClearUpperTriangle (A,n);
dSolveLCP(n, A, x, b, w, 0, lo, hi, findex);
// for (int i = 0; i < n; i++) {
// if (w[i] < 0.0 && abs(x[i] - hi[i]) > 0.000001)
// cout << "w[" << i << "] is negative, but x is " << x[i] << endl;
// else if (w[i] > 0.0 && abs(x[i] - lo[i]) > 0.000001)
// cout << "w[" << i << "] is positive, but x is " << x[i]
// << " lo is " << lo[i] << endl;
// else if (abs(w[i]) < 0.000001 && (x[i] > hi[i] || x[i] < lo[i]))
// cout << "w[i] " << i << " is zero, but x is " << x[i] << endl;
// }
*_x = Eigen::VectorXd(n);
for (int i = 0; i < n; ++i) {
(*_x)[i] = x[i];
}
// checkIfSolution(reducedA, reducedb, _x);
delete[] A;
delete[] b;
delete[] x;
delete[] w;
delete[] lo;
delete[] hi;
delete[] findex;
return 1;
}
}
