GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
Line | Branch | Exec | Source |
1 |
/* |
||
2 |
* Software License Agreement (BSD License) |
||
3 |
* |
||
4 |
* Copyright (c) 2022, INRIA |
||
5 |
* All rights reserved. |
||
6 |
* |
||
7 |
* Redistribution and use in source and binary forms, with or without |
||
8 |
* modification, are permitted provided that the following conditions |
||
9 |
* are met: |
||
10 |
* |
||
11 |
* * Redistributions of source code must retain the above copyright |
||
12 |
* notice, this list of conditions and the following disclaimer. |
||
13 |
* * Redistributions in binary form must reproduce the above |
||
14 |
* copyright notice, this list of conditions and the following |
||
15 |
* disclaimer in the documentation and/or other materials provided |
||
16 |
* with the distribution. |
||
17 |
* * Neither the name of Willow Garage, Inc. nor the names of its |
||
18 |
* contributors may be used to endorse or promote products derived |
||
19 |
* from this software without specific prior written permission. |
||
20 |
* |
||
21 |
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||
22 |
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||
23 |
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||
24 |
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||
25 |
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||
26 |
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||
27 |
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; |
||
28 |
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER |
||
29 |
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||
30 |
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||
31 |
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||
32 |
* POSSIBILITY OF SUCH DAMAGE. |
||
33 |
*/ |
||
34 |
|||
35 |
/** \author Louis Montaut */ |
||
36 |
|||
37 |
#include "hpp/fcl/data_types.h" |
||
38 |
#include <boost/test/tools/old/interface.hpp> |
||
39 |
#define BOOST_TEST_MODULE FCL_NESTEROV_GJK |
||
40 |
#include <boost/test/included/unit_test.hpp> |
||
41 |
|||
42 |
#include <Eigen/Geometry> |
||
43 |
#include <hpp/fcl/narrowphase/narrowphase.h> |
||
44 |
#include <hpp/fcl/shape/geometric_shapes.h> |
||
45 |
#include <hpp/fcl/internal/tools.h> |
||
46 |
|||
47 |
#include "utility.h" |
||
48 |
|||
49 |
using hpp::fcl::Box; |
||
50 |
using hpp::fcl::FCL_REAL; |
||
51 |
using hpp::fcl::GJKConvergenceCriterion; |
||
52 |
using hpp::fcl::GJKConvergenceCriterionType; |
||
53 |
using hpp::fcl::GJKSolver; |
||
54 |
using hpp::fcl::ShapeBase; |
||
55 |
using hpp::fcl::support_func_guess_t; |
||
56 |
using hpp::fcl::Transform3f; |
||
57 |
using hpp::fcl::Vec3f; |
||
58 |
using hpp::fcl::details::GJK; |
||
59 |
using hpp::fcl::details::MinkowskiDiff; |
||
60 |
using std::size_t; |
||
61 |
|||
62 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(set_cv_criterion) { |
63 |
✓✗ | 2 |
GJKSolver solver; |
64 |
✓✗ | 2 |
GJK gjk(128, 1e-6); |
65 |
|||
66 |
// Checking defaults |
||
67 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(solver.gjk_convergence_criterion == GJKConvergenceCriterion::VDB); |
68 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(solver.gjk_convergence_criterion_type == |
69 |
GJKConvergenceCriterionType::Relative); |
||
70 |
|||
71 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(gjk.convergence_criterion == GJKConvergenceCriterion::VDB); |
72 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(gjk.convergence_criterion_type == |
73 |
GJKConvergenceCriterionType::Relative); |
||
74 |
|||
75 |
// Checking set |
||
76 |
2 |
solver.gjk_convergence_criterion = GJKConvergenceCriterion::DualityGap; |
|
77 |
2 |
gjk.convergence_criterion = GJKConvergenceCriterion::DualityGap; |
|
78 |
2 |
solver.gjk_convergence_criterion_type = GJKConvergenceCriterionType::Absolute; |
|
79 |
2 |
gjk.convergence_criterion_type = GJKConvergenceCriterionType::Absolute; |
|
80 |
|||
81 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(solver.gjk_convergence_criterion == |
82 |
GJKConvergenceCriterion::DualityGap); |
||
83 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(solver.gjk_convergence_criterion_type == |
84 |
GJKConvergenceCriterionType::Absolute); |
||
85 |
|||
86 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(gjk.convergence_criterion == GJKConvergenceCriterion::DualityGap); |
87 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2 |
BOOST_CHECK(gjk.convergence_criterion_type == |
88 |
GJKConvergenceCriterionType::Absolute); |
||
89 |
2 |
} |
|
90 |
|||
91 |
2 |
void test_gjk_cv_criterion(const ShapeBase& shape0, const ShapeBase& shape1, |
|
92 |
const GJKConvergenceCriterionType cv_type) { |
||
93 |
// Solvers |
||
94 |
2 |
unsigned int max_iterations = 128; |
|
95 |
// by default GJK uses the VDB convergence criterion, which is relative. |
||
96 |
✓✗ | 2 |
GJK gjk1(max_iterations, 1e-6); |
97 |
|||
98 |
FCL_REAL tol; |
||
99 |
✓✓✗ | 2 |
switch (cv_type) { |
100 |
// need to lower the tolerance when absolute |
||
101 |
1 |
case GJKConvergenceCriterionType::Absolute: |
|
102 |
1 |
tol = 1e-8; |
|
103 |
1 |
break; |
|
104 |
1 |
case GJKConvergenceCriterionType::Relative: |
|
105 |
1 |
tol = 1e-6; |
|
106 |
1 |
break; |
|
107 |
default: |
||
108 |
throw std::logic_error("Wrong convergence criterion type"); |
||
109 |
} |
||
110 |
|||
111 |
✓✗ | 2 |
GJK gjk2(max_iterations, tol); |
112 |
2 |
gjk2.convergence_criterion = GJKConvergenceCriterion::DualityGap; |
|
113 |
2 |
gjk2.convergence_criterion_type = cv_type; |
|
114 |
|||
115 |
✓✗ | 2 |
GJK gjk3(max_iterations, tol); |
116 |
2 |
gjk3.convergence_criterion = GJKConvergenceCriterion::Hybrid; |
|
117 |
2 |
gjk3.convergence_criterion_type = cv_type; |
|
118 |
|||
119 |
// Minkowski difference |
||
120 |
✓✗ | 4 |
MinkowskiDiff mink_diff; |
121 |
|||
122 |
// Generate random transforms |
||
123 |
2 |
size_t n = 1000; |
|
124 |
2 |
FCL_REAL extents[] = {-3., -3., 0, 3., 3., 3.}; |
|
125 |
4 |
std::vector<Transform3f> transforms; |
|
126 |
✓✗ | 2 |
generateRandomTransforms(extents, transforms, n); |
127 |
✓✗ | 2 |
Transform3f identity = Transform3f::Identity(); |
128 |
|||
129 |
// Same init for both solvers |
||
130 |
✓✗ | 2 |
Vec3f init_guess = Vec3f(1, 0, 0); |
131 |
✓✗ | 2 |
support_func_guess_t init_support_guess; |
132 |
✓✗ | 2 |
init_support_guess.setZero(); |
133 |
|||
134 |
// Run for 3 different cv criterions |
||
135 |
✓✓ | 2002 |
for (size_t i = 0; i < n; ++i) { |
136 |
✓✗ | 2000 |
mink_diff.set(&shape0, &shape1, identity, transforms[i]); |
137 |
|||
138 |
✓✗ | 2000 |
GJK::Status res1 = gjk1.evaluate(mink_diff, init_guess, init_support_guess); |
139 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
BOOST_CHECK(gjk1.getIterations() <= max_iterations); |
140 |
✓✗ | 2000 |
Vec3f ray1 = gjk1.ray; |
141 |
✓✗ | 2000 |
res1 = gjk1.evaluate(mink_diff, init_guess, init_support_guess); |
142 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
BOOST_CHECK(res1 != GJK::Status::Failed); |
143 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
EIGEN_VECTOR_IS_APPROX(gjk1.ray, ray1, 1e-8); |
144 |
|||
145 |
✓✗ | 2000 |
GJK::Status res2 = gjk2.evaluate(mink_diff, init_guess, init_support_guess); |
146 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
BOOST_CHECK(gjk2.getIterations() <= max_iterations); |
147 |
✓✗ | 2000 |
Vec3f ray2 = gjk2.ray; |
148 |
✓✗ | 2000 |
res2 = gjk2.evaluate(mink_diff, init_guess, init_support_guess); |
149 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
BOOST_CHECK(res2 != GJK::Status::Failed); |
150 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
EIGEN_VECTOR_IS_APPROX(gjk2.ray, ray2, 1e-8); |
151 |
|||
152 |
✓✗ | 2000 |
GJK::Status res3 = gjk3.evaluate(mink_diff, init_guess, init_support_guess); |
153 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
BOOST_CHECK(gjk3.getIterations() <= max_iterations); |
154 |
✓✗ | 2000 |
Vec3f ray3 = gjk3.ray; |
155 |
✓✗ | 2000 |
res3 = gjk3.evaluate(mink_diff, init_guess, init_support_guess); |
156 |
✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
BOOST_CHECK(res3 != GJK::Status::Failed); |
157 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
EIGEN_VECTOR_IS_APPROX(gjk3.ray, ray3, 1e-8); |
158 |
|||
159 |
// check that solutions are close enough |
||
160 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
EIGEN_VECTOR_IS_APPROX(gjk1.ray, gjk2.ray, 1e-4); |
161 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✗✓ |
2000 |
EIGEN_VECTOR_IS_APPROX(gjk1.ray, gjk3.ray, 1e-4); |
162 |
} |
||
163 |
2 |
} |
|
164 |
|||
165 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
4 |
BOOST_AUTO_TEST_CASE(cv_criterion_same_solution) { |
166 |
✓✗ | 4 |
Box box0 = Box(0.1, 0.2, 0.3); |
167 |
✓✗ | 4 |
Box box1 = Box(1.1, 1.2, 1.3); |
168 |
✓✗ | 2 |
test_gjk_cv_criterion(box0, box1, GJKConvergenceCriterionType::Absolute); |
169 |
✓✗ | 2 |
test_gjk_cv_criterion(box0, box1, GJKConvergenceCriterionType::Relative); |
170 |
2 |
} |
Generated by: GCOVR (Version 4.2) |