hpp-fcl 1.8.1
HPP fork of FCL -- The Flexible Collision Library
Loading...
Searching...
No Matches
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
48namespace hpp
49{
50namespace fcl
51{
52
55{
56public:
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{
77public:
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
93
95
96 Vec3f a, b, c;
97
98 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
99};
100
103{
104public:
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
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{
151public:
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
170
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{
194public:
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
218
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{
251public:
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
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{
307public:
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
331
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{
357public:
370 static ConvexBase* convexHull (const Vec3f* points, unsigned 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((void*)copy.points, points, sizeof(Vec3f) * (size_t)copy.num_points);
385 }
386 copy.own_storage_ = true;
387
388 return copy_ptr;
389 }
390
393
396
399 unsigned int num_points;
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
418protected:
421 ConvexBase () : ShapeBase(), points(NULL), num_points(0),
422 neighbors(NULL), nneighbors_(NULL), own_storage_(false) {}
423
430 void initialize(bool ownStorage, Vec3f* points_, unsigned int num_points_);
431
437 void set(bool ownStorage, Vec3f* points_, unsigned int num_points_);
438
441 ConvexBase(const ConvexBase& other);
442
443 unsigned int* nneighbors_;
444
446
447private:
448 void computeCenter();
449
450public:
451
452 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
453};
454
455template <typename PolygonT> class Convex;
456
461{
462public:
464 Halfspace(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_)
465 {
466 unitNormalTest();
467 }
468
470 Halfspace(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : ShapeBase(), n(a, b, c), d(d_)
471 {
472 unitNormalTest();
473 }
474
475 Halfspace() : ShapeBase(), n(1, 0, 0), d(0)
476 {
477 }
478
479 Halfspace(const Halfspace & other)
480 : ShapeBase(other)
481 , n(other.n)
482 , d(other.d)
483 {
484 }
485
487 Halfspace& operator = (const Halfspace& other)
488 {
489 n = other.n;
490 d = other.d;
491 return *this;
492 }
493
495 virtual Halfspace* clone() const { return new Halfspace(*this); };
496
498 {
499 return n.dot(p) - d;
500 }
501
502 FCL_REAL distance(const Vec3f& p) const
503 {
504 return std::abs(n.dot(p) - d);
505 }
506
509
512
515
518
519protected:
520
523
524public:
525
526 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
527};
528
531{
532public:
534 Plane(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_)
535 {
536 unitNormalTest();
537 }
538
540 Plane(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : ShapeBase(), n(a, b, c), d(d_)
541 {
542 unitNormalTest();
543 }
544
545 Plane() : ShapeBase(), n(1, 0, 0), d(0)
546 {}
547
548 Plane(const Plane & other)
549 : ShapeBase(other)
550 , n(other.n)
551 , d(other.d)
552 {
553 }
554
556 Plane& operator = (const Plane& other)
557 {
558 n = other.n;
559 d = other.d;
560 return *this;
561 }
562
564 virtual Plane* clone() const { return new Plane(*this); };
565
567 {
568 return n.dot(p) - d;
569 }
570
571 FCL_REAL distance(const Vec3f& p) const
572 {
573 return std::abs(n.dot(p) - d);
574 }
575
578
580 NODE_TYPE getNodeType() const { return GEOM_PLANE; }
581
584
587
588protected:
589
592
593public:
594
595 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
596};
597
598}
599
600} // namespace hpp
601
602#endif
Center at zero point, axis aligned box.
Definition: geometric_shapes.h:103
Capsule It is where is the distance between the point x and the capsule segment AB,...
Definition: geometric_shapes.h:193
The geometry for the object for collision or distance computation.
Definition: collision_object.h:67
Cone The base of the cone is at and the top is at .
Definition: geometric_shapes.h:250
Base for convex polytope.
Definition: geometric_shapes.h:356
Convex polytope.
Definition: convex.h:54
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: geometric_shapes.h:306
Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; Poi...
Definition: geometric_shapes.h:461
Infinite plane.
Definition: geometric_shapes.h:531
Base class for all basic geometric shapes.
Definition: geometric_shapes.h:55
Center at zero point sphere.
Definition: geometric_shapes.h:150
Triangle stores the points instead of only indices of points.
Definition: geometric_shapes.h:76
#define HPP_FCL_DLLAPI
Definition: config.hh:64
@ OT_GEOM
Definition: collision_object.h:55
@ GEOM_TRIANGLE
Definition: collision_object.h:59
@ GEOM_CAPSULE
Definition: collision_object.h:59
@ GEOM_PLANE
Definition: collision_object.h:59
@ GEOM_HALFSPACE
Definition: collision_object.h:59
@ GEOM_BOX
Definition: collision_object.h:59
@ GEOM_CONVEX
Definition: collision_object.h:59
@ GEOM_CYLINDER
Definition: collision_object.h:59
@ GEOM_SPHERE
Definition: collision_object.h:59
@ GEOM_CONE
Definition: collision_object.h:59
void initialize(bool ownStorage, Vec3f *points_, unsigned int num_points_)
Initialize the points of the convex shape This also initializes the ConvexBase::center.
FCL_REAL computeVolume() const
compute the volume
Definition: geometric_shapes.h:279
void computeLocalAABB()
Compute AABB.
void computeLocalAABB()
Compute AABB.
Plane(const Vec3f &n_, FCL_REAL d_)
Construct a plane with normal direction and offset.
Definition: geometric_shapes.h:534
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:214
ShapeBase()
Definition: geometric_shapes.h:57
virtual Capsule * clone() const
Clone *this into a new Capsule.
Definition: geometric_shapes.h:208
Capsule(const Capsule &other)
Definition: geometric_shapes.h:200
virtual Halfspace * clone() const
Clone *this into a new Halfspace.
Definition: geometric_shapes.h:495
FCL_REAL computeVolume() const
compute the volume
Definition: geometric_shapes.h:133
NODE_TYPE getNodeType() const
Get node type: a plane.
Definition: geometric_shapes.h:580
FCL_REAL computeVolume() const
compute the volume
Definition: geometric_shapes.h:335
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:470
Vec3f * points
An array of the points of the polygon.
Definition: geometric_shapes.h:398
Sphere(const Sphere &other)
Definition: geometric_shapes.h:156
Halfspace(const Halfspace &other)
Definition: geometric_shapes.h:479
ConvexBase()
Construct an uninitialized convex object Initialization is done with ConvexBase::initialize.
Definition: geometric_shapes.h:421
Halfspace()
Definition: geometric_shapes.h:475
NODE_TYPE getNodeType() const
Get node type: a conex polytope.
Definition: geometric_shapes.h:395
FCL_REAL distance(const Vec3f &p) const
Definition: geometric_shapes.h:502
void unitNormalTest()
Turn non-unit normal into unit.
ShapeBase(const ShapeBase &other)
&#160;
Definition: geometric_shapes.h:60
NODE_TYPE getNodeType() const
Get node type: a capsule.
Definition: geometric_shapes.h:220
Box(FCL_REAL x, FCL_REAL y, FCL_REAL z)
Definition: geometric_shapes.h:105
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: geometric_shapes.h:284
TriangleP(const Vec3f &a_, const Vec3f &b_, const Vec3f &c_)
Definition: geometric_shapes.h:78
Halfspace(const Vec3f &n_, FCL_REAL d_)
Construct a half space with normal direction and offset.
Definition: geometric_shapes.h:464
void computeLocalAABB()
Compute AABB.
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: geometric_shapes.h:340
void computeLocalAABB()
Compute AABB.
Cylinder(const Cylinder &other)
Definition: geometric_shapes.h:313
unsigned int * nneighbors_
Definition: geometric_shapes.h:443
Vec3f a
Definition: geometric_shapes.h:96
FCL_REAL computeVolume() const
compute the volume
Definition: geometric_shapes.h:222
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:327
Vec3f n
Plane normal.
Definition: geometric_shapes.h:583
void computeLocalAABB()
Compute AABB.
virtual ConvexBase * clone() const
&#160;
Definition: geometric_shapes.h:376
void unitNormalTest()
Turn non-unit normal into unit.
NODE_TYPE getNodeType() const
get the node type
Definition: geometric_shapes.h:94
FCL_REAL radius
Radius of capsule.
Definition: geometric_shapes.h:211
void computeLocalAABB()
virtual function of compute AABB in local coordinate
static ConvexBase * convexHull(const Vec3f *points, unsigned int num_points, bool keepTriangles, const char *qhullCommand=NULL)
Build a convex hull based on Qhull library and store the vertices and optionally the triangles.
unsigned char const & count() const
Definition: geometric_shapes.h:406
virtual ~ShapeBase()
Definition: geometric_shapes.h:64
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: geometric_shapes.h:138
bool own_storage_
Definition: geometric_shapes.h:445
Box(const Vec3f &side_)
Definition: geometric_shapes.h:109
NODE_TYPE getNodeType() const
Get node type: a half space.
Definition: geometric_shapes.h:511
virtual Cone * clone() const
Clone *this into a new Cone.
Definition: geometric_shapes.h:265
Plane(const Plane &other)
Definition: geometric_shapes.h:548
Cylinder(FCL_REAL radius_, FCL_REAL lz_)
Definition: geometric_shapes.h:308
Vec3f n
Plane normal.
Definition: geometric_shapes.h:514
Plane()
Definition: geometric_shapes.h:545
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:271
Neighbors * neighbors
Definition: geometric_shapes.h:413
unsigned int * n_
Definition: geometric_shapes.h:404
FCL_REAL signedDistance(const Vec3f &p) const
Definition: geometric_shapes.h:497
unsigned int num_points
Definition: geometric_shapes.h:399
void computeLocalAABB()
Compute AABB.
FCL_REAL distance(const Vec3f &p) const
Definition: geometric_shapes.h:571
virtual Cylinder * clone() const
Clone *this into a new Cylinder.
Definition: geometric_shapes.h:321
FCL_REAL d
Plane offset.
Definition: geometric_shapes.h:586
Vec3f computeCOM() const
compute center of mass
Definition: geometric_shapes.h:295
FCL_REAL radius
Radius of the sphere.
Definition: geometric_shapes.h:166
FCL_REAL signedDistance(const Vec3f &p) const
Definition: geometric_shapes.h:566
Vec3f halfSide
box side half-length
Definition: geometric_shapes.h:125
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
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:540
TriangleP(const TriangleP &other)
Definition: geometric_shapes.h:83
void computeLocalAABB()
Compute AABB.
Sphere(FCL_REAL radius_)
Definition: geometric_shapes.h:152
FCL_REAL radius
Radius of the cone.
Definition: geometric_shapes.h:268
virtual Box * clone() const
Clone *this into a new Box.
Definition: geometric_shapes.h:120
ConvexBase(const ConvexBase &other)
Copy constructor Only the list of neighbors is copied.
FCL_REAL computeVolume() const
compute the volume
Definition: geometric_shapes.h:180
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: geometric_shapes.h:227
FCL_REAL radius
Radius of the cylinder.
Definition: geometric_shapes.h:324
virtual TriangleP * clone() const
Clone *this into a new TriangleP.
Definition: geometric_shapes.h:89
Box()
Definition: geometric_shapes.h:122
NODE_TYPE getNodeType() const
Get node type: a box.
Definition: geometric_shapes.h:131
Cone(const Cone &other)
Definition: geometric_shapes.h:257
Cone(FCL_REAL radius_, FCL_REAL lz_)
Definition: geometric_shapes.h:252
Capsule(FCL_REAL radius_, FCL_REAL lz_)
Definition: geometric_shapes.h:195
OBJECT_TYPE getObjectType() const
Get object type: a geometric shape.
Definition: geometric_shapes.h:67
NODE_TYPE getNodeType() const
Get node type: a cone.
Definition: geometric_shapes.h:277
virtual Plane * clone() const
Clone *this into a new Plane.
Definition: geometric_shapes.h:564
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: geometric_shapes.h:174
NODE_TYPE getNodeType() const
Get node type: a cylinder.
Definition: geometric_shapes.h:333
virtual Sphere * clone() const
Clone *this into a new Sphere.
Definition: geometric_shapes.h:163
NODE_TYPE getNodeType() const
Get node type: a sphere.
Definition: geometric_shapes.h:172
void computeLocalAABB()
Compute AABB.
void set(bool ownStorage, Vec3f *points_, unsigned int num_points_)
Set the points of the convex shape.
FCL_REAL d
Plane offset.
Definition: geometric_shapes.h:517
Box(const Box &other)
Definition: geometric_shapes.h:113
unsigned char count_
Definition: geometric_shapes.h:403
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:69
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:67
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:55
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_object.h:58
double FCL_REAL
Definition: data_types.h:66
Main namespace.
Definition: AABB.h:44
Definition: geometric_shapes.h:402