bodies.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan, E. Gil Jones */
36 
37 #ifndef GEOMETRIC_SHAPES_BODIES_
38 #define GEOMETRIC_SHAPES_BODIES_
39 
40 #if __cplusplus <= 199711L
41 #error This header requires at least C++11
42 #endif
43 
44 #include "geometric_shapes/aabb.h"
45 #include "geometric_shapes/obb.h"
46 #include "geometric_shapes/shapes.h"
47 #include <eigen_stl_containers/eigen_stl_containers.h>
48 #include <random_numbers/random_numbers.h>
49 #include <Eigen/Core>
50 #include <Eigen/Geometry>
51 #include <memory>
52 #include <vector>
53 
58 namespace bodies
59 {
61 struct BoundingSphere
62 {
63  Eigen::Vector3d center;
64  double radius;
65 
66  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
67 };
68 
70 struct BoundingCylinder
71 {
72  Eigen::Isometry3d pose;
73  double radius;
74  double length;
75  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
76 };
77 
78 class Body;
79 
81 typedef std::shared_ptr<Body> BodyPtr;
82 
84 typedef std::shared_ptr<const Body> BodyConstPtr;
85 
89 class Body
90 {
91 public:
92  Body() : scale_(1.0), padding_(0.0), type_(shapes::UNKNOWN_SHAPE)
93  {
94  pose_.setIdentity();
95  }
96 
97  virtual ~Body() = default;
98 
100  inline shapes::ShapeType getType() const
101  {
102  return type_;
103  }
104 
113  inline void setScaleDirty(double scale)
114  {
115  scale_ = scale;
116  }
117 
120  inline void setScale(double scale)
121  {
122  setScaleDirty(scale);
123  updateInternalData();
124  }
125 
127  inline double getScale() const
128  {
129  return scale_;
130  }
131 
140  inline void setPaddingDirty(double padd)
141  {
142  padding_ = padd;
143  }
144 
147  inline void setPadding(double padd)
148  {
149  setPaddingDirty(padd);
150  updateInternalData();
151  }
152 
154  inline double getPadding() const
155  {
156  return padding_;
157  }
158 
167  inline void setPoseDirty(const Eigen::Isometry3d& pose)
168  {
169  pose_ = pose;
170  }
171 
173  inline void setPose(const Eigen::Isometry3d& pose)
174  {
175  setPoseDirty(pose);
176  updateInternalData();
177  }
178 
180  inline const Eigen::Isometry3d& getPose() const
181  {
182  return pose_;
183  }
184 
193  inline void setDimensionsDirty(const shapes::Shape* shape)
194  {
195  useDimensions(shape);
196  }
197 
199  virtual std::vector<double> getDimensions() const = 0;
200 
202  virtual std::vector<double> getScaledDimensions() const = 0;
203 
205  inline void setDimensions(const shapes::Shape* shape)
206  {
207  setDimensionsDirty(shape);
208  updateInternalData();
209  }
210 
212  inline bool containsPoint(double x, double y, double z, bool verbose = false) const
213  {
214  Eigen::Vector3d pt(x, y, z);
215  return containsPoint(pt, verbose);
216  }
217 
219  virtual bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const = 0;
220 
226  virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
227  EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const = 0;
228 
231  virtual double computeVolume() const = 0;
232 
238  virtual bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
239  Eigen::Vector3d& result) const;
240 
243  virtual void computeBoundingSphere(BoundingSphere& sphere) const = 0;
244 
247  virtual void computeBoundingCylinder(BoundingCylinder& cylinder) const = 0;
248 
251  virtual void computeBoundingBox(AABB& bbox) const = 0;
252 
255  virtual void computeBoundingBox(OBB& bbox) const = 0;
256 
258  inline BodyPtr cloneAt(const Eigen::Isometry3d& pose) const
259  {
260  return cloneAt(pose, padding_, scale_);
261  }
262 
267  virtual BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scaling) const = 0;
268 
272  virtual void updateInternalData() = 0;
273 
274 protected:
276  virtual void useDimensions(const shapes::Shape* shape) = 0;
277 
279  double scale_;
280 
282  double padding_;
283 
285  shapes::ShapeType type_;
286 
288  Eigen::Isometry3d pose_;
289 
290 public:
291  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
292 };
293 
295 class Sphere : public Body
296 {
297 public:
298  Sphere() : Body()
299  {
300  type_ = shapes::SPHERE;
301  }
302 
303  Sphere(const shapes::Shape* shape) : Body()
304  {
305  type_ = shapes::SPHERE;
306  setDimensions(shape);
307  }
308 
309  explicit Sphere(const BoundingSphere& sphere);
310 
311  ~Sphere() override = default;
312 
314  std::vector<double> getDimensions() const override;
315  std::vector<double> getScaledDimensions() const override;
316 
317  bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override;
318  double computeVolume() const override;
319  bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
320  Eigen::Vector3d& result) const override;
321  void computeBoundingSphere(BoundingSphere& sphere) const override;
322  void computeBoundingCylinder(BoundingCylinder& cylinder) const override;
323  void computeBoundingBox(AABB& bbox) const override;
324  void computeBoundingBox(OBB& bbox) const override;
325  bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
326  EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override;
327 
328  BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const override;
329 
330  void updateInternalData() override;
331 
332 protected:
333  void useDimensions(const shapes::Shape* shape) override;
334 
335  // shape-dependent data
336  double radius_;
337 
338  // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
339  Eigen::Vector3d center_;
340  double radiusU_;
341  double radius2_;
342 
343 public:
344  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
345 };
346 
348 class Cylinder : public Body
349 {
350 public:
351  Cylinder() : Body()
352  {
353  type_ = shapes::CYLINDER;
354  }
355 
356  Cylinder(const shapes::Shape* shape) : Body()
357  {
358  type_ = shapes::CYLINDER;
359  setDimensions(shape);
360  }
361 
362  explicit Cylinder(const BoundingCylinder& cylinder);
363 
364  ~Cylinder() override = default;
365 
367  std::vector<double> getDimensions() const override;
368  std::vector<double> getScaledDimensions() const override;
369 
370  bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override;
371  double computeVolume() const override;
372  bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
373  Eigen::Vector3d& result) const override;
374  void computeBoundingSphere(BoundingSphere& sphere) const override;
375  void computeBoundingCylinder(BoundingCylinder& cylinder) const override;
376  void computeBoundingBox(AABB& bbox) const override;
377  void computeBoundingBox(OBB& bbox) const override;
378  bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
379  EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override;
380 
381  BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const override;
382 
383  void updateInternalData() override;
384 
385 protected:
386  void useDimensions(const shapes::Shape* shape) override;
387 
388  // shape-dependent data
389  double length_;
390  double radius_;
391 
392  // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
393  Eigen::Vector3d center_;
394  Eigen::Vector3d normalH_;
395  Eigen::Vector3d normalB1_;
396  Eigen::Vector3d normalB2_;
397 
398  double length2_;
399  double radiusU_;
400  double radiusB_;
401  double radiusBSqr_;
402  double radius2_;
403  double d1_;
404  double d2_;
405 
406 public:
407  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
408 };
409 
411 class Box : public Body
412 {
413 public:
414  Box() : Body()
415  {
416  type_ = shapes::BOX;
417  }
418 
419  Box(const shapes::Shape* shape) : Body()
420  {
421  type_ = shapes::BOX;
422  setDimensions(shape);
423  }
424 
425  explicit Box(const AABB& aabb);
426 
427  ~Box() override = default;
428 
430  std::vector<double> getDimensions() const override;
431  std::vector<double> getScaledDimensions() const override;
432 
433  bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override;
434  double computeVolume() const override;
435  bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
436  Eigen::Vector3d& result) const override;
437  void computeBoundingSphere(BoundingSphere& sphere) const override;
438  void computeBoundingCylinder(BoundingCylinder& cylinder) const override;
439  void computeBoundingBox(AABB& bbox) const override;
440  void computeBoundingBox(OBB& bbox) const override;
441  bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
442  EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override;
443 
444  BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const override;
445 
446  void updateInternalData() override;
447 
448 protected:
449  void useDimensions(const shapes::Shape* shape) override; // (x, y, z) = (length, width, height)
450 
451  // shape-dependent data
452  double length_;
453  double width_;
454  double height_;
455 
456  // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
457  Eigen::Vector3d center_;
458  Eigen::Matrix3d invRot_;
459 
460  Eigen::Vector3d minCorner_;
461  Eigen::Vector3d maxCorner_;
462 
463  double length2_;
464  double width2_;
465  double height2_;
466  double radiusB_;
467  double radius2_;
468 
469 public:
470  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
471 };
472 
474 class ConvexMesh : public Body
475 {
476 public:
477  ConvexMesh() : Body()
478  {
479  type_ = shapes::MESH;
480  scaled_vertices_ = nullptr;
481  }
482 
483  ConvexMesh(const shapes::Shape* shape) : Body()
484  {
485  type_ = shapes::MESH;
486  scaled_vertices_ = nullptr;
487  setDimensions(shape);
488  }
489 
490  ~ConvexMesh() override = default;
491 
493  std::vector<double> getDimensions() const override;
495  std::vector<double> getScaledDimensions() const override;
496 
497  bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override;
498  double computeVolume() const override;
499 
500  void computeBoundingSphere(BoundingSphere& sphere) const override;
501  void computeBoundingCylinder(BoundingCylinder& cylinder) const override;
502  void computeBoundingBox(AABB& bbox) const override;
503  void computeBoundingBox(OBB& bbox) const override;
504  bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
505  EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override;
506 
507  const std::vector<unsigned int>& getTriangles() const;
508  const EigenSTL::vector_Vector3d& getVertices() const;
509  const EigenSTL::vector_Vector3d& getScaledVertices() const;
510 
515  const EigenSTL::vector_Vector4d& getPlanes() const;
516 
517  BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const override;
518 
520  void computeScaledVerticesFromPlaneProjections();
521 
522  void correctVertexOrderFromPlanes();
523 
524  void updateInternalData() override;
525 
526 protected:
527  void useDimensions(const shapes::Shape* shape) override;
528 
530  unsigned int countVerticesBehindPlane(const Eigen::Vector4f& planeNormal) const;
531 
538  bool isPointInsidePlanes(const Eigen::Vector3d& point) const;
539 
540  // PIMPL structure
541  struct MeshData;
542 
543  // shape-dependent data; keep this in one struct so that a cheap pointer copy can be done in cloneAt()
544  std::shared_ptr<MeshData> mesh_data_;
545 
546  // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
547  Eigen::Isometry3d i_pose_;
548  Eigen::Vector3d center_;
549  double radiusB_;
550  double radiusBSqr_;
551  Box bounding_box_;
552 
553  // pointer to an array of scaled vertices
554  // If the padding is 0 & scaling is 1, then there is no need to have scaled vertices;
555  // we can just point to the vertices in mesh_data_.
556  // Otherwise, point to scaled_vertices_storage_
557  EigenSTL::vector_Vector3d* scaled_vertices_;
558 
559 private:
560  std::unique_ptr<EigenSTL::vector_Vector3d> scaled_vertices_storage_;
561 
562 public:
563  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
564 };
565 
569 class BodyVector
570 {
571 public:
572  BodyVector();
573 
575  BodyVector(const std::vector<shapes::Shape*>& shapes, const EigenSTL::vector_Isometry3d& poses, double padding = 0.0);
576 
577  ~BodyVector();
578 
580  void addBody(Body* body);
581 
583  void addBody(const shapes::Shape* shape, const Eigen::Isometry3d& pose, double padding = 0.0);
584 
586  void clear();
587 
589  void setPose(unsigned int i, const Eigen::Isometry3d& pose);
590 
592  std::size_t getCount() const;
593 
595  bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const;
596 
598  bool containsPoint(const Eigen::Vector3d& p, std::size_t& index, bool verbose = false) const;
599 
605  bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, std::size_t& index,
606  EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const;
607 
609  const Body* getBody(unsigned int i) const;
610 
611 private:
612  std::vector<Body*> bodies_;
613 };
614 
616 typedef std::shared_ptr<Body> BodyPtr;
617 
619 typedef std::shared_ptr<const Body> BodyConstPtr;
620 } // namespace bodies
621 
622 #endif
random_numbers.h
bodies::Body::Body
Body()
Definition: bodies.h:124
bodies::Box::cloneAt
BodyPtr cloneAt(const Eigen::Isometry3d &pose, double padding, double scale) const override
Get a clone of this body, but one that is located at the pose pose and has possibly different passing...
Definition: bodies.cpp:624
bodies::ConvexMesh::intersectsRay
bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections=nullptr, unsigned int count=0) const override
Check if a ray intersects the body, and find the set of intersections, in order, along the ray....
Definition: bodies.cpp:1237
obb.h
bodies::ConvexMesh::computeBoundingSphere
void computeBoundingSphere(BoundingSphere &sphere) const override
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for.
Definition: bodies.cpp:1159
bodies::ConvexMesh::getDimensions
std::vector< double > getDimensions() const override
Returns an empty vector.
Definition: bodies.cpp:1011
bodies::Box::intersectsRay
bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections=nullptr, unsigned int count=0) const override
Check if a ray intersects the body, and find the set of intersections, in order, along the ray....
Definition: bodies.cpp:691
bodies::ConvexMesh::isPointInsidePlanes
bool isPointInsidePlanes(const Eigen::Vector3d &point) const
Check if the point is inside all halfspaces this mesh consists of (mesh_data_->planes_).
Definition: bodies.cpp:1188
bodies::BoundingSphere::radius
double radius
Definition: bodies.h:128
bodies::ConvexMesh::computeVolume
double computeVolume() const override
Compute the volume of the body. This method includes changes induced by scaling and padding.
Definition: bodies.cpp:1222
bodies::ConvexMesh::mesh_data_
std::shared_ptr< MeshData > mesh_data_
Definition: bodies.h:573
shapes
Definition of various shapes. No properties such as position are included. These are simply the descr...
Definition: mesh_operations.h:47
EigenSTL::vector_Isometry3d
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
bodies::BodyVector::BodyVector
BodyVector()
Definition: bodies.cpp:1333
bodies::ConvexMesh::scaled_vertices_storage_
std::unique_ptr< EigenSTL::vector_Vector3d > scaled_vertices_storage_
Definition: bodies.h:592
bodies::Cylinder::radiusB_
double radiusB_
Definition: bodies.h:432
bodies::Box::center_
Eigen::Vector3d center_
Definition: bodies.h:489
bodies::Sphere
Definition of a sphere.
Definition: bodies.h:327
bodies::Box::computeVolume
double computeVolume() const override
Compute the volume of the body. This method includes changes induced by scaling and padding.
Definition: bodies.cpp:637
bodies::Body::scale_
double scale_
The scale that was set for this body.
Definition: bodies.h:311
bodies::Sphere::Sphere
Sphere()
Definition: bodies.h:330
bodies::Cylinder::normalB1_
Eigen::Vector3d normalB1_
Definition: bodies.h:427
bodies::Sphere::containsPoint
bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const override
Check if a point is inside the body. Surface points are included.
Definition: bodies.cpp:145
bodies::Body::~Body
virtual ~Body()=default
bodies::Box::useDimensions
void useDimensions(const shapes::Shape *shape) override
Depending on the shape, this function copies the relevant data to the body.
Definition: bodies.cpp:578
bodies::Sphere::radiusU_
double radiusU_
Definition: bodies.h:372
bodies::Cylinder::Cylinder
Cylinder()
Definition: bodies.h:383
bodies::Body::setPoseDirty
void setPoseDirty(const Eigen::Isometry3d &pose)
Set the pose of the body.
Definition: bodies.h:199
bodies::ConvexMesh::cloneAt
BodyPtr cloneAt(const Eigen::Isometry3d &pose, double padding, double scale) const override
Get a clone of this body, but one that is located at the pose pose and has possibly different passing...
Definition: bodies.cpp:1147
bodies::Body::pose_
Eigen::Isometry3d pose_
The location of the body (position and orientation)
Definition: bodies.h:320
bodies::Body::intersectsRay
virtual bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections=nullptr, unsigned int count=0) const =0
Check if a ray intersects the body, and find the set of intersections, in order, along the ray....
EigenSTL::vector_Vector4d
std::vector< Eigen::Vector4d, Eigen::aligned_allocator< Eigen::Vector4d > > vector_Vector4d
bodies::Cylinder::length2_
double length2_
Definition: bodies.h:430
bodies::ConvexMesh::useDimensions
void useDimensions(const shapes::Shape *shape) override
Depending on the shape, this function copies the relevant data to the body.
Definition: bodies.cpp:831
bodies::ConvexMesh::radiusB_
double radiusB_
Definition: bodies.h:581
bodies::Body::computeVolume
virtual double computeVolume() const =0
Compute the volume of the body. This method includes changes induced by scaling and padding.
shapes::UNKNOWN_SHAPE
@ UNKNOWN_SHAPE
Definition: shapes.h:63
bodies::Sphere::updateInternalData
void updateInternalData() override
This function is called every time a change to the body is made, so that intermediate values stored f...
Definition: bodies.cpp:165
bodies::Body::setDimensionsDirty
void setDimensionsDirty(const shapes::Shape *shape)
Set the dimensions of the body (from corresponding shape).
Definition: bodies.h:225
bodies::Sphere::getScaledDimensions
std::vector< double > getScaledDimensions() const override
Get the dimensions associated to this body (scaled and padded)
Definition: bodies.cpp:160
bodies::Cylinder::getDimensions
std::vector< double > getDimensions() const override
Get the radius & length of the cylinder.
Definition: bodies.cpp:345
bodies::Box::computeBoundingSphere
void computeBoundingSphere(BoundingSphere &sphere) const override
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for.
Definition: bodies.cpp:642
bodies::Cylinder::intersectsRay
bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections=nullptr, unsigned int count=0) const override
Check if a ray intersects the body, and find the set of intersections, in order, along the ray....
Definition: bodies.cpp:450
bodies::Cylinder::normalH_
Eigen::Vector3d normalH_
Definition: bodies.h:426
bodies::ConvexMesh::countVerticesBehindPlane
unsigned int countVerticesBehindPlane(const Eigen::Vector4f &planeNormal) const
(Used mainly for debugging) Count the number of vertices behind a plane
Definition: bodies.cpp:1208
bodies::Cylinder::containsPoint
bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const override
Check if a point is inside the body. Surface points are included.
Definition: bodies.cpp:319
bodies::Sphere::computeVolume
double computeVolume() const override
Compute the volume of the body. This method includes changes induced by scaling and padding.
Definition: bodies.cpp:186
bodies::Cylinder::computeBoundingBox
void computeBoundingBox(AABB &bbox) const override
Compute the axis-aligned bounding box for the body, in its current pose. Scaling and padding are acco...
Definition: bodies.cpp:428
shapes::SPHERE
@ SPHERE
Definition: shapes.h:64
bodies::Body::computeBoundingCylinder
virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const =0
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
bodies::Cylinder::d1_
double d1_
Definition: bodies.h:435
bodies::Body::containsPoint
bool containsPoint(double x, double y, double z, bool verbose=false) const
Check if a point is inside the body.
Definition: bodies.h:244
bodies::Cylinder::~Cylinder
~Cylinder() override=default
bodies::BoundingCylinder::radius
double radius
Definition: bodies.h:105
bodies::BodyVector::setPose
void setPose(unsigned int i, const Eigen::Isometry3d &pose)
Set the pose of a particular body in the vector of bodies.
Definition: bodies.cpp:1377
shapes::Shape
A basic definition of a shape. Shapes are considered centered at origin.
Definition: shapes.h:77
bodies::Sphere::cloneAt
BodyPtr cloneAt(const Eigen::Isometry3d &pose, double padding, double scale) const override
Get a clone of this body, but one that is located at the pose pose and has possibly different passing...
Definition: bodies.cpp:175
bodies::Box::containsPoint
bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const override
Check if a point is inside the body. Surface points are included.
Definition: bodies.cpp:572
bodies::Sphere::getDimensions
std::vector< double > getDimensions() const override
Get the radius of the sphere.
Definition: bodies.cpp:155
bodies::Body
A body is a shape + its pose. Point inclusion, ray intersection can be tested, volumes and bounding s...
Definition: bodies.h:121
bodies::Cylinder::computeBoundingCylinder
void computeBoundingCylinder(BoundingCylinder &cylinder) const override
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
Definition: bodies.cpp:421
bodies::Body::computeBoundingSphere
virtual void computeBoundingSphere(BoundingSphere &sphere) const =0
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for.
bodies::Body::getScaledDimensions
virtual std::vector< double > getScaledDimensions() const =0
Get the dimensions associated to this body (scaled and padded)
bodies::Box::getDimensions
std::vector< double > getDimensions() const override
Get the length & width & height (x, y, z) of the box.
Definition: bodies.cpp:586
bodies::Sphere::radius_
double radius_
Definition: bodies.h:368
shapes::MESH
@ MESH
Definition: shapes.h:69
bodies::Cylinder::computeVolume
double computeVolume() const override
Compute the volume of the body. This method includes changes induced by scaling and padding.
Definition: bodies.cpp:410
bodies::Sphere::radius2_
double radius2_
Definition: bodies.h:373
bodies::Cylinder::center_
Eigen::Vector3d center_
Definition: bodies.h:425
bodies::BodyVector::getBody
const Body * getBody(unsigned int i) const
Get the ith body in the vector.
Definition: bodies.cpp:1388
bodies::Box::radiusB_
double radiusB_
Definition: bodies.h:498
bodies::BodyVector::containsPoint
bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const
Check if any body in the vector contains the input point.
Definition: bodies.cpp:1410
bodies::Body::useDimensions
virtual void useDimensions(const shapes::Shape *shape)=0
Depending on the shape, this function copies the relevant data to the body.
bodies::Body::getPadding
double getPadding() const
Retrieve the current padding.
Definition: bodies.h:186
bodies::BoundingSphere
Definition of a sphere that bounds another object.
Definition: bodies.h:93
bodies::ConvexMesh::MeshData
Definition: bodies.cpp:777
bodies::Sphere::computeBoundingBox
void computeBoundingBox(AABB &bbox) const override
Compute the axis-aligned bounding box for the body, in its current pose. Scaling and padding are acco...
Definition: bodies.cpp:204
bodies::Cylinder::d2_
double d2_
Definition: bodies.h:436
bodies::Body::samplePointInside
virtual bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result) const
Sample a point that is included in the body using a given random number generator.
Definition: bodies.cpp:129
bodies::Body::setScaleDirty
void setScaleDirty(double scale)
If the dimension of the body should be scaled, this method sets the scale.
Definition: bodies.h:145
bodies::ConvexMesh::getScaledDimensions
std::vector< double > getScaledDimensions() const override
Returns an empty vector.
Definition: bodies.cpp:1016
bodies::Box::updateInternalData
void updateInternalData() override
This function is called every time a change to the body is made, so that intermediate values stored f...
Definition: bodies.cpp:596
bodies::ConvexMesh::ConvexMesh
ConvexMesh()
Definition: bodies.h:509
bodies::Box::computeBoundingBox
void computeBoundingBox(AABB &bbox) const override
Compute the axis-aligned bounding box for the body, in its current pose. Scaling and padding are acco...
Definition: bodies.cpp:679
bodies::Box::computeBoundingCylinder
void computeBoundingCylinder(BoundingCylinder &cylinder) const override
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
Definition: bodies.cpp:648
bodies::BodyVector::bodies_
std::vector< Body * > bodies_
Definition: bodies.h:644
bodies::OBB
Represents an oriented bounding box.
Definition: obb.h:85
bodies::ConvexMesh::updateInternalData
void updateInternalData() override
This function is called every time a change to the body is made, so that intermediate values stored f...
Definition: bodies.cpp:1085
bodies::Box::maxCorner_
Eigen::Vector3d maxCorner_
The translated, but not rotated max corner.
Definition: bodies.h:493
bodies::Body::type_
shapes::ShapeType type_
The type of shape this body was constructed from.
Definition: bodies.h:317
bodies::BoundingCylinder::length
double length
Definition: bodies.h:106
bodies::Box::length2_
double length2_
Definition: bodies.h:495
y
double y
Definition: mesh_operations.cpp:202
bodies::BoundingCylinder
Definition of a cylinder.
Definition: bodies.h:102
EigenSTL::vector_Vector3d
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
shapes::ShapeType
ShapeType
A list of known shape types.
Definition: shapes.h:61
bodies::Box::height2_
double height2_
Definition: bodies.h:497
bodies::Body::getDimensions
virtual std::vector< double > getDimensions() const =0
Get the dimensions associated to this body (as read from corresponding shape)
bodies::ConvexMesh::computeBoundingBox
void computeBoundingBox(AABB &bbox) const override
Compute the axis-aligned bounding box for the body, in its current pose. Scaling and padding are acco...
Definition: bodies.cpp:1176
bodies::BodyVector::intersectsRay
bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, std::size_t &index, EigenSTL::vector_Vector3d *intersections=nullptr, unsigned int count=0) const
Check if any of the bodies intersects the ray defined by origin and dir. When the first intersection ...
Definition: bodies.cpp:1416
bodies::Body::getType
shapes::ShapeType getType() const
Get the type of shape this body represents.
Definition: bodies.h:132
bodies::Box::width_
double width_
Definition: bodies.h:485
bodies::Box::height_
double height_
Definition: bodies.h:486
eigen_stl_containers.h
bodies::Box::getScaledDimensions
std::vector< double > getScaledDimensions() const override
Get the dimensions associated to this body (scaled and padded)
Definition: bodies.cpp:591
bodies::Box
Definition of a box.
Definition: bodies.h:443
bodies::ConvexMesh::getTriangles
const std::vector< unsigned int > & getTriangles() const
Definition: bodies.cpp:1124
bodies::Cylinder::radiusBSqr_
double radiusBSqr_
Definition: bodies.h:433
bodies::Sphere::useDimensions
void useDimensions(const shapes::Shape *shape) override
Depending on the shape, this function copies the relevant data to the body.
Definition: bodies.cpp:150
bodies::BoundingCylinder::pose
Eigen::Isometry3d pose
Definition: bodies.h:104
bodies::Body::getPose
const Eigen::Isometry3d & getPose() const
Retrieve the pose of the body.
Definition: bodies.h:212
bodies::Body::updateInternalData
virtual void updateInternalData()=0
This function is called every time a change to the body is made, so that intermediate values stored f...
bodies::Cylinder::useDimensions
void useDimensions(const shapes::Shape *shape) override
Depending on the shape, this function copies the relevant data to the body.
Definition: bodies.cpp:339
bodies::Box::minCorner_
Eigen::Vector3d minCorner_
The translated, but not rotated min corner.
Definition: bodies.h:492
bodies::BodyVector::getCount
std::size_t getCount() const
Get the number of bodies in this vector.
Definition: bodies.cpp:1372
aabb.h
bodies::ConvexMesh::radiusBSqr_
double radiusBSqr_
Definition: bodies.h:582
bodies::Sphere::center_
Eigen::Vector3d center_
Definition: bodies.h:371
bodies::Cylinder::normalB2_
Eigen::Vector3d normalB2_
Definition: bodies.h:428
bodies::Cylinder::radius_
double radius_
Definition: bodies.h:422
bodies::Body::setDimensions
void setDimensions(const shapes::Shape *shape)
Set the dimensions of the body (from corresponding shape)
Definition: bodies.h:237
bodies::BodyVector::addBody
void addBody(Body *body)
Add a body.
Definition: bodies.cpp:1356
bodies::BodyVector::~BodyVector
~BodyVector()
Definition: bodies.cpp:1344
bodies::ConvexMesh::bounding_box_
Box bounding_box_
Definition: bodies.h:583
bodies::Cylinder::samplePointInside
bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result) const override
Sample a point that is included in the body using a given random number generator.
Definition: bodies.cpp:381
bodies::ConvexMesh::i_pose_
Eigen::Isometry3d i_pose_
Definition: bodies.h:579
bodies::ConvexMesh::correctVertexOrderFromPlanes
void correctVertexOrderFromPlanes()
Definition: bodies.cpp:808
random_numbers::RandomNumberGenerator
point
std::chrono::system_clock::time_point point
bodies::BodyPtr
std::shared_ptr< Body > BodyPtr
Shared pointer to a Body.
Definition: bodies.h:110
bodies::Cylinder::getScaledDimensions
std::vector< double > getScaledDimensions() const override
Get the dimensions associated to this body (scaled and padded)
Definition: bodies.cpp:350
bodies::Cylinder::radius2_
double radius2_
Definition: bodies.h:434
bodies::ConvexMesh::~ConvexMesh
~ConvexMesh() override=default
bodies::Sphere::computeBoundingCylinder
void computeBoundingCylinder(BoundingCylinder &cylinder) const override
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
Definition: bodies.cpp:197
shapes.h
bodies::Body::getScale
double getScale() const
Retrieve the current scale.
Definition: bodies.h:159
bodies::BoundingSphere::center
Eigen::Vector3d center
Definition: bodies.h:127
bodies::Box::radius2_
double radius2_
Definition: bodies.h:499
bodies::ConvexMesh::center_
Eigen::Vector3d center_
Definition: bodies.h:580
bodies::Sphere::~Sphere
~Sphere() override=default
bodies::BodyConstPtr
std::shared_ptr< const Body > BodyConstPtr
Shared pointer to a const Body.
Definition: bodies.h:116
bodies::Body::setPaddingDirty
void setPaddingDirty(double padd)
If the dimension of the body should be padded, this method sets the pading.
Definition: bodies.h:172
bodies
This set of classes allows quickly detecting whether a given point is inside an object or not....
Definition: aabb.h:42
bodies::Body::setScale
void setScale(double scale)
If the dimension of the body should be scaled, this method sets the scale. Default is 1....
Definition: bodies.h:152
shapes::CYLINDER
@ CYLINDER
Definition: shapes.h:65
bodies::Body::computeBoundingBox
virtual void computeBoundingBox(AABB &bbox) const =0
Compute the axis-aligned bounding box for the body, in its current pose. Scaling and padding are acco...
index
unsigned int index
Definition: mesh_operations.cpp:203
bodies::Cylinder::length_
double length_
Definition: bodies.h:421
bodies::Body::setPose
void setPose(const Eigen::Isometry3d &pose)
Set the pose of the body. Default is identity.
Definition: bodies.h:205
bodies::Body::padding_
double padding_
The scale that was set for this body.
Definition: bodies.h:314
bodies::ConvexMesh::getScaledVertices
const EigenSTL::vector_Vector3d & getScaledVertices() const
Definition: bodies.cpp:1136
x
double x
Definition: mesh_operations.cpp:202
bodies::Cylinder::computeBoundingSphere
void computeBoundingSphere(BoundingSphere &sphere) const override
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for.
Definition: bodies.cpp:415
bodies::Body::setPadding
void setPadding(double padd)
If constant padding should be added to the body, this method sets the padding. Default is 0....
Definition: bodies.h:179
bodies::Box::~Box
~Box() override=default
bodies::Box::samplePointInside
bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result) const override
Sample a point that is included in the body using a given random number generator.
Definition: bodies.cpp:564
bodies::ConvexMesh::scaled_vertices_
EigenSTL::vector_Vector3d * scaled_vertices_
Definition: bodies.h:589
bodies::Sphere::intersectsRay
bool intersectsRay(const Eigen::Vector3d &origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections=nullptr, unsigned int count=0) const override
Check if a ray intersects the body, and find the set of intersections, in order, along the ray....
Definition: bodies.cpp:247
bodies::Box::invRot_
Eigen::Matrix3d invRot_
Definition: bodies.h:490
bodies::Cylinder::radiusU_
double radiusU_
Definition: bodies.h:431
bodies::Cylinder::cloneAt
BodyPtr cloneAt(const Eigen::Isometry3d &pose, double padding, double scale) const override
Get a clone of this body, but one that is located at the pose pose and has possibly different passing...
Definition: bodies.cpp:397
bodies::ConvexMesh::computeScaledVerticesFromPlaneProjections
void computeScaledVerticesFromPlaneProjections()
Project the original vertex to the scaled and padded planes and average.
Definition: bodies.cpp:1021
bodies::BodyVector
A vector of Body objects.
Definition: bodies.h:601
bodies::Cylinder
Definition of a cylinder.
Definition: bodies.h:380
bodies::Cylinder::updateInternalData
void updateInternalData() override
This function is called every time a change to the body is made, so that intermediate values stored f...
Definition: bodies.cpp:355
bodies::ConvexMesh
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Definition: bodies.h:506
bodies::BodyVector::clear
void clear()
Clear all bodies from the vector.
Definition: bodies.cpp:1349
bodies::ConvexMesh::containsPoint
bool containsPoint(const Eigen::Vector3d &p, bool verbose=false) const override
Check if a point is inside the body. Surface points are included.
Definition: bodies.cpp:794
z
double z
Definition: mesh_operations.cpp:202
bodies::Box::width2_
double width2_
Definition: bodies.h:496
shapes::BOX
@ BOX
Definition: shapes.h:67
bodies::ConvexMesh::computeBoundingCylinder
void computeBoundingCylinder(BoundingCylinder &cylinder) const override
Compute the bounding cylinder for the body, in its current pose. Scaling and padding are accounted fo...
Definition: bodies.cpp:1165
bodies::ConvexMesh::getPlanes
const EigenSTL::vector_Vector4d & getPlanes() const
Get the planes that define the convex shape.
Definition: bodies.cpp:1141
bodies::Sphere::samplePointInside
bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result) const override
Sample a point that is included in the body using a given random number generator.
Definition: bodies.cpp:224
bodies::ConvexMesh::getVertices
const EigenSTL::vector_Vector3d & getVertices() const
Definition: bodies.cpp:1130
bodies::Box::Box
Box()
Definition: bodies.h:446
bodies::AABB
Represents an axis-aligned bounding box.
Definition: aabb.h:77
bodies::Body::cloneAt
BodyPtr cloneAt(const Eigen::Isometry3d &pose) const
Get a clone of this body, but one that is located at the pose pose.
Definition: bodies.h:290
bodies::Box::length_
double length_
Definition: bodies.h:484
verbose
bool verbose
bodies::Sphere::computeBoundingSphere
void computeBoundingSphere(BoundingSphere &sphere) const override
Compute the bounding radius for the body, in its current pose. Scaling and padding are accounted for.
Definition: bodies.cpp:191

geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Tue Aug 13 2024 02:40:57