1 #include "gmock/gmock.h" 2 #include "gtest/gtest.h" 4 #include "../third-party/JCQP/ProblemGenerator.h" 5 #include "../third-party/JCQP/QpProblem.h" 6 #include "../third-party/JCQP/Timer.h" 9 TEST(JCQP, test_example) {
10 QpProblem<double> problem(2, 3);
11 problem.A << 1, 2, 4, 4, 5, 6;
12 problem.P << 12, 5, 5, 8;
14 problem.l << -1, -2, -3;
16 problem.runFromDense();
18 Vector<double> soln = problem.getSolution();
19 Vector<double> qf = 0.5 * soln.transpose() * problem.P * soln;
21 double objective = qf(0, 0) + problem.q.transpose() * soln;
22 printf(
"objective: %f\n", objective);
24 Vector<double> Ax = problem.A * soln;
25 for (
int i = 0; i < problem.m; i++) {
26 if (Ax[i] < problem.l[i]) {
27 printf(
"lb error (%.3f %.3f, %.3f)!\n", problem.l[i], problem.u[i],
31 if (Ax[i] > problem.u[i]) {
32 printf(
"ub error (%.3f %.3f, %.3f)!\n", problem.l[i], problem.u[i],
39 TEST(JCQP, test_result_dense) {
40 QpProblem<double> problem(2, 6);
41 problem.A << 1, 1, 1, 0, 0, 1, -1, -1, -1, 0, 0, -1;
42 problem.P << 4, 1, 1, 2;
43 problem.u << 1, 0.7, 0.7, -1, 0, 0;
44 problem.l << -1000, -1000, -1000, -1000, -1000, -1000;
46 problem.runFromDense();
48 EXPECT_TRUE(
fpEqual(problem.getSolution()[0], 0.3, .0001));
49 EXPECT_TRUE(
fpEqual(problem.getSolution()[1], 0.7, .0001));
52 TEST(JCQP, test_result_sparse) {
53 QpProblem<double> problem(2, 3);
54 problem.A << 1, 1, 1., 0., 0., 1.;
55 problem.P << 4., 1., 1., 2.;
56 problem.u << 1., 0.7, 0.7;
57 problem.l << 1., 0., 0.;
59 problem.runFromDense(-1,
true);
61 EXPECT_TRUE(
fpEqual(problem.getSolution()[0], 0.3, .0001));
62 EXPECT_TRUE(
fpEqual(problem.getSolution()[1], 0.7, .0001));
65 TEST(JCQP, test_sort_triples) {
66 std::vector<SparseTriple<double>> tris;
67 tris.push_back({1,1,1});
68 tris.push_back({0,1,0});
69 tris.push_back({0,0,1});
70 tris.push_back({0,0,0});
72 sortTriples(tris,
true);
73 EXPECT_TRUE(tris[0].r == 0 && tris[0].c == 0);
74 EXPECT_TRUE(tris[1].r == 1 && tris[1].c == 0);
75 EXPECT_TRUE(tris[2].r == 0 && tris[2].c == 1);
76 EXPECT_TRUE(tris[3].r == 1 && tris[3].c == 1);
78 tris.push_back({10,1,1});
79 EXPECT_THROW(sortTriples(tris,
true), std::runtime_error);
81 sortAndSumTriples(tris);
82 EXPECT_TRUE(tris[0].value == 10 + 1);
85 tris.push_back({1,1,1});
86 tris.push_back({0,1,0});
87 tris.push_back({0,0,1});
88 tris.push_back({0,0,0});
89 tris.push_back({10,1,1});
91 sortAndSumTriples(tris);
92 EXPECT_TRUE(tris[0].value == 10 + 1);
95 TEST(JCQP, test_result_sparse_triples) {
96 QpProblem<double> problem(2,3);
97 problem.A_triples = {{1.,0,0}, {1.,0,1}, {1., 1, 0},
99 problem.P_triples = {{4.,0,0}, {1.,0,1},
101 problem.u << 1., 0.7, 0.7;
102 problem.l << 1., 0., 0.;
104 problem.runFromTriples(-1,
true);
105 EXPECT_TRUE(
fpEqual(problem.getSolution()[0], 0.3, .0001));
106 EXPECT_TRUE(
fpEqual(problem.getSolution()[1], 0.7, .0001));
109 TEST(JCQP, test_solver_dense_double) {
110 DenseMatrix<double> A(100, 100);
112 A = A * A.transpose();
113 Vector<double> x(100);
116 Vector<double> b = x;
118 CholeskyDenseSolver<double> solver(
false);
119 for (
s64 i = 0; i < 3; i++) {
125 Vector<double> diff = A * x - b;
126 EXPECT_TRUE(
fpEqual(diff.minCoeff(), 0.0, 0.001));
127 EXPECT_TRUE(
fpEqual(diff.maxCoeff(), 0.0, 0.001));
130 TEST(JCQP, test_solver_dense_float) {
131 DenseMatrix<float> A(10, 10);
133 A = A * A.transpose();
139 CholeskyDenseSolver<float> solver(
false);
140 for (
s64 i = 0; i < 3; i++) {
146 Vector<float> diff = A * x - b;
147 EXPECT_TRUE(
fpEqual(diff.minCoeff(), 0.0f, 0.001f));
148 EXPECT_TRUE(
fpEqual(diff.maxCoeff(), 0.0f, 0.001f));
151 TEST(JCQP, test_solver_sparse) {
155 Vector<double> b = x;
156 DenseMatrix<double> A(n, n);
158 for (
int i = 0; i < n; i++) {
159 for (
int j = 0; j < n; j++) {
160 if ((i + 3 * j) % 7 == 1) {
166 A = A * A.transpose();
167 SparseMatrix<double> As = A.sparseView();
168 CholeskySparseSolver<double> solver;
172 Vector<double> diff = As * x - b;
173 EXPECT_TRUE(
fpEqual(diff.minCoeff(), 0.0, 0.001));
174 EXPECT_TRUE(
fpEqual(diff.maxCoeff(), 0.0, 0.001));
176 printf(
"diff %g %g\n", diff.minCoeff(), diff.maxCoeff());
bool fpEqual(T a, T b, T tol)