#include #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(); }