pr2_navigation_self_filter: pr2_navigation_self

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