geometric_shapes: bodies.h Source File

Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 /* Author: Ioan Sucan, E. Gil Jones */
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   // shape-dependent data
00260   double          radius_;
00261 
00262   // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
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   // shape-dependent data
00308   double          length_;
00309   double          radius_;
00310 
00311   // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
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); // (x, y, z) = (length, width, height)
00363   virtual void updateInternalData();
00364 
00365   // shape-dependent data
00366   double    length_;
00367   double    width_;
00368   double    height_;
00369 
00370   // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
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   // shape-dependent data; keep this in one struct so that a cheap pointer copy can be done in cloneAt()
00459   boost::shared_ptr<MeshData> mesh_data_;
00460 
00461   // pose/padding/scaling-dependent values & values computed for convenience and fast upcoming computations
00462   Eigen::Affine3d             i_pose_;
00463   Eigen::Vector3d             center_;
00464   double                      radiusB_;
00465   double                      radiusBSqr_;
00466   Box                         bounding_box_;
00467 
00468   // pointer to an array of scaled vertices
00469   // if the padding is 0 & scaling is 1, then there is no need to have scaled vertices; we can just point to the vertices in mesh_data_
00470   // otherwise, point to scaled_vertices_storage_
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