geometric_shapes.h 10.9 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
 *     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 */


Joseph Mirabel's avatar
Joseph Mirabel committed
39
40
#ifndef HPP_FCL_GEOMETRIC_SHAPES_H
#define HPP_FCL_GEOMETRIC_SHAPES_H
sachinc's avatar
sachinc committed
41

42
43
#include <boost/math/constants/constants.hpp>

44
#include <hpp/fcl/collision_object.h>
45
#include <hpp/fcl/data_types.h>
sachinc's avatar
sachinc committed
46
47
#include <string.h>

48
49
namespace hpp
{
sachinc's avatar
sachinc committed
50
51
52
namespace fcl
{

53
/// @brief Base class for all basic geometric shapes
54
class ShapeBase : public CollisionGeometry
sachinc's avatar
sachinc committed
55
56
{
public:
57
  ShapeBase() {}
sachinc's avatar
sachinc committed
58

59
60
  virtual ~ShapeBase () {};

61
  /// @brief Get object type: a geometric shape
sachinc's avatar
sachinc committed
62
63
64
  OBJECT_TYPE getObjectType() const { return OT_GEOM; }
};

65
66
67
68
/// @defgroup Geometric_Shapes
/// regroup class of differents types of geometric shapes.
/// @{

69
/// @brief Triangle stores the points instead of only indices of points
70
class TriangleP : public ShapeBase
71
72
{
public:
73
  TriangleP(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_) : ShapeBase(), a(a_), b(b_), c(c_)
jpan's avatar
jpan committed
74
75
  {
  }
76

77
  /// @brief virtual function of compute AABB in local coordinate
78
79
80
81
82
83
  void computeLocalAABB();
  
  NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; }

  Vec3f a, b, c;
};
sachinc's avatar
sachinc committed
84

85
/// @brief Center at zero point, axis aligned box
sachinc's avatar
sachinc committed
86
87
88
class Box : public ShapeBase
{
public:
89
  Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) : ShapeBase(), halfSide(x/2, y/2, z/2)
jpan's avatar
jpan committed
90
91
  {
  }
sachinc's avatar
sachinc committed
92

93
  Box(const Vec3f& side_) : ShapeBase(), halfSide(side_/2) 
jpan's avatar
jpan committed
94
95
96
  {
  }

97
98
  Box() {}

99
100
  /// @brief box side half-length
  Vec3f halfSide;
sachinc's avatar
sachinc committed
101

102
  /// @brief Compute AABB
103
  void computeLocalAABB();
sachinc's avatar
sachinc committed
104

105
  /// @brief Get node type: a box
sachinc's avatar
sachinc committed
106
  NODE_TYPE getNodeType() const { return GEOM_BOX; }
panjia1983's avatar
panjia1983 committed
107
108
109

  FCL_REAL computeVolume() const
  {
110
    return 8*halfSide.prod();
panjia1983's avatar
panjia1983 committed
111
112
113
114
115
  }

