hpp-fcl: include/hpp/fcl/collision_object.h Source File

hpp-fcl 1.8.0 HPP fork of FCL -- The Flexible Collision Library

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
39#ifndef HPP_FCL_COLLISION_OBJECT_BASE_H
40#define HPP_FCL_COLLISION_OBJECT_BASE_H
41
42#include <limits>
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
49namespace hpp
50{
51namespace fcl
52{
53
55 enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_HFIELD, OT_COUNT};
56
58 enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_OBBRSS, BV_KDOP16, BV_KDOP18, BV_KDOP24,
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};
60
61
64
66 class HPP_FCL_DLLAPI CollisionGeometry
67{
68public:
69
70 CollisionGeometry()
71 : aabb_center(Vec3f::Constant((std::numeric_limits<FCL_REAL>::max)()))
72 , aabb_radius(-1)
73 , cost_density(1)
74 , threshold_occupied(1)
75 , threshold_free(0)
76 {
77 }
78
80 CollisionGeometry(const CollisionGeometry & other)
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)
86 {
87 }
88
89 virtual ~CollisionGeometry() {}
90
92 virtual CollisionGeometry* clone() const = 0;
93
95 bool operator==(const CollisionGeometry & other) const
96 {
97 return
98 cost_density == other.cost_density
99 && threshold_occupied == other.threshold_occupied
100 && threshold_free == other.threshold_free
101 && aabb_center == other.aabb_center
102 && aabb_radius == other.aabb_radius
103 && aabb_local == other.aabb_local
104// && user_data == other.user_data
105 ;
106 }
107
109 bool operator!=(const CollisionGeometry & other) const
110 {
111 return !(*this == other);
112 }
113
115 virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; }
116
118 virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
119
121 virtual void computeLocalAABB() = 0;
122
124 void* getUserData() const
125 {
126 return user_data;
127 }
128
130 void setUserData(void *data)
131 {
132 user_data = data;
133 }
134
136 inline bool isOccupied() const
137 { return cost_density >= threshold_occupied; }
138
140 inline bool isFree() const
141 { return cost_density <= threshold_free; }
142
144 bool isUncertain() const;
145
147 Vec3f aabb_center;
148
150 FCL_REAL aabb_radius;
151
153 AABB aabb_local;
154
156 void *user_data;
157
159 FCL_REAL cost_density;
160
162 FCL_REAL threshold_occupied;
163
165 FCL_REAL threshold_free;
166
168 virtual Vec3f computeCOM() const { return Vec3f::Zero(); }
169
171 virtual Matrix3f computeMomentofInertia() const { return Matrix3f::Constant(NAN); }
172
174 virtual FCL_REAL computeVolume() const { return 0; }
175
177 virtual Matrix3f computeMomentofInertiaRelatedToCOM() const
178 {
179 Matrix3f C = computeMomentofInertia();
180 Vec3f com = computeCOM();
181 FCL_REAL V = computeVolume();
182
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();
192 }
193
194 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
195
196};
197
199 class HPP_FCL_DLLAPI CollisionObject
200{
201public:
202 CollisionObject(const shared_ptr<CollisionGeometry> &cgeom_) :
203 cgeom(cgeom_), cgeom_const(cgeom_)
204 {
205 if (cgeom)
206 {
207 cgeom->computeLocalAABB();
208 computeAABB();
209 }
210 }
211
212 CollisionObject(const shared_ptr<CollisionGeometry> &cgeom_, const Transform3f& tf) :
213 cgeom(cgeom_), cgeom_const(cgeom_), t(tf)
214 {
215 cgeom->computeLocalAABB();
216 computeAABB();
217 }
218
219 CollisionObject(const shared_ptr<CollisionGeometry> &cgeom_, const Matrix3f& R, const Vec3f& T):
220 cgeom(cgeom_), cgeom_const(cgeom_), t(Transform3f(R, T))
221 {
222 cgeom->computeLocalAABB();
223 computeAABB();
224 }
225
226 ~CollisionObject()
227 {
228 }
229
231 OBJECT_TYPE getObjectType() const
232 {
233 return cgeom->getObjectType();
234 }
235
237 NODE_TYPE getNodeType() const
238 {
239 return cgeom->getNodeType();
240 }
241
243 inline const AABB& getAABB() const
244 {
245 return aabb;
246 }
247
249 inline void computeAABB()
250 {
251 if(t.getRotation().isIdentity())
252 {
253 aabb = translate(cgeom->aabb_local, t.getTranslation());
254 }
255 else
256 {
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;
261 }
262 }
263
265 void* getUserData() const
266 {
267 return user_data;
268 }
269
271 void setUserData(void *data)
272 {
273 user_data = data;
274 }
275
277 inline const Vec3f& getTranslation() const
278 {
279 return t.getTranslation();
280 }
281
283 inline const Matrix3f& getRotation() const
284 {
285 return t.getRotation();
286 }
287
289 inline const Transform3f& getTransform() const
290 {
291 return t;
292 }
293
295 void setRotation(const Matrix3f& R)
296 {
297 t.setRotation(R);
298 }
299
301 void setTranslation(const Vec3f& T)
302 {
303 t.setTranslation(T);
304 }
305
306
307
309 void setTransform(const Matrix3f& R, const Vec3f& T)
310 {
311 t.setTransform(R, T);
312 }
313
314
315
317 void setTransform(const Transform3f& tf)
318 {
319 t = tf;
320 }
321
323 bool isIdentityTransform() const
324 {
325 return t.isIdentity();
326 }
327
329 void setIdentityTransform()
330 {
331 t.setIdentity();
332 }
333
335 const shared_ptr<const CollisionGeometry>& collisionGeometry() const
336 {
337 return cgeom_const;
338 }
339
341 const shared_ptr<CollisionGeometry>& collisionGeometry()
342 {
343 return cgeom;
344 }
345
346protected:
347
348 shared_ptr<CollisionGeometry> cgeom;
349 shared_ptr<const CollisionGeometry> cgeom_const;
350
351 Transform3f t;
352
354 mutable AABB aabb;
355
357 void *user_data;
358};
359
360}
361
362} // namespace hpp
363
364#endif
AABB.h
hpp::fcl::AABB
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:56
hpp::fcl::CollisionGeometry
The geometry for the object for collision or distance computation.
Definition: collision_object.h:67
hpp::fcl::CollisionObject
the object for collision or distance computation, contains the geometry and the transform information
Definition: collision_object.h:200
hpp::fcl::Transform3f
Simple transform class used locally by InterpMotion.
Definition: transform.h:59
fwd.hh
hpp::fcl::translate
KDOP< N > translate(const KDOP< N > &bv, const Vec3f &t)
translate the KDOP BV
hpp::fcl::CollisionObject::getAABB
const AABB & getAABB() const
get the AABB in world space
Definition: collision_object.h:243
hpp::fcl::CollisionGeometry::computeCOM
virtual Vec3f computeCOM() const
compute center of mass
Definition: collision_object.h:168
hpp::fcl::CollisionObject::CollisionObject
CollisionObject(const shared_ptr< CollisionGeometry > &cgeom_)
Definition: collision_object.h:202
hpp::fcl::CollisionGeometry::isUncertain
bool isUncertain() const
whether the object has some uncertainty
hpp::fcl::CollisionObject::getTransform
const Transform3f & getTransform() const
get object's transform
Definition: collision_object.h:289
hpp::fcl::CollisionGeometry::cost_density
FCL_REAL cost_density
collision cost for unit volume
Definition: collision_object.h:159
hpp::fcl::CollisionGeometry::aabb_local
AABB aabb_local
AABB in local coordinate, used for tight AABB when only translation transform.
Definition: collision_object.h:153
hpp::fcl::CollisionObject::setUserData
void setUserData(void *data)
set user data in object
Definition: collision_object.h:271
hpp::fcl::CollisionGeometry::isOccupied
bool isOccupied() const
whether the object is completely occupied
Definition: collision_object.h:136
hpp::fcl::CollisionObject::isIdentityTransform
bool isIdentityTransform() const
whether the object is in local coordinate
Definition: collision_object.h:323
hpp::fcl::CollisionObject::collisionGeometry
const shared_ptr< CollisionGeometry > & collisionGeometry()
get geometry from the object instance
Definition: collision_object.h:341
hpp::fcl::CollisionObject::getUserData
void * getUserData() const
get user data in object
Definition: collision_object.h:265
hpp::fcl::CollisionGeometry::operator!=
bool operator!=(const CollisionGeometry &other) const
Difference operator.
Definition: collision_object.h:109
hpp::fcl::CollisionGeometry::threshold_occupied
FCL_REAL threshold_occupied
threshold for occupied ( >= is occupied)
Definition: collision_object.h:162
hpp::fcl::CollisionGeometry::aabb_radius
FCL_REAL aabb_radius
AABB radius.
Definition: collision_object.h:150
hpp::fcl::OT_GEOM
@ OT_GEOM
Definition: collision_object.h:55
hpp::fcl::OT_OCTREE
@ OT_OCTREE
Definition: collision_object.h:55
hpp::fcl::OT_HFIELD
@ OT_HFIELD
Definition: collision_object.h:55
hpp::fcl::OT_BVH
@ OT_BVH
Definition: collision_object.h:55
hpp::fcl::OT_COUNT
@ OT_COUNT
Definition: collision_object.h:55
hpp::fcl::OT_UNKNOWN
@ OT_UNKNOWN
Definition: collision_object.h:55
hpp::fcl::CollisionObject::setTransform
void setTransform(const Transform3f &tf)
set object's transform
Definition: collision_object.h:317
hpp::fcl::CollisionGeometry::getObjectType
virtual OBJECT_TYPE getObjectType() const
get the type of the object
Definition: collision_object.h:115
hpp::fcl::CollisionObject::CollisionObject
CollisionObject(const shared_ptr< CollisionGeometry > &cgeom_, const Matrix3f &R, const Vec3f &T)
Definition: collision_object.h:219
hpp::fcl::CollisionGeometry::user_data
void * user_data
pointer to user defined data specific to this object
Definition: collision_object.h:156
hpp::fcl::CollisionGeometry::threshold_free
FCL_REAL threshold_free
threshold for free (<= is free)
Definition: collision_object.h:165
hpp::fcl::CollisionGeometry::isFree
bool isFree() const
whether the object is completely free
Definition: collision_object.h:140
hpp::fcl::CollisionGeometry::operator==
bool operator==(const CollisionGeometry &other) const
Equality operator.
Definition: collision_object.h:95
hpp::fcl::CollisionGeometry::CollisionGeometry
CollisionGeometry(const CollisionGeometry &other)
Copy constructor.
Definition: collision_object.h:80
hpp::fcl::CollisionObject::setTranslation
void setTranslation(const Vec3f &T)
set object's translation
Definition: collision_object.h:301
hpp::fcl::BV_UNKNOWN
@ BV_UNKNOWN
Definition: collision_object.h:58
hpp::fcl::GEOM_TRIANGLE
@ GEOM_TRIANGLE
Definition: collision_object.h:59
hpp::fcl::GEOM_CAPSULE
@ GEOM_CAPSULE
Definition: collision_object.h:59
hpp::fcl::NODE_COUNT
@ NODE_COUNT
Definition: collision_object.h:59
hpp::fcl::BV_kIOS
@ BV_kIOS
Definition: collision_object.h:58
hpp::fcl::HF_AABB
@ HF_AABB
Definition: collision_object.h:59
hpp::fcl::HF_OBBRSS
@ HF_OBBRSS
Definition: collision_object.h:59
hpp::fcl::BV_KDOP18
@ BV_KDOP18
Definition: collision_object.h:58
hpp::fcl::GEOM_PLANE
@ GEOM_PLANE
Definition: collision_object.h:59
hpp::fcl::BV_AABB
@ BV_AABB
Definition: collision_object.h:58
hpp::fcl::BV_OBBRSS
@ BV_OBBRSS
Definition: collision_object.h:58
hpp::fcl::GEOM_HALFSPACE
@ GEOM_HALFSPACE
Definition: collision_object.h:59
hpp::fcl::BV_KDOP16
@ BV_KDOP16
Definition: collision_object.h:58
hpp::fcl::GEOM_BOX
@ GEOM_BOX
Definition: collision_object.h:59
hpp::fcl::BV_OBB
@ BV_OBB
Definition: collision_object.h:58
hpp::fcl::BV_KDOP24
@ BV_KDOP24
Definition: collision_object.h:58
hpp::fcl::GEOM_CONVEX
@ GEOM_CONVEX
Definition: collision_object.h:59
hpp::fcl::GEOM_CYLINDER
@ GEOM_CYLINDER
Definition: collision_object.h:59
hpp::fcl::GEOM_SPHERE
@ GEOM_SPHERE
Definition: collision_object.h:59
hpp::fcl::BV_RSS
@ BV_RSS
Definition: collision_object.h:58
hpp::fcl::GEOM_CONE
@ GEOM_CONE
Definition: collision_object.h:59
hpp::fcl::GEOM_OCTREE
@ GEOM_OCTREE
Definition: collision_object.h:59
hpp::fcl::CollisionObject::cgeom_const
shared_ptr< const CollisionGeometry > cgeom_const
Definition: collision_object.h:349
hpp::fcl::CollisionObject::setTransform
void setTransform(const Matrix3f &R, const Vec3f &T)
set object's transform
Definition: collision_object.h:309
hpp::fcl::CollisionObject::getRotation
const Matrix3f & getRotation() const
get matrix rotation of the object
Definition: collision_object.h:283
hpp::fcl::CollisionGeometry::computeMomentofInertia
virtual Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: collision_object.h:171
hpp::fcl::CollisionObject::user_data
void * user_data
pointer to user defined data specific to this object
Definition: collision_object.h:357
hpp::fcl::CollisionGeometry::getUserData
void * getUserData() const
get user data in geometry
Definition: collision_object.h:124
hpp::fcl::CollisionGeometry::~CollisionGeometry
virtual ~CollisionGeometry()
Definition: collision_object.h:89
hpp::fcl::CollisionGeometry::computeMomentofInertiaRelatedToCOM
virtual Matrix3f computeMomentofInertiaRelatedToCOM() const
compute the inertia matrix, related to the com
Definition: collision_object.h:177
hpp::fcl::CollisionObject::computeAABB
void computeAABB()
compute the AABB in world space
Definition: collision_object.h:249
hpp::fcl::CollisionObject::CollisionObject
CollisionObject(const shared_ptr< CollisionGeometry > &cgeom_, const Transform3f &tf)
Definition: collision_object.h:212
hpp::fcl::CollisionGeometry::computeVolume
virtual FCL_REAL computeVolume() const
compute the volume
Definition: collision_object.h:174
hpp::fcl::CollisionObject::getObjectType
OBJECT_TYPE getObjectType() const
get the type of the object
Definition: collision_object.h:231
hpp::fcl::CollisionObject::setIdentityTransform
void setIdentityTransform()
set the object in local coordinate
Definition: collision_object.h:329
hpp::fcl::CollisionGeometry::aabb_center
Vec3f aabb_center
AABB center in local coordinate.
Definition: collision_object.h:147
hpp::fcl::CollisionObject::getTranslation
const Vec3f & getTranslation() const
get translation of the object
Definition: collision_object.h:277
hpp::fcl::CollisionObject::getNodeType
NODE_TYPE getNodeType() const
get the node type
Definition: collision_object.h:237
hpp::fcl::CollisionGeometry::setUserData
void setUserData(void *data)
set user data in geometry
Definition: collision_object.h:130
hpp::fcl::CollisionGeometry::CollisionGeometry
CollisionGeometry()
Definition: collision_object.h:70
hpp::fcl::CollisionGeometry::computeLocalAABB
virtual void computeLocalAABB()=0
compute the AABB for object in local coordinate
hpp::fcl::CollisionObject::setRotation
void setRotation(const Matrix3f &R)
set object's rotation matrix
Definition: collision_object.h:295
hpp::fcl::CollisionGeometry::clone
virtual CollisionGeometry * clone() const =0
Clone *this into a new CollisionGeometry.
hpp::fcl::CollisionObject::cgeom
shared_ptr< CollisionGeometry > cgeom
Definition: collision_object.h:348
hpp::fcl::CollisionObject::collisionGeometry
const shared_ptr< const CollisionGeometry > & collisionGeometry() const
get geometry from the object instance
Definition: collision_object.h:335
hpp::fcl::CollisionObject::~CollisionObject
~CollisionObject()
Definition: collision_object.h:226
hpp::fcl::CollisionObject::aabb
AABB aabb
AABB in global coordinate.
Definition: collision_object.h:354
hpp::fcl::CollisionGeometry::getNodeType
virtual NODE_TYPE getNodeType() const
get the node type
Definition: collision_object.h:118
hpp::fcl::CollisionObject::t
Transform3f t
Definition: collision_object.h:351
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:69
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
hpp::fcl::OBJECT_TYPE
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:55
hpp::fcl::NODE_TYPE
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_object.h:58
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:66
hpp
Main namespace.
Definition: AABB.h:44
transform.h