geometric_shapes: bodies.h Source 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_POINT_INCLUSION_
00038 #define GEOMETRIC_SHAPES_POINT_INCLUSION_
00039 
00040 #include "geometric_shapes/shapes.h"
00041 #include <LinearMath/btTransform.h>
00042 // #include <BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h>
00043 // #include <BulletCollision/CollisionShapes/btTriangleMesh.h>
00044 #include <vector>
00045 
00055 namespace bodies
00056 {
00057     
00059 struct BoundingSphere
00060 {
00061   btVector3 center;
00062   double    radius;
00063 };
00064 
00066 struct BoundingCylinder
00067 {
00068   btTransform pose;
00069   double radius;
00070   double length;
00071 };
00072   
00076   class Body
00077   {
00078   public:
00079         
00080     Body(void)
00081     {
00082       m_scale = 1.0;
00083       m_padding = 0.0;
00084       m_pose.setIdentity();
00085       m_type = shapes::UNKNOWN_SHAPE;
00086     }
00087         
00088     virtual ~Body(void)
00089     {
00090     }
00091         
00093     shapes::ShapeType getType(void) const
00094     {
00095       return m_type;
00096     }
00097         
00100     void setScale(double scale)
00101     {
00102       m_scale = scale;
00103       updateInternalData();
00104     }
00105         
00107     double getScale(void) const
00108     {
00109       return m_scale;
00110     }
00111         
00114     void setPadding(double padd)
00115     {
00116       m_padding = padd;
00117       updateInternalData();
00118     }
00119         
00121     double getPadding(void) const
00122     {
00123       return m_padding;
00124     }
00125         
00127     void setPose(const btTransform &pose)
00128     {
00129       m_pose = pose;
00130       updateInternalData();
00131     }
00132         
00134     const btTransform& getPose(void) const
00135     {
00136       return m_pose;
00137     }
00138         
00140     void setDimensions(const shapes::Shape *shape)
00141     {
00142       useDimensions(shape);
00143       updateInternalData();
00144     }
00145         
00147     bool containsPoint(double x, double y, double z) const
00148     {
00149       return containsPoint(btVector3(btScalar(x), btScalar(y), btScalar(z)));
00150     }
00151         
00156     virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL, unsigned int count = 0) const = 0;
00157         
00159     virtual bool containsPoint(const btVector3 &p, bool verbose = false) const = 0;     
00160         
00163     virtual double computeVolume(void) const = 0;
00164         
00167     virtual void computeBoundingSphere(BoundingSphere &sphere) const = 0;
00168 
00171     virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const = 0;
00172         
00173   protected:
00174         
00175     virtual void updateInternalData(void) = 0;
00176     virtual void useDimensions(const shapes::Shape *shape) = 0;
00177         
00178     shapes::ShapeType m_type;
00179     btTransform       m_pose;   
00180     double            m_scale;
00181     double            m_padding;        
00182   };
00183     
00185 class Sphere : public Body
00186 {
00187 public:
00188   Sphere(void) : Body()
00189   {
00190     m_type = shapes::SPHERE;
00191   }
00192         
00193   Sphere(const shapes::Shape *shape) : Body()
00194   {
00195     m_type = shapes::SPHERE;
00196     setDimensions(shape);
00197   }
00198         
00199   virtual ~Sphere(void)
00200   {
00201   }
00202         
00203   virtual bool containsPoint(const btVector3 &p, bool verbose = false) const;
00204   virtual double computeVolume(void) const;
00205   virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00206   virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const;
00207   virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL, unsigned int count = 0) const;
00208 
00209 protected:
00210         
00211   virtual void useDimensions(const shapes::Shape *shape);
00212   virtual void updateInternalData(void);
00213         
00214   btVector3 m_center;
00215   double    m_radius;   
00216   double    m_radiusU;
00217   double    m_radius2;              
00218 };
00219 
00221 class Cylinder : public Body
00222 {
00223 public:
00224   Cylinder(void) : Body()
00225   {
00226     m_type = shapes::CYLINDER;
00227   }
00228         
00229   Cylinder(const shapes::Shape *shape) : Body()
00230   {
00231     m_type = shapes::CYLINDER;
00232     setDimensions(shape);
00233   }
00234         
00235   virtual ~Cylinder(void)
00236   {
00237   }
00238         
00239   virtual bool containsPoint(const btVector3 &p, bool verbose = false) const;
00240   virtual double computeVolume(void) const;
00241   virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00242   virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const;
00243   virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL, unsigned int count = 0) const;
00244 
00245 protected:
00246         
00247   virtual void useDimensions(const shapes::Shape *shape);
00248   virtual void updateInternalData(void);
00249         
00250   btVector3 m_center;
00251   btVector3 m_normalH;
00252   btVector3 m_normalB1;
00253   btVector3 m_normalB2;
00254         
00255   double    m_length;
00256   double    m_length2;  
00257   double    m_radius;
00258   double    m_radiusU;
00259   double    m_radiusB;
00260   double    m_radiusBSqr;
00261   double    m_radius2;
00262   double    m_d1;
00263   double    m_d2;
00264 };
00265     
00267 class Box : public Body
00268 {
00269 public: 
00270   Box(void) : Body()
00271   {
00272     m_type = shapes::BOX;
00273   }
00274         
00275   Box(const shapes::Shape *shape) : Body()
00276   {
00277     m_type = shapes::BOX;
00278     setDimensions(shape);
00279   }
00280         
00281   virtual ~Box(void)
00282   {
00283   }
00284         
00285   virtual bool containsPoint(const btVector3 &p, bool verbose = false) const;
00286   virtual double computeVolume(void) const;
00287   virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00288   virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const;
00289   virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL, unsigned int count = 0) const;
00290 
00291 protected:
00292         
00293   virtual void useDimensions(const shapes::Shape *shape); // (x, y, z) = (length, width, height)            
00294   virtual void updateInternalData(void);
00295         
00296   btVector3 m_center;
00297   btVector3 m_normalL;
00298   btVector3 m_normalW;
00299   btVector3 m_normalH;
00300         
00301   btVector3 m_corner1;
00302   btVector3 m_corner2;
00303 
00304   double    m_length;
00305   double    m_width;
00306   double    m_height;   
00307   double    m_length2;
00308   double    m_width2;
00309   double    m_height2;  
00310   double    m_radiusB;
00311   double    m_radius2;
00312 };
00313 
00314 /*
00315   class Mesh : public Body
00316   {     
00317   public:
00319   Mesh(void) : Body()
00320   {
00321   m_type = shapes::MESH;
00322   m_btMeshShape = NULL;
00323   m_btMesh = NULL;
00324   }
00325         
00326   Mesh(const shapes::Shape *shape) : Body()
00327   {
00328   m_type = shapes::MESH;        
00329   m_btMeshShape = NULL;
00330   m_btMesh = NULL;
00331   setDimensions(shape);
00332   }
00333         
00334   virtual ~Mesh(void)
00335   {
00336   if (m_btMeshShape)
00337   delete m_btMeshShape;
00338   if (m_btMesh)
00339   delete m_btMesh;
00340   }
00341         
00342   \\\ \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.
00343   virtual bool containsPoint(const btVector3 &p) const;
00344 
00345   \\\ \brief This function is approximate. It returns the volume of the AABB enclosing the shape 
00346   virtual double computeVolume(void) const;
00347   virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00348   virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL, unsigned int count = 0) const;
00349         
00350   protected:
00351 
00352   virtual void useDimensions(const shapes::Shape *shape);
00353   virtual void updateInternalData(void);
00354         
00355   btBvhTriangleMeshShape  *m_btMeshShape;
00356   btTriangleMesh          *m_btMesh;
00357   btTransform              m_iPose;
00358   btVector3                m_center;
00359   btVector3                m_aabbMin;
00360   btVector3                m_aabbMax;
00361   double                   m_radiusB;
00362   double                   m_radiusBSqr;
00363         
00364   };
00365 */
00366     
00368 class ConvexMesh : public Body
00369 {
00370 public:
00371         
00372   ConvexMesh(void) : Body()
00373   {         
00374     m_type = shapes::MESH;
00375   }
00376         
00377   ConvexMesh(const shapes::Shape *shape) : Body()
00378   {       
00379     m_type = shapes::MESH;
00380     setDimensions(shape);
00381   }
00382         
00383   virtual ~ConvexMesh(void)
00384   {
00385   }     
00386 
00387   virtual bool containsPoint(const btVector3 &p, bool verbose = false) const;
00388   virtual double computeVolume(void) const;
00389         
00390   virtual void computeBoundingSphere(BoundingSphere &sphere) const;
00391   virtual void computeBoundingCylinder(BoundingCylinder &cylinder) const;
00392   virtual bool intersectsRay(const btVector3& origin, const btVector3 &dir, std::vector<btVector3> *intersections = NULL, unsigned int count = 0) const;
00393 
00394   const std::vector<unsigned int>& getTriangles() const {
00395     return m_triangles;
00396   }
00397 
00398   const std::vector<btVector3>& getVertices() const {
00399     return m_vertices;
00400   }
00401 
00402   const std::vector<btVector3>& getScaledVertices() const {
00403     return m_scaledVertices;
00404   }
00405 
00406 protected:
00407         
00408   virtual void useDimensions(const shapes::Shape *shape);
00409   virtual void updateInternalData(void);
00410         
00411   unsigned int countVerticesBehindPlane(const btVector4& planeNormal) const;
00412   bool isPointInsidePlanes(const btVector3& point) const;
00413         
00414   std::vector<btVector4>    m_planes;
00415   std::vector<btVector3>    m_vertices;
00416   std::vector<btVector3>    m_scaledVertices;
00417   std::vector<unsigned int> m_triangles;
00418   btTransform               m_iPose;
00419         
00420   btVector3                 m_center;
00421   btVector3                 m_meshCenter;
00422   double                    m_radiusB;
00423   double                    m_radiusBSqr;
00424   double                    m_meshRadiusB;
00425 
00426   btVector3                 m_boxOffset;
00427   Box                       m_boundingBox;
00428   BoundingCylinder          m_boundingCylinder;
00429 };
00430     
00431     
00433 Body* createBodyFromShape(const shapes::Shape *shape);
00434     
00436 void mergeBoundingSpheres(const std::vector<BoundingSphere> &spheres, BoundingSphere &mergedSphere);
00437     
00438 }
00439 
00440 #endif