GCC Code Coverage Report

Directory: ./
File: include/coal/shape/geometric_shapes_utility.h
Date: 2025-06-03 17:15:40
Exec Total Coverage
Lines: 5 13 38.5%
Functions: 7 51 13.7%
Branches: 3 42 7.1%
Line Branch Exec Source
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011-2014, Willow Garage, Inc.
5 * Copyright (c) 2014-2015, Open Source Robotics Foundation
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
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 * * Neither the name of Open Source Robotics Foundation nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 */
35
36 /** \author Jia Pan */
37
38 #ifndef COAL_GEOMETRIC_SHAPES_UTILITY_H
39 #define COAL_GEOMETRIC_SHAPES_UTILITY_H
40
41 #include <vector>
42 #include "coal/shape/geometric_shapes.h"
43 #include "coal/BV/BV.h"
44 #include "coal/internal/BV_fitter.h"
45
46 namespace coal {
47
48 /// @cond IGNORE
49 namespace details {
50 /// @brief get the vertices of some convex shape which can bound the given shape
51 /// in a specific configuration
52 COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const Box& box,
53 const Transform3s& tf);
54 COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const Sphere& sphere,
55 const Transform3s& tf);
56 COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const Ellipsoid& ellipsoid,
57 const Transform3s& tf);
58 COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const Capsule& capsule,
59 const Transform3s& tf);
60 COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const Cone& cone,
61 const Transform3s& tf);
62 COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const Cylinder& cylinder,
63 const Transform3s& tf);
64 COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const TriangleP& triangle,
65 const Transform3s& tf);
66 template <typename IndexType>
67 ✗ std::vector<Vec3s> getBoundVertices(const ConvexBaseTpl<IndexType>& convex,
68 const Transform3s& tf) {
69 ✗ std::vector<Vec3s> result(convex.num_points);
70 ✗ const std::vector<Vec3s>& points_ = *(convex.points);
71 ✗ for (std::size_t i = 0; i < convex.num_points; ++i) {
72 ✗ result[i] = tf.transform(points_[i]);
73 }
74
75 ✗ return result;
76 ✗ }
77 } // namespace details
78 /// @endcond
79
80 /// @brief calculate a bounding volume for a shape in a specific configuration
81 template <typename BV, typename S>
82 456 inline void computeBV(const S& s, const Transform3s& tf, BV& bv) {
83
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 228 times.
 456 if (s.getSweptSphereRadius() > 0) {
84 ✗ COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
85 std::runtime_error);
86 }
87
1/2
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
 456 std::vector<Vec3s> convex_bound_vertices = details::getBoundVertices(s, tf);
