00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef GEOMETRIC_SHAPES_BODIES_
00038 #define GEOMETRIC_SHAPES_BODIES_
00039
00040 #include "geometric_shapes/shapes.h"
00041 #include <eigen_stl_containers/eigen_stl_containers.h>
00042 #include <boost/scoped_ptr.hpp>
00043 #include <random_numbers/random_numbers.h>
00044 #include <vector>
00045 #include <Eigen/Core>
00046 #include <Eigen/Geometry>
00047
00052 namespace bodies
00053 {
00054
00056 struct BoundingSphere
00057 {
00058 Eigen::Vector3d center;
00059 double radius;
00060
00061 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00062 };
00063
00065 struct BoundingCylinder
00066 {
00067 Eigen::Affine3d pose;
00068 double radius;
00069 double length;
00070
00071 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00072 };
00073
00074 class Body;
00075
00077 typedef boost::shared_ptr<Body> BodyPtr;
00078
00080 typedef boost::shared_ptr<const Body> BodyConstPtr;
00081
00085 class Body
00086 {
00087 public:
00088
00089 Body() : scale_(1.0), padding_(0.0), type_(shapes::UNKNOWN_SHAPE)
00090 {
00091 pose_.setIdentity();
00092 }
00093
00094 virtual ~Body()
00095 {
00096 }
00097
00099 shapes::ShapeType getType() const
00100 {
00101 return type_;
00102 }
00103
00106 void setScale(double scale)
00107 {
00108 scale_ = scale;
00109 updateInternalData();
00110 }
00111
00113 double getScale() const
00114 {
00115 return scale_;
00116 }
00117
00120 void setPadding(double padd)
00121 {
00122 padding_ = padd;
00123 updateInternalData();
00124 }
00125
00127 double getPadding() const
00128 {
00129 return padding_;
00130 }
00131
00133 void setPose(const Eigen::Affine3d &pose)
00134 {
00135 pose_ = pose;
00136 updateInternalData();
00137 }
00138
00140 const Eigen::Affine3d& getPose() const
00141 {
00142 return pose_;
00143 }
00144
00146 virtual std::vector<double> getDimensions() const = 0;
00147
00149 void setDimensions(const shapes::Shape *shape);
00150
00152 bool containsPoint(double x, double y, double z, bool verbose = false) const
00153 {
00154 Eigen::Vector3d pt(x, y, z);
00155 return containsPoint(pt, verbose);
00156 }
00157
00159 virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose = false) const = 0;
00160
00165 virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections = NULL, unsigned int count = 0) const = 0;
00166
00169 virtual double computeVolume() const = 0;
00170
00174 virtual bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result);
00175
00178 virtual void computeBoundingSphere(BoundingSphere &sphere) const = 0;
00179
00182 virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const = 0;
00183
00185 BodyPtr cloneAt(const Eigen::Affine3d &pose) const
00186 {
00187 return cloneAt(pose, padding_, scale_);
00188 }
00189
00194 virtual BodyPtr cloneAt(const Eigen::Affine3d &pose, double padding, double scaling) const = 0;
00195
00196 protected:
00197
00201 virtual void updateInternalData() = 0;
00202
00205 virtual void useDimensions(const shapes::Shape *shape) = 0;
00206
00208 double scale_;
00209
00211 double padding_;
00212
00214 shapes::ShapeType type_;
00215
00217 Eigen::Affine3d pose_;
00218
00219 public:
00220 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00221 };
00222
00224 class Sphere : public Body
00225 {
00226 public:
00227 Sphere() : Body()
00228 {
00229 type_ = shapes::SPHERE;
00230 }
00231
00232 Sphere(const shapes::Shape *shape) : Body()
00233 {
00234 type_ = shapes::SPHERE;
00235 setDimensions(shape);
00236 }
00237
00238 virtual ~Sphere()
00239 {
00240 }
00241
00243 virtual std::vector<double> getDimensions() const;
00244
00245 virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose = false) const;
00246 virtual double computeVolume() const;
00247 virtual bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result);
00248 virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00249 virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const;
00250 virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections = NULL, unsigned int count = 0) const;
00251
00252 virtual BodyPtr cloneAt(const Eigen::Affine3d &pose, double padding, double scale) const;
00253
00254 protected:
00255
00256 virtual void useDimensions(const shapes::Shape *shape);
00257 virtual void updateInternalData();
00258
00259
00260 double radius_;
00261
00262
00263 Eigen::Vector3d center_;
00264 double radiusU_;
00265 double radius2_;
00266
00267 public:
00268 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00269 };
00270
00272 class Cylinder : public Body
00273 {
00274 public:
00275 Cylinder() : Body()
00276 {
00277 type_ = shapes::CYLINDER;
00278 }
00279
00280 Cylinder(const shapes::Shape *shape) : Body()
00281 {
00282 type_ = shapes::CYLINDER;
00283 setDimensions(shape);
00284 }
00285
00286 virtual ~Cylinder()
00287 {
00288 }
00289
00291 virtual std::vector<double> getDimensions() const;
00292
00293 virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose = false) const;
00294 virtual double computeVolume() const;
00295 virtual bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result);
00296 virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00297 virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const;
00298 virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections = NULL, unsigned int count = 0) const;
00299
00300 virtual BodyPtr cloneAt(const Eigen::Affine3d &pose, double padding, double scale) const;
00301
00302 protected:
00303
00304 virtual void useDimensions(const shapes::Shape *shape);
00305 virtual void updateInternalData();
00306
00307
00308 double length_;
00309 double radius_;
00310
00311
00312 Eigen::Vector3d center_;
00313 Eigen::Vector3d normalH_;
00314 Eigen::Vector3d normalB1_;
00315 Eigen::Vector3d normalB2_;
00316
00317 double length2_;
00318 double radiusU_;
00319 double radiusB_;
00320 double radiusBSqr_;
00321 double radius2_;
00322 double d1_;
00323 double d2_;
00324
00325 public:
00326 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00327 };
00328
00330 class Box : public Body
00331 {
00332 public:
00333 Box() : Body()
00334 {
00335 type_ = shapes::BOX;
00336 }
00337
00338 Box(const shapes::Shape *shape) : Body()
00339 {
00340 type_ = shapes::BOX;
00341 setDimensions(shape);
00342 }
00343
00344 virtual ~Box()
00345 {
00346 }
00347
00349 virtual std::vector<double> getDimensions() const;
00350
00351 virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose = false) const;
00352 virtual double computeVolume() const;
00353 virtual bool samplePointInside(random_numbers::RandomNumberGenerator &rng, unsigned int max_attempts, Eigen::Vector3d &result);
00354 virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00355 virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const;
00356 virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections = NULL, unsigned int count = 0) const;
00357
00358 virtual BodyPtr cloneAt(const Eigen::Affine3d &pose, double padding, double scale) const;
00359
00360 protected:
00361
00362 virtual void useDimensions(const shapes::Shape *shape);
00363 virtual void updateInternalData();
00364
00365
00366 double length_;
00367 double width_;
00368 double height_;
00369
00370
00371 Eigen::Vector3d center_;
00372 Eigen::Vector3d normalL_;
00373 Eigen::Vector3d normalW_;
00374 Eigen::Vector3d normalH_;
00375
00376 Eigen::Vector3d corner1_;
00377 Eigen::Vector3d corner2_;
00378
00379 double length2_;
00380 double width2_;
00381 double height2_;
00382 double radiusB_;
00383 double radius2_;
00384
00385 public:
00386 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00387 };
00388
00390 class ConvexMesh : public Body
00391 {
00392 public:
00393
00394 ConvexMesh() : Body()
00395 {
00396 type_ = shapes::MESH;
00397 scaled_vertices_ = NULL;
00398 }
00399
00400 ConvexMesh(const shapes::Shape *shape) : Body()
00401 {
00402 type_ = shapes::MESH;
00403 scaled_vertices_ = NULL;
00404 setDimensions(shape);
00405 }
00406
00407 virtual ~ConvexMesh()
00408 {
00409 }
00410
00412 virtual std::vector<double> getDimensions() const;
00413
00414 virtual bool containsPoint(const Eigen::Vector3d &p, bool verbose = false) const;
00415 virtual double computeVolume() const;
00416
00417 virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00418 virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const;
00419 virtual bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d &dir, EigenSTL::vector_Vector3d *intersections = NULL, unsigned int count = 0) const;
00420
00421 const std::vector<unsigned int>& getTriangles() const;
00422 const EigenSTL::vector_Vector3d& getVertices() const;
00423 const EigenSTL::vector_Vector3d& getScaledVertices() const;
00424
00425 virtual BodyPtr cloneAt(const Eigen::Affine3d &pose, double padding, double scale) const;
00426
00428 void computeScaledVerticesFromPlaneProjections();
00429
00430 void correctVertexOrderFromPlanes();
00431
00432 protected:
00433
00434 virtual void useDimensions(const shapes::Shape *shape);
00435 virtual void updateInternalData();
00436
00438 unsigned int countVerticesBehindPlane(const Eigen::Vector4f& planeNormal) const;
00439
00441 bool isPointInsidePlanes(const Eigen::Vector3d& point) const;
00442
00443 struct MeshData
00444 {
00445 EigenSTL::vector_Vector4f planes_;
00446 EigenSTL::vector_Vector3d vertices_;
00447 std::vector<unsigned int> triangles_;
00448 std::map<unsigned int, unsigned int> plane_for_triangle_;
00449 Eigen::Vector3d mesh_center_;
00450 double mesh_radiusB_;
00451 Eigen::Vector3d box_offset_;
00452 Eigen::Vector3d box_size_;
00453 BoundingCylinder bounding_cylinder_;
00454
00455 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00456 };
00457
00458
00459 boost::shared_ptr<MeshData> mesh_data_;
00460
00461
00462 Eigen::Affine3d i_pose_;
00463 Eigen::Vector3d center_;
00464 double radiusB_;
00465 double radiusBSqr_;
00466 Box bounding_box_;
00467
00468
00469
00470
00471 EigenSTL::vector_Vector3d *scaled_vertices_;
00472
00473 private:
00474 boost::scoped_ptr<EigenSTL::vector_Vector3d> scaled_vertices_storage_;
00475
00476 public:
00477 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00478 };
00479
00483 class BodyVector
00484 {
00485 public:
00486
00487 BodyVector();
00488
00490 BodyVector(const std::vector<shapes::Shape*>& shapes, const EigenSTL::vector_Affine3d& poses, double padding = 0.0);
00491
00492 ~BodyVector();
00493
00495 void addBody(Body* body);
00496
00498 void addBody(const shapes::Shape* shape, const Eigen::Affine3d& pose, double padding = 0.0);
00499
00501 void clear();
00502
00504 void setPose(unsigned int i, const Eigen::Affine3d& pose);
00505
00507 std::size_t getCount() const;
00508
00510 bool containsPoint(const Eigen::Vector3d &p, bool verbose = false) const;
00511
00513 bool containsPoint(const Eigen::Vector3d &p, std::size_t &index, bool verbose = false) const;
00514
00519 bool intersectsRay(const Eigen::Vector3d& origin, const Eigen::Vector3d &dir, std::size_t &index, EigenSTL::vector_Vector3d *intersections = NULL, unsigned int count = 0) const;
00520
00522 const Body* getBody(unsigned int i) const;
00523
00524 private:
00525
00526 std::vector<Body*> bodies_;
00527
00528 };
00529
00531 typedef boost::shared_ptr<Body> BodyPtr;
00532
00534 typedef boost::shared_ptr<const Body> BodyConstPtr;
00535
00536 }
00537
00538 #endif