hpp-fcl  1.7.8
HPP fork of FCL -- The Flexible Collision Library
geometric_shapes.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_GEOMETRIC_SHAPES_H
40 #define HPP_FCL_GEOMETRIC_SHAPES_H
41 
42 #include <boost/math/constants/constants.hpp>
43 
45 #include <hpp/fcl/data_types.h>
46 #include <string.h>
47 
48 namespace hpp
49 {
50 namespace fcl
51 {
52 
55 {
56 public:
57  ShapeBase() {}
58 
60  ShapeBase(const ShapeBase & other)
61  : CollisionGeometry(other)
62  {}
63 
64  virtual ~ShapeBase () {};
65 
67  OBJECT_TYPE getObjectType() const { return OT_GEOM; }
68 };
69 
73 
76 {
77 public:
78  TriangleP(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_) : ShapeBase(), a(a_), b(b_), c(c_)
79  {
80  }
81 
82 
83  TriangleP(const TriangleP & other)
84  : ShapeBase(other)
85  , a(other.a), b(other.b), c(other.c)
86  {}
87 
89  virtual TriangleP* clone() const { return new TriangleP(*this); };
90 
92  void computeLocalAABB();
93 
94  NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; }
95 
96  Vec3f a, b, c;
97 
98  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
99 };
100 
103 {
104 public:
105  Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) : ShapeBase(), halfSide(x/2, y/2, z/2)
106  {
107  }
108 
109  Box(const Vec3f& side_) : ShapeBase(), halfSide(side_/2)
110  {
111  }
112 
113  Box(const Box & other)
114  : ShapeBase(other)
115  , halfSide(other.halfSide)
116  {
117  }
118 
120  virtual Box* clone() const { return new Box(*this); };
121 
122  Box() {}
123 
126 
128  void computeLocalAABB();
129 
131  NODE_TYPE getNodeType() const { return GEOM_BOX; }
132 
134  {
135  return 8*halfSide.prod();
136  }
137 
139  {
140  FCL_REAL V = computeVolume();
141  Vec3f s (halfSide.cwiseAbs2() * V);
142  return (Vec3f (s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal();
143  }
144 
145  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
146 };
147 
150 {
151 public:
152  Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_)
153  {
154  }
155 
156  Sphere(const Sphere & other)
157  : ShapeBase(other)
158  , radius(other.radius)
159  {
160  }
161 
163  virtual Sphere* clone() const { return new Sphere(*this); };
164 
167 
169  void computeLocalAABB();
170 
172  NODE_TYPE getNodeType() const { return GEOM_SPHERE; }
173 
175  {
176  FCL_REAL I = 0.4 * radius * radius * computeVolume();
177  return I * Matrix3f::Identity();
178  }
179 
181  {
182  return 4 * boost::math::constants::pi<FCL_REAL>() * radius * radius * radius / 3;
183  }
184 
185  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
186 };
187 
193 {
194 public:
195  Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_)
196  {
197  halfLength = lz_/2;
198  }
199 
200  Capsule(const Capsule & other)
201  : ShapeBase(other)
202  , radius(other.radius)
203  , halfLength(other.halfLength)
204  {
205  }
206 
208  virtual Capsule* clone() const { return new Capsule(*this); };
209 
212 
215 
217  void computeLocalAABB();
218 
220  NODE_TYPE getNodeType() const { return GEOM_CAPSULE; }
221 
223  {
224  return boost::math::constants::pi<FCL_REAL>() * radius * radius *((halfLength * 2) + radius * 4/3.0);
225  }
226 
228  {
229  FCL_REAL v_cyl = radius * radius * (halfLength * 2) * boost::math::constants::pi<FCL_REAL>();
230  FCL_REAL v_sph = radius * radius * radius * boost::math::constants::pi<FCL_REAL>() * 4 / 3.0;
231 
232  FCL_REAL h2 = halfLength * halfLength;
233  FCL_REAL r2 = radius * radius;
234  FCL_REAL ix = v_cyl * (h2 / 3. + r2 / 4.) + v_sph * (0.4 * r2 + h2 + 0.75 * radius * halfLength);
235  FCL_REAL iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius;
236 
237  return (Matrix3f() << ix, 0, 0,
238  0, ix, 0,
239  0, 0, iz).finished();
240  }
241 
242  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
243 
244 };
245 
250 {
251 public:
252  Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_)
253  {
254  halfLength = lz_/2;
255  }
256 
257  Cone(const Cone & other)
258  : ShapeBase(other)
259  , radius(other.radius)
260  , halfLength(other.halfLength)
261  {
262  }
263 
265  virtual Cone* clone() const { return new Cone(*this); };
266 
269 
272 
274  void computeLocalAABB();
275 
277  NODE_TYPE getNodeType() const { return GEOM_CONE; }
278 
280  {
281  return boost::math::constants::pi<FCL_REAL>() * radius * radius * (halfLength * 2) / 3;
282  }
283 
285  {
286  FCL_REAL V = computeVolume();
287  FCL_REAL ix = V * (0.4 * halfLength * halfLength + 3 * radius * radius / 20);
288  FCL_REAL iz = 0.3 * V * radius * radius;
289 
290  return (Matrix3f() << ix, 0, 0,
291  0, ix, 0,
292  0, 0, iz).finished();
293  }
294 
296  {
297  return Vec3f(0, 0, -0.5 * halfLength);
298  }
299 
300  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
301 };
302 
306 {
307 public:
308  Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_)
309  {
310  halfLength = lz_/2;
311  }
312 
313  Cylinder(const Cylinder & other)
314  : ShapeBase(other)
315  , radius(other.radius)
316  , halfLength(other.halfLength)
317  {
318  }
319 
321  virtual Cylinder* clone() const { return new Cylinder(*this); };
322 
325 
328 
330  void computeLocalAABB();
331 
333  NODE_TYPE getNodeType() const { return GEOM_CYLINDER; }
334 
336  {
337  return boost::math::constants::pi<FCL_REAL>() * radius * radius * (halfLength * 2);
338  }
339 
341  {
342  FCL_REAL V = computeVolume();
343  FCL_REAL ix = V * (radius * radius / 4 + halfLength * halfLength / 3);
344  FCL_REAL iz = V * radius * radius / 2;
345  return (Matrix3f() << ix, 0, 0,
346  0, ix, 0,
347  0, 0, iz).finished();
348  }
349 
350  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
351 };
352 
356 {
357 public:
370  static ConvexBase* convexHull (const Vec3f* points, int num_points,
371  bool keepTriangles, const char* qhullCommand = NULL);
372 
373  virtual ~ConvexBase();
374 
376  virtual ConvexBase * clone() const
377  {
378  ConvexBase * copy_ptr = new ConvexBase(*this);
379  ConvexBase & copy = *copy_ptr;
380 
381  if(!copy.own_storage_)
382  {
383  copy.points = new Vec3f[copy.num_points];
384  memcpy(copy.points, points, sizeof(Vec3f) * (size_t)copy.num_points);
385  }
386  copy.own_storage_ = true;
387 
388  return copy_ptr;
389  }
390 
392  void computeLocalAABB();
393 
395  NODE_TYPE getNodeType() const { return GEOM_CONVEX; }
396 
400 
402  {
403  unsigned char count_;
404  unsigned int* n_;
405 
406  unsigned char const& count () const { return count_; }
407  unsigned int & operator[] (int i) { assert(i<count_); return n_[i]; }
408  unsigned int const& operator[] (int i) const { assert(i<count_); return n_[i]; }
409  };
414 
417 
418 protected:
421  ConvexBase () : ShapeBase(), points(NULL), num_points(0),
422  neighbors(NULL), nneighbors_(NULL), own_storage_(false) {}
423 
430  void initialize(bool ownStorage, Vec3f* points_, int num_points_);
431 
434  ConvexBase(const ConvexBase& other);
435 
436  unsigned int* nneighbors_;
437 
439 
440 private:
441  void computeCenter();
442 
443 public:
444 
445  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
446 };
447 
448 template <typename PolygonT> class Convex;
449 
454 {
455 public:
457  Halfspace(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_)
458  {
459  unitNormalTest();
460  }
461 
463  Halfspace(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : ShapeBase(), n(a, b, c), d(d_)
464  {
465  unitNormalTest();
466  }
467 
468  Halfspace() : ShapeBase(), n(1, 0, 0), d(0)
469  {
470  }
471 
472  Halfspace(const Halfspace & other)
473  : ShapeBase(other)
474  , n(other.n)
475  , d(other.d)
476  {
477  }
478 
480  Halfspace& operator = (const Halfspace& other)
481  {
482  n = other.n;
483  d = other.d;
484  return *this;
485  }
486 
488  virtual Halfspace* clone() const { return new Halfspace(*this); };
489 
490  FCL_REAL signedDistance(const Vec3f& p) const
491  {
492  return n.dot(p) - d;
493  }
494 
495  FCL_REAL distance(const Vec3f& p) const
496  {
497  return std::abs(n.dot(p) - d);
498  }
499 
501  void computeLocalAABB();
502 
505 
508 
511 
512 protected:
513 
515  void unitNormalTest();
516 
517 public:
518 
519  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
520 };
521 
524 {
525 public:
527  Plane(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_)
528  {
529  unitNormalTest();
530  }
531 
533  Plane(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : ShapeBase(), n(a, b, c), d(d_)
534  {
535  unitNormalTest();
536  }
537 
538  Plane() : ShapeBase(), n(1, 0, 0), d(0)
539  {}
540 
541  Plane(const Plane & other)
542  : ShapeBase(other)
543  , n(other.n)
544  , d(other.d)
545  {
546  }
547 
549  Plane& operator = (const Plane& other)
550  {
551  n = other.n;
552  d = other.d;
553  return *this;
554  }
555 
557  virtual Plane* clone() const { return new Plane(*this); };
558 
559  FCL_REAL signedDistance(const Vec3f& p) const
560  {
561  return n.dot(p) - d;
562  }
563 
564  FCL_REAL distance(const Vec3f& p) const
565  {
566  return std::abs(n.dot(p) - d);
567  }
568 
570  void computeLocalAABB();
571 
573  NODE_TYPE getNodeType() const { return GEOM_PLANE; }
574 
577 
580 
581 protected:
582 
584  void unitNormalTest();
585 
586 public:
587 
588  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
589 };
590 
591 }
592 
593 } // namespace hpp
594 
595 #endif
Definition: collision_object.h:59
Vec3f halfSide
box side half-length
Definition: geometric_shapes.h:125
NODE_TYPE getNodeType() const
Get node type: a box.
Definition: geometric_shapes.h:131
bool own_storage_
Definition: geometric_shapes.h:438
NODE_TYPE getNodeType() const
Get node type: a cone.
Definition: geometric_shapes.h:277
virtual TriangleP * clone() const
Clone *this into a new TriangleP.
Definition: geometric_shapes.h:89
Halfspace()
Definition: geometric_shapes.h:468
Vec3f n
Plane normal.
Definition: geometric_shapes.h:507
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: geometric_shapes.h:284
Vec3f computeCOM() const
compute center of mass
Definition: geometric_shapes.h:295
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:271
Sphere(FCL_REAL radius_)
Definition: geometric_shapes.h:152
Definition: collision_object.h:59
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:55
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: geometric_shapes.h:305
Main namespace.
Definition: AABB.h:43
FCL_REAL signedDistance(const Vec3f &p) const
Definition: geometric_shapes.h:490
Definition: geometric_shapes.h:401
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: geometric_shapes.h:227
Halfspace(const Vec3f &n_, FCL_REAL d_)
Construct a half space with normal direction and offset.
Definition: geometric_shapes.h:457
TriangleP(const TriangleP &other)
Definition: geometric_shapes.h:83
Capsule(const Capsule &other)
Definition: geometric_shapes.h:200
Plane(const Plane &other)
Definition: geometric_shapes.h:541
bool initialize(MeshCollisionTraversalNode< BV, RelativeTransformationIsIdentity > &node, BVHModel< BV > &model1, Transform3f &tf1, BVHModel< BV > &model2, Transform3f &tf2, CollisionResult &result, bool use_refit=false, bool refit_bottomup=false)
Initialize traversal node for collision between two meshes, given the current transforms.
Definition: traversal_node_setup.h:408
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
NODE_TYPE getNodeType() const
Get node type: a capsule.
Definition: geometric_shapes.h:220
Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; Poi...
Definition: geometric_shapes.h:453
virtual ConvexBase * clone() const
 .