  Matrix3f computeMomentofInertia() const
  {
    FCL_REAL V = computeVolume();
116
117
    Vec3f s (halfSide.cwiseAbs2() * V);
    return (Vec3f (s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal();
panjia1983's avatar
panjia1983 committed
118
  }
sachinc's avatar
sachinc committed
119
120
};

121
/// @brief Center at zero point sphere
sachinc's avatar
sachinc committed
122
123
124
class Sphere : public ShapeBase
{
public:
jpan's avatar
jpan committed
125
126
127
  Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_)
  {
  }
jpan's avatar
   
jpan committed
128
  
129
  /// @brief Radius of the sphere 
130
  FCL_REAL radius;
sachinc's avatar
sachinc committed
131

132
  /// @brief Compute AABB 
133
  void computeLocalAABB();
sachinc's avatar
sachinc committed
134

135
  /// @brief Get node type: a sphere 
sachinc's avatar
sachinc committed
136
  NODE_TYPE getNodeType() const { return GEOM_SPHERE; }
panjia1983's avatar
panjia1983 committed
137
138
139
140

  Matrix3f computeMomentofInertia() const
  {
    FCL_REAL I = 0.4 * radius * radius * computeVolume();
141
    return I * Matrix3f::Identity();
panjia1983's avatar
panjia1983 committed
142
143
144
145
  }

  FCL_REAL computeVolume() const
  {
146
    return 4 * boost::math::constants::pi<FCL_REAL>() * radius * radius * radius / 3;
panjia1983's avatar
panjia1983 committed
147
  }
sachinc's avatar
sachinc committed
148
149
};

150
/// @brief Center at zero point capsule 
sachinc's avatar
sachinc committed
151
152
153
class Capsule : public ShapeBase
{
public:
154
  Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_)
jpan's avatar
jpan committed
155
  {
156
    halfLength = lz_/2;
jpan's avatar
jpan committed
157
  }
jpan's avatar
   
jpan committed
158

159
  /// @brief Radius of capsule 
160
  FCL_REAL radius;
sachinc's avatar
sachinc committed
161

Lucile Remigy's avatar
Lucile Remigy committed
162
  /// @brief Half Length along z axis 
163
  FCL_REAL halfLength;
Lucile Remigy's avatar
Lucile Remigy committed
164

165
  /// @brief Compute AABB 
166
  void computeLocalAABB();
sachinc's avatar
sachinc committed
167

168
  /// @brief Get node type: a capsule 
sachinc's avatar
sachinc committed
169
  NODE_TYPE getNodeType() const { return GEOM_CAPSULE; }
panjia1983's avatar
panjia1983 committed
170
171
172

  FCL_REAL computeVolume() const
  {
173
    return boost::math::constants::pi<FCL_REAL>() * radius * radius *((halfLength * 2) + radius * 4/3.0);
panjia1983's avatar
panjia1983 committed
174
175
176
177
  }

  Matrix3f computeMomentofInertia() const
  {
178
    FCL_REAL v_cyl = radius * radius * (halfLength * 2) * boost::math::constants::pi<FCL_REAL>();
panjia1983's avatar
panjia1983 committed
179
180
    FCL_REAL v_sph = radius * radius * radius * boost::math::constants::pi<FCL_REAL>() * 4 / 3.0;
    
181
182
183
    FCL_REAL h2 = halfLength * halfLength;
    FCL_REAL r2 = radius * radius;
    FCL_REAL ix = v_cyl * (h2 / 3. + r2 / 4.) + v_sph * (0.4 * r2 + h2 + 0.75 * radius * halfLength);
panjia1983's avatar
panjia1983 committed
184
185
    FCL_REAL iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius;

186
187
188
    return (Matrix3f() << ix, 0, 0,
                          0, ix, 0,
                          0, 0, iz).finished();
panjia1983's avatar
panjia1983 committed
189
190
  }
  
sachinc's avatar
sachinc committed
191
192
};

193
/// @brief Center at zero cone 
sachinc's avatar
sachinc committed
194
195
196
class Cone : public ShapeBase
{
public:
197
  Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_)
jpan's avatar
jpan committed
198
  {
199
    halfLength = lz_/2;
jpan's avatar
jpan committed
200
201
  }

202
  /// @brief Radius of the cone 
203
  FCL_REAL radius;
sachinc's avatar
sachinc committed
204

Lucile Remigy's avatar
Lucile Remigy committed
205
  /// @brief Half Length along z axis 
206
  FCL_REAL halfLength;
Lucile Remigy's avatar
Lucile Remigy committed
207

208
  /// @brief Compute AABB 
209
  void computeLocalAABB();
sachinc's avatar
sachinc committed
210

211
  /// @brief Get node type: a cone 
sachinc's avatar
sachinc committed
212
  NODE_TYPE getNodeType() const { return GEOM_CONE; }
