bodies.h

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
* 
*  Copyright (c) 2008, Willow Garage, Inc.
*  All rights reserved.
* 
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
* 
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
* 
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

#ifndef GEOMETRIC_SHAPES_POINT_INCLUSION_
#define GEOMETRIC_SHAPES_POINT_INCLUSION_

#include "pr2_navigation_self_filter/shapes.h"
#include <tf/LinearMath/Transform.h>
#include <tf/LinearMath/Vector3.h>
// #include <BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h>
// #include <BulletCollision/CollisionShapes/btTriangleMesh.h>
#include <vector>

namespace bodies
{
    
    struct BoundingSphere
    {
        tf::Vector3 center;
        double    radius;
    };
    
    class Body
    {
    public:
        
        Body(void)
        {
            m_scale = 1.0;
            m_padding = 0.0;
            m_pose.setIdentity();
            m_type = shapes::UNKNOWN_SHAPE;
        }
        
        virtual ~Body(void)
        {
        }
        
        shapes::ShapeType getType(void) const
        {
            return m_type;
        }
        
        void setScale(double scale)
        {
            m_scale = scale;
            updateInternalData();
        }
        
        double getScale(void) const
        {
            return m_scale;
        }
        
        void setPadding(double padd)
        {
            m_padding = padd;
            updateInternalData();
        }
        
        double getPadding(void) const
        {
            return m_padding;
        }
        
        void setPose(const tf::Transform &pose)
        {
            m_pose = pose;
            updateInternalData();
        }
        
        const tf::Transform& getPose(void) const
        {
            return m_pose;
        }
        
        void setDimensions(const shapes::Shape *shape)
        {
            useDimensions(shape);
            updateInternalData();
        }
        
        bool containsPoint(double x, double y, double z) const
        {
            return containsPoint(tf::Vector3(tfScalar(x), tfScalar(y), tfScalar(z)));
        }
        
        virtual bool intersectsRay(const tf::Vector3& origin, const tf::Vector3 &dir, std::vector<tf::Vector3> *intersections = NULL, unsigned int count = 0) const = 0;
        
        virtual bool containsPoint(const tf::Vector3 &p, bool verbose = false) const = 0;       
        
        virtual double computeVolume(void) const = 0;
        
        virtual void computeBoundingSphere(BoundingSphere &sphere) const = 0;
        
    protected:
        
        virtual void updateInternalData(void) = 0;
        virtual void useDimensions(const shapes::Shape *shape) = 0;
        
        shapes::ShapeType m_type;
        tf::Transform       m_pose;     
        double            m_scale;
        double            m_padding;    
    };
    
    class Sphere : public Body
    {
    public:
        Sphere(void) : Body()
        {
            m_type = shapes::SPHERE;
        }
        
        Sphere(const shapes::Shape *shape) : Body()
        {
            m_type = shapes::SPHERE;
            setDimensions(shape);
        }
        
        virtual ~Sphere(void)
        {
        }
        
        virtual bool containsPoint(const tf::Vector3 &p, bool verbose=false) const;
        virtual double computeVolume(void) const;
        virtual void computeBoundingSphere(BoundingSphere &sphere) const;
        virtual bool intersectsRay(const tf::Vector3& origin, const tf::Vector3 &dir, std::vector<tf::Vector3> *intersections = NULL, unsigned int count = 0) const;

    protected:
        
        virtual void useDimensions(const shapes::Shape *shape);
        virtual void updateInternalData(void);
        
        tf::Vector3 m_center;
        double    m_radius;     
        double    m_radiusU;
        double    m_radius2;                
    };

    class Cylinder : public Body
    {
    public:
        Cylinder(void) : Body()
        {
            m_type = shapes::CYLINDER;
        }
        
        Cylinder(const shapes::Shape *shape) : Body()
        {
            m_type = shapes::CYLINDER;
            setDimensions(shape);
        }
        
        virtual ~Cylinder(void)
        {
        }
        
        virtual bool containsPoint(const tf::Vector3 &p, bool verbose=false) const;
        virtual double computeVolume(void) const;
        virtual void computeBoundingSphere(BoundingSphere &sphere) const;
        virtual bool intersectsRay(const tf::Vector3& origin, const tf::Vector3 &dir, std::vector<tf::Vector3> *intersections = NULL, unsigned int count = 0) const;

    protected:
        
        virtual void useDimensions(const shapes::Shape *shape);
        virtual void updateInternalData(void);
        
        tf::Vector3 m_center;
        tf::Vector3 m_normalH;
        tf::Vector3 m_normalB1;
        tf::Vector3 m_normalB2;
        
        double    m_length;
        double    m_length2;    
        double    m_radius;
        double    m_radiusU;
        double    m_radiusB;
        double    m_radiusBSqr;
        double    m_radius2;
        double    m_d1;
        double    m_d2;
    };
    