Definition: geometric_shapes.h:376
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: geometric_shapes.h:174
FCL_REAL computeVolume() const
compute the volume
Definition: geometric_shapes.h:335
ConvexBase()
Construct an uninitialized convex object Initialization is done with ConvexBase::initialize.
Definition: geometric_shapes.h:421
Infinite plane.
Definition: geometric_shapes.h:523
Vec3f c
Definition: geometric_shapes.h:96
FCL_REAL computeVolume() const
compute the volume
Definition: geometric_shapes.h:180
virtual ~ShapeBase()
Definition: geometric_shapes.h:64
Box()
Definition: geometric_shapes.h:122
Definition: collision_object.h:59
Base class for all basic geometric shapes.
Definition: geometric_shapes.h:54
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: geometric_shapes.h:340
Halfspace(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_)
Construct a plane with normal direction and offset.
Definition: geometric_shapes.h:463
Definition: collision_object.h:55
virtual Box * clone() const
Clone *this into a new Box.
Definition: geometric_shapes.h:120
TriangleP(const Vec3f &a_, const Vec3f &b_, const Vec3f &c_)
Definition: geometric_shapes.h:78
ShapeBase()
Definition: geometric_shapes.h:57
FCL_REAL radius
Radius of the cone.
Definition: geometric_shapes.h:265
double FCL_REAL
Definition: data_types.h:66
FCL_REAL distance(const Vec3f &p) const
Definition: geometric_shapes.h:564
Halfspace(const Halfspace &other)
Definition: geometric_shapes.h:472
Center at zero point, axis aligned box.
Definition: geometric_shapes.h:102
Definition: collision_object.h:59
unsigned int * n_
Definition: geometric_shapes.h:404
Plane(const Vec3f &n_, FCL_REAL d_)
Construct a plane with normal direction and offset.
Definition: geometric_shapes.h:527
NODE_TYPE getNodeType() const
Get node type: a plane.
Definition: geometric_shapes.h:573
Cone(FCL_REAL radius_, FCL_REAL lz_)
Definition: geometric_shapes.h:252
Triangle stores the points instead of only indices of points.
Definition: geometric_shapes.h:75
virtual Halfspace * clone() const
Clone *this into a new Halfspace.
Definition: geometric_shapes.h:488
FCL_REAL computeVolume() const
compute the volume
Definition: geometric_shapes.h:222
virtual Plane * clone() const
Clone *this into a new Plane.
Definition: geometric_shapes.h:557
Cone The base of the cone is at and the top is at .
Definition: geometric_shapes.h:249
Plane(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_)
Construct a plane with normal direction and offset.
Definition: geometric_shapes.h:533
FCL_REAL d
Plane offset.
Definition: geometric_shapes.h:510
FCL_REAL d
Plane offset.
Definition: geometric_shapes.h:579
unsigned char count_
Definition: geometric_shapes.h:403
FCL_REAL radius
Radius of the sphere.
Definition: geometric_shapes.h:163
int num_points
Definition: geometric_shapes.h:399
Plane()
Definition: geometric_shapes.h:538
Cone(const Cone &other)
Definition: geometric_shapes.h:257
FCL_REAL radius
Radius of the cylinder.
Definition: geometric_shapes.h:321
Vec3f n
Plane normal.
Definition: geometric_shapes.h:576
Box(const Vec3f &side_)
Definition: geometric_shapes.h:109
Center at zero point sphere.
Definition: geometric_shapes.h:149
FCL_REAL computeVolume() const
compute the volume
Definition: geometric_shapes.h:279
Capsule It is where is the distance between the point x and the capsule segment AB...
Definition: geometric_shapes.h:192
Definition: collision_object.h:59
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
Base for convex polytope.
Definition: geometric_shapes.h:355
FCL_REAL computeVolume() const
compute the volume
Definition: geometric_shapes.h:133
unsigned char const & count() const
Definition: geometric_shapes.h:406
Sphere(const Sphere &other)
Definition: geometric_shapes.h:156
Definition: collision_object.h:59
Cylinder(FCL_REAL radius_, FCL_REAL lz_)
Definition: geometric_shapes.h:308
NODE_TYPE getNodeType() const
Get node type: a conex polytope.
Definition: geometric_shapes.h:395
Capsule(FCL_REAL radius_, FCL_REAL lz_)
Definition: geometric_shapes.h:195
FCL_REAL signedDistance(const Vec3f &p) const
Definition: geometric_shapes.h:559
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:327
Box(const Box &other)
Definition: geometric_shapes.h:113
Cylinder(const Cylinder &other)
Definition: geometric_shapes.h:313
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
Convex polytope.
Definition: convex.h:53
NODE_TYPE getNodeType() const
get the node type
Definition: geometric_shapes.h:94
The geometry for the object for collision or distance computation.
Definition: collision_object.h:65
ShapeBase(const ShapeBase &other)
 .
