GCC Code Coverage Report


Directory: ./
File: include/coal/shape/geometric_shapes_utility.h
Date: 2025-04-01 09:23:31
Exec Total Coverage
Lines: 4 5 80.0%
Branches: 3 36 8.3%

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 ConvexBase& convex,
65 const Transform3s& tf);
66 COAL_DLLAPI std::vector<Vec3s> getBoundVertices(const TriangleP& triangle,
67 const Transform3s& tf);
68 } // namespace details
69 /// @endcond
70
71 /// @brief calculate a bounding volume for a shape in a specific configuration
72 template <typename BV, typename S>
73 456 inline void computeBV(const S& s, const Transform3s& tf, BV& bv) {
74
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 228 times.
456 if (s.getSweptSphereRadius() > 0) {
75 COAL_THROW_PRETTY("Swept-sphere radius not yet supported.",
76 std::runtime_error);
77 }
78
1/2
✓ Branch 1 taken 228 times.
✗ Branch 2 not taken.
456 std::vector<Vec3s> convex_bound_vertices = details::getBoundVertices(s, tf);
79
1/2
✓ Branch 3 taken 228 times.
✗ Branch 4 not taken.
456 fit(&convex_bound_vertices[0], (unsigned int)convex_bound_vertices.size(),
80 bv);
81 }
82
83 template <>
84 COAL_DLLAPI void computeBV<AABB, Box>(const Box& s, const Transform3s& tf,
85 AABB& bv);
86
87 template <>
88 COAL_DLLAPI void computeBV<AABB, Sphere>(const Sphere& s, const Transform3s& tf,
89 AABB& bv);
90
91 template <>
92 COAL_DLLAPI void computeBV<AABB, Ellipsoid>(const Ellipsoid& e,
93 const Transform3s& tf, AABB& bv);
94
95 template <>
96 COAL_DLLAPI void computeBV<AABB, Capsule>(const Capsule& s,
97 const Transform3s& tf, AABB& bv);
98
99 template <>
100 COAL_DLLAPI void computeBV<AABB, Cone>(const Cone& s, const Transform3s& tf,
101 AABB& bv);
102
103 template <>
104 COAL_DLLAPI void computeBV<AABB, Cylinder>(const Cylinder& s,
105 const Transform3s& tf, AABB& bv);
106
107 template <>
108 COAL_DLLAPI void computeBV<AABB, ConvexBase>(const ConvexBase& s,
109 const Transform3s& tf, AABB& bv);
110
111 template <>
112 COAL_DLLAPI void computeBV<AABB, TriangleP>(const TriangleP& s,
113 const Transform3s& tf, AABB& bv);
114
115 template <>
116 COAL_DLLAPI void computeBV<AABB, Halfspace>(const Halfspace& s,
117 const Transform3s& tf, AABB& bv);
118
119 template <>
120 COAL_DLLAPI void computeBV<AABB, Plane>(const Plane& s, const Transform3s& tf,
121 AABB& bv);
122
123 template <>
124 COAL_DLLAPI void computeBV<OBB, Box>(const Box& s, const Transform3s& tf,
125 OBB& bv);
126
127 template <>
128 COAL_DLLAPI void computeBV<OBB, Sphere>(const Sphere& s, const Transform3s& tf,
129 OBB& bv);
130
131 template <>
132 COAL_DLLAPI void computeBV<OBB, Capsule>(const Capsule& s,
133 const Transform3s& tf, OBB& bv);
134
135 template <>
136 COAL_DLLAPI void computeBV<OBB, Cone>(const Cone& s, const Transform3s& tf,
137 OBB& bv);
138
139 template <>
140 COAL_DLLAPI void computeBV<OBB, Cylinder>(const Cylinder& s,
141 const Transform3s& tf, OBB& bv);
142
143 template <>
144 COAL_DLLAPI void computeBV<OBB, ConvexBase>(const ConvexBase& s,
145 const Transform3s& tf, OBB& bv);
146
147 template <>
148 COAL_DLLAPI void computeBV<OBB, Halfspace>(const Halfspace& s,
149 const Transform3s& tf, OBB& bv);
150
151 template <>
152 COAL_DLLAPI void computeBV<RSS, Halfspace>(const Halfspace& s,
153 const Transform3s& tf, RSS& bv);
154
155 template <>
156 COAL_DLLAPI void computeBV<OBBRSS, Halfspace>(const Halfspace& s,
157 const Transform3s& tf,
158 OBBRSS& bv);
159
160 template <>
161 COAL_DLLAPI void computeBV<kIOS, Halfspace>(const Halfspace& s,
162 const Transform3s& tf, kIOS& bv);
163
164 template <>
165 COAL_DLLAPI void computeBV<KDOP<16>, Halfspace>(const Halfspace& s,
166 const Transform3s& tf,
167 KDOP<16>& bv);
168
169 template <>
170 COAL_DLLAPI void computeBV<KDOP<18>, Halfspace>(const Halfspace& s,
171 const Transform3s& tf,
172 KDOP<18>& bv);
173
174 template <>
175 COAL_DLLAPI void computeBV<KDOP<24>, Halfspace>(const Halfspace& s,
176 const Transform3s& tf,
177 KDOP<24>& bv);
178
179 template <>
180 COAL_DLLAPI void computeBV<OBB, Plane>(const Plane& s, const Transform3s& tf,
181 OBB& bv);
182
183 template <>
184 COAL_DLLAPI void computeBV<RSS, Plane>(const Plane& s, const Transform3s& tf,
185 RSS& bv);
186
187 template <>
188 COAL_DLLAPI void computeBV<OBBRSS, Plane>(const Plane& s, const Transform3s& tf,
189 OBBRSS& bv);
190
191 template <>
192 COAL_DLLAPI void computeBV<kIOS, Plane>(const Plane& s, const Transform3s& tf,
193 kIOS& bv);
194
195 template <>
196 COAL_DLLAPI void computeBV<KDOP<16>, Plane>(const Plane& s,
197 const Transform3s& tf,
198 KDOP<16>& bv);
199
200 template <>
201 COAL_DLLAPI void computeBV<KDOP<18>, Plane>(const Plane& s,
202 const Transform3s& tf,
203 KDOP<18>& bv);
204
205 template <>
206 COAL_DLLAPI void computeBV<KDOP<24>, Plane>(const Plane& s,
207 const Transform3s& tf,
208 KDOP<24>& bv);
209
210 /// @brief construct a box shape (with a configuration) from a given bounding
211 /// volume
212 COAL_DLLAPI void constructBox(const AABB& bv, Box& box, Transform3s& tf);
213
214 COAL_DLLAPI void constructBox(const OBB& bv, Box& box, Transform3s& tf);
215
216 COAL_DLLAPI void constructBox(const OBBRSS& bv, Box& box, Transform3s& tf);
217
218 COAL_DLLAPI void constructBox(const kIOS& bv, Box& box, Transform3s& tf);
219
220 COAL_DLLAPI void constructBox(const RSS& bv, Box& box, Transform3s& tf);
221
222 COAL_DLLAPI void constructBox(const KDOP<16>& bv, Box& box, Transform3s& tf);
223
224 COAL_DLLAPI void constructBox(const KDOP<18>& bv, Box& box, Transform3s& tf);
225
226 COAL_DLLAPI void constructBox(const KDOP<24>& bv, Box& box, Transform3s& tf);
227
228 COAL_DLLAPI void constructBox(const AABB& bv, const Transform3s& tf_bv,
229 Box& box, Transform3s& tf);
230
231 COAL_DLLAPI void constructBox(const OBB& bv, const Transform3s& tf_bv, Box& box,
232 Transform3s& tf);
233
234 COAL_DLLAPI void constructBox(const OBBRSS& bv, const Transform3s& tf_bv,
235 Box& box, Transform3s& tf);
236
237 COAL_DLLAPI void constructBox(const kIOS& bv, const Transform3s& tf_bv,
238 Box& box, Transform3s& tf);
239
240 COAL_DLLAPI void constructBox(const RSS& bv, const Transform3s& tf_bv, Box& box,
241 Transform3s& tf);
242
243 COAL_DLLAPI void constructBox(const KDOP<16>& bv, const Transform3s& tf_bv,
244 Box& box, Transform3s& tf);
245
246 COAL_DLLAPI void constructBox(const KDOP<18>& bv, const Transform3s& tf_bv,
247 Box& box, Transform3s& tf);
248
249 COAL_DLLAPI void constructBox(const KDOP<24>& bv, const Transform3s& tf_bv,
250 Box& box, Transform3s& tf);
251
252 COAL_DLLAPI Halfspace transform(const Halfspace& a, const Transform3s& tf);
253
254 COAL_DLLAPI Plane transform(const Plane& a, const Transform3s& tf);
255
256 COAL_DLLAPI std::array<Halfspace, 2> transformToHalfspaces(
257 const Plane& a, const Transform3s& tf);
258
259 } // namespace coal
260
261 #endif
262