    class Box : public Body
    {
    public: 
        Box(void) : Body()
        {
            m_type = shapes::BOX;
        }
        
        Box(const shapes::Shape *shape) : Body()
        {
            m_type = shapes::BOX;
            setDimensions(shape);
        }
        
        virtual ~Box(void)
        {
        }
        
        virtual bool containsPoint(const tf::Vector3 &p, bool verbose = false) const;
        virtual double computeVolume(void) const;
        virtual void computeBoundingSphere(BoundingSphere &sphere) const;
        virtual bool intersectsRay(const tf::Vector3& origin, const tf::Vector3 &dir, std::vector<tf::Vector3> *intersections = NULL, unsigned int count = 0) const;

    protected:
        
        virtual void useDimensions(const shapes::Shape *shape); // (x, y, z) = (length, width, height)      
        virtual void updateInternalData(void);
        
        tf::Vector3 m_center;
        tf::Vector3 m_normalL;
        tf::Vector3 m_normalW;
        tf::Vector3 m_normalH;
        
        tf::Vector3 m_corner1;
        tf::Vector3 m_corner2;

        double    m_length;
        double    m_width;
        double    m_height;     
        double    m_length2;
        double    m_width2;
        double    m_height2;    
        double    m_radiusB;
        double    m_radius2;
    };

    /*
    class Mesh : public Body
    {   
    public:
        Mesh(void) : Body()
        {
            m_type = shapes::MESH;
            m_btMeshShape = NULL;
            m_btMesh = NULL;
        }
        
        Mesh(const shapes::Shape *shape) : Body()
        {
            m_type = shapes::MESH;      
            m_btMeshShape = NULL;
            m_btMesh = NULL;
            setDimensions(shape);
        }
        
        virtual ~Mesh(void)
        {
            if (m_btMeshShape)
                delete m_btMeshShape;
            if (m_btMesh)
                delete m_btMesh;
        }
        
        \\\ \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.
        virtual bool containsPoint(const tf::Vector3 &p) const;

        \\\ \brief This function is approximate. It returns the volume of the AABB enclosing the shape 
        virtual double computeVolume(void) const;
        virtual void computeBoundingSphere(BoundingSphere &sphere) const;
        virtual bool intersectsRay(const tf::Vector3& origin, const tf::Vector3 &dir, std::vector<tf::Vector3> *intersections = NULL, unsigned int count = 0) const;
        
    protected:

        virtual void useDimensions(const shapes::Shape *shape);
        virtual void updateInternalData(void);
        
        btBvhTriangleMeshShape  *m_btMeshShape;
        btTriangleMesh          *m_btMesh;
        tf::Transform              m_iPose;
        tf::Vector3                m_center;
        tf::Vector3                m_aabbMin;
        tf::Vector3                m_aabbMax;
        double                   m_radiusB;
        double                   m_radiusBSqr;
        
    };
    */
    
    class ConvexMesh : public Body
    {
    public:
        
        ConvexMesh(void) : Body()
        {           
            m_type = shapes::MESH;
        }
        
        ConvexMesh(const shapes::Shape *shape) : Body()
        {         
            m_type = shapes::MESH;
            setDimensions(shape);
        }
        
        virtual ~ConvexMesh(void)
        {
        }       

        virtual bool containsPoint(const tf::Vector3 &p, bool verbose = false) const;
        virtual double computeVolume(void) const;
        
        virtual void computeBoundingSphere(BoundingSphere &sphere) const;
        virtual bool intersectsRay(const tf::Vector3& origin, const tf::Vector3 &dir, std::vector<tf::Vector3> *intersections = NULL, unsigned int count = 0) const;

    protected:
        
        virtual void useDimensions(const shapes::Shape *shape);
        virtual void updateInternalData(void);
        
        unsigned int countVerticesBehindPlane(const tf::tfVector4& planeNormal) const;
        bool isPointInsidePlanes(const tf::Vector3& point) const;
        
        std::vector<tf::tfVector4>    m_planes;
        std::vector<tf::Vector3>    m_vertices;
        std::vector<tf::Vector3>    m_scaledVertices;
        std::vector<unsigned int> m_triangles;
        tf::Transform               m_iPose;
        
        tf::Vector3                 m_center;
        tf::Vector3                 m_meshCenter;
        double                    m_radiusB;
        double                    m_radiusBSqr;
        double                    m_meshRadiusB;
        
        tf::Vector3                 m_boxOffset;
        Box                       m_boundingBox;
    };
    
    
    Body* createBodyFromShape(const shapes::Shape *shape);
    
    void mergeBoundingSpheres(const std::vector<BoundingSphere> &spheres, BoundingSphere &mergedSphere);
    
}

#endif