39#ifndef HPP_FCL_COLLISION_OBJECT_BASE_H
40#define HPP_FCL_COLLISION_OBJECT_BASE_H
59 GEOM_BOX,
GEOM_SPHERE,
GEOM_CAPSULE,
GEOM_CONE,
GEOM_CYLINDER,
GEOM_CONVEX,
GEOM_PLANE,
GEOM_HALFSPACE,
GEOM_TRIANGLE,
GEOM_OCTREE,
HF_AABB,
HF_OBBRSS,
NODE_COUNT};
71 : aabb_center(
Vec3f::Constant((std::numeric_limits<
FCL_REAL>::max)()))
74 , threshold_occupied(1)
81 : aabb_center(other.aabb_center)
82 , aabb_radius(other.aabb_radius)
83 , cost_density(other.cost_density)
84 , threshold_occupied(other.threshold_occupied)
85 , threshold_free(other.threshold_free)
111 return !(*
this == other);
137 {
return cost_density >= threshold_occupied; }
141 {
return cost_density <= threshold_free; }
179 Matrix3f C = computeMomentofInertia();
180 Vec3f com = computeCOM();
183 return (
Matrix3f() << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]),
184 C(0, 1) + V * com[0] * com[1],
185 C(0, 2) + V * com[0] * com[2],
186 C(1, 0) + V * com[1] * com[0],
187 C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]),
188 C(1, 2) + V * com[1] * com[2],
189 C(2, 0) + V * com[2] * com[0],
190 C(2, 1) + V * com[2] * com[1],
191 C(2, 2) - V * (com[0] * com[0] + com[1] * com[1])).finished();
194 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
203 cgeom(cgeom_), cgeom_const(cgeom_)
207 cgeom->computeLocalAABB();
213 cgeom(cgeom_), cgeom_const(cgeom_), t(tf)
215 cgeom->computeLocalAABB();
220 cgeom(cgeom_), cgeom_const(cgeom_), t(
Transform3f(R, T))
222 cgeom->computeLocalAABB();
233 return cgeom->getObjectType();
239 return cgeom->getNodeType();
251 if(t.getRotation().isIdentity())
253 aabb =
translate(cgeom->aabb_local, t.getTranslation());
257 Vec3f center (t.transform(cgeom->aabb_center));
258 Vec3f delta(Vec3f::Constant(cgeom->aabb_radius));
259 aabb.min_ = center - delta;
260 aabb.max_ = center + delta;
279 return t.getTranslation();
285 return t.getRotation();
311 t.setTransform(R, T);
325 return t.isIdentity();
348 shared_ptr<CollisionGeometry>
cgeom;
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:56
The geometry for the object for collision or distance computation.
Definition: collision_object.h:67
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:200
#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:243
virtual Vec3f computeCOM() const
compute center of mass
Definition: collision_object.h:168
CollisionObject(const shared_ptr< CollisionGeometry > &cgeom_)
Definition: collision_object.h:202
bool isUncertain() const
whether the object has some uncertainty
const Transform3f & getTransform() const
get object's transform
Definition: collision_object.h:289
FCL_REAL cost_density
collision cost for unit volume
Definition: collision_object.h:159
AABB aabb_local
AABB in local coordinate, used for tight AABB when only translation transform.
Definition: collision_object.h:153
void setUserData(void *data)
set user data in object
Definition: collision_object.h:271
bool isOccupied() const
whether the object is completely occupied
Definition: collision_object.h:136
bool isIdentityTransform() const
whether the object is in local coordinate
Definition: collision_object.h:323
const shared_ptr< CollisionGeometry > & collisionGeometry()
get geometry from the object instance
Definition: collision_object.h:341
void * getUserData() const
get user data in object
Definition: collision_object.h:265
bool operator!=(const CollisionGeometry &other) const
Difference operator.
Definition: collision_object.h:109
FCL_REAL threshold_occupied
threshold for occupied ( >= is occupied)
Definition: collision_object.h:162
FCL_REAL aabb_radius
AABB radius.
Definition: collision_object.h:150
@ OT_GEOM
Definition: collision_object.h:55
@ OT_OCTREE
Definition: collision_object.h:55
@ OT_HFIELD
Definition: collision_object.h:55
@ OT_BVH
Definition: collision_object.h:55
@ OT_COUNT
Definition: collision_object.h:55
@ OT_UNKNOWN
Definition: collision_object.h:55
void setTransform(const Transform3f &tf)
set object's transform
Definition: collision_object.h:317
virtual OBJECT_TYPE getObjectType() const
get the type of the object
Definition: collision_object.h:115
CollisionObject(const shared_ptr< CollisionGeometry > &cgeom_, const Matrix3f &R, const Vec3f &T)
Definition: collision_object.h:219
void * user_data
pointer to user defined data specific to this object
Definition: collision_object.h:156
FCL_REAL threshold_free
threshold for free (<= is free)
Definition: collision_object.h:165
bool isFree() const
whether the object is completely free
Definition: collision_object.h:140
bool operator==(const CollisionGeometry &other) const
Equality operator.
Definition: collision_object.h:95
CollisionGeometry(const CollisionGeometry &other)
Copy constructor.
Definition: collision_object.h:80
void setTranslation(const Vec3f &T)
set object's translation
Definition: collision_object.h:301
@ BV_UNKNOWN
Definition: collision_object.h:58
@ GEOM_TRIANGLE
Definition: collision_object.h:59
@ GEOM_CAPSULE
Definition: collision_object.h:59
@ NODE_COUNT
Definition: collision_object.h:59
@ BV_kIOS
Definition: collision_object.h:58
@ HF_AABB
Definition: collision_object.h:59
@ HF_OBBRSS
Definition: collision_object.h:59
@ BV_KDOP18
Definition: collision_object.h:58
@ GEOM_PLANE
Definition: collision_object.h:59
@ BV_AABB
Definition: collision_object.h:58
@ BV_OBBRSS
Definition: collision_object.h:58
@ GEOM_HALFSPACE
Definition: collision_object.h:59
@ BV_KDOP16
Definition: collision_object.h:58
@ GEOM_BOX
Definition: collision_object.h:59
@ BV_OBB
Definition: collision_object.h:58
@ BV_KDOP24
Definition: collision_object.h:58
@ GEOM_CONVEX
Definition: collision_object.h:59
@ GEOM_CYLINDER
Definition: collision_object.h:59
@ GEOM_SPHERE
Definition: collision_object.h:59
@ BV_RSS
Definition: collision_object.h:58
@ GEOM_CONE
Definition: collision_object.h:59
@ GEOM_OCTREE
Definition: collision_object.h:59
shared_ptr< const CollisionGeometry > cgeom_const
Definition: collision_object.h:349
void setTransform(const Matrix3f &R, const Vec3f &T)
set object's transform
Definition: collision_object.h:309
const Matrix3f & getRotation() const
get matrix rotation of the object
Definition: collision_object.h:283
virtual Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: collision_object.h:171
void * user_data
pointer to user defined data specific to this object
Definition: collision_object.h:357
void * getUserData() const
get user data in geometry
Definition: collision_object.h:124
virtual ~CollisionGeometry()
Definition: collision_object.h:89
virtual Matrix3f computeMomentofInertiaRelatedToCOM() const
compute the inertia matrix, related to the com
Definition: collision_object.h:177
void computeAABB()
compute the AABB in world space
Definition: collision_object.h:249
CollisionObject(const shared_ptr< CollisionGeometry > &cgeom_, const Transform3f &tf)
Definition: collision_object.h:212
virtual FCL_REAL computeVolume() const
compute the volume
Definition: collision_object.h:174
OBJECT_TYPE getObjectType() const
get the type of the object
Definition: collision_object.h:231
void setIdentityTransform()
set the object in local coordinate
Definition: collision_object.h:329
Vec3f aabb_center
AABB center in local coordinate.
Definition: collision_object.h:147
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:237
void setUserData(void *data)
set user data in geometry
Definition: collision_object.h:130
CollisionGeometry()
Definition: collision_object.h:70
virtual void computeLocalAABB()=0
compute the AABB for object in local coordinate
void setRotation(const Matrix3f &R)
set object's rotation matrix
Definition: collision_object.h:295
virtual CollisionGeometry * clone() const =0
Clone *this into a new CollisionGeometry.
shared_ptr< CollisionGeometry > cgeom
Definition: collision_object.h:348
const shared_ptr< const CollisionGeometry > & collisionGeometry() const
get geometry from the object instance
Definition: collision_object.h:335
~CollisionObject()
Definition: collision_object.h:226
AABB aabb
AABB in global coordinate.
Definition: collision_object.h:354
virtual NODE_TYPE getNodeType() const
get the node type
Definition: collision_object.h:118
Transform3f t
Definition: collision_object.h:351
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:69
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:55
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_object.h:58
double FCL_REAL
Definition: data_types.h:66
Main namespace.
Definition: AABB.h:44