Debian Codegraph @

unittests/.deprecated/testLemke.cpp

raw
1 #include <gtest/gtest.h>
2
3 #include "dart/lcpsolver/Lemke.h"
4 #include "TestHelpers.h"
5
6
7 //==============================================================================
8 TEST(Lemke, Lemke_1D)
9 {
10 Eigen::MatrixXd A;
11 Eigen::VectorXd b;
12 Eigen::VectorXd* f;
13 int err;
14
15 f = new Eigen::VectorXd(1);
16 A.resize(1,1);
17 A << 1;
18 b.resize(1);
19 b << -1.5;
20 err = dart::lcpsolver::Lemke(A,b,f);
21
22 EXPECT_EQ(err, 0);
23 EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b));
24 }
25
26 //==============================================================================
27 TEST(Lemke, Lemke_2D)
28 {
29 Eigen::MatrixXd A;
30 Eigen::VectorXd b;
31 Eigen::VectorXd* f;
32 int err;
33
34 f = new Eigen::VectorXd(2);
35 A.resize(2,2);
36 A << 3.12, 0.1877, 0.1877, 3.254;
37 b.resize(2);
38 b << -0.00662, -0.006711;
39 err = dart::lcpsolver::Lemke(A,b,f);
40
41 EXPECT_EQ(err, 0);
42 EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b));
43 }
44
45 //==============================================================================
46 TEST(Lemke, Lemke_4D)
47 {
48 Eigen::MatrixXd A;
49 Eigen::VectorXd b;
50 Eigen::VectorXd* f;
51 int err;
52
53 f = new Eigen::VectorXd(4);
54 A.resize(4,4);
55 A <<
56 3.999,0.9985, 1.001, -2,
57 0.9985, 3.998, -2,0.9995,
58 1.001, -2, 4.002, 1.001,
59 -2,0.9995, 1.001, 4.001;
60
61 b.resize(4);
62 b <<
63 -0.01008,
64 -0.009494,
65 -0.07234,
66 -0.07177;
67
68 err = dart::lcpsolver::Lemke(A,b,f);
69
70 EXPECT_EQ(err, 0);
71 EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b));
72 }
73
74 //==============================================================================
75 TEST(Lemke, Lemke_6D)
76 {
77 Eigen::MatrixXd A;
78 Eigen::VectorXd b;
79 Eigen::VectorXd* f;
80 int err;
81
82 f = new Eigen::VectorXd(6);
83 A.resize(6,6);
84 A <<
85 3.1360, -2.0370, 0.9723, 0.1096, -2.0370, 0.9723,
86 -2.0370, 3.7820, 0.8302, -0.0257, 2.4730, 0.0105,
87 0.9723, 0.8302, 5.1250, -2.2390, -1.9120, 3.4080,
88 0.1096, -0.0257, -2.2390, 3.1010, -0.0257, -2.2390,
89 -2.0370, 2.4730, -1.9120, -0.0257, 5.4870, -0.0242,
90 0.9723, 0.0105, 3.4080, -2.2390, -0.0242, 3.3860;
91
92 b.resize(6);
93 b <<
94 0.1649,
95 -0.0025,
96 -0.0904,
97 -0.0093,
98 -0.0000,
99 -0.0889;
100
101 err = dart::lcpsolver::Lemke(A,b,f);
102
103 EXPECT_EQ(err, 0);
104 EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b));
105 }
106
107 //==============================================================================
108 TEST(Lemke, Lemke_12D)
109 {
110 Eigen::MatrixXd A;
111 Eigen::VectorXd b;
112 Eigen::VectorXd* f;
113 int err;
114
115 f = new Eigen::VectorXd(12);
116 A.resize(12,12);
117 A <<
118 4.03, -1.014, -1.898, 1.03, -1.014, -1.898, 1, -1.014, -1.898, -2, -1.014, -1.898,
119 -1.014, 4.885, -1.259, 1.888, 3.81, 2.345, -1.879, 1.281, -2.334, 1.022, 0.206, 1.27,
120 -1.898, -1.259, 3.2, -1.032,-0.6849, 1.275, 1.003, 0.6657, 3.774, 1.869, 1.24, 1.85,
121 1.03, 1.888, -1.032, 4.03, 1.888, -1.032, -2, 1.888, -1.032, 1, 1.888, -1.032,
122 -1.014, 3.81,-0.6849, 1.888, 3.225, 1.275, -1.879, 1.85, -1.27, 1.022, 1.265, 0.6907,
123 -1.898, 2.345, 1.275, -1.032, 1.275, 4.86, 1.003, -1.24, 0.2059, 1.869, -2.309, 3.791,
124 1, -1.879, 1.003, -2, -1.879, 1.003, 3.97, -1.879, 1.003, 0.9703, -1.879, 1.003,
125 -1.014, 1.281, 0.6657, 1.888, 1.85, -1.24, -1.879, 3.187, 1.234, 1.022, 3.755,-0.6714,
126 -1.898, -2.334, 3.774, -1.032, -1.27, 0.2059, 1.003, 1.234, 4.839, 1.869, 2.299, 1.27,
127 -2, 1.022, 1.869, 1, 1.022, 1.869, 0.9703, 1.022, 1.869, 3.97, 1.022, 1.869,
128 -1.014, 0.206, 1.24, 1.888, 1.265, -2.309, -1.879, 3.755, 2.299, 1.022, 4.814, -1.25,
129 -1.898, 1.27, 1.85, -1.032, 0.6907, 3.791, 1.003,-0.6714, 1.27, 1.869, -1.25, 3.212;
130
131 b.resize(12);
132 b <<
133 -0.00981,
134 -1.458e-10,
135 5.357e-10,
136 -0.0098,
137 -1.44e-10,
138 5.298e-10,
139 -0.009807,
140 -1.399e-10,
141 5.375e-10,
142 -0.009807,
143 -1.381e-10,
144 5.316e-10;
145
146 err = dart::lcpsolver::Lemke(A,b,f);
147
148 EXPECT_EQ(err, 0);
149 EXPECT_TRUE(dart::lcpsolver::validate(A,(*f),b));
150 }
151
152 //==============================================================================
153 int main(int argc, char* argv[])
154 {
155 ::testing::InitGoogleTest(&argc,argv);
156 return RUN_ALL_TESTS();
157 }