hpp-fcl  1.4.4
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 
59  virtual ~ShapeBase () {};
60 
62  OBJECT_TYPE getObjectType() const { return OT_GEOM; }
63 };
64 
68 
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_)
74  {
75  }
76 
78  void computeLocalAABB();
79 
80  NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; }
81 
82  Vec3f a, b, c;
83 };
84 
86 class Box : public ShapeBase
87 {
88 public:
89  Box(FCL_REAL x, FCL_REAL y, FCL_REAL z) : ShapeBase(), halfSide(x/2, y/2, z/2)
90  {
91  }
92 
93  Box(const Vec3f& side_) : ShapeBase(), halfSide(side_/2)
94  {
95  }
96 
97  Box() {}
98 
101 
103  void computeLocalAABB();
104 
106  NODE_TYPE getNodeType() const { return GEOM_BOX; }
107 
109  {
110  return 8*halfSide.prod();
111  }
112 
114  {
115  FCL_REAL V = computeVolume();
116  Vec3f s (halfSide.cwiseAbs2() * V);
117  return (Vec3f (s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal();
118  }
119 };
120 
122 class Sphere : public ShapeBase
123 {
124 public:
125  Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_)
126  {
127  }
128 
131 
133  void computeLocalAABB();
134 
136  NODE_TYPE getNodeType() const { return GEOM_SPHERE; }
137 
139  {
140  FCL_REAL I = 0.4 * radius * radius * computeVolume();
141  return I * Matrix3f::Identity();
142  }
143 
145  {
146  return 4 * boost::math::constants::pi<FCL_REAL>() * radius * radius * radius / 3;
147  }
148 };
149 
154 class Capsule : public ShapeBase
155 {
156 public:
157  Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_)
158  {
159  halfLength = lz_/2;
160  }
161 
164 
167 
169  void computeLocalAABB();
170 
172  NODE_TYPE getNodeType() const { return GEOM_CAPSULE; }
173 
175  {
176  return boost::math::constants::pi<FCL_REAL>() * radius * radius *((halfLength * 2) + radius * 4/3.0);
177  }
178 
180  {
181  FCL_REAL v_cyl = radius * radius * (halfLength * 2) * boost::math::constants::pi<FCL_REAL>();
182  FCL_REAL v_sph = radius * radius * radius * boost::math::constants::pi<FCL_REAL>() * 4 / 3.0;
183 
185  FCL_REAL r2 = radius * radius;
186  FCL_REAL ix = v_cyl * (h2 / 3. + r2 / 4.) + v_sph * (0.4 * r2 + h2 + 0.75 * radius * halfLength);
187  FCL_REAL iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius;
188 
189  return (Matrix3f() << ix, 0, 0,
190  0, ix, 0,
191  0, 0, iz).finished();
192  }
193 
194 };
195 
199 class Cone : public ShapeBase
200 {
201 public:
202  Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_)
203  {
204  halfLength = lz_/2;
205  }
206 
209 
212 
214  void computeLocalAABB();
215 
217  NODE_TYPE getNodeType() const { return GEOM_CONE; }
218 
220  {
221  return boost::math::constants::pi<FCL_REAL>() * radius * radius * (halfLength * 2) / 3;
222  }
223 
225  {
226  FCL_REAL V = computeVolume();
227  FCL_REAL ix = V * (0.4 * halfLength * halfLength + 3 * radius * radius / 20);
228  FCL_REAL iz = 0.3 * V * radius * radius;
229 
230  return (Matrix3f() << ix, 0, 0,
231  0, ix, 0,
232  0, 0, iz).finished();
233  }
234 
236  {
237  return Vec3f(0, 0, -0.5 * halfLength);
238  }
239 };
240 
243 class Cylinder : public ShapeBase
244 {
245 public:
246  Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_)
247  {
248  halfLength = lz_/2;
249  }
250 
253 
256 
258  void computeLocalAABB();
259 
261  NODE_TYPE getNodeType() const { return GEOM_CYLINDER; }
262 
264  {
265  return boost::math::constants::pi<FCL_REAL>() * radius * radius * (halfLength * 2);
266  }
267 
269  {
270  FCL_REAL V = computeVolume();
271  FCL_REAL ix = V * (radius * radius / 4 + halfLength * halfLength / 3);
272  FCL_REAL iz = V * radius * radius / 2;
273  return (Matrix3f() << ix, 0, 0,
274  0, ix, 0,
275  0, 0, iz).finished();
276  }
277 };
278 
281 class ConvexBase : public ShapeBase
282 {
283 public:
284 
285  virtual ~ConvexBase();
286 
288  void computeLocalAABB();
289 
291  NODE_TYPE getNodeType() const { return GEOM_CONVEX; }
292 
296 
297  struct Neighbors
298  {
299  unsigned char count_;
300  unsigned int* n_;
301 
302  unsigned char const& count () const { return count_; }
303  unsigned int & operator[] (int i) { assert(i<count_); return n_[i]; }
304  unsigned int const& operator[] (int i) const { assert(i<count_); return n_[i]; }
305  };
310 
313 
314 protected:
318  ConvexBase(bool ownStorage, Vec3f* points_, int num_points_);
319 
322  ConvexBase(const ConvexBase& other);
323 
324  unsigned int* nneighbors_;
325 
327 
328 private:
329  void computeCenter();
330 };
331 
332 template <typename PolygonT> class Convex;
333 
337 class Halfspace : public ShapeBase
338 {
339 public:
341  Halfspace(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_)
342  {
343  unitNormalTest();
344  }
345 
347  Halfspace(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : ShapeBase(), n(a, b, c), d(d_)
348  {
349  unitNormalTest();
350  }
351 
352  Halfspace() : ShapeBase(), n(1, 0, 0), d(0)
353  {
354  }
355 
356  FCL_REAL signedDistance(const Vec3f& p) const
357  {
358  return n.dot(p) - d;
359  }
360 
361  FCL_REAL distance(const Vec3f& p) const
362  {
363  return std::abs(n.dot(p) - d);
364  }
365 
367  void computeLocalAABB();
368 
371 
374 
377 
378 protected:
379 
381  void unitNormalTest();
382 };
383 
385 class Plane : public ShapeBase
386 {
387 public:
389  Plane(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_)
390  {
391  unitNormalTest();
392  }
393 
395  Plane(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : ShapeBase(), n(a, b, c), d(d_)
396  {
397  unitNormalTest();
398  }
399 
400  Plane() : ShapeBase(), n(1, 0, 0), d(0)
401  {}
402 
403  FCL_REAL signedDistance(const Vec3f& p) const
404  {
405  return n.dot(p) - d;
406  }
407 
408  FCL_REAL distance(const Vec3f& p) const
409  {
410  return std::abs(n.dot(p) - d);
411  }
412 
414  void computeLocalAABB();
415 
417  NODE_TYPE getNodeType() const { return GEOM_PLANE; }
418 
421 
424 
425 protected:
426 
428  void unitNormalTest();
429 };
430 
431 }
432 
433 } // namespace hpp
434 
435 #endif
hpp::fcl::Cylinder::radius
FCL_REAL radius
Radius of the cylinder.
Definition: geometric_shapes.h:252
hpp::fcl::TriangleP::TriangleP
TriangleP(const Vec3f &a_, const Vec3f &b_, const Vec3f &c_)
Definition: geometric_shapes.h:73
hpp::fcl::GEOM_HALFSPACE
@ GEOM_HALFSPACE
Definition: collision_object.h:57
hpp::fcl::ConvexBase::computeLocalAABB
void computeLocalAABB()
Compute AABB.
data_types.h
hpp::fcl::Capsule::getNodeType
NODE_TYPE getNodeType() const
Get node type: a capsule.
Definition: geometric_shapes.h:172
hpp::fcl::Vec3f
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:73
hpp::fcl::Box::computeMomentofInertia
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: geometric_shapes.h:113
hpp::fcl::ConvexBase::neighbors
Neighbors * neighbors
Definition: geometric_shapes.h:309
hpp::fcl::ConvexBase::points
Vec3f * points
An array of the points of the polygon.
Definition: geometric_shapes.h:294
hpp::fcl::Cylinder::computeLocalAABB
void computeLocalAABB()
Compute AABB.
collision_object.h
hpp::fcl::GEOM_BOX
@ GEOM_BOX
Definition: collision_object.h:57
hpp::fcl::ConvexBase::getNodeType
NODE_TYPE getNodeType() const
Get node type: a conex polytope.
Definition: geometric_shapes.h:291
hpp::fcl::Halfspace::Halfspace
Halfspace()
Definition: geometric_shapes.h:352
hpp::fcl::GEOM_PLANE
@ GEOM_PLANE
Definition: collision_object.h:57
hpp::fcl::Cone::Cone
Cone(FCL_REAL radius_, FCL_REAL lz_)
Definition: geometric_shapes.h:202
hpp::fcl::Plane::signedDistance
FCL_REAL signedDistance(const Vec3f &p) const
Definition: geometric_shapes.h:403
hpp::fcl::TriangleP::c
Vec3f c
Definition: geometric_shapes.h:82
hpp::fcl::Cone::computeVolume
FCL_REAL computeVolume() const
compute the volume
Definition: geometric_shapes.h:219
hpp::fcl::Halfspace::signedDistance
FCL_REAL signedDistance(const Vec3f &p) const
Definition: geometric_shapes.h:356
hpp::fcl::Sphere
Center at zero point sphere.
Definition: geometric_shapes.h:122
hpp::fcl::Capsule::radius
FCL_REAL radius
Radius of capsule.
Definition: geometric_shapes.h:163
hpp::fcl::ShapeBase::ShapeBase
ShapeBase()
Definition: geometric_shapes.h:57
hpp::fcl::Halfspace::getNodeType
NODE_TYPE getNodeType() const
Get node type: a half space.
Definition: geometric_shapes.h:370
hpp::fcl::ConvexBase::own_storage_
bool own_storage_
Definition: geometric_shapes.h:326
hpp::fcl::ConvexBase::nneighbors_
unsigned int * nneighbors_
Definition: geometric_shapes.h:324
hpp::fcl::Box::Box
Box(FCL_REAL x, FCL_REAL y, FCL_REAL z)
Definition: geometric_shapes.h:89
hpp::fcl::Plane::unitNormalTest
void unitNormalTest()
Turn non-unit normal into unit.
hpp::fcl::GEOM_CONVEX
@ GEOM_CONVEX
Definition: collision_object.h:57
hpp::fcl::Cylinder::getNodeType
NODE_TYPE getNodeType() const
Get node type: a cylinder.
Definition: geometric_shapes.h:261
hpp::fcl::ShapeBase::~ShapeBase
virtual ~ShapeBase()
Definition: geometric_shapes.h:59
hpp::fcl::CollisionGeometry
The geometry for the object for collision or distance computation.
Definition: collision_object.h:63
hpp::fcl::Box::halfSide
Vec3f halfSide
box side half-length
Definition: geometric_shapes.h:100
hpp::fcl::NODE_TYPE
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_object.h:56
hpp::fcl::Cylinder
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: geometric_shapes.h:243
hpp::fcl::Capsule::halfLength
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:166
hpp::fcl::Halfspace::distance
FCL_REAL distance(const Vec3f &p) const
Definition: geometric_shapes.h:361
hpp::fcl::ConvexBase::Neighbors::count
unsigned char const & count() const
Definition: geometric_shapes.h:302
hpp::fcl::TriangleP::b
Vec3f b
Definition: geometric_shapes.h:82
hpp::fcl::Cylinder::Cylinder
Cylinder(FCL_REAL radius_, FCL_REAL lz_)
Definition: geometric_shapes.h:246
hpp::fcl::Halfspace::unitNormalTest
void unitNormalTest()
Turn non-unit normal into unit.
hpp::fcl::Cylinder::halfLength
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:255
hpp::fcl::Convex
Convex polytope.
Definition: convex.h:53
hpp::fcl::Capsule::Capsule
Capsule(FCL_REAL radius_, FCL_REAL lz_)
Definition: geometric_shapes.h:157
hpp::fcl::Plane::Plane
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:395
hpp::fcl::OT_GEOM
@ OT_GEOM
Definition: collision_object.h:53
hpp::fcl::Box::computeVolume
FCL_REAL computeVolume() const
compute the volume
Definition: geometric_shapes.h:108
hpp::fcl::FCL_REAL
double FCL_REAL
Definition: data_types.h:68
hpp::fcl::Sphere::radius
FCL_REAL radius
Radius of the sphere.
Definition: geometric_shapes.h:130
hpp::fcl::TriangleP::computeLocalAABB
void computeLocalAABB()
virtual function of compute AABB in local coordinate
hpp::fcl::OBJECT_TYPE
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:53
hpp::fcl::Cylinder::computeMomentofInertia
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: geometric_shapes.h:268
hpp::fcl::Sphere::computeVolume
FCL_REAL computeVolume() const
compute the volume
Definition: geometric_shapes.h:144
hpp::fcl::Cone::computeLocalAABB
void computeLocalAABB()
Compute AABB.
hpp::fcl::Sphere::computeMomentofInertia
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: geometric_shapes.h:138
hpp::fcl::Capsule::computeMomentofInertia
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: geometric_shapes.h:179
hpp::fcl::Capsule::computeVolume
FCL_REAL computeVolume() const
compute the volume
Definition: geometric_shapes.h:174
hpp::fcl::Box::getNodeType
NODE_TYPE getNodeType() const
Get node type: a box.
Definition: geometric_shapes.h:106
hpp::fcl::Plane::computeLocalAABB
void computeLocalAABB()
Compute AABB.
hpp::fcl::Cone::computeCOM
Vec3f computeCOM() const
compute center of mass
Definition: geometric_shapes.h:235
hpp::fcl::Box::computeLocalAABB
void computeLocalAABB()
Compute AABB.
hpp::fcl::GEOM_SPHERE
@ GEOM_SPHERE
Definition: collision_object.h:57
hpp
Main namespace.
Definition: AABB.h:43
hpp::fcl::Plane::Plane
Plane()
Definition: geometric_shapes.h:400
hpp::fcl::Cone::getNodeType
NODE_TYPE getNodeType() const
Get node type: a cone.
Definition: geometric_shapes.h:217
hpp::fcl::ConvexBase::Neighbors::count_
unsigned char count_
Definition: geometric_shapes.h:299
hpp::fcl::Halfspace::computeLocalAABB
void computeLocalAABB()
Compute AABB.
hpp::fcl::ConvexBase::~ConvexBase
virtual ~ConvexBase()
hpp::fcl::Plane::getNodeType
NODE_TYPE getNodeType() const
Get node type: a plane.
Definition: geometric_shapes.h:417
hpp::fcl::Cone
Cone The base of the cone is at and the top is at .
Definition: geometric_shapes.h:199
hpp::fcl::Capsule
Capsule It is where is the distance between the point x and the capsule segment AB,...
Definition: geometric_shapes.h:154
hpp::fcl::Cone::radius
FCL_REAL radius
Radius of the cone.
Definition: geometric_shapes.h:208
hpp::fcl::TriangleP::a
Vec3f a
Definition: geometric_shapes.h:82
hpp::fcl::Matrix3f
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:74
hpp::fcl::Plane::d
FCL_REAL d
Plane offset.
Definition: geometric_shapes.h:423
hpp::fcl::ConvexBase::ConvexBase
ConvexBase(bool ownStorage, Vec3f *points_, int num_points_)
Constructing a convex, providing normal and offset of each polytype surface, and the points and shape...
hpp::fcl::TriangleP::getNodeType
NODE_TYPE getNodeType() const
get the node type
Definition: geometric_shapes.h:80
hpp::fcl::Plane::Plane
Plane(const Vec3f &n_, FCL_REAL d_)
Construct a plane with normal direction and offset.
Definition: geometric_shapes.h:389
hpp::fcl::ConvexBase
Base for convex polytope.
Definition: geometric_shapes.h:281
hpp::fcl::ConvexBase::center
Vec3f center
center of the convex polytope, this is used for collision: center is guaranteed in the internal of th...
Definition: geometric_shapes.h:312
hpp::fcl::Box::Box
Box(const Vec3f &side_)
Definition: geometric_shapes.h:93
hpp::fcl::Halfspace
Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; Poi...
Definition: geometric_shapes.h:337
hpp::fcl::Cone::computeMomentofInertia
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: geometric_shapes.h:224
hpp::fcl::Halfspace::Halfspace
Halfspace(const Vec3f &n_, FCL_REAL d_)
Construct a half space with normal direction and offset.
Definition: geometric_shapes.h:341
hpp::fcl::Sphere::getNodeType
NODE_TYPE getNodeType() const
Get node type: a sphere.
Definition: geometric_shapes.h:136
hpp::fcl::GEOM_CYLINDER
@ GEOM_CYLINDER
Definition: collision_object.h:57
hpp::fcl::ConvexBase::Neighbors::operator[]
unsigned int & operator[](int i)
Definition: geometric_shapes.h:303
hpp::fcl::Sphere::Sphere
Sphere(FCL_REAL radius_)
Definition: geometric_shapes.h:125
hpp::fcl::GEOM_CONE
@ GEOM_CONE
Definition: collision_object.h:57
hpp::fcl::ShapeBase::getObjectType
OBJECT_TYPE getObjectType() const
Get object type: a geometric shape.
Definition: geometric_shapes.h:62
hpp::fcl::Box::Box
Box()
Definition: geometric_shapes.h:97
hpp::fcl::TriangleP
Triangle stores the points instead of only indices of points.
Definition: geometric_shapes.h:70
hpp::fcl::Cone::halfLength
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:211
hpp::fcl::Plane::distance
FCL_REAL distance(const Vec3f &p) const
Definition: geometric_shapes.h:408
hpp::fcl::Halfspace::Halfspace
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:347
hpp::fcl::Plane::n
Vec3f n
Plane normal.
Definition: geometric_shapes.h:420
hpp::fcl::ConvexBase::Neighbors::n_
unsigned int * n_
Definition: geometric_shapes.h:300
hpp::fcl::ConvexBase::num_points
int num_points
Definition: geometric_shapes.h:295
hpp::fcl::ShapeBase
Base class for all basic geometric shapes.
Definition: geometric_shapes.h:54
hpp::fcl::Capsule::computeLocalAABB
void computeLocalAABB()
Compute AABB.
hpp::fcl::GEOM_CAPSULE
@ GEOM_CAPSULE
Definition: collision_object.h:57
hpp::fcl::Sphere::computeLocalAABB
void computeLocalAABB()
Compute AABB.
hpp::fcl::Halfspace::d
FCL_REAL d
Plane offset.
Definition: geometric_shapes.h:376
hpp::fcl::ConvexBase::Neighbors
Definition: geometric_shapes.h:297
hpp::fcl::Cylinder::computeVolume
FCL_REAL computeVolume() const
compute the volume
Definition: geometric_shapes.h:263
hpp::fcl::GEOM_TRIANGLE
@ GEOM_TRIANGLE
Definition: collision_object.h:57
hpp::fcl::Halfspace::n
Vec3f n
Plane normal.
Definition: geometric_shapes.h:373
hpp::fcl::Box
Center at zero point, axis aligned box.
Definition: geometric_shapes.h:86
hpp::fcl::Plane
Infinite plane.
Definition: geometric_shapes.h:385