hpp-fcl  1.8.1
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 
49 namespace hpp
50 {
51 namespace fcl
52 {
53 
56 
60 
61 
64 
67 {
68 public:
69 
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 
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 
148 
151 
154 
156  void *user_data;
157 
160 
163 
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 
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 
200 {
201 public:
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 
227  {
228  }
229 
232  {
233  return cgeom->getObjectType();
234  }
235 
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 
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 
346 protected:
347 
348  shared_ptr<CollisionGeometry> cgeom;
349  shared_ptr<const CollisionGeometry> cgeom_const;
350 
352 
354  mutable AABB aabb;
355 
357  void *user_data;
358 };
359 
360 }
361 
362 } // namespace hpp
363 
364 #endif
Definition: collision_object.h:59
const Matrix3f & getRotation() const
get matrix rotation of the object
Definition: collision_object.h:283
Simple transform class used locally by InterpMotion.
Definition: transform.h:58
FCL_REAL cost_density
collision cost for unit volume
Definition: collision_object.h:159
bool operator==(const CollisionGeometry &other) const
Equality operator.
Definition: collision_object.h:95
virtual Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: collision_object.h:171
~CollisionObject()
Definition: collision_object.h:226
Definition: collision_object.h:58
Definition: collision_object.h:59
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:55
CollisionGeometry()
Definition: collision_object.h:70
Main namespace.
Definition: AABB.h:43
Definition: collision_object.h:58
NODE_TYPE getNodeType() const
get the node type
Definition: collision_object.h:237
void * user_data
pointer to user defined data specific to this object
Definition: collision_object.h:357
const shared_ptr< CollisionGeometry > & collisionGeometry()
get geometry from the object instance
Definition: collision_object.h:341
CollisionGeometry(const CollisionGeometry &other)
Copy constructor.
Definition: collision_object.h:80
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:69
virtual Vec3f computeCOM() const
compute center of mass
Definition: collision_object.h:168
CollisionObject(const shared_ptr< CollisionGeometry > &cgeom_, const Transform3f &tf)
Definition: collision_object.h:212
Definition: collision_object.h:59
Definition: collision_object.h:58
KDOP< N > translate(const KDOP< N > &bv, const Vec3f &t)
translate the KDOP BV
const shared_ptr< const CollisionGeometry > & collisionGeometry() const
get geometry from the object instance
Definition: collision_object.h:335
FCL_REAL threshold_occupied
threshold for occupied ( >= is occupied)
Definition: collision_object.h:162
Definition: collision_object.h:58
bool isOccupied() const
whether the object is completely occupied
Definition: collision_object.h:136
const Transform3f & getTransform() const
get object&#39;s transform
Definition: collision_object.h:289
bool isFree() const
whether the object is completely free
Definition: collision_object.h:140
void setIdentityTransform()
set the object in local coordinate
Definition: collision_object.h:329
Definition: collision_object.h:59
void setTransform(const Transform3f &tf)
set object&#39;s transform
Definition: collision_object.h:317
Definition: collision_object.h:59
virtual NODE_TYPE getNodeType() const
get the node type
Definition: collision_object.h:118
Definition: collision_object.h:55
AABB aabb_local
AABB in local coordinate, used for tight AABB when only translation transform.
Definition: collision_object.h:153
Definition: collision_object.h:55
void setTransform(const Matrix3f &R, const Vec3f &T)
set object&#39;s transform
Definition: collision_object.h:309
void setRotation(const Matrix3f &R)
set object&#39;s rotation matrix
Definition: collision_object.h:295
void * user_data
pointer to user defined data specific to this object
Definition: collision_object.h:156
double FCL_REAL
Definition: data_types.h:66
OBJECT_TYPE getObjectType() const
get the type of the object
Definition: collision_object.h:231
Definition: collision_object.h:55
Definition: collision_object.h:58
void setUserData(void *data)
set user data in geometry
Definition: collision_object.h:130
Definition: collision_object.h:59
Definition: collision_object.h:59
virtual OBJECT_TYPE getObjectType() const
get the type of the object
Definition: collision_object.h:115
Definition: collision_object.h:55
AABB aabb
AABB in global coordinate.
Definition: collision_object.h:354
void * getUserData() const
get user data in geometry
Definition: collision_object.h:124
CollisionObject(const shared_ptr< CollisionGeometry > &cgeom_, const Matrix3f &R, const Vec3f &T)
Definition: collision_object.h:219
A class describing the AABB collision structure, which is a box in 3D space determined by two diagona...
Definition: AABB.h:55
Definition: collision_object.h:58
FCL_REAL aabb_radius
AABB radius.
Definition: collision_object.h:150
Definition: collision_object.h:55
void setUserData(void *data)
set user data in object
Definition: collision_object.h:271
virtual Matrix3f computeMomentofInertiaRelatedToCOM() const
compute the inertia matrix, related to the com
Definition: collision_object.h:177
Definition: collision_object.h:58
Definition: collision_object.h:59
bool isIdentityTransform() const
whether the object is in local coordinate
Definition: collision_object.h:323
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, capsule, cone, cylinder, convex, plane, triangle), and octree
Definition: collision_object.h:58
virtual FCL_REAL computeVolume() const
compute the volume
Definition: collision_object.h:174
void setTranslation(const Vec3f &T)
set object&#39;s translation
Definition: collision_object.h:301
FCL_REAL threshold_free
threshold for free (<= is free)
Definition: collision_object.h:165
Vec3f aabb_center
AABB center in local coordinate.
Definition: collision_object.h:147
Definition: collision_object.h:59
shared_ptr< const CollisionGeometry > cgeom_const
Definition: collision_object.h:349
const AABB & getAABB() const
get the AABB in world space
Definition: collision_object.h:243
const Vec3f & getTranslation() const
get translation of the object
Definition: collision_object.h:277
Definition: collision_object.h:55
Definition: collision_object.h:59
Transform3f t
Definition: collision_object.h:351
CollisionObject(const shared_ptr< CollisionGeometry > &cgeom_)
Definition: collision_object.h:202
virtual ~CollisionGeometry()
Definition: collision_object.h:89
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
void computeAABB()
compute the AABB in world space
Definition: collision_object.h:249
the object for collision or distance computation, contains the geometry and the transform information...
Definition: collision_object.h:199
Definition: collision_object.h:58
void * getUserData() const
get user data in object
Definition: collision_object.h:265
The geometry for the object for collision or distance computation.
Definition: collision_object.h:66
Definition: collision_object.h:59
Definition: collision_object.h:59
shared_ptr< CollisionGeometry > cgeom
Definition: collision_object.h:348
Definition: collision_object.h:59
bool operator!=(const CollisionGeometry &other) const
Difference operator.
Definition: collision_object.h:109
#define HPP_FCL_DLLAPI
Definition: config.hh:64
Definition: collision_object.h:58