/home/slemaign/softs-local/BioMove3D-git/graspPlanning/include/gpKdTree.h

#ifndef GP_KDTREE_H
#define GP_KDTREE_H



class gpAABB
{
  private:
    class gpKdTree *tree_; 
    double xmin_, xmax_, ymin_, ymax_, zmin_, zmax_; 
    bool root_; 
    bool leaf_; 
    unsigned int level_;  
    class gpAABB *children_[2];  
    class gpAABB *brother_;  
    std::list<unsigned int> inner_points_; 
  public:
    gpAABB(gpKdTree *tree, std::list<unsigned int> &inner_points); 
    gpAABB(gpAABB *previous, std::list<unsigned int> &inner_points); 
    ~gpAABB();
    int divide();
    int draw(unsigned int level);
    int sphereIntersection(p3d_vector3 center, double radius, std::list<class gpContact> &points);
};

class gpKdTree
{
 friend class gpAABB; 
 private:
  class gpAABB *root_;
  std::vector<class gpContact> points;
  unsigned int depth_;
 public:
  gpKdTree();
  gpKdTree(std::list<class gpContact> &contactList);
  ~gpKdTree();
  int build(std::list<class gpContact> &contactList);
  int draw(unsigned int level);
  int sphereIntersection(p3d_vector3 center, double radius, std::list<class gpContact> &points);
  unsigned int depth()
  {  return depth_;  }
};

class gpAABBTris
{
  private:
    class gpKdTreeTris *tree_; 
    double xmin_, xmax_, ymin_, ymax_, zmin_, zmax_; 
    bool root_; 
    bool leaf_; 
    bool inside_; 
    unsigned int level_;  
    class gpAABBTris *children_[2];  
    class gpAABBTris *brother_;  
    std::list<unsigned int> inner_triangles_; 
  public:
    gpAABBTris(gpKdTreeTris *tree, std::list<unsigned int> &inner_triangles);
    gpAABBTris(gpAABBTris *previous, double xmin, double xmax, double ymin, double ymax, double zmin, double zmax);
    bool isTriangleOutside(p3d_vector3 p1, p3d_vector3 p2, p3d_vector3 p3);
    bool isInsidePolyhedre();
    int divide();
    int draw(unsigned int level);
    int sample(double step, std::list<gpVector3D> &points);
};

class gpKdTreeTris
{
 friend class gpAABBTris; 
 private:
  class gpAABBTris *root_;
  p3d_polyhedre *polyhedron_;
  std::list<unsigned int> inner_triangles_;
  unsigned int depth_;
 public:
  gpKdTreeTris(p3d_polyhedre *polyhedron);
  int draw(unsigned int level);
  unsigned int depth()
  {  return depth_;  }
  int pointCloud(double step, std::list<gpVector3D> &points);
};

#endif