Directory: | ./ |
---|---|
File: | test/shape_inflation.cpp |
Date: | 2025-04-01 09:23:31 |
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 |