collision_object.h 9.18 KB
Newer Older
sachinc's avatar
sachinc committed
1
2
3
/*
 * Software License Agreement (BSD License)
 *
4
5
 *  Copyright (c) 2011-2014, Willow Garage, Inc.
 *  Copyright (c) 2014-2015, Open Source Robotics Foundation
sachinc's avatar
sachinc committed
6
7
8
9
10
11
12
13
14
15
16
17
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
18
 *   * Neither the name of Open Source Robotics Foundation nor the names of its
sachinc's avatar
sachinc committed
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

/** \author Jia Pan */


#ifndef FCL_COLLISION_OBJECT_BASE_H
#define FCL_COLLISION_OBJECT_BASE_H

42
43
44
#include <hpp/fcl/deprecated.h>
#include <hpp/fcl/BV/AABB.h>
#include <hpp/fcl/math/transform.h>
45
#include <boost/shared_ptr.hpp>
sachinc's avatar
sachinc committed
46
47
48
49

namespace fcl
{

jpan's avatar
jpan committed
50
/// @brief object type: BVH (mesh, points), basic geometry, octree
jpan's avatar
jpan committed
51
enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_COUNT};
sachinc's avatar
sachinc committed
52

jpan's avatar
jpan committed
53
/// @brief 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
jpan's avatar
   
jpan committed
54
enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_OBBRSS, BV_KDOP16, BV_KDOP18, BV_KDOP24,
55
                GEOM_BOX, GEOM_SPHERE, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE, GEOM_HALFSPACE, GEOM_TRIANGLE, GEOM_OCTREE, NODE_COUNT};
sachinc's avatar
sachinc committed
56

jpan's avatar
jpan committed
57
/// @brief The geometry for the object for collision or distance computation
58
59
60
class CollisionGeometry
{
public:
61
62
63
  CollisionGeometry() : cost_density(1),
                        threshold_occupied(1),
                        threshold_free(0)
64
65
66
  {
  }

67
68
  virtual ~CollisionGeometry() {}

jpan's avatar
jpan committed
69
  /// @brief get the type of the object
70
71
  virtual OBJECT_TYPE getObjectType() const { return OT_UNKNOWN; }

72
  /// @brief get the node type
73
74
  virtual NODE_TYPE getNodeType() const { return BV_UNKNOWN; }

jpan's avatar
jpan committed
75
  /// @brief compute the AABB for object in local coordinate
76
77
  virtual void computeLocalAABB() = 0;

jpan's avatar
jpan committed
78
  /// @brief get user data in geometry
isucan's avatar
isucan committed
79
80
81
82
83
  void* getUserData() const
  {
    return user_data;
  }

jpan's avatar
jpan committed
84
  /// @brief set user data in geometry
isucan's avatar
isucan committed
85
86
87
88
89
  void setUserData(void *data)
  {
    user_data = data;
  }

jpan's avatar
jpan committed
90
  /// @brief whether the object is completely occupied
91
92
  inline bool isOccupied() const HPP_FCL_DEPRECATED
  { return cost_density >= threshold_occupied; }
jpan's avatar
jpan committed
93
94

  /// @brief whether the object is completely free
95
96
  inline bool isFree() const HPP_FCL_DEPRECATED
  { return cost_density <= threshold_free; }
jpan's avatar
jpan committed
97
98

  /// @brief whether the object has some uncertainty
99
  bool isUncertain() const HPP_FCL_DEPRECATED;
100

jpan's avatar
jpan committed
101
  /// @brief AABB center in local coordinate
102
103
  Vec3f aabb_center;

jpan's avatar
jpan committed
104
  /// @brief AABB radius
105
  FCL_REAL aabb_radius;
isucan's avatar
isucan committed
106

jpan's avatar
jpan committed
107
  /// @brief AABB in local coordinate, used for tight AABB when only translation transform
jpan's avatar
jpan committed
108
109
  AABB aabb_local;

jpan's avatar
jpan committed
110
  /// @brief pointer to user defined data specific to this object
isucan's avatar
isucan committed
111
  void *user_data;
jpan's avatar
jpan committed
112

113
114
115
  /// @brief collision cost for unit volume
  FCL_REAL cost_density;

jpan's avatar
jpan committed
116
  /// @brief threshold for occupied ( >= is occupied)
117
118
  FCL_REAL threshold_occupied;

jpan's avatar
jpan committed
119
  /// @brief threshold for free (<= is free)
120
  FCL_REAL threshold_free;
121

panjia1983's avatar
panjia1983 committed
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
  /// @brief compute center of mass
  virtual Vec3f computeCOM() const { return Vec3f(); }

