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 |