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