  /// @brief compute the inertia matrix, related to the origin
  virtual Matrix3f computeMomentofInertia() const { return Matrix3f(); }

  /// @brief compute the volume
  virtual FCL_REAL computeVolume() const { return 0; }

  /// @brief compute the inertia matrix, related to the com
  virtual Matrix3f computeMomentofInertiaRelatedToCOM() const
  {
    Matrix3f C = computeMomentofInertia();
    Vec3f com = computeCOM();
    FCL_REAL V = computeVolume();

138
139
140
141
142
143
144
145
146
    return (Matrix3f() << C(0, 0) - V * (com[1] * com[1] + com[2] * com[2]),
                          C(0, 1) + V * com[0] * com[1],
                          C(0, 2) + V * com[0] * com[2],
                          C(1, 0) + V * com[1] * com[0],
                          C(1, 1) - V * (com[0] * com[0] + com[2] * com[2]),
                          C(1, 2) + V * com[1] * com[2],
                          C(2, 0) + V * com[2] * com[0],
                          C(2, 1) + V * com[2] * com[1],
                          C(2, 2) - V * (com[0] * com[0] + com[1] * com[1])).finished();
panjia1983's avatar
panjia1983 committed
147
148
  }

149
150
};

jpan's avatar
jpan committed
151
/// @brief the object for collision or distance computation, contains the geometry and the transform information
sachinc's avatar
sachinc committed
152
153
154
class CollisionObject
{
public:
155
156
 CollisionObject(const boost::shared_ptr<CollisionGeometry> &cgeom_) :
    cgeom(cgeom_), cgeom_const(cgeom_)
157
  {
158
159
160
161
162
    if (cgeom)
    {
      cgeom->computeLocalAABB();
      computeAABB();
    }
163
164
  }

165
166
  CollisionObject(const boost::shared_ptr<CollisionGeometry> &cgeom_, const Transform3f& tf) :
    cgeom(cgeom_), cgeom_const(cgeom_), t(tf)
167
  {
168
    cgeom->computeLocalAABB();
jpan's avatar
   
jpan committed
169
    computeAABB();
170
171
  }

172
  CollisionObject(const boost::shared_ptr<CollisionGeometry> &cgeom_, const Matrix3f& R, const Vec3f& T):
173
      cgeom(cgeom_), cgeom_const(cgeom_), t(Transform3f(R, T))
174
  {
175
    cgeom->computeLocalAABB();
jpan's avatar
   
jpan committed
176
    computeAABB();
177
178
179
180
181
182
  }

  ~CollisionObject()
  {
  }

jpan's avatar
jpan committed
183
  /// @brief get the type of the object
184
185
186
187
188
  OBJECT_TYPE getObjectType() const
  {
    return cgeom->getObjectType();
  }

jpan's avatar
jpan committed
189
  /// @brief get the node type
190
191
192
193
194
  NODE_TYPE getNodeType() const
  {
    return cgeom->getNodeType();
  }

jpan's avatar
jpan committed
195
  /// @brief get the AABB in world space
196
197
198
199
200
  inline const AABB& getAABB() const
  {
    return aabb;
  }

jpan's avatar
jpan committed
201
  /// @brief compute the AABB in world space
202
203
  inline void computeAABB()
  {
Joseph Mirabel's avatar
Joseph Mirabel committed
204
    if(isQuatIdentity(t.getQuatRotation()))
jpan's avatar
jpan committed
205
    {
jpan's avatar
jpan committed
206
      aabb = translate(cgeom->aabb_local, t.getTranslation());
jpan's avatar
jpan committed
207
208
209
    }
    else
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
210
      Vec3f center (t.transform(cgeom->aabb_center));
211
      Vec3f delta(Vec3f::Constant(cgeom->aabb_radius));
jpan's avatar
jpan committed
212
213
214
      aabb.min_ = center - delta;
      aabb.max_ = center + delta;
    }
215
216
  }

