GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
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 HPP_FCL_COLLISION_OBJECT_BASE_H |
||
39 |
#define HPP_FCL_COLLISION_OBJECT_BASE_H |
||
40 |
|||
41 |
#include <limits> |
||
42 |
#include <typeinfo> |
||
43 |
|||
44 |
#include <hpp/fcl/deprecated.hh> |
||
45 |
#include <hpp/fcl/fwd.hh> |
||
46 |
#include <hpp/fcl/BV/AABB.h> |
||
47 |
#include <hpp/fcl/math/transform.h> |
||
48 |
|||
49 |
namespace hpp { |
||
50 |
namespace fcl { |
||
51 |
|||
52 |
/// @brief object type: BVH (mesh, points), basic geometry, octree |
||
53 |
enum OBJECT_TYPE { |
||
54 |
OT_UNKNOWN, |
||
55 |
OT_BVH, |
||
56 |
OT_GEOM, |
||
57 |
OT_OCTREE, |
||
58 |
OT_HFIELD, |
||
59 |
OT_COUNT |
||
60 |
}; |
||
61 |
|||
62 |
/// @brief traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, |
||
63 |
/// KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone, |
||
64 |
/// cylinder, convex, plane, triangle), and octree |
||
65 |
enum NODE_TYPE { |
||
66 |
BV_UNKNOWN, |
||
67 |
BV_AABB, |
||
68 |
BV_OBB, |
||
69 |
BV_RSS, |
||
70 |
BV_kIOS, |
||
71 |
BV_OBBRSS, |
||
72 |
BV_KDOP16, |
||
73 |
BV_KDOP18, |
||
74 |
BV_KDOP24, |
||
75 |
GEOM_BOX, |
||
76 |
GEOM_SPHERE, |
||
77 |
GEOM_CAPSULE, |
||
78 |
GEOM_CONE, |
||
79 |
GEOM_CYLINDER, |
||
80 |
GEOM_CONVEX, |
||
81 |
GEOM_PLANE, |
||
82 |
GEOM_HALFSPACE, |
||
83 |
GEOM_TRIANGLE, |
||
84 |
GEOM_OCTREE, |
||
85 |
GEOM_ELLIPSOID, |
||
86 |
HF_AABB, |
||
87 |
HF_OBBRSS, |
||
88 |
NODE_COUNT |
||
89 |
}; |
||
90 |
|||
91 |
/// @addtogroup Construction_Of_BVH |
||
92 |
/// @{ |
||
93 |
|||
94 |
/// @brief The geometry for the object for collision or distance computation |
||
95 |
class HPP_FCL_DLLAPI CollisionGeometry { |
||
96 |
public: |
||
97 |
116890211 |
CollisionGeometry() |
|
98 |
✓✗ | 116890211 |
: aabb_center(Vec3f::Constant((std::numeric_limits<FCL_REAL>::max)())), |
99 |
aabb_radius(-1), |
||
100 |
cost_density(1), |
||
101 |
threshold_occupied(1), |
||
102 |
✓✗ | 233780422 |
threshold_free(0) {} |
103 |
|||
104 |
/// \brief Copy constructor |
||
105 |
84 |
CollisionGeometry(const CollisionGeometry& other) = default; |
|
106 |
|||
107 |
233780318 |
virtual ~CollisionGeometry() {} |
|
108 |
|||
109 |
/// @brief Clone *this into a new CollisionGeometry |
||
110 |
virtual CollisionGeometry* clone() const = 0; |
||
111 |
|||
112 |
/// \brief Equality operator |
||
113 |
88 |
bool operator==(const CollisionGeometry& other) const { |
|
114 |
176 |
return cost_density == other.cost_density && |
|
115 |
✓✗ | 88 |
threshold_occupied == other.threshold_occupied && |
116 |
✓✗ | 88 |
threshold_free == other.threshold_free && |
117 |
✓✗ | 88 |
aabb_center == other.aabb_center && |
118 |
✓✗✓✗ ✓✗ |
264 |
aabb_radius == other.aabb_radius && aabb_local == other.aabb_local && |
119 |
✓✓ | 176 |
isEqual(other); |
120 |
} |
||
121 |
|||
122 |
/// \brief Difference operator |
||
123 |
1 |
bool operator!=(const CollisionGeometry& other) const { |
|
124 |
1 |
return isNotEqual(other); |
|
125 |
} |
||
126 |
|||
127 |
/// @brief get the type of the object |
||
128 |
virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; } |
||
129 |
|||
130 |
/// @brief get the node type |
||
131 |
virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; } |
||
132 |
|||
133 |
/// @brief compute the AABB for object in local coordinate |
||
134 |
virtual void computeLocalAABB() = 0; |
||
135 |
|||
136 |
/// @brief get user data in geometry |
||
137 |
void* getUserData() const { return user_data; } |
||
138 |
|||
139 |
/// @brief set user data in geometry |
||
140 |
void setUserData(void* data) { user_data = data; } |
||
141 |
|||
142 |
/// @brief whether the object is completely occupied |
||
143 |
28982 |
inline bool isOccupied() const { return cost_density >= threshold_occupied; } |
|
144 |
|||
145 |
/// @brief whether the object is completely free |
||
146 |
inline bool isFree() const { return cost_density <= threshold_free; } |
||
147 |
|||
148 |
/// @brief whether the object has some uncertainty |
||
149 |
bool isUncertain() const; |
||
150 |
|||
151 |
/// @brief AABB center in local coordinate |
||
152 |
Vec3f aabb_center; |
||
153 |
|||
154 |
/// @brief AABB radius |
||
155 |
FCL_REAL aabb_radius; |
||
156 |
|||
157 |
/// @brief AABB in local coordinate, used for tight AABB when only translation |
||
158 |
/// transform |
||
159 |
AABB aabb_local; |
||
160 |
|||
161 |
/// @brief pointer to user defined data specific to this object |
||
162 |
void* user_data; |
||
163 |
|||
164 |
/// @brief collision cost for unit volume |
||
165 |
FCL_REAL cost_density; |
||
166 |
|||
167 |
/// @brief threshold for occupied ( >= is occupied) |
||
168 |
FCL_REAL threshold_occupied; |
||
169 |
|||
170 |
/// @brief threshold for free (<= is free) |
||
171 |
FCL_REAL threshold_free; |
||
172 |
|||
173 |
/// @brief compute center of mass |
||
174 |
✓✗ | 16 |
virtual Vec3f computeCOM() const { return Vec3f::Zero(); } |
175 |
|||
176 |
/// @brief compute the inertia matrix, related to the origin |
||
177 |
virtual Matrix3f computeMomentofInertia() const { |
||
178 |
return Matrix3f::Constant(NAN); |
||
179 |
} |
||
180 |
|||
181 |
/// @brief compute the volume |
||
182 |
virtual FCL_REAL computeVolume() const { return 0; } |
||
183 |
|||
184 |
/// @brief compute the inertia matrix, related to the com |
||
185 |
5 |
virtual Matrix3f computeMomentofInertiaRelatedToCOM() const { |
|
186 |
✓✗ | 5 |
Matrix3f C = computeMomentofInertia(); |
187 |
✓✗ | 5 |
Vec3f com = computeCOM(); |
188 |
✓✗ | 5 |
FCL_REAL V = computeVolume(); |
189 |
|||
190 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗ |
10 |
return (Matrix3f() << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]), |
191 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
5 |
C(0, 1) + V * com[0] * com[1], C(0, 2) + V * com[0] * com[2], |
192 |
✓✗✓✗ ✓✗✓✗ |
5 |
C(1, 0) + V * com[1] * com[0], |
193 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
5 |
C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]), |
194 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
5 |
C(1, 2) + V * com[1] * com[2], C(2, 0) + V * com[2] * com[0], |
195 |
✓✗✓✗ ✓✗✓✗ |
5 |
C(2, 1) + V * com[2] * com[1], |
196 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
10 |
C(2, 2) - V * (com[0] * com[0] + com[1] * com[1])) |
197 |
✓✗✓✗ |
10 |
.finished(); |
198 |
} |
||
199 |
|||
200 |
private: |
||
201 |
/// @brief equal operator with another object of derived type. |
||
202 |
virtual bool isEqual(const CollisionGeometry& other) const = 0; |
||
203 |
|||
204 |
/// @brief not equal operator with another object of derived type. |
||
205 |
1 |
virtual bool isNotEqual(const CollisionGeometry& other) const { |
|
206 |
1 |
return !(*this == other); |
|
207 |
} |
||
208 |
|||
209 |
public: |
||
210 |
11316 |
EIGEN_MAKE_ALIGNED_OPERATOR_NEW |
|
211 |
}; |
||
212 |
|||
213 |
/// @brief the object for collision or distance computation, contains the |
||
214 |
/// geometry and the transform information |
||
215 |
class HPP_FCL_DLLAPI CollisionObject { |
||
216 |
public: |
||
217 |
6966 |
CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_, |
|
218 |
bool compute_local_aabb = true) |
||
219 |
✓✗✓✗ |
6966 |
: cgeom(cgeom_), user_data(nullptr) { |
220 |
✓✗ | 6966 |
init(compute_local_aabb); |
221 |
6966 |
} |
|
222 |
|||
223 |
38485 |
CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_, |
|
224 |
const Transform3f& tf, bool compute_local_aabb = true) |
||
225 |
✓✗✓✗ |
38485 |
: cgeom(cgeom_), t(tf), user_data(nullptr) { |
226 |
✓✗ | 38485 |
init(compute_local_aabb); |
227 |
38485 |
} |
|
228 |
|||
229 |
CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_, |
||
230 |
const Matrix3f& R, const Vec3f& T, |
||
231 |
bool compute_local_aabb = true) |
||
232 |
: cgeom(cgeom_), t(R, T), user_data(nullptr) { |
||
233 |
init(compute_local_aabb); |
||
234 |
} |
||
235 |
|||
236 |
bool operator==(const CollisionObject& other) const { |
||
237 |
return cgeom == other.cgeom && t == other.t && user_data == other.user_data; |
||
238 |
} |
||
239 |
|||
240 |
bool operator!=(const CollisionObject& other) const { |
||
241 |
return !(*this == other); |
||
242 |
} |
||
243 |
|||
244 |
45315 |
~CollisionObject() {} |
|
245 |
|||
246 |
/// @brief get the type of the object |
||
247 |
OBJECT_TYPE getObjectType() const { return cgeom->getObjectType(); } |
||
248 |
|||
249 |
/// @brief get the node type |
||
250 |
NODE_TYPE getNodeType() const { return cgeom->getNodeType(); } |
||
251 |
|||
252 |
/// @brief get the AABB in world space |
||
253 |
1229844 |
const AABB& getAABB() const { return aabb; } |
|
254 |
|||
255 |
/// @brief get the AABB in world space |
||
256 |
6795084 |
AABB& getAABB() { return aabb; } |
|
257 |
|||
258 |
/// @brief compute the AABB in world space |
||
259 |
39839 |
void computeAABB() { |
|
260 |
✓✗✓✓ |
39839 |
if (t.getRotation().isIdentity()) { |
261 |
✓✗ | 7525 |
aabb = translate(cgeom->aabb_local, t.getTranslation()); |
262 |
} else { |
||
263 |
✓✗ | 32314 |
Vec3f center(t.transform(cgeom->aabb_center)); |
264 |
✓✗✓✗ |
32314 |
Vec3f delta(Vec3f::Constant(cgeom->aabb_radius)); |
265 |
✓✗✓✗ |
32314 |
aabb.min_ = center - delta; |
266 |
✓✗✓✗ |
32314 |
aabb.max_ = center + delta; |
267 |
} |
||
268 |
39839 |
} |
|
269 |
|||
270 |
/// @brief get user data in object |
||
271 |
void* getUserData() const { return user_data; } |
||
272 |
|||
273 |
/// @brief set user data in object |
||
274 |
void setUserData(void* data) { user_data = data; } |
||
275 |
|||
276 |
/// @brief get translation of the object |
||
277 |
1344 |
inline const Vec3f& getTranslation() const { return t.getTranslation(); } |
|
278 |
|||
279 |
/// @brief get matrix rotation of the object |
||
280 |
1344 |
inline const Matrix3f& getRotation() const { return t.getRotation(); } |
|
281 |
|||
282 |
/// @brief get object's transform |
||
283 |
2503782 |
inline const Transform3f& getTransform() const { return t; } |
|
284 |
|||
285 |
/// @brief set object's rotation matrix |
||
286 |
void setRotation(const Matrix3f& R) { t.setRotation(R); } |
||
287 |
|||
288 |
/// @brief set object's translation |
||
289 |
2 |
void setTranslation(const Vec3f& T) { t.setTranslation(T); } |
|
290 |
|||
291 |
/// @brief set object's transform |
||
292 |
1344 |
void setTransform(const Matrix3f& R, const Vec3f& T) { t.setTransform(R, T); } |
|
293 |
|||
294 |
/// @brief set object's transform |
||
295 |
4 |
void setTransform(const Transform3f& tf) { t = tf; } |
|
296 |
|||
297 |
/// @brief whether the object is in local coordinate |
||
298 |
bool isIdentityTransform() const { return t.isIdentity(); } |
||
299 |
|||
300 |
/// @brief set the object in local coordinate |
||
301 |
void setIdentityTransform() { t.setIdentity(); } |
||
302 |
|||
303 |
/// @brief get shared pointer to collision geometry of the object instance |
||
304 |
const shared_ptr<const CollisionGeometry> collisionGeometry() const { |
||
305 |
return cgeom; |
||
306 |
} |
||
307 |
|||
308 |
/// @brief get shared pointer to collision geometry of the object instance |
||
309 |
32822 |
const shared_ptr<CollisionGeometry>& collisionGeometry() { return cgeom; } |
|
310 |
|||
311 |
/// @brief get raw pointer to collision geometry of the object instance |
||
312 |
2503782 |
const CollisionGeometry* collisionGeometryPtr() const { return cgeom.get(); } |
|
313 |
|||
314 |
/// @brief get raw pointer to collision geometry of the object instance |
||
315 |
CollisionGeometry* collisionGeometryPtr() { return cgeom.get(); } |
||
316 |
|||
317 |
/// @brief Associate a new CollisionGeometry |
||
318 |
/// |
||
319 |
/// @param[in] collision_geometry The new CollisionGeometry |
||
320 |
/// @param[in] compute_local_aabb Whether the local aabb of the input new has |
||
321 |
/// to be computed. |
||
322 |
/// |
||
323 |
void setCollisionGeometry( |
||
324 |
const shared_ptr<CollisionGeometry>& collision_geometry, |
||
325 |
bool compute_local_aabb = true) { |
||
326 |
if (collision_geometry.get() != cgeom.get()) { |
||
327 |
cgeom = collision_geometry; |
||
328 |
init(compute_local_aabb); |
||
329 |
} |
||
330 |
} |
||
331 |
|||
332 |
protected: |
||
333 |
45451 |
void init(bool compute_local_aabb = true) { |
|
334 |
✓✓ | 45451 |
if (cgeom) { |
335 |
✓✗ | 38493 |
if (compute_local_aabb) cgeom->computeLocalAABB(); |
336 |
38493 |
computeAABB(); |
|
337 |
} |
||
338 |
45451 |
} |
|
339 |
|||
340 |
shared_ptr<CollisionGeometry> cgeom; |
||
341 |
|||
342 |
Transform3f t; |
||
343 |
|||
344 |
/// @brief AABB in global coordinate |
||
345 |
mutable AABB aabb; |
||
346 |
|||
347 |
/// @brief pointer to user defined data specific to this object |
||
348 |
void* user_data; |
||
349 |
}; |
||
350 |
|||
351 |
} // namespace fcl |
||
352 |
|||
353 |
} // namespace hpp |
||
354 |
|||
355 |
#endif |
Generated by: GCOVR (Version 4.2) |