88
1/2
✓ Branch 3 taken 228 times.
✗ Branch 4 not taken.
 456 fit(&convex_bound_vertices[0], (unsigned int)convex_bound_vertices.size(),
89 bv);
90 456 }
91
92 template <>
93 COAL_DLLAPI void computeBV<AABB, Box>(const Box& s, const Transform3s& tf,
94 AABB& bv);
95
96 template <>
97 COAL_DLLAPI void computeBV<AABB, Sphere>(const Sphere& s, const Transform3s& tf,
98 AABB& bv);
99
100 template <>
101 COAL_DLLAPI void computeBV<AABB, Ellipsoid>(const Ellipsoid& e,
102 const Transform3s& tf, AABB& bv);
103
104 template <>
105 COAL_DLLAPI void computeBV<AABB, Capsule>(const Capsule& s,
106 const Transform3s& tf, AABB& bv);
107
108 template <>
109 COAL_DLLAPI void computeBV<AABB, Cone>(const Cone& s, const Transform3s& tf,
110 AABB& bv);
111
112 template <>
113 COAL_DLLAPI void computeBV<AABB, Cylinder>(const Cylinder& s,
114 const Transform3s& tf, AABB& bv);
115 template <>
116 COAL_DLLAPI void computeBV<AABB, ConvexBase32>(const ConvexBase32& s,
117 const Transform3s& tf, AABB& bv);
118
119 template <>
120 COAL_DLLAPI void computeBV<AABB, ConvexBase16>(const ConvexBase16& s,
121 const Transform3s& tf, AABB& bv);
122
123 template <>
124 COAL_DLLAPI void computeBV<AABB, TriangleP>(const TriangleP& s,
125 const Transform3s& tf, AABB& bv);
126
127 template <>
128 COAL_DLLAPI void computeBV<AABB, Halfspace>(const Halfspace& s,
129 const Transform3s& tf, AABB& bv);
130
131 template <>
132 COAL_DLLAPI void computeBV<AABB, Plane>(const Plane& s, const Transform3s& tf,
133 AABB& bv);
134
135 template <>
136 COAL_DLLAPI void computeBV<OBB, Box>(const Box& s, const Transform3s& tf,
137 OBB& bv);
138
139 template <>
140 COAL_DLLAPI void computeBV<OBB, Sphere>(const Sphere& s, const Transform3s& tf,
141 OBB& bv);
142
143 template <>
144 COAL_DLLAPI void computeBV<OBB, Capsule>(const Capsule& s,
145 const Transform3s& tf, OBB& bv);
146
147 template <>
148 COAL_DLLAPI void computeBV<OBB, Cone>(const Cone& s, const Transform3s& tf,
149 OBB& bv);
150
151 template <>
152 COAL_DLLAPI void computeBV<OBB, Cylinder>(const Cylinder& s,
153 const Transform3s& tf, OBB& bv);
154
155 template <>
156 COAL_DLLAPI void computeBV<OBB, ConvexBase32>(const ConvexBase32& s,
157 const Transform3s& tf, OBB& bv);
158
159 template <>
160 COAL_DLLAPI void computeBV<OBB, ConvexBase16>(const ConvexBase16& s,
161 const Transform3s& tf, OBB& bv);
162
163 template <>
164 COAL_DLLAPI void computeBV<OBB, Halfspace>(const Halfspace& s,
165 const Transform3s& tf, OBB& bv);
166
167 template <>
168 COAL_DLLAPI void computeBV<RSS, Halfspace>(const Halfspace& s,
169 const Transform3s& tf, RSS& bv);
170
171 template <>
172 COAL_DLLAPI void computeBV<OBBRSS, Halfspace>(const Halfspace& s,
173 const Transform3s& tf,
174 OBBRSS& bv);
175
176 template <>
177 COAL_DLLAPI void computeBV<kIOS, Halfspace>(const Halfspace& s,
178 const Transform3s& tf, kIOS& bv);
179
180 template <>
181 COAL_DLLAPI void computeBV<KDOP<16>, Halfspace>(const Halfspace& s,
182 const Transform3s& tf,
183 KDOP<16>& bv);
184
185 template <>
186 COAL_DLLAPI void computeBV<KDOP<18>, Halfspace>(const Halfspace& s,
187 const Transform3s& tf,
188 KDOP<18>& bv);
189
190 template <>
191 COAL_DLLAPI void computeBV<KDOP<24>, Halfspace>(const Halfspace& s,
192 const Transform3s& tf,
193 KDOP<24>& bv);
194
195 template <>
196 COAL_DLLAPI void computeBV<OBB, Plane>(const Plane& s, const Transform3s& tf,
197 OBB& bv);
198
199 template <>
200 COAL_DLLAPI void computeBV<RSS, Plane>(const Plane& s, const Transform3s& tf,
201 RSS& bv);
202
203 template <>
204 COAL_DLLAPI void computeBV<OBBRSS, Plane>(const Plane& s, const Transform3s& tf,
205 OBBRSS& bv);
206
207 template <>
208 COAL_DLLAPI void computeBV<kIOS, Plane>(const Plane& s, const Transform3s& tf,
209 kIOS& bv);
210
211 template <>
212 COAL_DLLAPI void computeBV<KDOP<16>, Plane>(const Plane& s,
213 const Transform3s& tf,
214 KDOP<16>& bv);
215
216 template <>
217 COAL_DLLAPI void computeBV<KDOP<18>, Plane>(const Plane& s,
218 const Transform3s& tf,
219 KDOP<18>& bv);
220
221 template <>
222 COAL_DLLAPI void computeBV<KDOP<24>, Plane>(const Plane& s,
223 const Transform3s& tf,
224 KDOP<24>& bv);
225
226 /// @brief construct a box shape (with a configuration) from a given bounding
227 /// volume
228 COAL_DLLAPI void constructBox(const AABB& bv, Box& box, Transform3s& tf);
229
230 COAL_DLLAPI void constructBox(const OBB& bv, Box& box, Transform3s& tf);
231
232 COAL_DLLAPI void constructBox(const OBBRSS& bv, Box& box, Transform3s& tf);
233
234 COAL_DLLAPI void constructBox(const kIOS& bv, Box& box, Transform3s& tf);
235
236 COAL_DLLAPI void constructBox(const RSS& bv, Box& box, Transform3s& tf);
237
238 COAL_DLLAPI void constructBox(const KDOP<16>& bv, Box& box, Transform3s& tf);
239
240 COAL_DLLAPI void constructBox(const KDOP<18>& bv, Box& box, Transform3s& tf);
241
242 COAL_DLLAPI void constructBox(const KDOP<24>& bv, Box& box, Transform3s& tf);
243
244 COAL_DLLAPI void constructBox(const AABB& bv, const Transform3s& tf_bv,
245 Box& box, Transform3s& tf);
246
247 COAL_DLLAPI void constructBox(const OBB& bv, const Transform3s& tf_bv, Box& box,
248 Transform3s& tf);
249
250 COAL_DLLAPI void constructBox(const OBBRSS& bv, const Transform3s& tf_bv,
251 Box& box, Transform3s& tf);
252
253 COAL_DLLAPI void constructBox(const kIOS& bv, const Transform3s& tf_bv,
254 Box& box, Transform3s& tf);
255
256 COAL_DLLAPI void constructBox(const RSS& bv, const Transform3s& tf_bv, Box& box,
257 Transform3s& tf);
258
259 COAL_DLLAPI void constructBox(const KDOP<16>& bv, const Transform3s& tf_bv,
260 Box& box, Transform3s& tf);
261
262 COAL_DLLAPI void constructBox(const KDOP<18>& bv, const Transform3s& tf_bv,
263 Box& box, Transform3s& tf);
264
265 COAL_DLLAPI void constructBox(const KDOP<24>& bv, const Transform3s& tf_bv,
266 Box& box, Transform3s& tf);
267
268 COAL_DLLAPI Halfspace transform(const Halfspace& a, const Transform3s& tf);
269
270 COAL_DLLAPI Plane transform(const Plane& a, const Transform3s& tf);
271
272 COAL_DLLAPI std::array<Halfspace, 2> transformToHalfspaces(
273 const Plane& a, const Transform3s& tf);
274
275 } // namespace coal
276
277 #endif
278