38 #ifndef HPP_FCL_COLLISION_OBJECT_BASE_H 39 #define HPP_FCL_COLLISION_OBJECT_BASE_H 98 : aabb_center(
Vec3f::Constant((std::numeric_limits<
FCL_REAL>::max)())),
101 threshold_occupied(1),
124 return isNotEqual(other);
134 virtual void computeLocalAABB() = 0;
143 inline bool isOccupied()
const {
return cost_density >= threshold_occupied; }
146 inline bool isFree()
const {
return cost_density <= threshold_free; }
149 bool isUncertain()
const;
178 return Matrix3f::Constant(NAN);
186 Matrix3f C = computeMomentofInertia();
187 Vec3f com = computeCOM();
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]))
206 return !(*
this == other);
210 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
218 bool compute_local_aabb =
true)
219 : cgeom(cgeom_), user_data(nullptr) {
220 init(compute_local_aabb);
224 const Transform3f& tf,
bool compute_local_aabb =
true)
225 : cgeom(cgeom_), t(tf), user_data(nullptr) {
226 init(compute_local_aabb);
231 bool compute_local_aabb =
true)
232 : cgeom(cgeom_), t(R, T), user_data(nullptr) {
233 init(compute_local_aabb);
237 return cgeom == other.
cgeom && t == other.
t && user_data == other.
user_data;
241 return !(*
this == other);
260 if (t.getRotation().isIdentity()) {
261 aabb =
translate(cgeom->aabb_local, t.getTranslation());
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;
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);
333 void init(
bool compute_local_aabb =
true) {
335 if (compute_local_aabb) cgeom->computeLocalAABB();
340 shared_ptr<CollisionGeometry>
cgeom;
Definition: collision_object.h:75
const Matrix3f & getRotation() const
get matrix rotation of the object
Definition: collision_object.h:280
FCL_REAL cost_density
collision cost for unit volume
Definition: collision_object.h:165
bool operator==(const CollisionGeometry &other) const
Equality operator.
Definition: collision_object.h:113
virtual Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: collision_object.h:177
~CollisionObject()
Definition: collision_object.h:244
Definition: collision_object.h:71
Definition: collision_object.h:78
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:53
CollisionGeometry()
Definition: collision_object.h:97
Main namespace.
Definition: broadphase_bruteforce.h:44
void setCollisionGeometry(const shared_ptr< CollisionGeometry > &collision_geometry, bool compute_local_aabb=true)
Associate a new CollisionGeometry.
Definition: collision_object.h:323
Definition: collision_object.h:66
CollisionObject(const shared_ptr< CollisionGeometry > &cgeom_, const Transform3f &tf, bool compute_local_aabb=true)
Definition: collision_object.h:223
NODE_TYPE getNodeType() const
get the node type
Definition: collision_object.h:250
void * user_data
pointer to user defined data specific to this object
Definition: collision_object.h:348
const shared_ptr< CollisionGeometry > & collisionGeometry()
get shared pointer to collision geometry of the object instance
Definition: collision_object.h:309
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
virtual Vec3f computeCOM() const
compute center of mass
Definition: collision_object.h:174
CollisionGeometry * collisionGeometryPtr()
get raw pointer to collision geometry of the object instance
Definition: collision_object.h:315
Definition: collision_object.h:84
Definition: collision_object.h:69
KDOP< N > translate(const KDOP< N > &bv, const Vec3f &t)
translate the KDOP BV
FCL_REAL threshold_occupied
threshold for occupied ( >= is occupied)
Definition: collision_object.h:168
Definition: collision_object.h:67
bool isOccupied() const
whether the object is completely occupied
Definition: collision_object.h:143
const Transform3f & getTransform() const
get object's transform
Definition: collision_object.h:283
bool isFree() const
whether the object is completely free
Definition: collision_object.h:146
void setIdentityTransform()
set the object in local coordinate
Definition: collision_object.h:301
Definition: collision_object.h:86
void setTransform(const Transform3f &tf)
set object's transform
Definition: collision_object.h:295
Definition: collision_object.h:82
virtual NODE_TYPE getNodeType() const
get the node type
Definition: collision_object.h:131
Definition: collision_object.h:56
AABB aabb_local
AABB in local coordinate, used for tight AABB when only translation transform.
Definition: collision_object.h:159
Definition: collision_object.h:59
void setTransform(const Matrix3f &R, const Vec3f &T)
set object's transform
Definition: collision_object.h:292
void setRotation(const Matrix3f &R)
set object's rotation matrix
Definition: collision_object.h:286
void * user_data
pointer to user defined data specific to this object
Definition: collision_object.h:162
double FCL_REAL
Definition: data_types.h:65
OBJECT_TYPE getObjectType() const
get the type of the object
Definition: collision_object.h:247
Definition: collision_object.h:58
Definition: collision_object.h:72
void setUserData(void *data)
set user data in geometry
Definition: collision_object.h:140
Definition: collision_object.h:83
Definition: collision_object.h:87
virtual OBJECT_TYPE getObjectType() const
get the type of the object
Definition: collision_object.h:128
Definition: collision_object.h:54
bool operator!=(const CollisionObject &other) const
Definition: collision_object.h:240
AABB aabb
AABB in global coordinate.
Definition: collision_object.h:345
void * getUserData() const
get user data in geometry
Definition: collision_object.h:137
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:54
Definition: collision_object.h:70
FCL_REAL aabb_radius
AABB radius.
Definition: collision_object.h:155
Definition: collision_object.h:57
void setUserData(void *data)
set user data in object
Definition: collision_object.h:274
const shared_ptr< const CollisionGeometry > collisionGeometry() const
get shared pointer to collision geometry of the object instance
Definition: collision_object.h:304
virtual Matrix3f computeMomentofInertiaRelatedToCOM() const
compute the inertia matrix, related to the com
Definition: collision_object.h:185
void init(bool compute_local_aabb=true)
Definition: collision_object.h:333
Definition: collision_object.h:73
Definition: collision_object.h:76
bool isIdentityTransform() const
whether the object is in local coordinate
Definition: collision_object.h:298
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, ellipsoid, capsule, cone, cylinder, convex, plane, triangle), and octree
Definition: collision_object.h:65
virtual FCL_REAL computeVolume() const
compute the volume
Definition: collision_object.h:182
void setTranslation(const Vec3f &T)
set object's translation
Definition: collision_object.h:289
FCL_REAL threshold_free
threshold for free (<= is free)
Definition: collision_object.h:171
CollisionObject(const shared_ptr< CollisionGeometry > &cgeom_, bool compute_local_aabb=true)
Definition: collision_object.h:217
Definition: collision_object.h:85
AABB & getAABB()
get the AABB in world space
Definition: collision_object.h:256
Vec3f aabb_center
AABB center in local coordinate.
Definition: collision_object.h:152
Definition: collision_object.h:79
const AABB & getAABB() const
get the AABB in world space
Definition: collision_object.h:253
const Vec3f & getTranslation() const
get translation of the object
Definition: collision_object.h:277
Definition: collision_object.h:55
Definition: collision_object.h:88
Transform3f t
Definition: collision_object.h:342
virtual ~CollisionGeometry()
Definition: collision_object.h:107
const CollisionGeometry * collisionGeometryPtr() const
get raw pointer to collision geometry of the object instance
Definition: collision_object.h:312
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
void computeAABB()
compute the AABB in world space
Definition: collision_object.h:259
the object for collision or distance computation, contains the geometry and the transform information...
Definition: collision_object.h:215
Definition: collision_object.h:68
void * getUserData() const
get user data in object
Definition: collision_object.h:271
The geometry for the object for collision or distance computation.
Definition: collision_object.h:95
Definition: collision_object.h:81
Definition: collision_object.h:80
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
shared_ptr< CollisionGeometry > cgeom
Definition: collision_object.h:340
bool operator==(const CollisionObject &other) const
Definition: collision_object.h:236
Definition: collision_object.h:77
bool operator!=(const CollisionGeometry &other) const
Difference operator.
Definition: collision_object.h:123
#define HPP_FCL_DLLAPI
Definition: config.hh:64
CollisionObject(const shared_ptr< CollisionGeometry > &cgeom_, const Matrix3f &R, const Vec3f &T, bool compute_local_aabb=true)
Definition: collision_object.h:229
Definition: collision_object.h:74