jpan's avatar
jpan committed
217
  /// @brief get user data in object
218
219
220
221
222
  void* getUserData() const
  {
    return user_data;
  }

jpan's avatar
jpan committed
223
  /// @brief set user data in object
224
225
226
227
228
  void setUserData(void *data)
  {
    user_data = data;
  }

jpan's avatar
jpan committed
229
  /// @brief get translation of the object
230
231
232
233
234
  inline const Vec3f& getTranslation() const
  {
    return t.getTranslation();
  }

jpan's avatar
jpan committed
235
  /// @brief get matrix rotation of the object
236
237
238
239
240
  inline const Matrix3f& getRotation() const
  {
    return t.getRotation();
  }

jpan's avatar
jpan committed
241
  /// @brief get quaternion rotation of the object
242
  inline const Quaternion3f& getQuatRotation() const
243
244
245
246
  {
    return t.getQuatRotation();
  }

jpan's avatar
jpan committed
247
  /// @brief get object's transform
248
  inline const Transform3f& getTransform() const
249
250
251
252
  {
    return t;
  }

jpan's avatar
jpan committed
253
  /// @brief set object's rotation matrix
254
255
256
257
258
  void setRotation(const Matrix3f& R)
  {
    t.setRotation(R);
  }

jpan's avatar
jpan committed
259
  /// @brief set object's translation
260
261
262
263
264
  void setTranslation(const Vec3f& T)
  {
    t.setTranslation(T);
  }

jpan's avatar
jpan committed
265
  /// @brief set object's quatenrion rotation
266
  void setQuatRotation(const Quaternion3f& q)
267
268
269
270
  {
    t.setQuatRotation(q);
  }

jpan's avatar
jpan committed
271
  /// @brief set object's transform
272
273
274
275
276
  void setTransform(const Matrix3f& R, const Vec3f& T)
  {
    t.setTransform(R, T);
  }

jpan's avatar
jpan committed
277
  /// @brief set object's transform
278
  void setTransform(const Quaternion3f& q, const Vec3f& T)
279
280
281
282
  {
    t.setTransform(q, T);
  }

jpan's avatar
jpan committed
283
  /// @brief set object's transform
284
  void setTransform(const Transform3f& tf)
285
286
287
288
  {
    t = tf;
  }

jpan's avatar
jpan committed
289
  /// @brief whether the object is in local coordinate
290
291
292
293
294
  bool isIdentityTransform() const
  {
    return t.isIdentity();
  }

jpan's avatar
jpan committed
295
  /// @brief set the object in local coordinate
296
297
298
299
300
  void setIdentityTransform()
  {
    t.setIdentity();
  }

jpan's avatar
jpan committed
301
  /// @brief get geometry from the object instance
302
303
  FCL_DEPRECATED
  const CollisionGeometry* getCollisionGeometry() const
304
305
306
307
  {
    return cgeom.get();
  }

308
  /// @brief get geometry from the object instance
309
  const boost::shared_ptr<const CollisionGeometry>& collisionGeometry() const
310
  {
311
    return cgeom_const;
312
313
  }

314
315
316
317
318
319
  /// @brief get geometry from the object instance
  const boost::shared_ptr<CollisionGeometry>& collisionGeometry()
  {
    return cgeom;
  }

320
321
322
protected:

  boost::shared_ptr<CollisionGeometry> cgeom;
323
  boost::shared_ptr<const CollisionGeometry> cgeom_const;
324

325
  Transform3f t;
326

jpan's avatar
jpan committed
327
  /// @brief AABB in global coordinate
328
329
  mutable AABB aabb;

jpan's avatar
jpan committed
330
  /// @brief pointer to user defined data specific to this object
331
332
333
  void *user_data;
};

sachinc's avatar
sachinc committed
334
335
336
}

#endif