panjia1983's avatar
panjia1983 committed
213
214
215

  FCL_REAL computeVolume() const
  {
216
    return boost::math::constants::pi<FCL_REAL>() * radius * radius * (halfLength * 2) / 3;
panjia1983's avatar
panjia1983 committed
217
218
219
220
221
  }

  Matrix3f computeMomentofInertia() const
  {
    FCL_REAL V = computeVolume();
222
    FCL_REAL ix = V * (0.4 * halfLength * halfLength + 3 * radius * radius / 20);
panjia1983's avatar
panjia1983 committed
223
224
    FCL_REAL iz = 0.3 * V * radius * radius;

225
226
227
    return (Matrix3f() << ix, 0, 0,
                          0, ix, 0,
                          0, 0, iz).finished();
panjia1983's avatar
panjia1983 committed
228
229
230
231
  }

  Vec3f computeCOM() const
  {
232
    return Vec3f(0, 0, -0.5 * halfLength);
panjia1983's avatar
panjia1983 committed
233
  }
sachinc's avatar
sachinc committed
234
235
};

236
/// @brief Center at zero cylinder 
sachinc's avatar
sachinc committed
237
238
239
class Cylinder : public ShapeBase
{
public:
240
  Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_)
jpan's avatar
jpan committed
241
  {
242
    halfLength = lz_/2;
jpan's avatar
jpan committed
243
  }
jpan's avatar
   
jpan committed
244
  
245
  /// @brief Radius of the cylinder 
246
  FCL_REAL radius;
sachinc's avatar
sachinc committed
247

Lucile Remigy's avatar
Lucile Remigy committed
248
  /// @brief Half Length along z axis 
249
  FCL_REAL halfLength;
Lucile Remigy's avatar
Lucile Remigy committed
250

251
  /// @brief Compute AABB 
252
  void computeLocalAABB();
sachinc's avatar
sachinc committed
253

254
  /// @brief Get node type: a cylinder 
sachinc's avatar
sachinc committed
255
  NODE_TYPE getNodeType() const { return GEOM_CYLINDER; }
panjia1983's avatar
panjia1983 committed
256
257
258

  FCL_REAL computeVolume() const
  {
259
    return boost::math::constants::pi<FCL_REAL>() * radius * radius * (halfLength * 2);
panjia1983's avatar
panjia1983 committed
260
261
262
263
264
  }

  Matrix3f computeMomentofInertia() const
  {
    FCL_REAL V = computeVolume();
265
    FCL_REAL ix = V * (radius * radius / 4 + halfLength * halfLength / 3);
panjia1983's avatar
panjia1983 committed
266
    FCL_REAL iz = V * radius * radius / 2;
267
268
269
    return (Matrix3f() << ix, 0, 0,
                          0, ix, 0,
                          0, 0, iz).finished();
panjia1983's avatar
panjia1983 committed
270
  }
sachinc's avatar
sachinc committed
271
272
};

273
274
275
/// @brief Base for convex polytope.
/// @note Inherited classes are responsible for filling ConvexBase::neighbors;
class ConvexBase : public ShapeBase
sachinc's avatar
sachinc committed
276
277
{
public:
Lucile Remigy's avatar
Lucile Remigy committed
278

279
  virtual ~ConvexBase();
sachinc's avatar
sachinc committed
280

281
  /// @brief Compute AABB 
282
  void computeLocalAABB();
sachinc's avatar
sachinc committed
283

284
  /// @brief Get node type: a conex polytope 
sachinc's avatar
sachinc committed
285
286
  NODE_TYPE getNodeType() const { return GEOM_CONVEX; }

287
  /// @brief An array of the points of the polygon.
sachinc's avatar
sachinc committed
288
289
290
  Vec3f* points;
  int num_points;

291
  struct Neighbors
sachinc's avatar
sachinc committed
292
  {
293
294
295
296
297
298
    unsigned char count_;
    unsigned int* n_;

    unsigned char const& count () const { return count_; }
    unsigned int      & operator[] (int i)       { assert(i<count_); return n_[i]; }
    unsigned int const& operator[] (int i) const { assert(i<count_); return n_[i]; }
sachinc's avatar
sachinc committed
299
  };
300
301
302
303
  /// Neighbors of each vertex.
  /// It is an array of size num_points. For each vertex, it contains the number
  /// of neighbors and a list of indices to them.
  Neighbors* neighbors;
sachinc's avatar
sachinc committed
304

305
  /// @brief center of the convex polytope, this is used for collision: center is guaranteed in the internal of the polytope (as it is convex) 
sachinc's avatar
sachinc committed
306
307
  Vec3f center;

308
309
310
311
protected:
  /// @brief Constructing a convex, providing normal and offset of each polytype surface, and the points and shape topology information 
  /// \param points_ list of 3D points
  /// \param num_points_ number of 3D points
312
  ConvexBase(bool ownStorage, Vec3f* points_, int num_points_);
panjia1983's avatar
panjia1983 committed
313

314
315
316
  /// @brief Copy constructor 
  /// Only the list of neighbors is copied.
  ConvexBase(const ConvexBase& other);
panjia1983's avatar
panjia1983 committed
317

318
  unsigned int* nneighbors_;
sachinc's avatar
sachinc committed
319

320
321
  bool own_storage_;

322
private:
323
  void computeCenter();
324
};
325

