Debian Codegraph @

unittests/integration/test_Lemke.cpp

raw
1 /*
2 * Copyright (c) 2011-2022, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include <gtest/gtest.h>
34
35 #include "dart/lcpsolver/Lemke.hpp"
36
37 #include "TestHelpers.hpp"
38
39 //==============================================================================
40 TEST(Lemke, Lemke_1D)
41 {
42 Eigen::MatrixXd A;
43 Eigen::VectorXd b;
44 Eigen::VectorXd* f;
45 int err;
46
47 f = new Eigen::VectorXd(1);
48 A.resize(1, 1);
49 A << 1;
50 b.resize(1);
51 b << -1.5;
52 err = dart::lcpsolver::Lemke(A, b, f);
53
54 EXPECT_EQ(err, 0);
55 EXPECT_TRUE(dart::lcpsolver::validate(A, (*f), b));
56 }
57
58 //==============================================================================
59 TEST(Lemke, Lemke_2D)
60 {
61 Eigen::MatrixXd A;
62 Eigen::VectorXd b;
63 Eigen::VectorXd* f;
64 int err;
65
66 f = new Eigen::VectorXd(2);
67 A.resize(2, 2);
68 A << 3.12, 0.1877, 0.1877, 3.254;
69 b.resize(2);
70 b << -0.00662, -0.006711;
71 err = dart::lcpsolver::Lemke(A, b, f);
72
73 EXPECT_EQ(err, 0);
74 EXPECT_TRUE(dart::lcpsolver::validate(A, (*f), b));
75 }
76
77 //==============================================================================
78 TEST(Lemke, Lemke_4D)
79 {
80 Eigen::MatrixXd A;
81 Eigen::VectorXd b;
82 Eigen::VectorXd* f;
83 int err;
84
85 f = new Eigen::VectorXd(4);
86 A.resize(4, 4);
87 A << 3.999, 0.9985, 1.001, -2, 0.9985, 3.998, -2, 0.9995, 1.001, -2, 4.002,
88 1.001, -2, 0.9995, 1.001, 4.001;
89
90 b.resize(4);
91 b << -0.01008, -0.009494, -0.07234, -0.07177;
92
93 err = dart::lcpsolver::Lemke(A, b, f);
94
95 EXPECT_EQ(err, 0);
96 EXPECT_TRUE(dart::lcpsolver::validate(A, (*f), b));
97 }
98
99 //==============================================================================
100 TEST(Lemke, Lemke_6D)
101 {
102 Eigen::MatrixXd A;
103 Eigen::VectorXd b;
104 Eigen::VectorXd* f;
105 int err;
106
107 f = new Eigen::VectorXd(6);
108 A.resize(6, 6);
109 A << 3.1360, -2.0370, 0.9723, 0.1096, -2.0370, 0.9723, -2.0370, 3.7820,
110 0.8302, -0.0257, 2.4730, 0.0105, 0.9723, 0.8302, 5.1250, -2.2390, -1.9120,
111 3.4080, 0.1096, -0.0257, -2.2390, 3.1010, -0.0257, -2.2390, -2.0370,
112 2.4730, -1.9120, -0.0257, 5.4870, -0.0242, 0.9723, 0.0105, 3.4080,
113 -2.2390, -0.0242, 3.3860;
114
115 b.resize(6);
116 b << 0.1649, -0.0025, -0.0904, -0.0093, -0.0000, -0.0889;
117
118 err = dart::lcpsolver::Lemke(A, b, f);
119
120 EXPECT_EQ(err, 0);
121 EXPECT_TRUE(dart::lcpsolver::validate(A, (*f), b));
122 }
123
124 //==============================================================================
125 TEST(Lemke, Lemke_12D)
126 {
127 Eigen::MatrixXd A;
128 Eigen::VectorXd b;
129 Eigen::VectorXd* f;
130 int err;
131
132 f = new Eigen::VectorXd(12);
133 A.resize(12, 12);
134 A << 4.03, -1.014, -1.898, 1.03, -1.014, -1.898, 1, -1.014, -1.898, -2,
135 -1.014, -1.898, -1.014, 4.885, -1.259, 1.888, 3.81, 2.345, -1.879, 1.281,
136 -2.334, 1.022, 0.206, 1.27, -1.898, -1.259, 3.2, -1.032, -0.6849, 1.275,
137 1.003, 0.6657, 3.774, 1.869, 1.24, 1.85, 1.03, 1.888, -1.032, 4.03, 1.888,
138 -1.032, -2, 1.888, -1.032, 1, 1.888, -1.032, -1.014, 3.81, -0.6849, 1.888,
139 3.225, 1.275, -1.879, 1.85, -1.27, 1.022, 1.265, 0.6907, -1.898, 2.345,
140 1.275, -1.032, 1.275, 4.86, 1.003, -1.24, 0.2059, 1.869, -2.309, 3.791, 1,
141 -1.879, 1.003, -2, -1.879, 1.003, 3.97, -1.879, 1.003, 0.9703, -1.879,
142 1.003, -1.014, 1.281, 0.6657, 1.888, 1.85, -1.24, -1.879, 3.187, 1.234,
143 1.022, 3.755, -0.6714, -1.898, -2.334, 3.774, -1.032, -1.27, 0.2059,
144 1.003, 1.234, 4.839, 1.869, 2.299, 1.27, -2, 1.022, 1.869, 1, 1.022,
145 1.869, 0.9703, 1.022, 1.869, 3.97, 1.022, 1.869, -1.014, 0.206, 1.24,
146 1.888, 1.265, -2.309, -1.879, 3.755, 2.299, 1.022, 4.814, -1.25, -1.898,
147 1.27, 1.85, -1.032, 0.6907, 3.791, 1.003, -0.6714, 1.27, 1.869, -1.25,
148 3.212;
149
150 b.resize(12);
151 b << -0.00981, -1.458e-10, 5.357e-10, -0.0098, -1.44e-10, 5.298e-10,
152 -0.009807, -1.399e-10, 5.375e-10, -0.009807, -1.381e-10, 5.316e-10;
153
154 err = dart::lcpsolver::Lemke(A, b, f);
155
156 EXPECT_EQ(err, 0);
157 EXPECT_TRUE(dart::lcpsolver::validate(A, (*f), b));
158 }
159
160 //==============================================================================
161 int main(int argc, char* argv[])
162 {
163 ::testing::InitGoogleTest(&argc, argv);
164 return RUN_ALL_TESTS();
165 }