Skip to content

Commit

Permalink
Fix another Lemke bug: tmpMinRatio ==> tmpd
Browse files Browse the repository at this point in the history 
Provide unittest instead of another apps example
  • Loading branch information
yang-lab committed Oct 28, 2016
1 parent fc04bba commit cae33a3
Show file tree
Hide file tree
Showing 4 changed files with 171 additions and 132 deletions.
7 changes: 0 additions & 7 deletions apps/Lemke_fix/CMakeLists.txt
View file

This file was deleted.

111 changes: 0 additions & 111 deletions apps/Lemke_fix/main.cpp
View file

This file was deleted.

28 changes: 14 additions & 14 deletions dart/lcpsolver/Lemke.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -224,11 +224,11 @@ for (iter = 0; iter < maxiter; ++iter) {
double theta = minRatio.minCoeff();

std::vector<int> tmpJ;
std::vector<double> tmpMinRatio;
std::vector<double> tmpd;
for (size_t i = 0; i < jSize; ++i) {
if (x[j[i]] / d[j[i]] <= theta) {
tmpJ.push_back(j[i]);
tmpMinRatio.push_back(minRatio[i]);
tmpd.push_back(d[j[i]]);
}
}

Expand Down Expand Up @@ -260,11 +260,11 @@ for (iter = 0; iter < maxiter; ++iter) {
if (lvindex != -1) {
lvindex = j[lvindex]; // Always use artificial if possible
} else {
theta = tmpMinRatio[0];
theta = tmpd[0];
lvindex = 0;
for (size_t i = 0; i < jSize; ++i) {
if (tmpMinRatio[i] - theta > piv_tol) { // Bubble sorting
theta = tmpMinRatio[i];
if (tmpd[i] - theta > piv_tol) { // Bubble sorting
theta = tmpd[i];
lvindex = i;
}
}
Expand All @@ -275,17 +275,17 @@ for (iter = 0; iter < maxiter; ++iter) {

ratio = x[lvindex] / d[lvindex];

// bool bDiverged = false;
// for (int i = 0; i < n; ++i) {
// if (isnan(x[i]) || isinf(x[i])) {
// bDiverged = true;
// bool bDiverged = false;
// for (int i = 0; i < n; ++i) {
// if (isnan(x[i]) || isinf(x[i])) {
// bDiverged = true;
// break;
// }
// }
// if (bDiverged) {
// err = 4;
// break;
// }
// }
// if (bDiverged) {
// err = 4;
// break;
// }

// Perform pivot
x = x - ratio * d;
Expand Down
157 changes: 157 additions & 0 deletions unittests/testLemke.cpp
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,157 @@
#include <gtest/gtest.h>

#include "dart/lcpsolver/Lemke.h"
#include "TestHelpers.h"


//==============================================================================
TEST(Lemke, Lemke_1D)
{
Eigen::MatrixXd A;
Eigen::VectorXd b;
Eigen::VectorXd* f;
int err;

f = new Eigen::VectorXd(1);
A.resize(1,1);
A << 1;
b.resize(1);
b << -1.5;
err = dart::lcpsolver::Lemke(A,b,f);

EXPECT_EQ(err, 0);
EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b));
}

//==============================================================================
TEST(Lemke, Lemke_2D)
{
Eigen::MatrixXd A;
Eigen::VectorXd b;
Eigen::VectorXd* f;
int err;

f = new Eigen::VectorXd(2);
A.resize(2,2);
A << 3.12, 0.1877, 0.1877, 3.254;
b.resize(2);
b << -0.00662, -0.006711;
err = dart::lcpsolver::Lemke(A,b,f);

EXPECT_EQ(err, 0);
EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b));
}

//==============================================================================
TEST(Lemke, Lemke_4D)
{
Eigen::MatrixXd A;
Eigen::VectorXd b;
Eigen::VectorXd* f;
int err;

f = new Eigen::VectorXd(4);
A.resize(4,4);
A <<
3.999,0.9985, 1.001, -2,
0.9985, 3.998, -2,0.9995,
1.001, -2, 4.002, 1.001,
-2,0.9995, 1.001, 4.001;

b.resize(4);
b <<
-0.01008,
-0.009494,
-0.07234,
-0.07177;

err = dart::lcpsolver::Lemke(A,b,f);

EXPECT_EQ(err, 0);
EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b));
}