326
327
template <typename PolygonT> class Convex;

328
329
330
331
332
333
334
335
336
337
338
339
340
/// @brief Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d;
/// Points in the negative side of the separation plane (i.e. {x | n * x < d}) are inside the half space and points
/// in the positive side of the separation plane (i.e. {x | n * x > d}) are outside the half space
class Halfspace : public ShapeBase
{
public:
  /// @brief Construct a half space with normal direction and offset
  Halfspace(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_)
  {
    unitNormalTest();
  }

  /// @brief Construct a plane with normal direction and offset
341
  Halfspace(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : ShapeBase(), n(a, b, c), d(d_)
342
343
344
345
  {
    unitNormalTest();
  }

346
347
348
349
350
351
352
353
354
355
356
357
358
359
  Halfspace() : ShapeBase(), n(1, 0, 0), d(0)
  {
  }

  FCL_REAL signedDistance(const Vec3f& p) const
  {
    return n.dot(p) - d;
  }

  FCL_REAL distance(const Vec3f& p) const
  {
    return std::abs(n.dot(p) - d);
  }

360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
  /// @brief Compute AABB
  void computeLocalAABB();

  /// @brief Get node type: a half space
  NODE_TYPE getNodeType() const { return GEOM_HALFSPACE; }
  
  /// @brief Plane normal
  Vec3f n;
  
  /// @brief Plane offset
  FCL_REAL d;

protected:

  /// @brief Turn non-unit normal into unit
  void unitNormalTest();
};

378
/// @brief Infinite plane 
sachinc's avatar
sachinc committed
379
380
381
class Plane : public ShapeBase
{
public:
382
  /// @brief Construct a plane with normal direction and offset 
jpan's avatar
jpan committed
383
384
385
386
  Plane(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) 
  { 
    unitNormalTest(); 
  }
jpan's avatar
   
jpan committed
387
  
388
  /// @brief Construct a plane with normal direction and offset 
389
  Plane(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : ShapeBase(), n(a, b, c), d(d_)
sachinc's avatar
sachinc committed
390
391
392
393
  {
    unitNormalTest();
  }

394
395
396
397
398
399
400
401
402
403
404
405
406
  Plane() : ShapeBase(), n(1, 0, 0), d(0)
  {}

  FCL_REAL signedDistance(const Vec3f& p) const
  {
    return n.dot(p) - d;
  }

  FCL_REAL distance(const Vec3f& p) const
  {
    return std::abs(n.dot(p) - d);
  }

407
  /// @brief Compute AABB 
408
  void computeLocalAABB();
sachinc's avatar
sachinc committed
409

410
  /// @brief Get node type: a plane 
sachinc's avatar
sachinc committed
411
412
  NODE_TYPE getNodeType() const { return GEOM_PLANE; }

413
  /// @brief Plane normal 
sachinc's avatar
sachinc committed
414
  Vec3f n;
jpan's avatar
   
jpan committed
415

416
  /// @brief Plane offset 
417
  FCL_REAL d;
sachinc's avatar
sachinc committed
418
419

protected:
jpan's avatar
   
jpan committed
420
  
421
  /// @brief Turn non-unit normal into unit 
sachinc's avatar
sachinc committed
422
423
424
425
426
  void unitNormalTest();
};

}

427
428
} // namespace hpp

sachinc's avatar
sachinc committed
429
#endif