bodies.h
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 
00037 #ifndef GEOMETRIC_SHAPES_BODIES_
00038 #define GEOMETRIC_SHAPES_BODIES_
00039 
00040 #include "geometric_shapes/shapes.h"
00041 #include <tf/LinearMath/Transform.h>
00042 #include <tf/LinearMath/Vector3.h>
00043 // #include <BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h>
00044 // #include <BulletCollision/CollisionShapes/btTriangleMesh.h>
00045 #include <vector>
00046 
00054 namespace bodies
00055 {
00056     
00058 struct BoundingSphere
00059 {
00060   tf::Vector3 center;
00061   double    radius;
00062 };
00063 
00065 struct BoundingCylinder
00066 {
00067   tf::Transform pose;
00068   double radius;
00069   double length;
00070 };
00071   
00075   class Body
00076   {
00077   public:
00078         
00079     Body(void)
00080     {
00081       m_scale = 1.0;
00082       m_padding = 0.0;
00083       m_pose.setIdentity();
00084       m_type = shapes::UNKNOWN_SHAPE;
00085     }
00086         
00087     virtual ~Body(void)
00088     {
00089     }
00090         
00092     shapes::ShapeType getType(void) const
00093     {
00094       return m_type;
00095     }
00096         
00099     void setScale(double scale)
00100     {
00101       m_scale = scale;
00102       updateInternalData();
00103     }
00104         
00106     double getScale(void) const
00107     {
00108       return m_scale;
00109     }
00110         
00113     void setPadding(double padd)
00114     {
00115       m_padding = padd;
00116       updateInternalData();
00117     }
00118         
00120     double getPadding(void) const
00121     {
00122       return m_padding;
00123     }
00124         
00126     void setPose(const tf::Transform &pose)
00127     {
00128       m_pose = pose;
00129       updateInternalData();
00130     }
00131         
00133     const tf::Transform& getPose(void) const
00134     {
00135       return m_pose;
00136     }
00137         
00139     void setDimensions(const shapes::Shape *shape)
00140     {
00141       useDimensions(shape);
00142       updateInternalData();
00143     }
00144         
00146     bool containsPoint(double x, double y, double z) const
00147     {
00148       return containsPoint(tf::Vector3(tfScalar(x), tfScalar(y), tfScalar(z)));
00149     }
00150         
00155     virtual bool intersectsRay(const tf::Vector3& origin, const tf::Vector3 &dir, std::vector<tf::Vector3> *intersections = NULL, unsigned int count = 0) const = 0;
00156         
00158     virtual bool containsPoint(const tf::Vector3 &p, bool verbose = false) const = 0;   
00159         
00162     virtual double computeVolume(void) const = 0;
00163         
00166     virtual void computeBoundingSphere(BoundingSphere &sphere) const = 0;
00167 
00170     virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const = 0;
00171         
00172   protected:
00173         
00174     virtual void updateInternalData(void) = 0;
00175     virtual void useDimensions(const shapes::Shape *shape) = 0;
00176         
00177     shapes::ShapeType m_type;
00178     tf::Transform       m_pose; 
00179     double            m_scale;
00180     double            m_padding;        
00181   };
00182     
00184 class Sphere : public Body
00185 {
00186 public:
00187   Sphere(void) : Body()
00188   {
00189     m_type = shapes::SPHERE;
00190   }
00191         
00192   Sphere(const shapes::Shape *shape) : Body()
00193   {
00194     m_type = shapes::SPHERE;
00195     setDimensions(shape);
00196   }
00197         
00198   virtual ~Sphere(void)
00199   {
00200   }
00201         
00202   virtual bool containsPoint(const tf::Vector3 &p, bool verbose = false) const;
00203   virtual double computeVolume(void) const;
00204   virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00205   virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const;
00206   virtual bool intersectsRay(const tf::Vector3& origin, const tf::Vector3 &dir, std::vector<tf::Vector3> *intersections = NULL, unsigned int count = 0) const;
00207 
00208 protected:
00209         
00210   virtual void useDimensions(const shapes::Shape *shape);
00211   virtual void updateInternalData(void);
00212         
00213   tf::Vector3 m_center;
00214   double    m_radius;   
00215   double    m_radiusU;
00216   double    m_radius2;              
00217 };
00218 
00220 class Cylinder : public Body
00221 {
00222 public:
00223   Cylinder(void) : Body()
00224   {
00225     m_type = shapes::CYLINDER;
00226   }
00227         
00228   Cylinder(const shapes::Shape *shape) : Body()
00229   {
00230     m_type = shapes::CYLINDER;
00231     setDimensions(shape);
00232   }
00233         
00234   virtual ~Cylinder(void)
00235   {
00236   }
00237         
00238   virtual bool containsPoint(const tf::Vector3 &p, bool verbose = false) const;
00239   virtual double computeVolume(void) const;
00240   virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00241   virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const;
00242   virtual bool intersectsRay(const tf::Vector3& origin, const tf::Vector3 &dir, std::vector<tf::Vector3> *intersections = NULL, unsigned int count = 0) const;
00243 
00244 protected:
00245         
00246   virtual void useDimensions(const shapes::Shape *shape);
00247   virtual void updateInternalData(void);
00248         
00249   tf::Vector3 m_center;
00250   tf::Vector3 m_normalH;
00251   tf::Vector3 m_normalB1;
00252   tf::Vector3 m_normalB2;
00253         
00254   double    m_length;
00255   double    m_length2;  
00256   double    m_radius;
00257   double    m_radiusU;
00258   double    m_radiusB;
00259   double    m_radiusBSqr;
00260   double    m_radius2;
00261   double    m_d1;
00262   double    m_d2;
00263 };
00264     
00266 class Box : public Body
00267 {
00268 public: 
00269   Box(void) : Body()
00270   {
00271     m_type = shapes::BOX;
00272   }
00273         
00274   Box(const shapes::Shape *shape) : Body()
00275   {
00276     m_type = shapes::BOX;
00277     setDimensions(shape);
00278   }
00279         
00280   virtual ~Box(void)
00281   {
00282   }
00283         
00284   virtual bool containsPoint(const tf::Vector3 &p, bool verbose = false) const;
00285   virtual double computeVolume(void) const;
00286   virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00287   virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const;
00288   virtual bool intersectsRay(const tf::Vector3& origin, const tf::Vector3 &dir, std::vector<tf::Vector3> *intersections = NULL, unsigned int count = 0) const;
00289 
00290 protected:
00291         
00292   virtual void useDimensions(const shapes::Shape *shape); // (x, y, z) = (length, width, height)            
00293   virtual void updateInternalData(void);
00294         
00295   tf::Vector3 m_center;
00296   tf::Vector3 m_normalL;
00297   tf::Vector3 m_normalW;
00298   tf::Vector3 m_normalH;
00299         
00300   tf::Vector3 m_corner1;
00301   tf::Vector3 m_corner2;
00302 
00303   double    m_length;
00304   double    m_width;
00305   double    m_height;   
00306   double    m_length2;
00307   double    m_width2;
00308   double    m_height2;  
00309   double    m_radiusB;
00310   double    m_radius2;
00311 };
00312 
00313 /*
00314   class Mesh : public Body
00315   {     
00316   public:
00318   Mesh(void) : Body()
00319   {
00320   m_type = shapes::MESH;
00321   m_btMeshShape = NULL;
00322   m_btMesh = NULL;
00323   }
00324         
00325   Mesh(const shapes::Shape *shape) : Body()
00326   {
00327   m_type = shapes::MESH;        
00328   m_btMeshShape = NULL;
00329   m_btMesh = NULL;
00330   setDimensions(shape);
00331   }
00332         
00333   virtual ~Mesh(void)
00334   {
00335   if (m_btMeshShape)
00336   delete m_btMeshShape;
00337   if (m_btMesh)
00338   delete m_btMesh;
00339   }
00340         
00341   \\\ \brief The mesh is considered to be concave, so this function is implemented with raycasting. This is a bit slow and not so accurate for very small triangles.
00342   virtual bool containsPoint(const tf::Vector3 &p) const;
00343 
00344   \\\ \brief This function is approximate. It returns the volume of the AABB enclosing the shape 
00345   virtual double computeVolume(void) const;
00346   virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00347   virtual bool intersectsRay(const tf::Vector3& origin, const tf::Vector3 &dir, std::vector<tf::Vector3> *intersections = NULL, unsigned int count = 0) const;
00348         
00349   protected:
00350 
00351   virtual void useDimensions(const shapes::Shape *shape);
00352   virtual void updateInternalData(void);
00353         
00354   btBvhTriangleMeshShape  *m_btMeshShape;
00355   btTriangleMesh          *m_btMesh;
00356   tf::Transform              m_iPose;
00357   tf::Vector3                m_center;
00358   tf::Vector3                m_aabbMin;
00359   tf::Vector3                m_aabbMax;
00360   double                   m_radiusB;
00361   double                   m_radiusBSqr;
00362         
00363   };
00364 */
00365     
00367 class ConvexMesh : public Body
00368 {
00369 public:
00370         
00371   ConvexMesh(void) : Body()
00372   {         
00373     m_type = shapes::MESH;
00374   }
00375         
00376   ConvexMesh(const shapes::Shape *shape) : Body()
00377   {       
00378     m_type = shapes::MESH;
00379     setDimensions(shape);
00380   }
00381         
00382   virtual ~ConvexMesh(void)
00383   {
00384   }     
00385 
00386   virtual bool containsPoint(const tf::Vector3 &p, bool verbose = false) const;
00387   virtual double computeVolume(void) const;
00388         
00389   virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00390   virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const;
00391   virtual bool intersectsRay(const tf::Vector3& origin, const tf::Vector3 &dir, std::vector<tf::Vector3> *intersections = NULL, unsigned int count = 0) const;
00392 
00393   const std::vector<unsigned int>& getTriangles() const {
00394     return m_triangles;
00395   }
00396 
00397   const std::vector<tf::Vector3>& getVertices() const {
00398     return m_vertices;
00399   }
00400 
00401   const std::vector<tf::Vector3>& getScaledVertices() const {
00402     return m_scaledVertices;
00403   }
00404 
00405 protected:
00406         
00407   virtual void useDimensions(const shapes::Shape *shape);
00408   virtual void updateInternalData(void);
00409         
00410   unsigned int countVerticesBehindPlane(const tf::tfVector4& planeNormal) const;
00411   bool isPointInsidePlanes(const tf::Vector3& point) const;
00412         
00413   std::vector<tf::tfVector4>    m_planes;
00414   std::vector<tf::Vector3>    m_vertices;
00415   std::vector<tf::Vector3>    m_scaledVertices;
00416   std::vector<unsigned int> m_triangles;
00417   tf::Transform               m_iPose;
00418         
00419   tf::Vector3                 m_center;
00420   tf::Vector3                 m_meshCenter;
00421   double                    m_radiusB;
00422   double                    m_radiusBSqr;
00423   double                    m_meshRadiusB;
00424 
00425   tf::Vector3                 m_boxOffset;
00426   Box                       m_boundingBox;
00427   BoundingCylinder          m_boundingCylinder;
00428 };
00429     
00430     
00432 Body* createBodyFromShape(const shapes::Shape *shape);
00433     
00435 void mergeBoundingSpheres(const std::vector<BoundingSphere> &spheres, BoundingSphere &mergedSphere);
00436 
00437 class BodyVector {
00438 public:
00439 
00440   BodyVector();
00441 
00442   BodyVector(const std::vector<shapes::Shape*>& shapes, 
00443              const std::vector<tf::Transform>& poses,
00444              double padding);
00445   
00446   ~BodyVector();
00447 
00448   void addBody(const shapes::Shape* shape, const tf::Transform& pose, double padding = 0.0);
00449 
00450   void setPose(unsigned int i, const tf::Transform& pose);
00451 
00452   unsigned int getSize() const {
00453     return bodies_.size();
00454   }
00455 
00456   double getPadding() const {
00457     return padding_;
00458   }
00459 
00460   const Body* getBody(unsigned int i) const;
00461   const Body* getPaddedBody(unsigned int i) const;
00462 
00463   BoundingSphere getBoundingSphere(unsigned int i) const;
00464   BoundingSphere getPaddedBoundingSphere(unsigned int i) const;
00465   
00466   double getBoundingSphereRadiusSquared(unsigned int i) const;
00467   double getPaddedBoundingSphereRadiusSquared(unsigned int i) const;
00468 
00469 private:
00470 
00471   std::vector<Body*> bodies_;
00472   double padding_;
00473   std::vector<Body*> padded_bodies_;
00474   std::vector<double> rsqrs_;
00475   std::vector<double> padded_rsqrs_;
00476 
00477 };
00478     
00479 }
00480 
00481 #endif


geometric_shapes
Author(s): Ioan Sucan
autogenerated on Thu Dec 12 2013 11:08:55