hpp-fcl 2.4.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
38#ifndef HPP_FCL_GEOMETRIC_SHAPES_H
39#define HPP_FCL_GEOMETRIC_SHAPES_H
40
41#include <boost/math/constants/constants.hpp>
42
44#include <hpp/fcl/data_types.h>
45#include <string.h>
46
47namespace hpp {
48namespace fcl {
49
52 public:
54
56 ShapeBase(const ShapeBase& other) : CollisionGeometry(other) {}
57
58 ShapeBase& operator=(const ShapeBase& other) = default;
59
60 virtual ~ShapeBase(){};
61
63 OBJECT_TYPE getObjectType() const { return OT_GEOM; }
64};
65
69
72 public:
74
75 TriangleP(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_)
76 : ShapeBase(), a(a_), b(b_), c(c_) {}
77
78 TriangleP(const TriangleP& other)
79 : ShapeBase(other), a(other.a), b(other.b), c(other.c) {}
80
82 virtual TriangleP* clone() const { return new TriangleP(*this); };
83
86
88
89 // std::pair<ShapeBase*, Transform3f> inflated(const FCL_REAL value) const {
90 // if (value == 0) return std::make_pair(new TriangleP(*this),
91 // Transform3f()); Vec3f AB(b - a), BC(c - b), CA(a - c); AB.normalize();
92 // BC.normalize();
93 // CA.normalize();
94 //
95 // Vec3f new_a(a + value * Vec3f(-AB + CA).normalized());
96 // Vec3f new_b(b + value * Vec3f(-BC + AB).normalized());
97 // Vec3f new_c(c + value * Vec3f(-CA + BC).normalized());
98 //
99 // return std::make_pair(new TriangleP(new_a, new_b, new_c),
100 // Transform3f());
101 // }
102 //
103 // FCL_REAL minInflationValue() const
104 // {
105 // return (std::numeric_limits<FCL_REAL>::max)(); // TODO(jcarpent):
106 // implement
107 // }
108
109 Vec3f a, b, c;
110
111 private:
112 virtual bool isEqual(const CollisionGeometry& _other) const {
113 const TriangleP* other_ptr = dynamic_cast<const TriangleP*>(&_other);
114 if (other_ptr == nullptr) return false;
115 const TriangleP& other = *other_ptr;
116
117 return a == other.a && b == other.b && c == other.c;
118 }
119
120 public:
121 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
122};
123
126 public:
128 : ShapeBase(), halfSide(x / 2, y / 2, z / 2) {}
129
130 Box(const Vec3f& side_) : ShapeBase(), halfSide(side_ / 2) {}
131
132 Box(const Box& other) : ShapeBase(other), halfSide(other.halfSide) {}
133
134 Box& operator=(const Box& other) {
135 if (this == &other) return *this;
136
137 this->halfSide = other.halfSide;
138 return *this;
139 }
140
142 virtual Box* clone() const { return new Box(*this); };
143
145 Box() {}
146
149
152
154 NODE_TYPE getNodeType() const { return GEOM_BOX; }
155
156 FCL_REAL computeVolume() const { return 8 * halfSide.prod(); }
157
159 FCL_REAL V = computeVolume();
160 Vec3f s(halfSide.cwiseAbs2() * V);
161 return (Vec3f(s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal();
162 }
163
164 FCL_REAL minInflationValue() const { return -halfSide.minCoeff(); }
165
172 std::pair<Box, Transform3f> inflated(const FCL_REAL value) const {
173 if (value <= minInflationValue())
174 HPP_FCL_THROW_PRETTY("value (" << value << ") "
175 << "is two small. It should be at least: "
176 << minInflationValue(),
177 std::invalid_argument);
178 return std::make_pair(Box(2 * (halfSide + Vec3f::Constant(value))),
179 Transform3f());
180 }
181
182 private:
183 virtual bool isEqual(const CollisionGeometry& _other) const {
184 const Box* other_ptr = dynamic_cast<const Box*>(&_other);
185 if (other_ptr == nullptr) return false;
186 const Box& other = *other_ptr;
187
188 return halfSide == other.halfSide;
189 }
190
191 public:
192 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
193};
194
197 public:
200
201 explicit Sphere(FCL_REAL radius_) : ShapeBase(), radius(radius_) {}
202
203 Sphere(const Sphere& other) : ShapeBase(other), radius(other.radius) {}
204
206 virtual Sphere* clone() const { return new Sphere(*this); };
207
210
213
216
218 FCL_REAL I = 0.4 * radius * radius * computeVolume();
219 return I * Matrix3f::Identity();
220 }
221
223 return 4 * boost::math::constants::pi<FCL_REAL>() * radius * radius *
224 radius / 3;
225 }
226
227 FCL_REAL minInflationValue() const { return -radius; }
228
235 std::pair<Sphere, Transform3f> inflated(const FCL_REAL value) const {
236 if (value <= minInflationValue())
238 "value (" << value << ") is two small. It should be at least: "
239 << minInflationValue(),
240 std::invalid_argument);
241 return std::make_pair(Sphere(radius + value), Transform3f());
242 }
243
244 private:
245 virtual bool isEqual(const CollisionGeometry& _other) const {
246 const Sphere* other_ptr = dynamic_cast<const Sphere*>(&_other);
247 if (other_ptr == nullptr) return false;
248 const Sphere& other = *other_ptr;
249
250 return radius == other.radius;
251 }
252
253 public:
254 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
255};
256
259 public:
262
264 : ShapeBase(), radii(rx, ry, rz) {}
265
266 explicit Ellipsoid(const Vec3f& radii) : radii(radii) {}
267
268 Ellipsoid(const Ellipsoid& other) : ShapeBase(other), radii(other.radii) {}
269
271 virtual Ellipsoid* clone() const { return new Ellipsoid(*this); };
272
276
279
282
284 FCL_REAL V = computeVolume();
285 FCL_REAL a2 = V * radii[0] * radii[0];
286 FCL_REAL b2 = V * radii[1] * radii[1];
287 FCL_REAL c2 = V * radii[2] * radii[2];
288 return (Matrix3f() << 0.2 * (b2 + c2), 0, 0, 0, 0.2 * (a2 + c2), 0, 0, 0,
289 0.2 * (a2 + b2))
290 .finished();
291 }
292
294 return 4 * boost::math::constants::pi<FCL_REAL>() * radii[0] * radii[1] *
295 radii[2] / 3;
296 }
297
298 FCL_REAL minInflationValue() const { return -radii.minCoeff(); }
299
306 std::pair<Ellipsoid, Transform3f> inflated(const FCL_REAL value) const {
307 if (value <= minInflationValue())
309 "value (" << value << ") is two small. It should be at least: "
310 << minInflationValue(),
311 std::invalid_argument);
312 return std::make_pair(Ellipsoid(radii + Vec3f::Constant(value)),
313 Transform3f());
314 }
315
316 private:
317 virtual bool isEqual(const CollisionGeometry& _other) const {
318 const Ellipsoid* other_ptr = dynamic_cast<const Ellipsoid*>(&_other);
319 if (other_ptr == nullptr) return false;
320 const Ellipsoid& other = *other_ptr;
321
322 return radii == other.radii;
323 }
324
325 public:
326 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
327};
328
334 public:
337
338 Capsule(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) {
339 halfLength = lz_ / 2;
340 }
341
342 Capsule(const Capsule& other)
343 : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {}
344
346 virtual Capsule* clone() const { return new Capsule(*this); };
347
350
353
356
359
361 return boost::math::constants::pi<FCL_REAL>() * radius * radius *
362 ((halfLength * 2) + radius * 4 / 3.0);
363 }
364
366 FCL_REAL v_cyl = radius * radius * (halfLength * 2) *
367 boost::math::constants::pi<FCL_REAL>();
368 FCL_REAL v_sph = radius * radius * radius *
369 boost::math::constants::pi<FCL_REAL>() * 4 / 3.0;
370
371 FCL_REAL h2 = halfLength * halfLength;
372 FCL_REAL r2 = radius * radius;
373 FCL_REAL ix = v_cyl * (h2 / 3. + r2 / 4.) +
374 v_sph * (0.4 * r2 + h2 + 0.75 * radius * halfLength);
375 FCL_REAL iz = (0.5 * v_cyl + 0.4 * v_sph) * radius * radius;
376
377 return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
378 }
379
380 FCL_REAL minInflationValue() const { return -radius; }
381
388 std::pair<Capsule, Transform3f> inflated(const FCL_REAL value) const {
389 if (value <= minInflationValue())
391 "value (" << value << ") is two small. It should be at least: "
392 << minInflationValue(),
393 std::invalid_argument);
394 return std::make_pair(Capsule(radius + value, 2 * halfLength),
395 Transform3f());
396 }
397
398 private:
399 virtual bool isEqual(const CollisionGeometry& _other) const {
400 const Capsule* other_ptr = dynamic_cast<const Capsule*>(&_other);
401 if (other_ptr == nullptr) return false;
402 const Capsule& other = *other_ptr;
403
404 return radius == other.radius && halfLength == other.halfLength;
405 }
406
407 public:
408 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
409};
410
415 public:
417 Cone() {}
418
419 Cone(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) {
420 halfLength = lz_ / 2;
421 }
422
423 Cone(const Cone& other)
424 : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {}
425
427 virtual Cone* clone() const { return new Cone(*this); };
428
431
434
437
439 NODE_TYPE getNodeType() const { return GEOM_CONE; }
440
442 return boost::math::constants::pi<FCL_REAL>() * radius * radius *
443 (halfLength * 2) / 3;
444 }
445
447 FCL_REAL V = computeVolume();
448 FCL_REAL ix =
449 V * (0.4 * halfLength * halfLength + 3 * radius * radius / 20);
450 FCL_REAL iz = 0.3 * V * radius * radius;
451
452 return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
453 }
454
455 Vec3f computeCOM() const { return Vec3f(0, 0, -0.5 * halfLength); }
456
457 FCL_REAL minInflationValue() const { return -(std::min)(radius, halfLength); }
458
465 std::pair<Cone, Transform3f> inflated(const FCL_REAL value) const {
466 if (value <= minInflationValue())
468 "value (" << value << ") is two small. It should be at least: "
469 << minInflationValue(),
470 std::invalid_argument);
471
472 // tan(alpha) = 2*halfLength/radius;
473 const FCL_REAL tan_alpha = 2 * halfLength / radius;
474 const FCL_REAL sin_alpha = tan_alpha / std::sqrt(1 + tan_alpha * tan_alpha);
475 const FCL_REAL top_inflation = value / sin_alpha;
476 const FCL_REAL bottom_inflation = value;
477
478 const FCL_REAL new_lz = 2 * halfLength + top_inflation + bottom_inflation;
479 const FCL_REAL new_cz = (top_inflation + bottom_inflation) / 2.;
480 const FCL_REAL new_radius = new_lz / tan_alpha;
481
482 return std::make_pair(Cone(new_radius, new_lz),
483 Transform3f(Vec3f(0., 0., new_cz)));
484 }
485
486 private:
487 virtual bool isEqual(const CollisionGeometry& _other) const {
488 const Cone* other_ptr = dynamic_cast<const Cone*>(&_other);
489 if (other_ptr == nullptr) return false;
490 const Cone& other = *other_ptr;
491
492 return radius == other.radius && halfLength == other.halfLength;
493 }
494
495 public:
496 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
497};
498
502 public:
505
506 Cylinder(FCL_REAL radius_, FCL_REAL lz_) : ShapeBase(), radius(radius_) {
507 halfLength = lz_ / 2;
508 }
509
510 Cylinder(const Cylinder& other)
511 : ShapeBase(other), radius(other.radius), halfLength(other.halfLength) {}
512
513 Cylinder& operator=(const Cylinder& other) {
514 if (this == &other) return *this;
515
516 this->radius = other.radius;
517 this->halfLength = other.halfLength;
518 return *this;
519 }
520
522 virtual Cylinder* clone() const { return new Cylinder(*this); };
523
526
529
532
535
537 return boost::math::constants::pi<FCL_REAL>() * radius * radius *
538 (halfLength * 2);
539 }
540
542 FCL_REAL V = computeVolume();
543 FCL_REAL ix = V * (radius * radius / 4 + halfLength * halfLength / 3);
544 FCL_REAL iz = V * radius * radius / 2;
545 return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished();
546 }
547
548 FCL_REAL minInflationValue() const { return -(std::min)(radius, halfLength); }
549
556 std::pair<Cylinder, Transform3f> inflated(const FCL_REAL value) const {
557 if (value <= minInflationValue())
559 "value (" << value << ") is two small. It should be at least: "
560 << minInflationValue(),
561 std::invalid_argument);
562 return std::make_pair(Cylinder(radius + value, 2 * (halfLength + value)),
563 Transform3f());
564 }
565
566 private:
567 virtual bool isEqual(const CollisionGeometry& _other) const {
568 const Cylinder* other_ptr = dynamic_cast<const Cylinder*>(&_other);
569 if (other_ptr == nullptr) return false;
570 const Cylinder& other = *other_ptr;
571
572 return radius == other.radius && halfLength == other.halfLength;
573 }
574
575 public:
576 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
577};
578
582 public:
595 static ConvexBase* convexHull(const Vec3f* points, unsigned int num_points,
596 bool keepTriangles,
597 const char* qhullCommand = NULL);
598
599 virtual ~ConvexBase();
600
602 virtual ConvexBase* clone() const {
603 ConvexBase* copy_ptr = new ConvexBase(*this);
604 ConvexBase& copy = *copy_ptr;
605
606 if (!copy.own_storage_) {
607 copy.points = new Vec3f[copy.num_points];
608 std::copy(points, points + num_points, copy.points);
609 }
610 copy.own_storage_ = true;
611 copy.ShapeBase::operator=(*this);
612
613 return copy_ptr;
614 }
615
618
621
624 unsigned int num_points;
625
627 unsigned char count_;
628 unsigned int* n_;
629
630 unsigned char const& count() const { return count_; }
631 unsigned int& operator[](int i) {
632 assert(i < count_);
633 return n_[i];
634 }
635 unsigned int const& operator[](int i) const {
636 assert(i < count_);
637 return n_[i];
638 }
639
640 bool operator==(const Neighbors& other) const {
641 if (count_ != other.count_) return false;
642
643 for (int i = 0; i < count_; ++i) {
644 if (n_[i] != other.n_[i]) return false;
645 }
646
647 return true;
648 }
649
650 bool operator!=(const Neighbors& other) const { return !(*this == other); }
651 };
656
660
661 protected:
665 : ShapeBase(),
666 points(NULL),
667 num_points(0),
668 neighbors(NULL),
669 nneighbors_(NULL),
670 own_storage_(false) {}
671
678 void initialize(bool ownStorage, Vec3f* points_, unsigned int num_points_);
679
685 void set(bool ownStorage, Vec3f* points_, unsigned int num_points_);
686
689 ConvexBase(const ConvexBase& other);
690
691 unsigned int* nneighbors_;
692
694
695 private:
696 void computeCenter();
697
698 private:
699 virtual bool isEqual(const CollisionGeometry& _other) const {
700 const ConvexBase* other_ptr = dynamic_cast<const ConvexBase*>(&_other);
701 if (other_ptr == nullptr) return false;
702 const ConvexBase& other = *other_ptr;
703
704 if (num_points != other.num_points) return false;
705
706 for (unsigned int i = 0; i < num_points; ++i) {
707 if (points[i] != other.points[i]) return false;
708 }
709
710 for (unsigned int i = 0; i < num_points; ++i) {
711 if (neighbors[i] != other.neighbors[i]) return false;
712 }
713
714 return center == other.center;
715 }
716
717 public:
718 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
719};
720
721template <typename PolygonT>
722class Convex;
723
730 public:
732 Halfspace(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) {
733 unitNormalTest();
734 }
735
738 : ShapeBase(), n(a, b, c), d(d_) {
739 unitNormalTest();
740 }
741
742 Halfspace() : ShapeBase(), n(1, 0, 0), d(0) {}
743
744 Halfspace(const Halfspace& other)
745 : ShapeBase(other), n(other.n), d(other.d) {}
746
749 n = other.n;
750 d = other.d;
751 return *this;
752 }
753
755 virtual Halfspace* clone() const { return new Halfspace(*this); };
756
757 FCL_REAL signedDistance(const Vec3f& p) const { return n.dot(p) - d; }
758
759 FCL_REAL distance(const Vec3f& p) const { return std::abs(n.dot(p) - d); }
760
763
766
768 return std::numeric_limits<FCL_REAL>::lowest();
769 }
770
777 std::pair<Halfspace, Transform3f> inflated(const FCL_REAL value) const {
778 if (value <= minInflationValue())
780 "value (" << value << ") is two small. It should be at least: "
781 << minInflationValue(),
782 std::invalid_argument);
783 return std::make_pair(Halfspace(n, d + value), Transform3f());
784 }
785
788
791
792 protected:
795
796 private:
797 virtual bool isEqual(const CollisionGeometry& _other) const {
798 const Halfspace* other_ptr = dynamic_cast<const Halfspace*>(&_other);
799 if (other_ptr == nullptr) return false;
800 const Halfspace& other = *other_ptr;
801
802 return n == other.n && d == other.d;
803 }
804
805 public:
806 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
807};
808
811 public:
813 Plane(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_) {
814 unitNormalTest();
815 }
816
819 : ShapeBase(), n(a, b, c), d(d_) {
820 unitNormalTest();
821 }
822
823 Plane() : ShapeBase(), n(1, 0, 0), d(0) {}
824
825 Plane(const Plane& other) : ShapeBase(other), n(other.n), d(other.d) {}
826
828 Plane& operator=(const Plane& other) {
829 n = other.n;
830 d = other.d;
831 return *this;
832 }
833
835 virtual Plane* clone() const { return new Plane(*this); };
836
837 FCL_REAL signedDistance(const Vec3f& p) const { return n.dot(p) - d; }
838
839 FCL_REAL distance(const Vec3f& p) const { return std::abs(n.dot(p) - d); }
840
843
845 NODE_TYPE getNodeType() const { return GEOM_PLANE; }
846
849
852
853 protected:
856
857 private:
858 virtual bool isEqual(const CollisionGeometry& _other) const {
859 const Plane* other_ptr = dynamic_cast<const Plane*>(&_other);
860 if (other_ptr == nullptr) return false;
861 const Plane& other = *other_ptr;
862
863 return n == other.n && d == other.d;
864 }
865
866 public:
867 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
868};
869
870} // namespace fcl
871
872} // namespace hpp
873
874#endif
Center at zero point, axis aligned box.
Definition: geometric_shapes.h:125
Capsule It is where is the distance between the point x and the capsule segment AB,...
Definition: geometric_shapes.h:333
The geometry for the object for collision or distance computation.
Definition: collision_object.h:95
Cone The base of the cone is at and the top is at .
Definition: geometric_shapes.h:414
Base for convex polytope.
Definition: geometric_shapes.h:581
Cylinder along Z axis. The cylinder is defined at its centroid.
Definition: geometric_shapes.h:501
Ellipsoid centered at point zero.
Definition: geometric_shapes.h:258
Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d; Poi...
Definition: geometric_shapes.h:729
Infinite plane.
Definition: geometric_shapes.h:810
Base class for all basic geometric shapes.
Definition: geometric_shapes.h:51
Center at zero point sphere.
Definition: geometric_shapes.h:196
Simple transform class used locally by InterpMotion.
Definition: transform.h:54
Triangle stores the points instead of only indices of points.
Definition: geometric_shapes.h:71
#define HPP_FCL_DLLAPI
Definition: config.hh:88
#define HPP_FCL_THROW_PRETTY(message, exception)
Definition: fwd.hh:57
@ OT_GEOM
Definition: collision_object.h:56
@ GEOM_TRIANGLE
Definition: collision_object.h:83
@ GEOM_CAPSULE
Definition: collision_object.h:77
@ GEOM_PLANE
Definition: collision_object.h:81
@ GEOM_HALFSPACE
Definition: collision_object.h:82
@ GEOM_ELLIPSOID
Definition: collision_object.h:85
@ GEOM_BOX
Definition: collision_object.h:75
@ GEOM_CONVEX
Definition: collision_object.h:80
@ GEOM_CYLINDER
Definition: collision_object.h:79
@ GEOM_SPHERE
Definition: collision_object.h:76
@ GEOM_CONE
Definition: collision_object.h:78
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:441
void computeLocalAABB()
Compute AABB.
Vec3f c
Definition: geometric_shapes.h:109
void computeLocalAABB()
Compute AABB.
Plane(const Vec3f &n_, FCL_REAL d_)
Construct a plane with normal direction and offset.
Definition: geometric_shapes.h:813
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:352
ShapeBase()
Definition: geometric_shapes.h:53
virtual Capsule * clone() const
Clone *this into a new Capsule.
Definition: geometric_shapes.h:346
Capsule(const Capsule &other)
Definition: geometric_shapes.h:342
virtual Halfspace * clone() const
Clone *this into a new Halfspace.
Definition: geometric_shapes.h:755
FCL_REAL computeVolume() const
compute the volume
Definition: geometric_shapes.h:156
FCL_REAL minInflationValue() const
Definition: geometric_shapes.h:767
NODE_TYPE getNodeType() const
Get node type: a plane.
Definition: geometric_shapes.h:845
FCL_REAL computeVolume() const
compute the volume
Definition: geometric_shapes.h:536
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:737
Vec3f * points
An array of the points of the polygon.
Definition: geometric_shapes.h:623
Sphere(const Sphere &other)
Definition: geometric_shapes.h:203
std::pair< Halfspace, Transform3f > inflated(const FCL_REAL value) const
Inflate the cylinder by an amount given by value.
Definition: geometric_shapes.h:777
Halfspace(const Halfspace &other)
Definition: geometric_shapes.h:744
ConvexBase()
Construct an uninitialized convex object Initialization is done with ConvexBase::initialize.
Definition: geometric_shapes.h:664
Halfspace()
Definition: geometric_shapes.h:742
NODE_TYPE getNodeType() const
Get node type: a conex polytope.
Definition: geometric_shapes.h:620
FCL_REAL distance(const Vec3f &p) const
Definition: geometric_shapes.h:759
void unitNormalTest()
Turn non-unit normal into unit.
ShapeBase(const ShapeBase &other)
&#160;
Definition: geometric_shapes.h:56
NODE_TYPE getNodeType() const
Get node type: an ellipsoid.
Definition: geometric_shapes.h:281
NODE_TYPE getNodeType() const
Get node type: a capsule.
Definition: geometric_shapes.h:358
Box(FCL_REAL x, FCL_REAL y, FCL_REAL z)
Definition: geometric_shapes.h:127
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: geometric_shapes.h:446
unsigned int const & operator[](int i) const
Definition: geometric_shapes.h:635
TriangleP(const Vec3f &a_, const Vec3f &b_, const Vec3f &c_)
Definition: geometric_shapes.h:75
Halfspace(const Vec3f &n_, FCL_REAL d_)
Construct a half space with normal direction and offset.
Definition: geometric_shapes.h:732
Ellipsoid(const Vec3f &radii)
Definition: geometric_shapes.h:266
void computeLocalAABB()
Compute AABB.
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: geometric_shapes.h:541
void computeLocalAABB()
Compute AABB.
Cylinder(const Cylinder &other)
Definition: geometric_shapes.h:510
Ellipsoid(const Ellipsoid &other)
Definition: geometric_shapes.h:268
unsigned int * nneighbors_
Definition: geometric_shapes.h:691
Halfspace & operator=(const Halfspace &other)
operator =
Definition: geometric_shapes.h:748
Vec3f a
Definition: geometric_shapes.h:109
FCL_REAL computeVolume() const
compute the volume
Definition: geometric_shapes.h:360
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:528
Vec3f n
Plane normal.
Definition: geometric_shapes.h:848
void computeLocalAABB()
Compute AABB.
virtual ConvexBase * clone() const
&#160;
Definition: geometric_shapes.h:602
void unitNormalTest()
Turn non-unit normal into unit.
std::pair< Box, Transform3f > inflated(const FCL_REAL value) const
Inflate the box by an amount given by value.
Definition: geometric_shapes.h:172
NODE_TYPE getNodeType() const
get the node type
Definition: geometric_shapes.h:87
std::pair< Capsule, Transform3f > inflated(const FCL_REAL value) const
Inflate the capsule by an amount given by value.
Definition: geometric_shapes.h:388
FCL_REAL radius
Radius of capsule.
Definition: geometric_shapes.h:349
void computeLocalAABB()
virtual function of compute AABB in local coordinate
ShapeBase & operator=(const ShapeBase &other)=default
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:630
virtual ~ShapeBase()
Definition: geometric_shapes.h:60
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: geometric_shapes.h:158
bool own_storage_
Definition: geometric_shapes.h:693
Box(const Vec3f &side_)
Definition: geometric_shapes.h:130
TriangleP()
Definition: geometric_shapes.h:73
FCL_REAL minInflationValue() const
Definition: geometric_shapes.h:457
NODE_TYPE getNodeType() const
Get node type: a half space.
Definition: geometric_shapes.h:765
virtual Cone * clone() const
Clone *this into a new Cone.
Definition: geometric_shapes.h:427
Plane(const Plane &other)
Definition: geometric_shapes.h:825
Cylinder(FCL_REAL radius_, FCL_REAL lz_)
Definition: geometric_shapes.h:506
Vec3f n
Plane normal.
Definition: geometric_shapes.h:787
Cylinder()
Default constructor.
Definition: geometric_shapes.h:504
Plane()
Definition: geometric_shapes.h:823
FCL_REAL halfLength
Half Length along z axis.
Definition: geometric_shapes.h:433
bool operator!=(const Neighbors &other) const
Definition: geometric_shapes.h:650
Ellipsoid()
Default constructor.
Definition: geometric_shapes.h:261
Neighbors * neighbors
Definition: geometric_shapes.h:655
bool operator==(const Neighbors &other) const
Definition: geometric_shapes.h:640
unsigned int * n_
Definition: geometric_shapes.h:628
FCL_REAL signedDistance(const Vec3f &p) const
Definition: geometric_shapes.h:757
unsigned int num_points
Definition: geometric_shapes.h:624
void computeLocalAABB()
Compute AABB.
FCL_REAL distance(const Vec3f &p) const
Definition: geometric_shapes.h:839
virtual Cylinder * clone() const
Clone *this into a new Cylinder.
Definition: geometric_shapes.h:522
FCL_REAL d
Plane offset.
Definition: geometric_shapes.h:851
Vec3f computeCOM() const
compute center of mass
Definition: geometric_shapes.h:455
FCL_REAL radius
Radius of the sphere.
Definition: geometric_shapes.h:209
FCL_REAL signedDistance(const Vec3f &p) const
Definition: geometric_shapes.h:837
Sphere()
Default constructor.
Definition: geometric_shapes.h:199
Vec3f halfSide
box side half-length
Definition: geometric_shapes.h:148
Vec3f center
center of the convex polytope, this is used for collision: center is guaranteed in the internal of th...
Definition: geometric_shapes.h:659
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:818
std::pair< Sphere, Transform3f > inflated(const FCL_REAL value) const
Inflate the sphere by an amount given by value.
Definition: geometric_shapes.h:235
FCL_REAL minInflationValue() const
Definition: geometric_shapes.h:380
TriangleP(const TriangleP &other)
Definition: geometric_shapes.h:78
void computeLocalAABB()
Compute AABB.
Sphere(FCL_REAL radius_)
Definition: geometric_shapes.h:201
virtual Ellipsoid * clone() const
Clone *this into a new Ellipsoid.
Definition: geometric_shapes.h:271
FCL_REAL radius
Radius of the cone.
Definition: geometric_shapes.h:430
Capsule()
Default constructor.
Definition: geometric_shapes.h:336
virtual Box * clone() const
Clone *this into a new Box.
Definition: geometric_shapes.h:142
Cone()
Default constructor.
Definition: geometric_shapes.h:417
ConvexBase(const ConvexBase &other)
Copy constructor Only the list of neighbors is copied.
FCL_REAL computeVolume() const
compute the volume
Definition: geometric_shapes.h:222
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: geometric_shapes.h:365
FCL_REAL radius
Radius of the cylinder.
Definition: geometric_shapes.h:525
virtual TriangleP * clone() const
Clone *this into a new TriangleP.
Definition: geometric_shapes.h:82
std::pair< Ellipsoid, Transform3f > inflated(const FCL_REAL value) const
Inflate the ellipsoid by an amount given by value.
Definition: geometric_shapes.h:306
Box()
Default constructor.
Definition: geometric_shapes.h:145
NODE_TYPE getNodeType() const
Get node type: a box.
Definition: geometric_shapes.h:154
Ellipsoid(FCL_REAL rx, FCL_REAL ry, FCL_REAL rz)
Definition: geometric_shapes.h:263
Cone(const Cone &other)
Definition: geometric_shapes.h:423
Cylinder & operator=(const Cylinder &other)
Definition: geometric_shapes.h:513
std::pair< Cone, Transform3f > inflated(const FCL_REAL value) const
Inflate the cone by an amount given by value.
Definition: geometric_shapes.h:465
std::pair< Cylinder, Transform3f > inflated(const FCL_REAL value) const
Inflate the cylinder by an amount given by value.
Definition: geometric_shapes.h:556
FCL_REAL minInflationValue() const
Definition: geometric_shapes.h:164
Vec3f b
Definition: geometric_shapes.h:109
Cone(FCL_REAL radius_, FCL_REAL lz_)
Definition: geometric_shapes.h:419
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: geometric_shapes.h:283
FCL_REAL minInflationValue() const
Definition: geometric_shapes.h:548
Capsule(FCL_REAL radius_, FCL_REAL lz_)
Definition: geometric_shapes.h:338
void computeLocalAABB()
Compute AABB.
OBJECT_TYPE getObjectType() const
Get object type: a geometric shape.
Definition: geometric_shapes.h:63
NODE_TYPE getNodeType() const
Get node type: a cone.
Definition: geometric_shapes.h:439
virtual Plane * clone() const
Clone *this into a new Plane.
Definition: geometric_shapes.h:835
FCL_REAL minInflationValue() const
Definition: geometric_shapes.h:227
Matrix3f computeMomentofInertia() const
compute the inertia matrix, related to the origin
Definition: geometric_shapes.h:217
NODE_TYPE getNodeType() const
Get node type: a cylinder.
Definition: geometric_shapes.h:534
virtual Sphere * clone() const
Clone *this into a new Sphere.
Definition: geometric_shapes.h:206
Vec3f radii
Radii of the Ellipsoid (such that on boundary: x^2/rx^2 + y^2/ry^2.
Definition: geometric_shapes.h:275
NODE_TYPE getNodeType() const
Get node type: a sphere.
Definition: geometric_shapes.h:215
Box & operator=(const Box &other)
Definition: geometric_shapes.h:134
unsigned int & operator[](int i)
Definition: geometric_shapes.h:631
Plane & operator=(const Plane &other)
operator =
Definition: geometric_shapes.h:828
void computeLocalAABB()
Compute AABB.
FCL_REAL computeVolume() const
compute the volume
Definition: geometric_shapes.h:293
void set(bool ownStorage, Vec3f *points_, unsigned int num_points_)
Set the points of the convex shape.
FCL_REAL minInflationValue() const
Definition: geometric_shapes.h:298
FCL_REAL d
Plane offset.
Definition: geometric_shapes.h:790
Box(const Box &other)
Definition: geometric_shapes.h:132
unsigned char count_
Definition: geometric_shapes.h:627
Eigen::Matrix< FCL_REAL, 3, 3 > Matrix3f
Definition: data_types.h:68
Eigen::Matrix< FCL_REAL, 3, 1 > Vec3f
Definition: data_types.h:66
OBJECT_TYPE
object type: BVH (mesh, points), basic geometry, octree
Definition: collision_object.h:53
NODE_TYPE
traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18,...
Definition: collision_object.h:65
bool isEqual(const Eigen::MatrixBase< Derived > &lhs, const Eigen::MatrixBase< OtherDerived > &rhs, const FCL_REAL tol=std::numeric_limits< FCL_REAL >::epsilon() *100)
Definition: tools.h:209
double FCL_REAL
Definition: data_types.h:65
Main namespace.
Definition: broadphase_bruteforce.h:44
Definition: geometric_shapes.h:626