hpp-fcl  1.4.4
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 <hpp/fcl/deprecated.hh>
43 #include <hpp/fcl/BV/AABB.h>
44 #include <hpp/fcl/math/transform.h>
45 #include <boost/shared_ptr.hpp>
46 
47 namespace hpp
48 {
49 namespace fcl
50 {
51 
54 
58 
61 
64 {
65 public:
69  {
70  }
71 
72  virtual ~CollisionGeometry() {}
73 
75  virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; }
76 
78  virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; }
79 
81  virtual void computeLocalAABB() = 0;
82 
84  void* getUserData() const
85  {
86  return user_data;
87  }
88 
90  void setUserData(void *data)
91  {
92  user_data = data;
93  }
94 
96  inline bool isOccupied() const
97  { return cost_density >= threshold_occupied; }
98 
100  inline bool isFree() const
101  { return cost_density <= threshold_free; }
102 
104  bool isUncertain() const;
105 
108 
111 
114 
116  void *user_data;
117 
120 
123 
126 
128  virtual Vec3f computeCOM() const { return Vec3f::Zero(); }
129 
131  virtual Matrix3f computeMomentofInertia() const { return Matrix3f::Constant(NAN); }
132 
134  virtual FCL_REAL computeVolume() const { return 0; }
135 
138  {
140  Vec3f com = computeCOM();
141  FCL_REAL V = computeVolume();
142 
143  return (Matrix3f() << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]),
144  C(0, 1) + V * com[0] * com[1],
145  C(0, 2) + V * com[0] * com[2],
146  C(1, 0) + V * com[1] * com[0],
147  C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]),
148  C(1, 2) + V * com[1] * com[2],
149  C(2, 0) + V * com[2] * com[0],
150  C(2, 1) + V * com[2] * com[1],
151  C(2, 2) - V * (com[0] * com[0] + com[1] * com[1])).finished();
152  }
153 
154 };
155 
158 {
159 public:
160  CollisionObject(const boost::shared_ptr<CollisionGeometry> &cgeom_) :
161  cgeom(cgeom_), cgeom_const(cgeom_)
162  {
163  if (cgeom)
164  {
165  cgeom->computeLocalAABB();
166  computeAABB();
167  }
168  }
169 
170  CollisionObject(const boost::shared_ptr<CollisionGeometry> &cgeom_, const Transform3f& tf) :
171  cgeom(cgeom_), cgeom_const(cgeom_), t(tf)
172  {
173  cgeom->computeLocalAABB();
174  computeAABB();
175  }
176 
177  CollisionObject(const boost::shared_ptr<CollisionGeometry> &cgeom_, const Matrix3f& R, const Vec3f& T):
178  cgeom(cgeom_), cgeom_const(cgeom_), t(Transform3f(R, T))
179  {
180  cgeom->computeLocalAABB();
181  computeAABB();
182  }
183 
185  {
186  }
187 
190  {
191  return cgeom->getObjectType();
192  }
193 
196  {
197  return cgeom->getNodeType();
198  }
199 
201  inline const AABB& getAABB() const
202  {
203  return aabb;
204  }
205 
207  inline void computeAABB()
208  {
209  if(t.getRotation().isIdentity())
210  {
211  aabb = translate(cgeom->aabb_local, t.getTranslation());
212  }
213  else
214  {
215  Vec3f center (t.transform(cgeom->aabb_center));
216  Vec3f delta(Vec3f::Constant(cgeom->aabb_radius));
217  aabb.min_ = center - delta;
218  aabb.max_ = center + delta;
219  }
220  }
221 
223  void* getUserData() const
224  {
225  return user_data;
226  }
227 
229  void setUserData(void *data)
230  {
231  user_data = data;
232  }
233 
235  inline const Vec3f& getTranslation() const
236  {
237  return t.getTranslation();
238  }
239 
241  inline const Matrix3f& getRotation() const
242  {
243  return t.getRotation();
244  }
245 
247  inline const Transform3f& getTransform() const
248  {
249  return t;
250  }
251 
253  void setRotation(const Matrix3f& R)
254  {
255  t.setRotation(R);
256  }
257 
259  void setTranslation(const Vec3f& T)
260  {
261  t.setTranslation(T);
262  }
263 
264 
265 
267  void setTransform(const Matrix3f& R, const Vec3f& T)
268  {
269  t.setTransform(R, T);
270  }
271 
272 
273 
275  void setTransform(const Transform3f& tf)
276  {
277  t = tf;
278  }
279 
281  bool isIdentityTransform() const
282  {
283  return t.isIdentity();
284  }
285 
288  {
289  t.setIdentity();
290  }
291 
295  {
296  return cgeom.get();
297  }
298 
300  const boost::shared_ptr<const CollisionGeometry>& collisionGeometry() const
301  {
302  return cgeom_const;
303  }
304 
306  const boost::shared_ptr<CollisionGeometry>& collisionGeometry()
307  {
308  return cgeom;
309  }
310 
311 protected:
312 
313  boost::shared_ptr<CollisionGeometry> cgeom;
314  boost::shared_ptr<const CollisionGeometry> cgeom_const;
315 
317 
319  mutable AABB aabb;
320 
322  void *user_data;
323 };
324 
325 }
326 
327 } // namespace hpp
328 
329 #endif
Definition: collision_object.h:57
const Matrix3f & getRotation() const
get matrix rotation of the object
Definition: collision_object.h:241
Simple transform class used locally by InterpMotion.
Definition: transform.h:59
FCL_REAL cost_density
collision cost for unit volume
Definition: collision_object.h:119
const int GST_INDEP HPP_FCL_DEPRECATED
Definition: collision_data.h:56
virtual Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: collision_object.h:131
const boost::shared_ptr< CollisionGeometry > & collisionGeometry()
get geometry from the object instance
Definition: collision_object.h:306
~CollisionObject()
Definition: collision_object.h:184
Definition: collision_object.h:56
boost::shared_ptr< const CollisionGeometry > cgeom_const
Definition: collision_object.h:314
Definition: collision_object.h:57
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:53
CollisionGeometry()
Definition: collision_object.h:66
Main namespace.
Definition: AABB.h:43
const boost::shared_ptr< const CollisionGeometry > & collisionGeometry() const
get geometry from the object instance
Definition: collision_object.h:300
Definition: collision_object.h:56
NODE_TYPE getNodeType() const
get the node type
Definition: collision_object.h:195
void * user_data
pointer to user defined data specific to this object
Definition: collision_object.h:322
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:74
virtual Vec3f computeCOM() const
compute center of mass
Definition: collision_object.h:128
CollisionObject(const boost::shared_ptr< CollisionGeometry > &cgeom_, const Transform3f &tf)
Definition: collision_object.h:170
HPP_FCL_DEPRECATED const CollisionGeometry * getCollisionGeometry() const
get geometry from the object instance
Definition: collision_object.h:294
Definition: collision_object.h:57
Definition: collision_object.h:56
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:122
Definition: collision_object.h:56
bool isOccupied() const
whether the object is completely occupied
Definition: collision_object.h:96
const Transform3f & getTransform() const
get object&#39;s transform
Definition: collision_object.h:247
bool isFree() const
whether the object is completely free
Definition: collision_object.h:100
void setIdentityTransform()
set the object in local coordinate
Definition: collision_object.h:287
void setTransform(const Transform3f &tf)
set object&#39;s transform
Definition: collision_object.h:275
Definition: collision_object.h:57
CollisionObject(const boost::shared_ptr< CollisionGeometry > &cgeom_, const Matrix3f &R, const Vec3f &T)
Definition: collision_object.h:177
virtual void computeLocalAABB()=0
compute the AABB for object in local coordinate
virtual NODE_TYPE getNodeType() const
get the node type
Definition: collision_object.h:78
Definition: collision_object.h:53
AABB aabb_local
AABB in local coordinate, used for tight AABB when only translation transform.
Definition: collision_object.h:113
Definition: collision_object.h:53
void setTransform(const Matrix3f &R, const Vec3f &T)
set object&#39;s transform
Definition: collision_object.h:267
void setRotation(const Matrix3f &R)
set object&#39;s rotation matrix
Definition: collision_object.h:253
void * user_data
pointer to user defined data specific to this object
Definition: collision_object.h:116
double FCL_REAL
Definition: data_types.h:68
OBJECT_TYPE getObjectType() const
get the type of the object
Definition: collision_object.h:189
Definition: collision_object.h:56
void setUserData(void *data)
set user data in geometry
Definition: collision_object.h:90
Definition: collision_object.h:57
virtual OBJECT_TYPE getObjectType() const
get the type of the object
Definition: collision_object.h:75
Definition: collision_object.h:53
AABB aabb
AABB in global coordinate.
Definition: collision_object.h:319
void * getUserData() const
get user data in geometry
Definition: collision_object.h:84
CollisionObject(const boost::shared_ptr< CollisionGeometry > &cgeom_)
Definition: collision_object.h:160
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:56
FCL_REAL aabb_radius
AABB radius.
Definition: collision_object.h:110
Definition: collision_object.h:53
void setUserData(void *data)
set user data in object
Definition: collision_object.h:229
virtual Matrix3f computeMomentofInertiaRelatedToCOM() const
compute the inertia matrix, related to the com
Definition: collision_object.h:137
Definition: collision_object.h:56
Definition: collision_object.h:57
bool isIdentityTransform() const
whether the object is in local coordinate
Definition: collision_object.h:281
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:56
virtual FCL_REAL computeVolume() const
compute the volume
Definition: collision_object.h:134
void setTranslation(const Vec3f &T)
set object&#39;s translation
Definition: collision_object.h:259
FCL_REAL threshold_free
threshold for free (<= is free)
Definition: collision_object.h:125
Vec3f aabb_center
AABB center in local coordinate.
Definition: collision_object.h:107
Definition: collision_object.h:57
const AABB & getAABB() const
get the AABB in world space
Definition: collision_object.h:201
const Vec3f & getTranslation() const
get translation of the object
Definition: collision_object.h:235
Definition: collision_object.h:53
Definition: collision_object.h:57
Transform3f t
Definition: collision_object.h:316
virtual ~CollisionGeometry()
Definition: collision_object.h:72
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:73
void computeAABB()
compute the AABB in world space
Definition: collision_object.h:207
the object for collision or distance computation, contains the geometry and the transform information...
Definition: collision_object.h:157
Definition: collision_object.h:56
void * getUserData() const
get user data in object
Definition: collision_object.h:223
bool isUncertain() const
whether the object has some uncertainty
The geometry for the object for collision or distance computation.
Definition: collision_object.h:63
boost::shared_ptr< CollisionGeometry > cgeom
Definition: collision_object.h:313
Definition: collision_object.h:57
Definition: collision_object.h:57
Definition: collision_object.h:57
Definition: collision_object.h:56