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"
46 #include <eigen_stl_containers/eigen_stl_containers.h>
48 #include <Eigen/Core>
49 #include <Eigen/Geometry>
50 #include <memory>
51 #include <vector>
52 
57 namespace bodies
58 {
60 struct BoundingSphere
61 {
62  Eigen::Vector3d center;
63  double radius;
64 
65  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
66 };
67 
69 struct BoundingCylinder
70 {
71  Eigen::Isometry3d pose;
72  double radius;
73  double length;
74  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
75 };
76 
77 class Body;
78 
80 typedef std::shared_ptr<Body> BodyPtr;
81 
83 typedef std::shared_ptr<const Body> BodyConstPtr;
84 
88 class Body
89 {
90 public:
91  Body() : scale_(1.0), padding_(0.0), type_(shapes::UNKNOWN_SHAPE)
92  {
93  pose_.setIdentity();
94  }
95 
96  virtual ~Body() = default;
97 
99  inline shapes::ShapeType getType() const
100  {
101  return type_;
102  }
103 
112  inline void setScaleDirty(double scale)
113  {
114  scale_ = scale;
115  }
116 
119  inline void setScale(double scale)
120  {
121  setScaleDirty(scale);
123  }
124 
126  inline double getScale() const
127  {
128  return scale_;
129  }
130 
139  inline void setPaddingDirty(double padd)
140  {
141  padding_ = padd;
142  }
143 
146  inline void setPadding(double padd)
147  {
148  setPaddingDirty(padd);
150  }
151 
153  inline double getPadding() const
154  {
155  return padding_;
156  }
157 
166  inline void setPoseDirty(const Eigen::Isometry3d& pose)
167  {
168  pose_ = pose;
169  }
170 
172  inline void setPose(const Eigen::Isometry3d& pose)
173  {
174  setPoseDirty(pose);
176  }
177 
179  inline const Eigen::Isometry3d& getPose() const
180  {
181  return pose_;
182  }
183 
192  inline void setDimensionsDirty(const shapes::Shape* shape)
193  {
194  useDimensions(shape);
195  }
196 
198  virtual std::vector<double> getDimensions() const = 0;
199 
201  virtual std::vector<double> getScaledDimensions() const = 0;
202 
204  inline void setDimensions(const shapes::Shape* shape)
205  {
206  setDimensionsDirty(shape);
208  }
209 
211  inline bool containsPoint(double x, double y, double z, bool verbose = false) const
212  {
213  Eigen::Vector3d pt(x, y, z);
214  return containsPoint(pt, verbose);
215  }
216 
218  virtual bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const = 0;
219 
225  virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
226  EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const = 0;
227 
230  virtual double computeVolume() const = 0;
231 
237  virtual bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
238  Eigen::Vector3d& result) const;
239 
242  virtual void computeBoundingSphere(BoundingSphere& sphere) const = 0;
243 
246  virtual void computeBoundingCylinder(BoundingCylinder& cylinder) const = 0;
247 
250  virtual void computeBoundingBox(AABB& bbox) const = 0;
251 
253  inline BodyPtr cloneAt(const Eigen::Isometry3d& pose) const
254  {
255  return cloneAt(pose, padding_, scale_);
256  }
257 
262  virtual BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scaling) const = 0;
263 
267  virtual void updateInternalData() = 0;
268 
269 protected:
271  virtual void useDimensions(const shapes::Shape* shape) = 0;
272 
274  double scale_;
275 
277  double padding_;
278 
281 
283  Eigen::Isometry3d pose_;
284 
285 public:
286  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
287 };
288 
290 class Sphere : public Body
291 {
292 public:
293  Sphere() : Body()
294  {
296  }
297 
298  Sphere(const shapes::Shape* shape) : Body()
299  {
301  setDimensions(shape);
302  }
303 
304  explicit Sphere(const BoundingSphere& sphere);
305 
306  ~Sphere() override = default;
307 
309  std::vector<double> getDimensions() const override;
310  std::vector<double> getScaledDimensions() const override;
311 
312  bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override;
313  double computeVolume() const override;
314  bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
315  Eigen::Vector3d& result) const override;
316  void computeBoundingSphere(BoundingSphere& sphere) const override;
317  void computeBoundingCylinder(BoundingCylinder& cylinder) const override;
318  void computeBoundingBox(AABB& bbox) const override;
319  bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
320  EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override;
321 
322  BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const override;
323 
324  void updateInternalData() override;
325 
326 protected:
327  void useDimensions(const shapes::Shape* shape) override;
328 
329  // shape-dependent data
330  double radius_;
331 
332  // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
333  Eigen::Vector3d center_;
334  double radiusU_;
335  double radius2_;
336 
337 public:
338  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
339 };
340 
342 class Cylinder : public Body
343 {
344 public:
345  Cylinder() : Body()
346  {
348  }
349 
350  Cylinder(const shapes::Shape* shape) : Body()
351  {
353  setDimensions(shape);
354  }
355 
356  explicit Cylinder(const BoundingCylinder& cylinder);
357 
358  ~Cylinder() override = default;
359 
361  std::vector<double> getDimensions() const override;
362  std::vector<double> getScaledDimensions() const override;
363 
364  bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override;
365  double computeVolume() const override;
366  bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
367  Eigen::Vector3d& result) const override;
368  void computeBoundingSphere(BoundingSphere& sphere) const override;
369  void computeBoundingCylinder(BoundingCylinder& cylinder) const override;
370  void computeBoundingBox(AABB& bbox) const override;
371  bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
372  EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override;
373 
374  BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const override;
375 
376  void updateInternalData() override;
377 
378 protected:
379  void useDimensions(const shapes::Shape* shape) override;
380 
381  // shape-dependent data
382  double length_;
383  double radius_;
384 
385  // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
386  Eigen::Vector3d center_;
387  Eigen::Vector3d normalH_;
388  Eigen::Vector3d normalB1_;
389  Eigen::Vector3d normalB2_;
390 
391  double length2_;
392  double radiusU_;
393  double radiusB_;
394  double radiusBSqr_;
395  double radius2_;
396  double d1_;
397  double d2_;
398 
399 public:
400  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
401 };
402 
404 class Box : public Body
405 {
406 public:
407  Box() : Body()
408  {
409  type_ = shapes::BOX;
410  }
411 
412  Box(const shapes::Shape* shape) : Body()
413  {
416  }
417 
418  explicit Box(const AABB& aabb);
419 
420  ~Box() override = default;
421 
423  std::vector<double> getDimensions() const override;
424  std::vector<double> getScaledDimensions() const override;
425 
426  bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override;
427  double computeVolume() const override;
428  bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
429  Eigen::Vector3d& result) const override;
430  void computeBoundingSphere(BoundingSphere& sphere) const override;
431  void computeBoundingCylinder(BoundingCylinder& cylinder) const override;
432  void computeBoundingBox(AABB& bbox) const override;
433  bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
434  EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override;
435 
436  BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const override;
437 
438  void updateInternalData() override;
439 
440 protected:
441  void useDimensions(const shapes::Shape* shape) override; // (x, y, z) = (length, width, height)
442 
443  // shape-dependent data
444  double length_;
445  double width_;
446  double height_;
447 
448  // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
449  Eigen::Vector3d center_;
450  Eigen::Matrix3d invRot_;
451 
452  Eigen::Vector3d minCorner_;
453  Eigen::Vector3d maxCorner_;
454 
455  double length2_;
456  double width2_;
457  double height2_;
458  double radiusB_;
459  double radius2_;
460 
461 public:
462  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
463 };
464 
466 class ConvexMesh : public Body
467 {
468 public:
469  ConvexMesh() : Body()
470  {
472  scaled_vertices_ = nullptr;
473  }
474 
475  ConvexMesh(const shapes::Shape* shape) : Body()
476  {
478  scaled_vertices_ = nullptr;
479  setDimensions(shape);
480  }
481 
482  ~ConvexMesh() override = default;
483 
485  std::vector<double> getDimensions() const override;
487  std::vector<double> getScaledDimensions() const override;
488 
489  bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const override;
490  double computeVolume() const override;
491 
492  void computeBoundingSphere(BoundingSphere& sphere) const override;
493  void computeBoundingCylinder(BoundingCylinder& cylinder) const override;
494  void computeBoundingBox(AABB& bbox) const override;
495  bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
496  EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const override;
497 
498  const std::vector<unsigned int>& getTriangles() const;
499  const EigenSTL::vector_Vector3d& getVertices() const;
500  const EigenSTL::vector_Vector3d& getScaledVertices() const;
501 
506  const EigenSTL::vector_Vector4d& getPlanes() const;
507 
508  BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const override;
509 
512 
514 
515  void updateInternalData() override;
516 
517 protected:
518  void useDimensions(const shapes::Shape* shape) override;
519 
521  unsigned int countVerticesBehindPlane(const Eigen::Vector4f& planeNormal) const;
522 
529  bool isPointInsidePlanes(const Eigen::Vector3d& point) const;
530 
531  // PIMPL structure
532  struct MeshData;
533 
534  // shape-dependent data; keep this in one struct so that a cheap pointer copy can be done in cloneAt()
535  std::shared_ptr<MeshData> mesh_data_;
536 
537  // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
538  Eigen::Isometry3d i_pose_;
539  Eigen::Vector3d center_;
540  double radiusB_;
541  double radiusBSqr_;
543 
544  // pointer to an array of scaled vertices
545  // If the padding is 0 & scaling is 1, then there is no need to have scaled vertices;
546  // we can just point to the vertices in mesh_data_.
547  // Otherwise, point to scaled_vertices_storage_
548  EigenSTL::vector_Vector3d* scaled_vertices_;
549 
550 private:
551  std::unique_ptr<EigenSTL::vector_Vector3d> scaled_vertices_storage_;
552 
553 public:
554  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
555 };
556 
560 class BodyVector
561 {
562 public:
563  BodyVector();
564 
566  BodyVector(const std::vector<shapes::Shape*>& shapes, const EigenSTL::vector_Isometry3d& poses, double padding = 0.0);
567 
568  ~BodyVector();
569 
571  void addBody(Body* body);
572 
574  void addBody(const shapes::Shape* shape, const Eigen::Isometry3d& pose, double padding = 0.0);
575 
577  void clear();
578 
580  void setPose(unsigned int i, const Eigen::Isometry3d& pose);
581 
583  std::size_t getCount() const;
584 
586  bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const;
587 
589  bool containsPoint(const Eigen::Vector3d& p, std::size_t& index, bool verbose = false) const;
590 
596  bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, std::size_t& index,
597  EigenSTL::vector_Vector3d* intersections = nullptr, unsigned int count = 0) const;
598 
600  const Body* getBody(unsigned int i) const;
601 
602 private:
603  std::vector<Body*> bodies_;
604 };
605 
607 typedef std::shared_ptr<Body> BodyPtr;
608 
610 typedef std::shared_ptr<const Body> BodyConstPtr;
611 } // namespace bodies
612 
613 #endif
random_numbers.h
bodies::Body::Body
Body()
Definition: bodies.h:123
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:610
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:1213
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:1140
bodies::ConvexMesh::getDimensions
std::vector< double > getDimensions() const override
Returns an empty vector.
Definition: bodies.cpp:992
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:672
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:1164
bodies::BoundingSphere::radius
double radius
Definition: bodies.h:127
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:1198
bodies::ConvexMesh::mesh_data_
std::shared_ptr< MeshData > mesh_data_
Definition: bodies.h:564
shapes
Definition of various shapes. No properties such as position are included. These are simply the descr...
Definition: mesh_operations.h:47
bodies::BodyVector::BodyVector
BodyVector()
Definition: bodies.cpp:1309
bodies::ConvexMesh::scaled_vertices_storage_
std::unique_ptr< EigenSTL::vector_Vector3d > scaled_vertices_storage_
Definition: bodies.h:583
bodies::Cylinder::radiusB_
double radiusB_
Definition: bodies.h:425
bodies::Box::center_
Eigen::Vector3d center_
Definition: bodies.h:481
bodies::Sphere
Definition of a sphere.
Definition: bodies.h:322
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:623
bodies::Body::scale_
double scale_
The scale that was set for this body.
Definition: bodies.h:306
bodies::Sphere::Sphere
Sphere()
Definition: bodies.h:325
bodies::Cylinder::normalB1_
Eigen::Vector3d normalB1_
Definition: bodies.h:420
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:564
bodies::Sphere::radiusU_
double radiusU_
Definition: bodies.h:366
bodies::Cylinder::Cylinder
Cylinder()
Definition: bodies.h:377
bodies::Body::setPoseDirty
void setPoseDirty(const Eigen::Isometry3d &pose)
Set the pose of the body.
Definition: bodies.h:198
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:1128
bodies::Body::pose_
Eigen::Isometry3d pose_
The location of the body (position and orientation)
Definition: bodies.h:315
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....
bodies::Cylinder::length2_
double length2_
Definition: bodies.h:423
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:812
bodies::ConvexMesh::radiusB_
double radiusB_
Definition: bodies.h:572
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:224
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:336
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:628
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:436
bodies::Cylinder::normalH_
Eigen::Vector3d normalH_
Definition: bodies.h:419
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:1184
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:310
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:419
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:428
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:243
bodies::Cylinder::~Cylinder
~Cylinder() override=default
bodies::BoundingCylinder::radius
double radius
Definition: bodies.h:104
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:1353
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:558
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:120
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:412
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:572
bodies::Sphere::radius_
double radius_
Definition: bodies.h:362
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:401
bodies::Sphere::radius2_
double radius2_
Definition: bodies.h:367
bodies::Cylinder::center_
Eigen::Vector3d center_
Definition: bodies.h:418
bodies::BodyVector::getBody
const Body * getBody(unsigned int i) const
Get the ith body in the vector.
Definition: bodies.cpp:1364
bodies::Box::radiusB_
double radiusB_
Definition: bodies.h:490
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:1386
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:185
bodies::BoundingSphere
Definition of a sphere that bounds another object.
Definition: bodies.h:92
bodies::ConvexMesh::MeshData
Definition: bodies.cpp:758
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:429
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:144
bodies::ConvexMesh::getScaledDimensions
std::vector< double > getScaledDimensions() const override
Returns an empty vector.
Definition: bodies.cpp:997
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:582
bodies::ConvexMesh::ConvexMesh
ConvexMesh()
Definition: bodies.h:501
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:665
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:634
bodies::BodyVector::bodies_
std::vector< Body * > bodies_
Definition: bodies.h:635
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:1066
bodies::Box::maxCorner_
Eigen::Vector3d maxCorner_
The translated, but not rotated max corner.
Definition: bodies.h:485
bodies::Body::type_
shapes::ShapeType type_
The type of shape this body was constructed from.
Definition: bodies.h:312
bodies::BoundingCylinder::length
double length
Definition: bodies.h:105
bodies::Box::length2_
double length2_
Definition: bodies.h:487
y
double y
Definition: mesh_operations.cpp:202
bodies::BoundingCylinder
Definition of a cylinder.
Definition: bodies.h:101
shapes::ShapeType
ShapeType
A list of known shape types.
Definition: shapes.h:61
bodies::Box::height2_
double height2_
Definition: bodies.h:489
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:1157
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:1392
bodies::Body::getType
shapes::ShapeType getType() const
Get the type of shape this body represents.
Definition: bodies.h:131
bodies::Box::width_
double width_
Definition: bodies.h:477
bodies::Box::height_
double height_
Definition: bodies.h:478
bodies::Box::getScaledDimensions
std::vector< double > getScaledDimensions() const override
Get the dimensions associated to this body (scaled and padded)
Definition: bodies.cpp:577
bodies::Box
Definition of a box.
Definition: bodies.h:436
bodies::ConvexMesh::getTriangles
const std::vector< unsigned int > & getTriangles() const
Definition: bodies.cpp:1105
bodies::Cylinder::radiusBSqr_
double radiusBSqr_
Definition: bodies.h:426
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:103
bodies::Body::getPose
const Eigen::Isometry3d & getPose() const
Retrieve the pose of the body.
Definition: bodies.h:211
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:330
bodies::Box::minCorner_
Eigen::Vector3d minCorner_
The translated, but not rotated min corner.
Definition: bodies.h:484
bodies::BodyVector::getCount
std::size_t getCount() const
Get the number of bodies in this vector.
Definition: bodies.cpp:1348
aabb.h
bodies::ConvexMesh::radiusBSqr_
double radiusBSqr_
Definition: bodies.h:573
bodies::Sphere::center_
Eigen::Vector3d center_
Definition: bodies.h:365
bodies::Cylinder::normalB2_
Eigen::Vector3d normalB2_
Definition: bodies.h:421
bodies::Cylinder::radius_
double radius_
Definition: bodies.h:415
bodies::Body::setDimensions
void setDimensions(const shapes::Shape *shape)
Set the dimensions of the body (from corresponding shape)
Definition: bodies.h:236
bodies::BodyVector::addBody
void addBody(Body *body)
Add a body.
Definition: bodies.cpp:1332
bodies::BodyVector::~BodyVector
~BodyVector()
Definition: bodies.cpp:1320
bodies::ConvexMesh::bounding_box_
Box bounding_box_
Definition: bodies.h:574
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:372
bodies::ConvexMesh::i_pose_
Eigen::Isometry3d i_pose_
Definition: bodies.h:570
bodies::ConvexMesh::correctVertexOrderFromPlanes
void correctVertexOrderFromPlanes()
Definition: bodies.cpp:789
random_numbers::RandomNumberGenerator
bodies::BodyPtr
std::shared_ptr< Body > BodyPtr
Shared pointer to a Body.
Definition: bodies.h:109
bodies::Cylinder::getScaledDimensions
std::vector< double > getScaledDimensions() const override
Get the dimensions associated to this body (scaled and padded)
Definition: bodies.cpp:341
bodies::Cylinder::radius2_
double radius2_
Definition: bodies.h:427
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:158
bodies::BoundingSphere::center
Eigen::Vector3d center
Definition: bodies.h:126
bodies::Box::radius2_
double radius2_
Definition: bodies.h:491
bodies::ConvexMesh::center_
Eigen::Vector3d center_
Definition: bodies.h:571
bodies::Sphere::~Sphere
~Sphere() override=default
bodies::BodyConstPtr
std::shared_ptr< const Body > BodyConstPtr
Shared pointer to a const Body.
Definition: bodies.h:115
bodies::Body::setPaddingDirty
void setPaddingDirty(double padd)
If the dimension of the body should be padded, this method sets the pading.
Definition: bodies.h:171
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:151
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:414
bodies::Body::setPose
void setPose(const Eigen::Isometry3d &pose)
Set the pose of the body. Default is identity.
Definition: bodies.h:204
bodies::Body::padding_
double padding_
The scale that was set for this body.
Definition: bodies.h:309
bodies::ConvexMesh::getScaledVertices
const EigenSTL::vector_Vector3d & getScaledVertices() const
Definition: bodies.cpp:1117
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:406
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:178
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:550
bodies::ConvexMesh::scaled_vertices_
EigenSTL::vector_Vector3d * scaled_vertices_
Definition: bodies.h:580
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:238
bodies::Box::invRot_
Eigen::Matrix3d invRot_
Definition: bodies.h:482
bodies::Cylinder::radiusU_
double radiusU_
Definition: bodies.h:424
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:388
bodies::ConvexMesh::computeScaledVerticesFromPlaneProjections
void computeScaledVerticesFromPlaneProjections()
Project the original vertex to the scaled and padded planes and average.
Definition: bodies.cpp:1002
bodies::BodyVector
A vector of Body objects.
Definition: bodies.h:592
bodies::Cylinder
Definition of a cylinder.
Definition: bodies.h:374
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:346
bodies::ConvexMesh
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Definition: bodies.h:498
bodies::BodyVector::clear
void clear()
Clear all bodies from the vector.
Definition: bodies.cpp:1325
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:775
z
double z
Definition: mesh_operations.cpp:202
bodies::Box::width2_
double width2_
Definition: bodies.h:488
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:1146
bodies::ConvexMesh::getPlanes
const EigenSTL::vector_Vector4d & getPlanes() const
Get the planes that define the convex shape.
Definition: bodies.cpp:1122
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:215
bodies::ConvexMesh::getVertices
const EigenSTL::vector_Vector3d & getVertices() const
Definition: bodies.cpp:1111
bodies::Box::Box
Box()
Definition: bodies.h:439
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:285
bodies::Box::length_
double length_
Definition: bodies.h:476
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 Sun Aug 22 2021 02:42:25