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