GCC Code Coverage Report

Directory: ./
File: test/shape_inflation.cpp
Date: 2025-04-13 14:25:19
Exec Total Coverage
Lines: 81 84 96.4%
Branches: 155 332 46.7%
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 #define BOOST_TEST_MODULE COAL_SECURITY_MARGIN
36 #include <boost/test/included/unit_test.hpp>
37
38 #include <cmath>
39 #include <iostream>
40 #include "coal/distance.h"
41 #include "coal/math/transform.h"
42 #include "coal/collision.h"
43 #include "coal/collision_object.h"
44 #include "coal/shape/geometric_shapes.h"
45 #include "coal/shape/geometric_shapes_utility.h"
46
47 #include "utility.h"
48
49 using namespace coal;
50 using coal::CollisionGeometryPtr_t;
51 using coal::CollisionObject;
52 using coal::CollisionRequest;
53 using coal::CollisionResult;
54 using coal::DistanceRequest;
55 using coal::DistanceResult;
56 using coal::Transform3s;
57 using coal::Vec3s;
58
59 #define MATH_SQUARED(x) (x * x)
60
61 template <typename Shape>
62 bool isApprox(const Shape &s1, const Shape &s2, const Scalar tol);
63
64 34 bool isApprox(const Scalar &v1, const Scalar &v2, const Scalar tol) {
65 typedef Eigen::Matrix<Scalar, 1, 1> Matrix;
66
1/2
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
 34 Matrix m1;
67
1/2
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
 34 m1 << v1;
68
1/2
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
 34 Matrix m2;
69
1/2
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
 34 m2 << v2;
70
1/2
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
 68 return m1.isApprox(m2, tol);
71 }
72
73 5 bool isApprox(const Box &s1, const Box &s2, const Scalar tol) {
74 5 return s1.halfSide.isApprox(s2.halfSide, tol);
75 }
76
77 5 bool isApprox(const Sphere &s1, const Sphere &s2, const Scalar tol) {
78 5 return isApprox(s1.radius, s2.radius, tol);
79 }
80
81 5 bool isApprox(const Ellipsoid &s1, const Ellipsoid &s2, const Scalar tol) {
82 5 return s1.radii.isApprox(s2.radii, tol);
83 }
84
85 5 bool isApprox(const Capsule &s1, const Capsule &s2, const Scalar tol) {
86
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 2 times.
 8 return isApprox(s1.radius, s2.radius, tol) &&
87
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 8 isApprox(s1.halfLength, s2.halfLength, tol);
88 }
89
90 5 bool isApprox(const Cylinder &s1, const Cylinder &s2, const Scalar tol) {
91
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 2 times.
 8 return isApprox(s1.radius, s2.radius, tol) &&
92
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 8 isApprox(s1.halfLength, s2.halfLength, tol);
93 }
94
95 5 bool isApprox(const Cone &s1, const Cone &s2, const Scalar tol) {
96
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 2 times.
 8 return isApprox(s1.radius, s2.radius, tol) &&
97
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 8 isApprox(s1.halfLength, s2.halfLength, tol);
98 }
99
100 ✗ bool isApprox(const TriangleP &s1, const TriangleP &s2, const Scalar tol) {
101 ✗ return s1.a.isApprox(s2.a, tol) && s1.b.isApprox(s2.b, tol) &&
102 ✗ s1.c.isApprox(s2.c, tol);
103 }
104
105 5 bool isApprox(const Halfspace &s1, const Halfspace &s2, const Scalar tol) {
106
3/4
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 2 times.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
 5 return isApprox(s1.d, s2.d, tol) && s1.n.isApprox(s2.n, tol);
107 }
108
109 template <typename Shape>
110 14 void test(const Shape &original_shape, const Scalar inflation,
111 const Scalar tol = Scalar(1e-8)) {
112 // Zero inflation
113 {
114 14 const Scalar inflation = 0.;
115
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 14 const auto &inflation_result = original_shape.inflated(inflation);
116 14 const Transform3s &shift = inflation_result.second;
117 14 const Shape &inflated_shape = inflation_result.first;
118
119
7/14
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 7 times.
 14 BOOST_CHECK(isApprox(original_shape, inflated_shape, tol));
120
7/14
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 7 times.
 14 BOOST_CHECK(shift.isIdentity(tol));
121 14 }
122
123 // Positive inflation
124 {
125
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 14 const auto &inflation_result = original_shape.inflated(inflation);
126 14 const Shape &inflated_shape = inflation_result.first;
127 14 const Transform3s &inflation_shift = inflation_result.second;
128
129
7/14
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 7 times.
 14 BOOST_CHECK(!isApprox(original_shape, inflated_shape, tol));
130
131
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 14 const auto &deflation_result = inflated_shape.inflated(-inflation);
132 14 const Shape &deflated_shape = deflation_result.first;
133 14 const Transform3s &deflation_shift = deflation_result.second;
134
135
7/14
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 7 times.
 14 BOOST_CHECK(isApprox(original_shape, deflated_shape, tol));
136
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 14 BOOST_CHECK((inflation_shift * deflation_shift).isIdentity(tol));
137 14 }
138
139 // Negative inflation
140 {
141
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 14 const auto &inflation_result = original_shape.inflated(-inflation);
142 14 const Shape &inflated_shape = inflation_result.first;
143 14 const Transform3s &inflation_shift = inflation_result.second;
144
145
7/14
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 7 times.
 14 BOOST_CHECK(!isApprox(original_shape, inflated_shape, tol));
146
147
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 14 const auto &deflation_result = inflated_shape.inflated(+inflation);
148 14 const Shape &deflated_shape = deflation_result.first;
149 14 const Transform3s &deflation_shift = deflation_result.second;
150
151
7/14
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 7 times.
 14 BOOST_CHECK(isApprox(original_shape, deflated_shape, tol));
152
8/16
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 7 times.
✗ Branch 6 not taken.
✓ Branch 11 taken 7 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 7 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 7 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 7 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 7 times.
✗ Branch 24 not taken.
✗ Branch 28 not taken.
✓ Branch 29 taken 7 times.
 14 BOOST_CHECK((inflation_shift * deflation_shift).isIdentity(tol));
153 14 }
154 14 }
155
156 template <typename Shape>
157 12 void test_throw(const Shape &shape, const Scalar inflation) {
158
11/34
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✓ Branch 9 taken 6 times.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 21 not taken.
✗ Branch 22 not taken.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 35 not taken.
✓ Branch 36 taken 6 times.
✗ Branch 39 not taken.
✓ Branch 40 taken 6 times.
✓ Branch 43 taken 6 times.
✗ Branch 44 not taken.
✓ Branch 47 taken 6 times.
✗ Branch 48 not taken.
✓ Branch 52 taken 6 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 6 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 6 times.
✗ Branch 59 not taken.
✗ Branch 63 not taken.
✓ Branch 64 taken 6 times.
 24 BOOST_REQUIRE_THROW(shape.inflated(inflation), std::invalid_argument);
159 12 }
160
161 template <typename Shape>
162 1 void test_no_throw(const Shape &shape, const Scalar inflation) {
163
8/28
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 1 times.
✗ Branch 10 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 1 times.
✗ Branch 21 not taken.
✗ Branch 25 not taken.
✓ Branch 26 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 29 taken 1 times.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
✗ Branch 46 not taken.
✗ Branch 47 not taken.
✗ Branch 49 not taken.
✗ Branch 50 not taken.
✗ Branch 54 not taken.
✗ Branch 55 not taken.
 1 BOOST_REQUIRE_NO_THROW(shape.inflated(inflation));
164 1 }
165
166
33/66
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 23 taken 1 times.
✗ Branch 24 not taken.
✓ Branch 27 taken 1 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 1 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 1 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 1 times.
✗ Branch 37 not taken.
✓ Branch 41 taken 1 times.
✗ Branch 42 not taken.
✓ Branch 45 taken 1 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 1 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 1 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 1 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 1 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 1 times.
✗ Branch 62 not taken.
✓ Branch 66 taken 1 times.
✗ Branch 67 not taken.
✓ Branch 70 taken 1 times.
✗ Branch 71 not taken.
✓ Branch 73 taken 1 times.
✗ Branch 74 not taken.
✓ Branch 77 taken 1 times.
✗ Branch 78 not taken.
✓ Branch 80 taken 1 times.
✗ Branch 81 not taken.
✓ Branch 83 taken 1 times.
✗ Branch 84 not taken.
✓ Branch 86 taken 1 times.
✗ Branch 87 not taken.
✓ Branch 91 taken 1 times.
✗ Branch 92 not taken.
✓ Branch 95 taken 1 times.
✗ Branch 96 not taken.
✓ Branch 98 taken 1 times.
✗ Branch 99 not taken.
✓ Branch 102 taken 1 times.
✗ Branch 103 not taken.
✓ Branch 105 taken 1 times.
✗ Branch 106 not taken.
✓ Branch 108 taken 1 times.
✗ Branch 109 not taken.
✓ Branch 111 taken 1 times.
✗ Branch 112 not taken.
✓ Branch 116 taken 1 times.
✗ Branch 117 not taken.
 4 BOOST_AUTO_TEST_CASE(test_inflate) {
167
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 const coal::Sphere sphere(1);
168
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 test(sphere, Scalar(0.01), Scalar(1e-8));
169
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 test_throw(sphere, Scalar(-1.1));
170
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 test_no_throw(sphere, 1.);
171
172
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 const coal::Box box(1, 1, 1);
173
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 test(box, Scalar(0.01), Scalar(1e-8));
174
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 test_throw(box, Scalar(-0.6));
175
176
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 const coal::Ellipsoid ellipsoid(1, 2, 3);
177
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 test(ellipsoid, Scalar(0.01), Scalar(1e-8));
178
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 test_throw(ellipsoid, Scalar(-1.1));
179
180
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 const coal::Capsule capsule(1, 2);
181
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 test(capsule, Scalar(0.01), Scalar(1e-8));
182
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 test_throw(capsule, Scalar(-1.1));
183
184
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 const coal::Cylinder cylinder(1, 2);
185
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 test(cylinder, Scalar(0.01), Scalar(1e-8));
186
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 test_throw(cylinder, Scalar(-1.1));
187
188
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 const coal::Cone cone(1, 4);
189
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 test(cone, Scalar(0.01), Scalar(1e-8));
190
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 test_throw(cone, Scalar(-1.1));
191
192
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 2 const coal::Halfspace halfspace(Vec3s::UnitZ(), 0);
193
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 test(halfspace, Scalar(0.01), Scalar(1e-8));
194
195 // const coal::TriangleP triangle(Vec3s::UnitX(), Vec3s::UnitY(),
196 // Vec3s::UnitZ());
197 // test(triangle, 0.01, 1e-8);
198 2 }
199