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"
48 #include <Eigen/Core>
49 #include <Eigen/Geometry>
50 #include <memory>
51 #include <vector>
52 
57 namespace bodies
58 {
61 {
62  Eigen::Vector3d center;
63  double radius;
64 
65  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
66 };
67 
70 {
71  Eigen::Isometry3d pose;
72  double radius;
73  double length;
74 
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()
98  {
99  }
100 
103  {
104  return type_;
105  }
106 
109  void setScale(double scale)
110  {
111  scale_ = scale;
112  updateInternalData();
113  }
114 
116  double getScale() const
117  {
118  return scale_;
119  }
120 
123  void setPadding(double padd)
124  {
125  padding_ = padd;
126  updateInternalData();
127  }
128 
130  double getPadding() const
131  {
132  return padding_;
133  }
134 
136  void setPose(const Eigen::Isometry3d& pose)
137  {
138  pose_ = pose;
139  updateInternalData();
140  }
141 
143  const Eigen::Isometry3d& getPose() const
144  {
145  return pose_;
146  }
147 
149  virtual std::vector<double> getDimensions() const = 0;
150 
152  void setDimensions(const shapes::Shape* shape);
153 
155  bool containsPoint(double x, double y, double z, bool verbose = false) const
156  {
157  Eigen::Vector3d pt(x, y, z);
158  return containsPoint(pt, verbose);
159  }
160 
162  virtual bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const = 0;
163 
168  virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
169  EigenSTL::vector_Vector3d* intersections = NULL, unsigned int count = 0) const = 0;
170 
173  virtual double computeVolume() const = 0;
174 
180  virtual bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
181  Eigen::Vector3d& result);
182 
185  virtual void computeBoundingSphere(BoundingSphere& sphere) const = 0;
186 
189  virtual void computeBoundingCylinder(BoundingCylinder& cylinder) const = 0;
190 
193  virtual void computeBoundingBox(AABB& bbox) const = 0;
194 
196  BodyPtr cloneAt(const Eigen::Isometry3d& pose) const
197  {
198  return cloneAt(pose, padding_, scale_);
199  }
200 
205  virtual BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scaling) const = 0;
206 
207 protected:
211  virtual void updateInternalData() = 0;
212 
214  virtual void useDimensions(const shapes::Shape* shape) = 0;
215 
217  double scale_;
218 
220  double padding_;
221 
224 
226  Eigen::Isometry3d pose_;
227 
228 public:
229  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
230 };
231 
233 class Sphere : public Body
234 {
235 public:
236  Sphere() : Body()
237  {
238  type_ = shapes::SPHERE;
239  }
240 
241  Sphere(const shapes::Shape* shape) : Body()
242  {
243  type_ = shapes::SPHERE;
244  setDimensions(shape);
245  }
246 
247  virtual ~Sphere()
248  {
249  }
250 
252  virtual std::vector<double> getDimensions() const;
253 
254  virtual bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const;
255  virtual double computeVolume() const;
256  virtual bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
257  Eigen::Vector3d& result);
258  virtual void computeBoundingSphere(BoundingSphere& sphere) const;
259  virtual void computeBoundingCylinder(BoundingCylinder& cylinder) const;
260  virtual void computeBoundingBox(AABB& bbox) const;
261  virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
262  EigenSTL::vector_Vector3d* intersections = NULL, unsigned int count = 0) const;
263 
264  virtual BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const;
265 
266 protected:
267  virtual void useDimensions(const shapes::Shape* shape);
268  virtual void updateInternalData();
269 
270  // shape-dependent data
271  double radius_;
272 
273  // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
274  Eigen::Vector3d center_;
275  double radiusU_;
276  double radius2_;
277 
278 public:
279  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
280 };
281 
283 class Cylinder : public Body
284 {
285 public:
287  {
288  type_ = shapes::CYLINDER;
289  }
290 
291  Cylinder(const shapes::Shape* shape) : Body()
292  {
293  type_ = shapes::CYLINDER;
294  setDimensions(shape);
295  }
296 
297  virtual ~Cylinder()
298  {
299  }
300 
302  virtual std::vector<double> getDimensions() const;
303 
304  virtual bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const;
305  virtual double computeVolume() const;
306  virtual bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
307  Eigen::Vector3d& result);
308  virtual void computeBoundingSphere(BoundingSphere& sphere) const;
309  virtual void computeBoundingCylinder(BoundingCylinder& cylinder) const;
310  virtual void computeBoundingBox(AABB& bbox) const;
311  virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
312  EigenSTL::vector_Vector3d* intersections = NULL, unsigned int count = 0) const;
313 
314  virtual BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const;
315 
316 protected:
317  virtual void useDimensions(const shapes::Shape* shape);
318  virtual void updateInternalData();
319 
320  // shape-dependent data
321  double length_;
322  double radius_;
323 
324  // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
325  Eigen::Vector3d center_;
326  Eigen::Vector3d normalH_;
327  Eigen::Vector3d normalB1_;
328  Eigen::Vector3d normalB2_;
329 
330  double length2_;
331  double radiusU_;
332  double radiusB_;
333  double radiusBSqr_;
334  double radius2_;
335  double d1_;
336  double d2_;
337 
338 public:
339  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
340 };
341 
343 class Box : public Body
344 {
345 public:
346  Box() : Body()
347  {
348  type_ = shapes::BOX;
349  }
350 
351  Box(const shapes::Shape* shape) : Body()
352  {
353  type_ = shapes::BOX;
354  setDimensions(shape);
355  }
356 
357  virtual ~Box()
358  {
359  }
360 
362  virtual std::vector<double> getDimensions() const;
363 
364  virtual bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const;
365  virtual double computeVolume() const;
366  virtual bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
367  Eigen::Vector3d& result);
368  virtual void computeBoundingSphere(BoundingSphere& sphere) const;
369  virtual void computeBoundingCylinder(BoundingCylinder& cylinder) const;
370  virtual void computeBoundingBox(AABB& bbox) const;
371  virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
372  EigenSTL::vector_Vector3d* intersections = NULL, unsigned int count = 0) const;
373 
374  virtual BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const;
375 
376 protected:
377  virtual void useDimensions(const shapes::Shape* shape); // (x, y, z) = (length, width, height)
378  virtual void updateInternalData();
379 
380  // shape-dependent data
381  double length_;
382  double width_;
383  double height_;
384 
385  // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
386  Eigen::Vector3d center_;
387  Eigen::Vector3d normalL_;
388  Eigen::Vector3d normalW_;
389  Eigen::Vector3d normalH_;
390 
391  Eigen::Vector3d corner1_;
392  Eigen::Vector3d corner2_;
393 
394  double length2_;
395  double width2_;
396  double height2_;
397  double radiusB_;
398  double radius2_;
399 
400 public:
401  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
402 };
403 
405 class ConvexMesh : public Body
406 {
407 public:
409  {
410  type_ = shapes::MESH;
411  scaled_vertices_ = NULL;
412  }
413 
414  ConvexMesh(const shapes::Shape* shape) : Body()
415  {
416  type_ = shapes::MESH;
417  scaled_vertices_ = NULL;
418  setDimensions(shape);
419  }
420 
421  virtual ~ConvexMesh()
422  {
423  }
424 
426  virtual std::vector<double> getDimensions() const;
427 
428  virtual bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const;
429  virtual double computeVolume() const;
430 
431  virtual void computeBoundingSphere(BoundingSphere& sphere) const;
432  virtual void computeBoundingCylinder(BoundingCylinder& cylinder) const;
433  virtual void computeBoundingBox(AABB& bbox) const;
434  virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
435  EigenSTL::vector_Vector3d* intersections = NULL, unsigned int count = 0) const;
436 
437  const std::vector<unsigned int>& getTriangles() const;
438  const EigenSTL::vector_Vector3d& getVertices() const;
439  const EigenSTL::vector_Vector3d& getScaledVertices() const;
440 
445  const EigenSTL::vector_Vector4d& getPlanes() const;
446 
447  virtual BodyPtr cloneAt(const Eigen::Isometry3d& pose, double padding, double scale) const;
448 
450  void computeScaledVerticesFromPlaneProjections();
451 
452  void correctVertexOrderFromPlanes();
453 
454 protected:
455  virtual void useDimensions(const shapes::Shape* shape);
456  virtual void updateInternalData();
457 
459  unsigned int countVerticesBehindPlane(const Eigen::Vector4f& planeNormal) const;
460 
462  bool isPointInsidePlanes(const Eigen::Vector3d& point) const;
463 
464  struct MeshData
465  {
468  std::vector<unsigned int> triangles_;
469  std::map<unsigned int, unsigned int> plane_for_triangle_;
470  Eigen::Vector3d mesh_center_;
472  Eigen::Vector3d box_offset_;
473  Eigen::Vector3d box_size_;
475 
476  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
477  };
478 
479  // shape-dependent data; keep this in one struct so that a cheap pointer copy can be done in cloneAt()
480  std::shared_ptr<MeshData> mesh_data_;
481 
482  // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
483  Eigen::Isometry3d i_pose_;
484  Eigen::Vector3d center_;
485  double radiusB_;
486  double radiusBSqr_;
488 
489  // pointer to an array of scaled vertices
490  // If the padding is 0 & scaling is 1, then there is no need to have scaled vertices;
491  // we can just point to the vertices in mesh_data_.
492  // Otherwise, point to scaled_vertices_storage_
494 
495 private:
496  std::unique_ptr<EigenSTL::vector_Vector3d> scaled_vertices_storage_;
497 
498 public:
499  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
500 };
501 
506 {
507 public:
508  BodyVector();
509 
511  BodyVector(const std::vector<shapes::Shape*>& shapes, const EigenSTL::vector_Isometry3d& poses, double padding = 0.0);
512 
513  ~BodyVector();
514 
516  void addBody(Body* body);
517 
519  void addBody(const shapes::Shape* shape, const Eigen::Isometry3d& pose, double padding = 0.0);
520 
522  void clear();
523 
525  void setPose(unsigned int i, const Eigen::Isometry3d& pose);
526 
528  std::size_t getCount() const;
529 
531  bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const;
532 
534  bool containsPoint(const Eigen::Vector3d& p, std::size_t& index, bool verbose = false) const;
535 
541  bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, std::size_t& index,
542  EigenSTL::vector_Vector3d* intersections = NULL, unsigned int count = 0) const;
543 
545  const Body* getBody(unsigned int i) const;
546 
547 private:
548  std::vector<Body*> bodies_;
549 };
550 
552 typedef std::shared_ptr<Body> BodyPtr;
553 
555 typedef std::shared_ptr<const Body> BodyConstPtr;
556 }
557 
558 #endif
double radiusBSqr_
Definition: bodies.h:486
Definition of various shapes. No properties such as position are included. These are simply the descr...
Eigen::Vector3d normalH_
Definition: bodies.h:389
std::vector< unsigned int > triangles_
Definition: bodies.h:468
double radius2_
Definition: bodies.h:276
void setPose(const Eigen::Isometry3d &pose)
Set the pose of the body. Default is identity.
Definition: bodies.h:136
void computeBoundingSphere(const std::vector< const Body *> &bodies, BoundingSphere &mergedSphere)
Compute the bounding sphere for a set of bodies and store the resulting sphere in mergedSphere...
Eigen::Vector3d corner2_
Definition: bodies.h:392
Eigen::Vector3d center_
Definition: bodies.h:386
Eigen::Vector3d normalL_
Definition: bodies.h:387
double width2_
Definition: bodies.h:395
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
bool containsPoint(double x, double y, double z, bool verbose=false) const
Check if a point is inside the body.
Definition: bodies.h:155
virtual ~ConvexMesh()
Definition: bodies.h:421
Definition of a cylinder.
Definition: bodies.h:283
EigenSTL::vector_Vector3d vertices_
Definition: bodies.h:467
double radiusB_
Definition: bodies.h:397
double radius2_
Definition: bodies.h:398
std::map< unsigned int, unsigned int > plane_for_triangle_
Definition: bodies.h:469
std::shared_ptr< Body > BodyPtr
Shared pointer to a Body.
Definition: bodies.h:78
Definition of a sphere.
Definition: bodies.h:233
double radius_
Definition: bodies.h:271
virtual ~Body()
Definition: bodies.h:97
virtual ~Sphere()
Definition: bodies.h:247
std::shared_ptr< MeshData > mesh_data_
Definition: bodies.h:480
std::vector< Body * > bodies_
Definition: bodies.h:548
Box(const shapes::Shape *shape)
Definition: bodies.h:351
Eigen::Vector3d normalW_
Definition: bodies.h:388
Eigen::Vector3d mesh_center_
Definition: bodies.h:470
void setScale(double scale)
If the dimension of the body should be scaled, this method sets the scale. Default is 1...
Definition: bodies.h:109
double radiusBSqr_
Definition: bodies.h:333
double length_
Definition: bodies.h:321
Eigen::Vector3d center
Definition: bodies.h:62
Eigen::Isometry3d pose_
The location of the body (position and orientation)
Definition: bodies.h:226
double length2_
Definition: bodies.h:394
double y
Eigen::Vector3d center_
Definition: bodies.h:274
Definition of a sphere that bounds another object.
Definition: bodies.h:60
double radiusU_
Definition: bodies.h:331
Eigen::Vector3d box_size_
Definition: bodies.h:473
shapes::ShapeType type_
The type of shape this body was constructed from.
Definition: bodies.h:223
double radiusB_
Definition: bodies.h:332
std::shared_ptr< const Body > BodyConstPtr
Shared pointer to a const Body.
Definition: bodies.h:84
double width_
Definition: bodies.h:382
double height2_
Definition: bodies.h:396
unsigned int index
A basic definition of a shape. Shapes are considered centered at origin.
Definition: shapes.h:77
double radiusU_
Definition: bodies.h:275
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Definition: bodies.h:405
Eigen::Vector3d normalB1_
Definition: bodies.h:327
Definition of a box.
Definition: bodies.h:343
Definition of a cylinder.
Definition: bodies.h:69
EigenSTL::vector_Vector3d * scaled_vertices_
Definition: bodies.h:493
BoundingCylinder bounding_cylinder_
Definition: bodies.h:474
double radius2_
Definition: bodies.h:334
double length2_
Definition: bodies.h:330
double z
ShapeType
A list of known shape types.
Definition: shapes.h:61
double padding_
The scale that was set for this body.
Definition: bodies.h:220
double height_
Definition: bodies.h:383
Eigen::Vector3d center_
Definition: bodies.h:325
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:196
std::vector< Eigen::Vector4d, Eigen::aligned_allocator< Eigen::Vector4d > > vector_Vector4d
EigenSTL::vector_Vector4d planes_
Definition: bodies.h:466
A vector of Body objects.
Definition: bodies.h:505
std::unique_ptr< EigenSTL::vector_Vector3d > scaled_vertices_storage_
Definition: bodies.h:496
double getScale() const
Retrieve the current scale.
Definition: bodies.h:116
Eigen::Vector3d center_
Definition: bodies.h:484
double d1_
Definition: bodies.h:335
double getPadding() const
Retrieve the current padding.
Definition: bodies.h:130
Eigen::Vector3d normalH_
Definition: bodies.h:326
Eigen::Vector3d normalB2_
Definition: bodies.h:328
Cylinder(const shapes::Shape *shape)
Definition: bodies.h:291
Eigen::Isometry3d pose
Definition: bodies.h:71
ConvexMesh(const shapes::Shape *shape)
Definition: bodies.h:414
Eigen::Vector3d corner1_
Definition: bodies.h:391
double radius_
Definition: bodies.h:322
Sphere(const shapes::Shape *shape)
Definition: bodies.h:241
Eigen::Vector3d box_offset_
Definition: bodies.h:472
Represents an axis-aligned bounding box.
Definition: aabb.h:45
std::vector< Eigen::Isometry3d, Eigen::aligned_allocator< Eigen::Isometry3d > > vector_Isometry3d
const Eigen::Isometry3d & getPose() const
Retrieve the pose of the body.
Definition: bodies.h:143
shapes::ShapeType getType() const
Get the type of shape this body represents.
Definition: bodies.h:102
virtual ~Cylinder()
Definition: bodies.h:297
double d2_
Definition: bodies.h:336
double length_
Definition: bodies.h:381
double x
A body is a shape + its pose. Point inclusion, ray intersection can be tested, volumes and bounding s...
Definition: bodies.h:89
double radiusB_
Definition: bodies.h:485
Eigen::Isometry3d i_pose_
Definition: bodies.h:483
void setPadding(double padd)
If constant padding should be added to the body, this method sets the padding. Default is 0...
Definition: bodies.h:123
double scale_
The scale that was set for this body.
Definition: bodies.h:217
This set of classes allows quickly detecting whether a given point is inside an object or not...
Definition: aabb.h:42
virtual ~Box()
Definition: bodies.h:357


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Mon Sep 2 2019 03:27:16