//==============================================================================
TEST(Lemke, Lemke_6D)
{
Eigen::MatrixXd A;
Eigen::VectorXd b;
Eigen::VectorXd* f;
int err;

f = new Eigen::VectorXd(6);
A.resize(6,6);
A <<
3.1360, -2.0370, 0.9723, 0.1096, -2.0370, 0.9723,
-2.0370, 3.7820, 0.8302, -0.0257, 2.4730, 0.0105,
0.9723, 0.8302, 5.1250, -2.2390, -1.9120, 3.4080,
0.1096, -0.0257, -2.2390, 3.1010, -0.0257, -2.2390,
-2.0370, 2.4730, -1.9120, -0.0257, 5.4870, -0.0242,
0.9723, 0.0105, 3.4080, -2.2390, -0.0242, 3.3860;

b.resize(6);
b <<
0.1649,
-0.0025,
-0.0904,
-0.0093,
-0.0000,
-0.0889;

err = dart::lcpsolver::Lemke(A,b,f);

EXPECT_EQ(err, 0);
EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b));
}

//==============================================================================
TEST(Lemke, Lemke_12D)
{
Eigen::MatrixXd A;
Eigen::VectorXd b;
Eigen::VectorXd* f;
int err;

f = new Eigen::VectorXd(12);
A.resize(12,12);
A <<
4.03, -1.014, -1.898, 1.03, -1.014, -1.898, 1, -1.014, -1.898, -2, -1.014, -1.898,
-1.014, 4.885, -1.259, 1.888, 3.81, 2.345, -1.879, 1.281, -2.334, 1.022, 0.206, 1.27,
-1.898, -1.259, 3.2, -1.032,-0.6849, 1.275, 1.003, 0.6657, 3.774, 1.869, 1.24, 1.85,
1.03, 1.888, -1.032, 4.03, 1.888, -1.032, -2, 1.888, -1.032, 1, 1.888, -1.032,
-1.014, 3.81,-0.6849, 1.888, 3.225, 1.275, -1.879, 1.85, -1.27, 1.022, 1.265, 0.6907,
-1.898, 2.345, 1.275, -1.032, 1.275, 4.86, 1.003, -1.24, 0.2059, 1.869, -2.309, 3.791,
1, -1.879, 1.003, -2, -1.879, 1.003, 3.97, -1.879, 1.003, 0.9703, -1.879, 1.003,
-1.014, 1.281, 0.6657, 1.888, 1.85, -1.24, -1.879, 3.187, 1.234, 1.022, 3.755,-0.6714,
-1.898, -2.334, 3.774, -1.032, -1.27, 0.2059, 1.003, 1.234, 4.839, 1.869, 2.299, 1.27,
-2, 1.022, 1.869, 1, 1.022, 1.869, 0.9703, 1.022, 1.869, 3.97, 1.022, 1.869,
-1.014, 0.206, 1.24, 1.888, 1.265, -2.309, -1.879, 3.755, 2.299, 1.022, 4.814, -1.25,
-1.898, 1.27, 1.85, -1.032, 0.6907, 3.791, 1.003,-0.6714, 1.27, 1.869, -1.25, 3.212;

b.resize(12);
b <<
-0.00981,
-1.458e-10,
5.357e-10,
-0.0098,
-1.44e-10,
5.298e-10,
-0.009807,
-1.399e-10,
5.375e-10,
-0.009807,
-1.381e-10,
5.316e-10;

err = dart::lcpsolver::Lemke(A,b,f);

EXPECT_EQ(err, 0);
EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b));
}

//==============================================================================
int main(int argc, char* argv[])
{
::testing::InitGoogleTest(&argc,argv);
return RUN_ALL_TESTS();
}

0 comments on commit cae33a3

Please sign in to comment.