hpp-fcl 2.3.0
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
collision_object.h
Go to the documentation of this file.
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
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>
48
49namespace hpp {
50namespace fcl {
51
60};
61
89};
90
93
96 public:
98 : aabb_center(Vec3f::Constant((std::numeric_limits<FCL_REAL>::max)())),
99 aabb_radius(-1),
100 cost_density(1),
101 threshold_occupied(1),
102 threshold_free(0) {}
103
105 CollisionGeometry(const CollisionGeometry& other) = default;
106
108
110 virtual CollisionGeometry* clone() const = 0;
111
113 bool operator==(const CollisionGeometry& other) const {
114 return cost_density == other.cost_density &&
115 threshold_occupied == other.threshold_occupied &&
116 threshold_free == other.threshold_free &&
117 aabb_center == other.aabb_center &&
118 aabb_radius == other.aabb_radius && aabb_local == other.aabb_local &&
119 isEqual(other);
120 }
121
123 bool operator!=(const CollisionGeometry& other) const {
124 return isNotEqual(other);
125 }
126
128 virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; }
129
131 virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
132
134 virtual void computeLocalAABB() = 0;
135
137 void* getUserData() const { return user_data; }
138
140 void setUserData(void* data) { user_data = data; }
141
143 inline bool isOccupied() const { return cost_density >= threshold_occupied; }
144
146 inline bool isFree() const { return cost_density <= threshold_free; }
147
149 bool isUncertain() const;
150
153
156
160
163
166
169
172
174 virtual Vec3f computeCOM() const { return Vec3f::Zero(); }
175
178 return Matrix3f::Constant(NAN);
179 }
180
182 virtual FCL_REAL computeVolume() const { return 0; }
183
186 Matrix3f C = computeMomentofInertia();
187 Vec3f com = computeCOM();
188 FCL_REAL V = computeVolume();
189
190 return (Matrix3f() << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]),
191 C(0, 1) + V * com[0] * com[1], C(0, 2) + V * com[0] * com[2],
192 C(1, 0) + V * com[1] * com[0],
193 C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]),
194 C(1, 2) + V * com[1] * com[2], C(2, 0) + V * com[2] * com[0],
195 C(2, 1) + V * com[2] * com[1],
196 C(2, 2) - V * (com[0] * com[0] + com[1] * com[1]))
197 .finished();
198 }
199
200 private:
202 virtual bool isEqual(const CollisionGeometry& other) const = 0;
203
205 virtual bool isNotEqual(const CollisionGeometry& other) const {
206 return !(*this == other);
207 }
208
209 public:
210 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
211};
212
216 public:
217 CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_,
218 bool compute_local_aabb = true)
219 : cgeom(cgeom_), user_data(nullptr) {
220 init(compute_local_aabb);
221 }
222
223 CollisionObject(const shared_ptr<CollisionGeometry>& cgeom_,
224 const Transform3f& tf, bool compute_local_aabb = true)
225 : cgeom(cgeom_), t(tf), user_data(nullptr) {
226 init(compute_local_aabb);
227 }
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
245
247 OBJECT_TYPE getObjectType() const { return cgeom->getObjectType(); }
248
250 NODE_TYPE getNodeType() const { return cgeom->getNodeType(); }
251
253 const AABB& getAABB() const { return aabb; }
254
256 AABB& getAABB() { return aabb; }
257
259 void computeAABB() {
260 if (t.getRotation().isIdentity()) {
261 aabb = translate(cgeom->aabb_local, t.getTranslation());
262 } else {
263 Vec3f center(t.transform(cgeom->aabb_center));
264 Vec3f delta(Vec3f::Constant(cgeom->aabb_radius));
265 aabb.min_ = center - delta;
266 aabb.max_ = center + delta;
267 }
268 }
269
271 void* getUserData() const { return user_data; }
272
274 void setUserData(void* data) { user_data = data; }
275
277 inline const Vec3f& getTranslation() const { return t.getTranslation(); }
278
280 inline const Matrix3f& getRotation() const { return t.getRotation(); }
281
283 inline const Transform3f& getTransform() const { return t; }
284
286 void setRotation(const Matrix3f& R) { t.setRotation(R); }
287
289 void setTranslation(const Vec3f& T) { t.setTranslation(T); }
290
292 void setTransform(const Matrix3f& R, const Vec3f& T) { t.setTransform(R, T); }
293
295 void setTransform(const Transform3f& tf) { t = tf; }
296
298 bool isIdentityTransform() const { return t.isIdentity(); }
299
301 void setIdentityTransform() { t.setIdentity(); }
302
304 const shared_ptr<const CollisionGeometry> collisionGeometry() const {
305 return cgeom;
306 }
307
309 const shared_ptr<CollisionGeometry>& collisionGeometry() { return cgeom; }
310
318 const shared_ptr<CollisionGeometry>& collision_geometry,
319 bool compute_local_aabb = true) {
320 if (collision_geometry.get() != cgeom.get()) {
321 cgeom = collision_geometry;
322 init(compute_local_aabb);
323 }
324 }
325
326 protected:
327 void init(bool compute_local_aabb = true) {
328 if (cgeom) {
329 if (compute_local_aabb) cgeom->computeLocalAABB();
330 computeAABB();
331 }
332 }
333
334 shared_ptr<CollisionGeometry> cgeom;
335
337
339 mutable AABB aabb;
340
343};
344
345} // namespace fcl
346
347} // namespace hpp
348
349#endif
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:54
The geometry for the object for collision or distance computation.
Definition: collision_object.h:95
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:215
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
#define HPP_FCL_DLLAPI
Definition: config.hh:64
KDOP< N > translate(const KDOP< N > &bv, const Vec3f &t)
translate the KDOP BV
const AABB & getAABB() const
get the AABB in world space
Definition: collision_object.h:253
virtual Vec3f computeCOM() const
compute center of mass
Definition: collision_object.h:174
const shared_ptr< const CollisionGeometry > collisionGeometry() const
get geometry from the object instance
Definition: collision_object.h:304
CollisionObject(const shared_ptr< CollisionGeometry > &cgeom_, const Matrix3f &R, const Vec3f &T, bool compute_local_aabb=true)
Definition: collision_object.h:229
bool isUncertain() const
whether the object has some uncertainty
const Transform3f & getTransform() const
get object's transform
Definition: collision_object.h:283
CollisionGeometry(const CollisionGeometry &other)=default
Copy constructor.
void setCollisionGeometry(const shared_ptr< CollisionGeometry > &collision_geometry, bool compute_local_aabb=true)
Associate a new CollisionGeometry.
Definition: collision_object.h:317
FCL_REAL cost_density
collision cost for unit volume
Definition: collision_object.h:165
AABB aabb_local
AABB in local coordinate, used for tight AABB when only translation transform.
Definition: collision_object.h:159
void setUserData(void *data)
set user data in object
Definition: collision_object.h:274
bool isOccupied() const
whether the object is completely occupied
Definition: collision_object.h:143
bool isIdentityTransform() const
whether the object is in local coordinate
Definition: collision_object.h:298
const shared_ptr< CollisionGeometry > & collisionGeometry()
get geometry from the object instance
Definition: collision_object.h:309
void * getUserData() const
get user data in object
Definition: collision_object.h:271
bool operator!=(const CollisionGeometry &other) const
Difference operator.
Definition: collision_object.h:123
FCL_REAL threshold_occupied
threshold for occupied ( >= is occupied)
Definition: collision_object.h:168
FCL_REAL aabb_radius
AABB radius.
Definition: collision_object.h:155
@ OT_GEOM
Definition: collision_object.h:56
@ OT_OCTREE
Definition: collision_object.h:57
@ OT_HFIELD
Definition: collision_object.h:58
@ OT_BVH
Definition: collision_object.h:55
@ OT_COUNT
Definition: collision_object.h:59
@ OT_UNKNOWN
Definition: collision_object.h:54
void setTransform(const Transform3f &tf)
set object's transform
Definition: collision_object.h:295
virtual OBJECT_TYPE getObjectType() const
get the type of the object
Definition: collision_object.h:128
void * user_data
pointer to user defined data specific to this object
Definition: collision_object.h:162
FCL_REAL threshold_free
threshold for free (<= is free)
Definition: collision_object.h:171
bool isFree() const
whether the object is completely free
Definition: collision_object.h:146
bool operator==(const CollisionGeometry &other) const
Equality operator.
Definition: collision_object.h:113
void setTranslation(const Vec3f &T)
set object's translation
Definition: collision_object.h:289
@ BV_UNKNOWN
Definition: collision_object.h:66
@ GEOM_TRIANGLE
Definition: collision_object.h:83
@ GEOM_CAPSULE
Definition: collision_object.h:77
@ NODE_COUNT
Definition: collision_object.h:88
@ BV_kIOS
Definition: collision_object.h:70
@ HF_AABB
Definition: collision_object.h:86
@ HF_OBBRSS
Definition: collision_object.h:87
@ BV_KDOP18
Definition: collision_object.h:73
@ GEOM_PLANE
Definition: collision_object.h:81
@ BV_AABB
Definition: collision_object.h:67
@ BV_OBBRSS
Definition: collision_object.h:71
@ GEOM_HALFSPACE
Definition: collision_object.h:82
@ BV_KDOP16
Definition: collision_object.h:72
@ GEOM_ELLIPSOID
Definition: collision_object.h:85
@ GEOM_BOX
Definition: collision_object.h:75
@ BV_OBB
Definition: collision_object.h:68
@ BV_KDOP24
Definition: collision_object.h:74
@ GEOM_CONVEX
Definition: collision_object.h:80
@ GEOM_CYLINDER
Definition: collision_object.h:79
@ GEOM_SPHERE
Definition: collision_object.h:76
@ BV_RSS
Definition: collision_object.h:69
@ GEOM_CONE
Definition: collision_object.h:78
@ GEOM_OCTREE
Definition: collision_object.h:84
void init(bool compute_local_aabb=true)
Definition: collision_object.h:327
void setTransform(const Matrix3f &R, const Vec3f &T)
set object's transform
Definition: collision_object.h:292
const Matrix3f & getRotation() const
get matrix rotation of the object
Definition: collision_object.h:280
virtual Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: collision_object.h:177
void * user_data
pointer to user defined data specific to this object
Definition: collision_object.h:342
void * getUserData() const
get user data in geometry
Definition: collision_object.h:137
virtual ~CollisionGeometry()
Definition: collision_object.h:107
bool operator==(const CollisionObject &other) const
Definition: collision_object.h:236
virtual Matrix3f computeMomentofInertiaRelatedToCOM() const
compute the inertia matrix, related to the com
Definition: collision_object.h:185
void computeAABB()
compute the AABB in world space
Definition: collision_object.h:259
AABB & getAABB()
get the AABB in world space
Definition: collision_object.h:256
virtual FCL_REAL computeVolume() const
compute the volume
Definition: collision_object.h:182
OBJECT_TYPE getObjectType() const
get the type of the object
Definition: collision_object.h:247
void setIdentityTransform()
set the object in local coordinate
Definition: collision_object.h:301
Vec3f aabb_center
AABB center in local coordinate.
Definition: collision_object.h:152
const Vec3f & getTranslation() const
get translation of the object
Definition: collision_object.h:277
NODE_TYPE getNodeType() const
get the node type
Definition: collision_object.h:250
CollisionObject(const shared_ptr< CollisionGeometry > &cgeom_, const Transform3f &tf, bool compute_local_aabb=true)
Definition: collision_object.h:223
void setUserData(void *data)
set user data in geometry
Definition: collision_object.h:140
CollisionGeometry()
Definition: collision_object.h:97
virtual void computeLocalAABB()=0
compute the AABB for object in local coordinate
CollisionObject(const shared_ptr< CollisionGeometry > &cgeom_, bool compute_local_aabb=true)
Definition: collision_object.h:217
bool operator!=(const CollisionObject &other) const
Definition: collision_object.h:240
void setRotation(const Matrix3f &R)
set object's rotation matrix
Definition: collision_object.h:286
virtual CollisionGeometry * clone() const =0
Clone *this into a new CollisionGeometry.
shared_ptr< CollisionGeometry > cgeom
Definition: collision_object.h:334
~CollisionObject()
Definition: collision_object.h:244
AABB aabb
AABB in global coordinate.
Definition: collision_object.h:339
virtual NODE_TYPE getNodeType() const
get the node type
Definition: collision_object.h:131
Transform3f t
Definition: collision_object.h:336
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:53
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_object.h:65
bool isEqual(const Eigen::MatrixBase< Derived > &lhs, const Eigen::MatrixBase< OtherDerived > &rhs, const FCL_REAL tol=std::numeric_limits< FCL_REAL >::epsilon() *100)
Definition: tools.h:209
double FCL_REAL
Definition: data_types.h:65
Main namespace.
Definition: broadphase_bruteforce.h:44