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 
47 #include <Eigen/Core>
48 #include <Eigen/Geometry>
49 #include <memory>
50 #include <vector>
51 
56 namespace bodies
57 {
60 {
61  Eigen::Vector3d center;
62  double radius;
63 
64  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
65 };
66 
69 {
70  Eigen::Affine3d pose;
71  double radius;
72  double length;
73 
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()
97  {
98  }
99 
102  {
103  return type_;
104  }
105 
108  void setScale(double scale)
109  {
110  scale_ = scale;
111  updateInternalData();
112  }
113 
115  double getScale() const
116  {
117  return scale_;
118  }
119 
122  void setPadding(double padd)
123  {
124  padding_ = padd;
125  updateInternalData();
126  }
127 
129  double getPadding() const
130  {
131  return padding_;
132  }
133 
135  void setPose(const Eigen::Affine3d& pose)
136  {
137  pose_ = pose;
138  updateInternalData();
139  }
140 
142  const Eigen::Affine3d& getPose() const
143  {
144  return pose_;
145  }
146 
148  virtual std::vector<double> getDimensions() const = 0;
149 
151  void setDimensions(const shapes::Shape* shape);
152 
154  bool containsPoint(double x, double y, double z, bool verbose = false) const
155  {
156  Eigen::Vector3d pt(x, y, z);
157  return containsPoint(pt, verbose);
158  }
159 
161  virtual bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const = 0;
162 
167  virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
168  EigenSTL::vector_Vector3d* intersections = NULL, unsigned int count = 0) const = 0;
169 
172  virtual double computeVolume() const = 0;
173 
179  virtual bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
180  Eigen::Vector3d& result);
181 
184  virtual void computeBoundingSphere(BoundingSphere& sphere) const = 0;
185 
188  virtual void computeBoundingCylinder(BoundingCylinder& cylinder) const = 0;
189 
191  BodyPtr cloneAt(const Eigen::Affine3d& pose) const
192  {
193  return cloneAt(pose, padding_, scale_);
194  }
195 
200  virtual BodyPtr cloneAt(const Eigen::Affine3d& pose, double padding, double scaling) const = 0;
201 
202 protected:
206  virtual void updateInternalData() = 0;
207 
209  virtual void useDimensions(const shapes::Shape* shape) = 0;
210 
212  double scale_;
213 
215  double padding_;
216 
219 
221  Eigen::Affine3d pose_;
222 
223 public:
224  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
225 };
226 
228 class Sphere : public Body
229 {
230 public:
231  Sphere() : Body()
232  {
233  type_ = shapes::SPHERE;
234  }
235 
236  Sphere(const shapes::Shape* shape) : Body()
237  {
238  type_ = shapes::SPHERE;
239  setDimensions(shape);
240  }
241 
242  virtual ~Sphere()
243  {
244  }
245 
247  virtual std::vector<double> getDimensions() const;
248 
249  virtual bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const;
250  virtual double computeVolume() const;
251  virtual bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
252  Eigen::Vector3d& result);
253  virtual void computeBoundingSphere(BoundingSphere& sphere) const;
254  virtual void computeBoundingCylinder(BoundingCylinder& cylinder) const;
255  virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
256  EigenSTL::vector_Vector3d* intersections = NULL, unsigned int count = 0) const;
257 
258  virtual BodyPtr cloneAt(const Eigen::Affine3d& pose, double padding, double scale) const;
259 
260 protected:
261  virtual void useDimensions(const shapes::Shape* shape);
262  virtual void updateInternalData();
263 
264  // shape-dependent data
265  double radius_;
266 
267  // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
268  Eigen::Vector3d center_;
269  double radiusU_;
270  double radius2_;
271 
272 public:
273  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
274 };
275 
277 class Cylinder : public Body
278 {
279 public:
281  {
282  type_ = shapes::CYLINDER;
283  }
284 
285  Cylinder(const shapes::Shape* shape) : Body()
286  {
287  type_ = shapes::CYLINDER;
288  setDimensions(shape);
289  }
290 
291  virtual ~Cylinder()
292  {
293  }
294 
296  virtual std::vector<double> getDimensions() const;
297 
298  virtual bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const;
299  virtual double computeVolume() const;
300  virtual bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
301  Eigen::Vector3d& result);
302  virtual void computeBoundingSphere(BoundingSphere& sphere) const;
303  virtual void computeBoundingCylinder(BoundingCylinder& cylinder) const;
304  virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
305  EigenSTL::vector_Vector3d* intersections = NULL, unsigned int count = 0) const;
306 
307  virtual BodyPtr cloneAt(const Eigen::Affine3d& pose, double padding, double scale) const;
308 
309 protected:
310  virtual void useDimensions(const shapes::Shape* shape);
311  virtual void updateInternalData();
312 
313  // shape-dependent data
314  double length_;
315  double radius_;
316 
317  // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
318  Eigen::Vector3d center_;
319  Eigen::Vector3d normalH_;
320  Eigen::Vector3d normalB1_;
321  Eigen::Vector3d normalB2_;
322 
323  double length2_;
324  double radiusU_;
325  double radiusB_;
326  double radiusBSqr_;
327  double radius2_;
328  double d1_;
329  double d2_;
330 
331 public:
332  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
333 };
334 
336 class Box : public Body
337 {
338 public:
339  Box() : Body()
340  {
341  type_ = shapes::BOX;
342  }
343 
344  Box(const shapes::Shape* shape) : Body()
345  {
346  type_ = shapes::BOX;
347  setDimensions(shape);
348  }
349 
350  virtual ~Box()
351  {
352  }
353 
355  virtual std::vector<double> getDimensions() const;
356 
357  virtual bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const;
358  virtual double computeVolume() const;
359  virtual bool samplePointInside(random_numbers::RandomNumberGenerator& rng, unsigned int max_attempts,
360  Eigen::Vector3d& result);
361  virtual void computeBoundingSphere(BoundingSphere& sphere) const;
362  virtual void computeBoundingCylinder(BoundingCylinder& cylinder) const;
363  virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
364  EigenSTL::vector_Vector3d* intersections = NULL, unsigned int count = 0) const;
365 
366  virtual BodyPtr cloneAt(const Eigen::Affine3d& pose, double padding, double scale) const;
367 
368 protected:
369  virtual void useDimensions(const shapes::Shape* shape); // (x, y, z) = (length, width, height)
370  virtual void updateInternalData();
371 
372  // shape-dependent data
373  double length_;
374  double width_;
375  double height_;
376 
377  // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
378  Eigen::Vector3d center_;
379  Eigen::Vector3d normalL_;
380  Eigen::Vector3d normalW_;
381  Eigen::Vector3d normalH_;
382 
383  Eigen::Vector3d corner1_;
384  Eigen::Vector3d corner2_;
385 
386  double length2_;
387  double width2_;
388  double height2_;
389  double radiusB_;
390  double radius2_;
391 
392 public:
393  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
394 };
395 
397 class ConvexMesh : public Body
398 {
399 public:
401  {
402  type_ = shapes::MESH;
403  scaled_vertices_ = NULL;
404  }
405 
406  ConvexMesh(const shapes::Shape* shape) : Body()
407  {
408  type_ = shapes::MESH;
409  scaled_vertices_ = NULL;
410  setDimensions(shape);
411  }
412 
413  virtual ~ConvexMesh()
414  {
415  }
416 
418  virtual std::vector<double> getDimensions() const;
419 
420  virtual bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const;
421  virtual double computeVolume() const;
422 
423  virtual void computeBoundingSphere(BoundingSphere& sphere) const;
424  virtual void computeBoundingCylinder(BoundingCylinder& cylinder) const;
425  virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir,
426  EigenSTL::vector_Vector3d* intersections = NULL, unsigned int count = 0) const;
427 
428  const std::vector<unsigned int>& getTriangles() const;
429  const EigenSTL::vector_Vector3d& getVertices() const;
430  const EigenSTL::vector_Vector3d& getScaledVertices() const;
431 
432  virtual BodyPtr cloneAt(const Eigen::Affine3d& pose, double padding, double scale) const;
433 
435  void computeScaledVerticesFromPlaneProjections();
436 
437  void correctVertexOrderFromPlanes();
438 
439 protected:
440  virtual void useDimensions(const shapes::Shape* shape);
441  virtual void updateInternalData();
442 
444  unsigned int countVerticesBehindPlane(const Eigen::Vector4f& planeNormal) const;
445 
447  bool isPointInsidePlanes(const Eigen::Vector3d& point) const;
448 
449  struct MeshData
450  {
453  std::vector<unsigned int> triangles_;
454  std::map<unsigned int, unsigned int> plane_for_triangle_;
455  Eigen::Vector3d mesh_center_;
457  Eigen::Vector3d box_offset_;
458  Eigen::Vector3d box_size_;
460 
461  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
462  };
463 
464  // shape-dependent data; keep this in one struct so that a cheap pointer copy can be done in cloneAt()
465  std::shared_ptr<MeshData> mesh_data_;
466 
467  // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
468  Eigen::Affine3d i_pose_;
469  Eigen::Vector3d center_;
470  double radiusB_;
471  double radiusBSqr_;
473 
474  // pointer to an array of scaled vertices
475  // If the padding is 0 & scaling is 1, then there is no need to have scaled vertices;
476  // we can just point to the vertices in mesh_data_.
477  // Otherwise, point to scaled_vertices_storage_
479 
480 private:
481  std::unique_ptr<EigenSTL::vector_Vector3d> scaled_vertices_storage_;
482 
483 public:
484  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
485 };
486 
491 {
492 public:
493  BodyVector();
494 
496  BodyVector(const std::vector<shapes::Shape*>& shapes, const EigenSTL::vector_Affine3d& poses, double padding = 0.0);
497 
498  ~BodyVector();
499 
501  void addBody(Body* body);
502 
504  void addBody(const shapes::Shape* shape, const Eigen::Affine3d& pose, double padding = 0.0);
505 
507  void clear();
508 
510  void setPose(unsigned int i, const Eigen::Affine3d& pose);
511 
513  std::size_t getCount() const;
514 
516  bool containsPoint(const Eigen::Vector3d& p, bool verbose = false) const;
517 
519  bool containsPoint(const Eigen::Vector3d& p, std::size_t& index, bool verbose = false) const;
520 
526  bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d& dir, std::size_t& index,
527  EigenSTL::vector_Vector3d* intersections = NULL, unsigned int count = 0) const;
528 
530  const Body* getBody(unsigned int i) const;
531 
532 private:
533  std::vector<Body*> bodies_;
534 };
535 
537 typedef std::shared_ptr<Body> BodyPtr;
538 
540 typedef std::shared_ptr<const Body> BodyConstPtr;
541 }
542 
543 #endif
const Eigen::Affine3d & getPose() const
Retrieve the pose of the body.
Definition: bodies.h:142
double radiusBSqr_
Definition: bodies.h:471
Definition of various shapes. No properties such as position are included. These are simply the descr...
Eigen::Vector3d normalH_
Definition: bodies.h:381
void setPose(const Eigen::Affine3d &pose)
Set the pose of the body. Default is identity.
Definition: bodies.h:135
std::vector< unsigned int > triangles_
Definition: bodies.h:453
double radius2_
Definition: bodies.h:270
BodyPtr cloneAt(const Eigen::Affine3d &pose) const
Get a clone of this body, but one that is located at the pose pose.
Definition: bodies.h:191
Eigen::Vector3d corner2_
Definition: bodies.h:384
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
Eigen::Vector3d center_
Definition: bodies.h:378
Eigen::Vector3d normalL_
Definition: bodies.h:379
double width2_
Definition: bodies.h:387
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
shapes::ShapeType getType() const
Get the type of shape this body represents.
Definition: bodies.h:101
bool containsPoint(double x, double y, double z, bool verbose=false) const
Check if a point is inside the body.
Definition: bodies.h:154
virtual ~ConvexMesh()
Definition: bodies.h:413
Definition of a cylinder.
Definition: bodies.h:277
EigenSTL::vector_Vector3d vertices_
Definition: bodies.h:452
double radiusB_
Definition: bodies.h:389
double radius2_
Definition: bodies.h:390
double getPadding() const
Retrieve the current padding.
Definition: bodies.h:129
std::map< unsigned int, unsigned int > plane_for_triangle_
Definition: bodies.h:454
std::shared_ptr< Body > BodyPtr
Shared pointer to a Body.
Definition: bodies.h:77
Definition of a sphere.
Definition: bodies.h:228
double radius_
Definition: bodies.h:265
virtual ~Body()
Definition: bodies.h:96
virtual ~Sphere()
Definition: bodies.h:242
std::shared_ptr< MeshData > mesh_data_
Definition: bodies.h:465
std::vector< Body * > bodies_
Definition: bodies.h:533
Box(const shapes::Shape *shape)
Definition: bodies.h:344
Eigen::Vector3d normalW_
Definition: bodies.h:380
Eigen::Vector3d mesh_center_
Definition: bodies.h:455
void setScale(double scale)
If the dimension of the body should be scaled, this method sets the scale. Default is 1...
Definition: bodies.h:108
double radiusBSqr_
Definition: bodies.h:326
Eigen::Affine3d pose
Definition: bodies.h:70
double length_
Definition: bodies.h:314
Eigen::Vector3d center
Definition: bodies.h:61
EigenSTL::vector_Vector4f planes_
Definition: bodies.h:451
double length2_
Definition: bodies.h:386
double y
Eigen::Vector3d center_
Definition: bodies.h:268
Definition of a sphere that bounds another object.
Definition: bodies.h:59
double radiusU_
Definition: bodies.h:324
Eigen::Vector3d box_size_
Definition: bodies.h:458
shapes::ShapeType type_
The type of shape this body was constructed from.
Definition: bodies.h:218
double radiusB_
Definition: bodies.h:325
std::shared_ptr< const Body > BodyConstPtr
Shared pointer to a const Body.
Definition: bodies.h:83
double width_
Definition: bodies.h:374
double height2_
Definition: bodies.h:388
unsigned int index
A basic definition of a shape. Shapes are considered centered at origin.
Definition: shapes.h:77
double radiusU_
Definition: bodies.h:269
Definition of a convex mesh. Convex hull is computed for a given shape::Mesh.
Definition: bodies.h:397
Eigen::Vector3d normalB1_
Definition: bodies.h:320
Definition of a box.
Definition: bodies.h:336
Definition of a cylinder.
Definition: bodies.h:68
EigenSTL::vector_Vector3d * scaled_vertices_
Definition: bodies.h:478
BoundingCylinder bounding_cylinder_
Definition: bodies.h:459
double radius2_
Definition: bodies.h:327
Eigen::Affine3d pose_
The location of the body (position and orientation)
Definition: bodies.h:221
double length2_
Definition: bodies.h:323
std::vector< Eigen::Vector4f, Eigen::aligned_allocator< Eigen::Vector4f > > vector_Vector4f
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:215
double height_
Definition: bodies.h:375
Eigen::Vector3d center_
Definition: bodies.h:318
A vector of Body objects.
Definition: bodies.h:490
std::unique_ptr< EigenSTL::vector_Vector3d > scaled_vertices_storage_
Definition: bodies.h:481
Eigen::Vector3d center_
Definition: bodies.h:469
double d1_
Definition: bodies.h:328
Eigen::Vector3d normalH_
Definition: bodies.h:319
Eigen::Vector3d normalB2_
Definition: bodies.h:321
Cylinder(const shapes::Shape *shape)
Definition: bodies.h:285
ConvexMesh(const shapes::Shape *shape)
Definition: bodies.h:406
Eigen::Vector3d corner1_
Definition: bodies.h:383
double radius_
Definition: bodies.h:315
Sphere(const shapes::Shape *shape)
Definition: bodies.h:236
Eigen::Vector3d box_offset_
Definition: bodies.h:457
virtual ~Cylinder()
Definition: bodies.h:291
double d2_
Definition: bodies.h:329
double length_
Definition: bodies.h:373
double x
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...
A body is a shape + its pose. Point inclusion, ray intersection can be tested, volumes and bounding s...
Definition: bodies.h:88
double radiusB_
Definition: bodies.h:470
void setPadding(double padd)
If constant padding should be added to the body, this method sets the padding. Default is 0...
Definition: bodies.h:122
double scale_
The scale that was set for this body.
Definition: bodies.h:212
double getScale() const
Retrieve the current scale.
Definition: bodies.h:115
This set of classes allows quickly detecting whether a given point is inside an object or not...
Definition: bodies.h:56
virtual ~Box()
Definition: bodies.h:350
Eigen::Affine3d i_pose_
Definition: bodies.h:468


geometric_shapes
Author(s): Ioan Sucan , Gil Jones
autogenerated on Fri Jun 7 2019 21:59:29