Definition: geometric_shapes.h:60
NODE_TYPE getNodeType() const
Get node type: a half space.
Definition: geometric_shapes.h:504
Neighbors * neighbors
Definition: geometric_shapes.h:413
Definition: collision_object.h:59
Definition: collision_object.h:59
Definition: collision_object.h:59
Vec3f center
center of the convex polytope, this is used for collision: center is guaranteed in the internal of th...
Definition: geometric_shapes.h:416
FCL_REAL radius
Radius of capsule.
Definition: geometric_shapes.h:208
unsigned int * nneighbors_
Definition: geometric_shapes.h:436
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:214
OBJECT_TYPE getObjectType() const
Get object type: a geometric shape.
Definition: geometric_shapes.h:67
FCL_REAL distance(const Vec3f &p) const
Definition: geometric_shapes.h:495
#define HPP_FCL_DLLAPI
Definition: config.hh:64
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: geometric_shapes.h:138
NODE_TYPE getNodeType() const
Get node type: a sphere.
Definition: geometric_shapes.h:172
Vec3f * points
An array of the points of the polygon.
Definition: geometric_shapes.h:398
NODE_TYPE getNodeType() const
Get node type: a cylinder.
Definition: geometric_shapes.h:333
Box(FCL_REAL x, FCL_REAL y, FCL_REAL z)
Definition: geometric_shapes.h:105