ColdetModel.h

Go to the documentation of this file.

/*
 * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
 * All rights reserved. This program is made available under the terms of the
 * Eclipse Public License v1.0 which accompanies this distribution, and is
 * available at http://www.eclipse.org/legal/epl-v10.html
 * Contributors:
 * National Institute of Advanced Industrial Science and Technology (AIST)
 */
#ifndef HRPCOLLISION_COLDET_MODEL_H_INCLUDED
#define HRPCOLLISION_COLDET_MODEL_H_INCLUDED
 
#include "config.h"
#include <string>
#include <hrpUtil/Eigen3d.h>
#include <hrpUtil/Referenced.h>
#include <vector>
#include <map>
 
 
namespace IceMaths {
    class Matrix4x4;
}
 
namespace hrp {
 
    class ColdetModelSharedDataSet;
    class Edge
    {
        int vertex[2];
    public :
        Edge(int vertexIndex1, int vertexIndex2){
            if(vertexIndex1 < vertexIndex2){
                vertex[0] = vertexIndex1;
                vertex[1] = vertexIndex2;
            } else {
                vertex[0] = vertexIndex2;
                vertex[1] = vertexIndex1;
            }
        }
        bool operator<(const Edge& rhs) const {
            if(vertex[0] < rhs.vertex[0]){
                return true;
            } else if(vertex[0] == rhs.vertex[0]){
                return (vertex[1] < rhs.vertex[1]);
            } else {
                return false;
            }
        }
    };
 
    struct trianglePair {
        int t[2];
    };
 
    typedef std::map< Edge, trianglePair > EdgeToTriangleMap;
 
    class HRP_COLLISION_EXPORT ColdetModel : public Referenced
    {
      public:
        enum PrimitiveType { SP_MESH, SP_BOX, SP_CYLINDER, SP_CONE, SP_SPHERE, SP_PLANE };
 
        ColdetModel();
 
        ColdetModel(const ColdetModel& org);
 
        virtual ~ColdetModel();
 
        void setName(const std::string& name) { name_ = name; }
 
        const std::string& name() const { return name_; }
 
        void setNumVertices(int n);
 
        int getNumVertices() const;
 
        void setNumTriangles(int n);
 
        int getNumTriangles() const;
 
        void setVertex(int index, float x, float y, float z);
 
        void addVertex(float x, float y, float z);
 
        void getVertex(int index, float& out_x, float& out_y, float& out_z) const;
 
        void setTriangle(int index, int v1, int v2, int v3);
 
        void addTriangle(int v1, int v2, int v3);
 
        void getTriangle(int index, int& out_v1, int& out_v2, int& out_v3) const;
 
        void build();
 
        bool isValid() const { return isValid_; }
 
        void setPosition(const Matrix33& R, const Vector3& p);
 
        void setPosition(const double* R, const double* p);
 
        void setPrimitiveType(PrimitiveType ptype);
 
        PrimitiveType getPrimitiveType() const;
 
        void setNumPrimitiveParams(unsigned int nparam);
 
        bool setPrimitiveParam(unsigned int index, float value);
 
        bool getPrimitiveParam(unsigned int index, float &value) const;
 
        void setPrimitivePosition(const double* R, const double* p);
        
        double computeDistanceWithRay(const double *point, const double *dir);
 
        bool checkCollisionWithPointCloud(const std::vector<Vector3> &i_cloud,
                                          double i_radius);
 
        void getBoundingBoxData(const int depth, std::vector<Vector3>& out_boxes);
        
        int getAABBTreeDepth();
        int getAABBmaxNum();
        int numofBBtoDepth(int minNumofBB);
 
        ColdetModelSharedDataSet *getDataSet() { return dataSet; }
      private:
        void initialize();
        void setNeighborTriangle(int triangle, int vertex0, int vertex1, int vertex2);
        void initNeighbor(int n);
 
        
        ColdetModelSharedDataSet* dataSet;
        IceMaths::Matrix4x4* transform;
        IceMaths::Matrix4x4* pTransform; 
        std::string name_;
        bool isValid_;
        EdgeToTriangleMap triangleMap;
 
        bool setNeighborTriangleSub(int triangle, int vertex0, int vertex1);
        void setNeighbor(int triangle0, int triangle1);
 
        friend class ColdetModelPair;
    };
 
    typedef boost::intrusive_ptr<ColdetModel> ColdetModelPtr;
}
 
 
#endif