Point_set.cpp

Go to the documentation of this file.

/*
**    Mesh Converter
**    Copyright (C) 2008  Damien Rohmer
**
**    This program is free software: you can redistribute it and/or modify
**    it under the terms of the GNU General Public License as published by
**    the Free Software Foundation, either version 3 of the License, or
**    (at your option) any later version.
**
**   This program is distributed in the hope that it will be useful,
**    but WITHOUT ANY WARRANTY; without even the implied warranty of
**    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
**    GNU General Public License for more details.
**
**    You should have received a copy of the GNU General Public License
**    along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/


#include <Point_set.h>

Point_set::Point_set():vertex(){vertex.resize(0);}
Point_set::Point_set(const Point_set &p):vertex(p.vertex){}
Point_set::~Point_set(){vertex.resize(0);}
  
int Point_set::destroy(){vertex.resize(0);return 0;}



int Point_set::get_vertex_number() const{return size();}
int Point_set::size() const{
  if( vertex.size()%3 != 0)
    {printf("Error vertex size is not correct in Point_set (not divisible by 3) [%d]\n",int(vertex.size()));exit(-1);}
  return vertex.size()/3;
}

int Point_set::resize(int _N_vertex){vertex.resize(3*_N_vertex);return 0;}

double& Point_set::operator[](int index)
{
  if(index<0)
    {printf("Error index<0 (%d) in operator[] in Point_set\n",index);exit(-1);}
  else if(index>=int(vertex.size()))
    {printf("Error in operator[], index too large compared to size in Point_set (%d/%d)\n",index,vertex.size());exit(-1);}
  return vertex[index];
}
double Point_set::operator[](int index) const
{
  if(index<0)
    {printf("Error index<0 (%d) in operator[] {const} in Point_set\n",index);exit(-1);}
  else if(index>=int(vertex.size()))
    {printf("Error in operator[] {const}, index too large compared to size in Point_set (%d/%d)\n",index,vertex.size());exit(-1);}
  return vertex[index];
}


double& Point_set::get_vertex(int k_vertex,int k_dim){return (*this)[3*k_vertex+k_dim];}
double  Point_set::get_vertex(int k_vertex,int k_dim) const{return (*this)[3*k_vertex+k_dim];}
const double* Point_set::get_vertex_p(int k_vertex)
{
  if(k_vertex<0)
    {printf("Error index<0 (%d) in get_vertex_p in Point_set\n",k_vertex);exit(-1);}
  else if(3*k_vertex+2>=int(vertex.size()))
    {printf("Error in get_vertex_p, index too large compared to size in Point_set (%d/%d)\n",k_vertex,vertex.size());exit(-1);}
  return &vertex[3*k_vertex];
}

V_3D Point_set::get_vertex(int k_vertex) const
{
  V_3D v(get_vertex(k_vertex,0),get_vertex(k_vertex,1),get_vertex(k_vertex,2));
  return v;
}
V_3D Point_set::get_vertex_v3d(int k_vertex) const{return get_vertex(k_vertex);}



int Point_set::set_vertex(int k_vertex,double x,double y,double z)
{
  (*this)[3*k_vertex+0]=x;
  (*this)[3*k_vertex+1]=y;
  (*this)[3*k_vertex+2]=z;
  return 0;
}
int Point_set::set_vertex(int k_vertex,int k_dim,double value)
{V_3D v;v.set(k_dim,value);return set_vertex(k_vertex,v);}
int Point_set::set_vertex(int k_vertex,V_3D v)
{return set_vertex(k_vertex,v[0],v[1],v[2]);}
int Point_set::set_vertex(int k_vertex,double* v)
{return set_vertex(k_vertex,v[0],v[1],v[2]);}
int Point_set::set_vertex(int k_vertex,int dim,double *v)
{for(int k=0;k<dim;k++){set_vertex(k_vertex+k,&v[3*k]);}return 0;}
int Point_set::set_vertex(const Point_set& p)
{(*this)=p;return 0;}


int Point_set::add_vertex(double x,double y,double z)
{vertex.push_back(x);vertex.push_back(y);vertex.push_back(z);return 0;}
int Point_set::add_vertex(const V_3D& v)
{return add_vertex(v[0],v[1],v[2]);}
int Point_set::add_vertex(const std::vector <double>& to_add)
{
  if((to_add.size()%3)!=0)
    {printf("Error in add_vertex in Point_set, size of to_add is not multiple of 3 (%d)\n",to_add.size());exit(-1);}
  int k_vertex=0;
  for(k_vertex=0;k_vertex<int(to_add.size())/3;k_vertex++)
    add_vertex(to_add[3*k_vertex+0],to_add[3*k_vertex+1],to_add[3*k_vertex+2]);
  return 0;
}
int Point_set::add_vertex(const std::vector <V_3D>& to_add)
{
  int k_vertex=0;
  for(k_vertex=0;k_vertex<int(to_add.size());k_vertex++)
    add_vertex(to_add[k_vertex]);
  return 0;
}
int Point_set::add_vertex(const double* v){return add_vertex(v[0],v[1],v[2]);}
int Point_set::add_vertex(int dim,double *v)
{for(int k=0;k<dim;k++){add_vertex(v[3*k+0],v[3*k+1],v[3*k+2]);}return 0;}
int Point_set::add_vertex(const Point_set& p)
{return add_vertex(p.get_vertex_vector());}







int Point_set::exist(const V_3D& v,double epsilon) const
{
  int k_vertex=0;
  int N_vertex=size();
  V_3D x;
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    {
      x=get_vertex(k_vertex);
      if(v.equal(x,epsilon)==1)
        return k_vertex;
    }
  return -1;
}
int Point_set::exist(const V_3D& v) const{return exist(v,0.00001);}
int Point_set::exist(double x,double y,double z) const{return exist(x,y,z,0.00001);}
int Point_set::exist(double x,double y,double z,double epsilon) const{V_3D X(x,y,z);return exist(X,epsilon);}


int Point_set::add_unique(double x,double y,double z,double epsilon)
{
  if(exist(x,y,z,epsilon)==-1)
    return add_vertex(x,y,z);
  return -1;
}
int Point_set::add_unique(const V_3D& v,double epsilon)
{
  if(exist(v,epsilon)==-1)
    return add_vertex(v);
  return -1;
}
int Point_set::add_unique(const double* v,double epsilon)
{
  if(exist(v[0],v[1],v[2],epsilon)==-1)
    return add_vertex(v);
  return -1;
}


int Point_set::eliminate(int k_index)
{
  if(k_index<0 || k_index>=get_vertex_number())
    {printf("Error k_index too large in eliminate (%d/%d)\n",k_index,get_vertex_number());exit(-1);}
  int k_vertex=0,N_vertex=get_vertex_number();
  int k_dim=0;
  for(k_vertex=k_index;k_vertex<N_vertex-1;k_vertex++)
    for(k_dim=0;k_dim<3;k_dim++)
      vertex[3*k_vertex+k_dim]=vertex[3*(k_vertex+1)+k_dim];
  vertex.resize(N_vertex-1);
  return 0;
}
int Point_set::eliminate(const std::vector <int>& k_index)
{
  int k_vertex=0;
  int N_vertex=k_index.size();
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    eliminate(k_index[k_vertex]);
  return 0;
}
int Point_set::eliminate(double x,double y,double z){return eliminate(x,y,z,0.00001);}
int Point_set::eliminate(double x,double y,double z,double epsilon)
{
  int k_index=0;
  k_index=exist(x,y,z,epsilon);
  if(k_index!=-1)
    return eliminate(k_index);
  
  return -1;
}
int Point_set::eliminate(const V_3D& x,double epsilon){return eliminate(x[0],x[1],x[2],epsilon);}
int Point_set::eliminate(const V_3D& x){return eliminate(x[0],x[1],x[2],0.00001);}
int Point_set::eliminate(const double* x,double epsilon){return eliminate(x[0],x[1],x[2],epsilon);}
int Point_set::eliminate(const double* x){return eliminate(x[0],x[1],x[2],0.00001);}
int Point_set::eliminate(const std::vector <double>& k_index)
{
  int k_vertex=0;
  int N_index=0;
  if((k_index.size()%3)!=0)
    {printf("Error size of k_index in eliminate is not divisible by 3 (%d)\n",k_index.size());exit(-1);}
  for(k_vertex=0;k_vertex<N_index/3;k_vertex++)
    eliminate(k_index[3*k_vertex+0],k_index[3*k_vertex+1],k_index[3*k_vertex+2]);
  return 0;
}








int Point_set::matrix_transform(const Matrix& M)
{
  int k_vertex=0,N_vertex=get_vertex_number();
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    set_vertex(k_vertex,M*get_vertex_v3d(k_vertex));
  return 0;  
}



int Point_set::rotate(const V_3D& n,double angle){return rotate(n[0],n[1],n[2],angle);}
int Point_set::rotate(double n_x,double n_y,double n_z,double angle)
{
  Matrix R;
  R.set_rotation_matrix(n_x,n_y,n_z,angle);
  return matrix_transform(R);
}
int Point_set::rotate(const double *n,double angle){return rotate(n[0],n[1],n[2],angle);}



int Point_set::rotate(const V_3D& n,double angle,const V_3D& center)
{return rotate(n[0],n[1],n[2],angle,center);}
int Point_set::rotate(double n_x,double n_y,double n_z,double angle,const V_3D& center)
{
  Matrix R;R.set_rotation_matrix(n_x,n_y,n_z,angle);
  Matrix T;T.set_translation(center[0],center[1],center[2]);
  Matrix iT;iT.set_translation(-center[0],-center[1],-center[2]);
  Matrix M = T*R*iT;

  return matrix_transform(M);
}
int Point_set::rotate(const double *n,double angle,const V_3D& center)
{return rotate(n[0],n[1],n[2],angle,center);}



int Point_set::translate(double t_x,double t_y,double t_z)
{
  int k_vertex=0,N_vertex=get_vertex_number();
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    set_vertex(k_vertex,vertex[3*k_vertex+0]+t_x,vertex[3*k_vertex+1]+t_y,vertex[3*k_vertex+2]+t_z);
  return 0;
}
int Point_set::translate(const double *t) {return translate(t[0],t[1],t[2]);}
int Point_set::translate(const V_3D& t) {return translate(t[0],t[1],t[2]);}






int Point_set::scale(double s_x,double s_y,double s_z)
{
  double epsilon=0.000001;
  if(fabs(s_x)<epsilon)
    {printf("Warning scaling x=0.0 \n");}
  if(fabs(s_y)<epsilon)
    {printf("Warning scaling y=0.0 \n");}
  if(fabs(s_z)<epsilon)
    {printf("Warning scaling z=0.0 \n");}

  int k_vertex=0,N_vertex=get_vertex_number();
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    set_vertex(k_vertex,vertex[3*k_vertex+0]*s_x,vertex[3*k_vertex+1]*s_y,vertex[3*k_vertex+2]*s_z);
  return 0;  
}
int Point_set::scale(const double *s) {return scale(s[0],s[1],s[2]);}
int Point_set::scale(const V_3D& s) {return scale(s[0],s[1],s[2]);}
int Point_set::scale(double s) {return scale(s,s,s);}






int Point_set::rotate_centered(const V_3D& n,double angle)
{
  V_3D center=get_middle_point();
  translate(-1.0*center);
  rotate(n,angle);
  translate( center);
  return 0;
}
int Point_set::rotate_centered(double n_x,double n_y,double n_z,double angle)
{V_3D n(n_x,n_y,n_z);return rotate_centered(n,angle);}
int Point_set::rotate_centered(const double *n,double angle)
{V_3D n_v3d(n[0],n[1],n[2]);return rotate_centered(n_v3d,angle);}



int Point_set::scale_centered(double s_x,double s_y,double s_z)
{
  V_3D center=get_middle_point();
  translate(-1.0*center);
  scale(s_x,s_y,s_z);
  translate( center);
  return 0;
}
int Point_set::scale_centered(const double *s){return scale_centered(s[0],s[1],s[2]);}
int Point_set::scale_centered(const V_3D& s){return scale_centered(s[0],s[1],s[2]);}
int Point_set::scale_centered(double s){return scale_centered(s,s,s);}









V_3D Point_set::get_middle_point() const
{
  V_3D min_point = get_min_point();
  V_3D max_point = get_max_point();

  return 0.5*(min_point+max_point); 
}
V_3D Point_set::get_min_point() const
{
  V_3D min_point(9999,9999,9999);
  V_3D current;
  int k_vertex=0,N_vertex=get_vertex_number();
  int k_dim=0;
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    {
      current = get_vertex_v3d(k_vertex);
      for(k_dim=0;k_dim<3;k_dim++)
        if(current[k_dim]<min_point[k_dim])
          min_point[k_dim] = current[k_dim];
    }

  return min_point;
}
V_3D Point_set::get_max_point() const
{
  V_3D max_point(-9999,-9999,-9999);
  V_3D current;
  int k_vertex=0,N_vertex=get_vertex_number();
  int k_dim=0;
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    {
      current = get_vertex_v3d(k_vertex);
      for(k_dim=0;k_dim<3;k_dim++)
        if(current[k_dim]>max_point[k_dim])
          max_point[k_dim] = current[k_dim];
    }
  return max_point;
}
V_3D Point_set::get_size_set() const
{
  V_3D min_point = get_min_point();
  V_3D max_point = get_max_point();

  return (max_point-min_point);
}


int Point_set::center_point_set(){return translate(-1.0*get_middle_point());}










double Point_set::get_closest_distance(double x,double y,double z,int *index) const
{V_3D X(x,y,z);return get_closest_distance(X,index);}
double Point_set::get_closest_distance(const double* x,int *index) const
{return get_closest_distance(x[0],x[1],x[2],index);}
double Point_set::get_closest_distance(const V_3D& x,int *index) const
{
  int k_vertex=0;
  double min_dist=9999999.0;
  V_3D current;
  double current_dist=0.0;
  for(k_vertex=0;k_vertex<size();k_vertex++)
    {
      current = get_vertex_v3d(k_vertex);
      current_dist=(x-current).norm();
      if(min_dist>current_dist)
        {
          *index=k_vertex;
          min_dist=current_dist;
        }
    }
  return min_dist;
}






double Point_set::get_further_distance(double x,double y,double z,int *index) const
{V_3D X(x,y,z);return get_further_distance(X,index);}
double Point_set::get_further_distance(const double* x,int *index) const
{return get_further_distance(x[0],x[1],x[2],index);}
double Point_set::get_further_distance(const V_3D& x,int *index) const
{
  int k_vertex=0;
  double max_dist=0.0;
  V_3D current;
  double current_dist=0.0;
  for(k_vertex=0;k_vertex<size();k_vertex++)
    {
      current = get_vertex_v3d(k_vertex);
      current_dist=(x-current).norm();
      if(max_dist<current_dist)
        {
          *index=k_vertex;
          max_dist=current_dist;
        }
    }
  return max_dist;
}


double Point_set::distance_to_index(double x,double y,double z,int index) const
{V_3D X(x,y,z);return distance_to_index(X,index);}
double Point_set::distance_to_index(const double* x,int index) const
{V_3D X(x[0],x[1],x[2]);return distance_to_index(X,index);}
double Point_set::distance_to_index(const V_3D& x,int index) const
{return (x-get_vertex_v3d(index)).norm();}




std::vector <int> Point_set::N_closest(const V_3D& center,int _N_closest) const
{
  std::vector <int> closest;
  std::vector <double> dist;
  std::vector <double> full_dist(size());

  closest.resize(_N_closest);
  dist.resize(_N_closest);

  int k_vertex=0,N_vertex=size();
  V_3D current;

  //calculate every distance to every vertex
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    {
      current=get_vertex_v3d(k_vertex);
      full_dist[k_vertex] = (current-center).norm();
    }
  
  int k_closest=0;
  //first case
  for(k_vertex=0,dist[0]=9999.9;k_vertex<N_vertex;k_vertex++)
    if(full_dist[k_vertex]<dist[0]){closest[0]=k_vertex;dist[0]=full_dist[k_vertex];}
  //other case
  for(k_closest=1;k_closest<_N_closest;k_closest++)
    for(k_vertex=0,dist[k_closest]=9999.9;k_vertex<N_vertex;k_vertex++)
      if(full_dist[k_vertex]<dist[k_closest] && dist[k_closest]>dist[k_closest-1])
        {closest[k_closest]=k_vertex;dist[k_closest]=full_dist[k_vertex];}
  
  return closest;
}
std::vector <int> Point_set::N_closest(double x,double y,double z,int _N_closest) const
{V_3D X(x,y,z);return N_closest(X,_N_closest);}
std::vector <int> Point_set::N_closest(double *x,int _N_closest) const
{return N_closest(x[0],x[1],x[2],_N_closest);}






std::vector <int> Point_set::inside_circle(const V_3D& center,double radius) const
{
  std::vector <int> points;
  int k_vertex=0,N_vertex=size();
  V_3D current;
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    if( (current-center).norm()<=radius )
      points.push_back(k_vertex);

  return points;
}
std::vector <int> Point_set::inside_circle(double x,double y,double z,double radius) const
{V_3D X(x,y,z);return inside_circle(X,radius);}









std::vector <int> Point_set::outside_circle(const V_3D& center,double radius) const
{
  std::vector <int> points;
  int k_vertex=0,N_vertex=size();
  V_3D current;
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    if( (current-center).norm()>radius )
      points.push_back(k_vertex);

  return points;
}
std::vector <int> Point_set::outside_circle(double x,double y,double z,double radius) const
{V_3D X(x,y,z);return outside_circle(X,radius);}







std::vector <int> Point_set::inside_box(const V_3D& p1,const V_3D& p2) const
{
  std::vector <int> point;
  int k_vertex=0,N_vertex=size();
  V_3D current;

  V_3D A=p1.get_min_scalar(p2);
  V_3D B=p1.get_max_scalar(p2);
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    {
      current=get_vertex_v3d(k_vertex);
      if(current>=A && current<=B)
        point.push_back(k_vertex);
    }
  return point;
}
std::vector <int> Point_set::outside_box(const V_3D& p1,const V_3D& p2) const
{
  std::vector <int> point;
  int k_vertex=0,N_vertex=size();
  V_3D current;

  V_3D A=p1.get_min_scalar(p2);
  V_3D B=p1.get_max_scalar(p2);
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    {
      current=get_vertex_v3d(k_vertex);
      if(!(current>=A && current<=B))
        point.push_back(k_vertex);
    }
  return point;
}

std::vector <int> Point_set::inside_tetrahedra(const V_3D& p1,const V_3D& p2,const V_3D& p3,const V_3D& p4) const
{
  std::vector <int> point;
  int k_vertex=0,N_vertex=size();
  V_3D current;

  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    {
      current=get_vertex_v3d(k_vertex);
      if(current.is_inside_volume(p1,p2,p3,p4)==1)
        point.push_back(k_vertex);
    }
  return point;
}

std::vector <int> Point_set::outside_tetrahedra(const V_3D& p1,const V_3D& p2,const V_3D& p3,const V_3D& p4) const
{
  std::vector <int> point;
  int k_vertex=0,N_vertex=size();
  V_3D current;

  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    {
      current=get_vertex_v3d(k_vertex);
      if(current.is_inside_volume(p1,p2,p3,p4)==0)
        point.push_back(k_vertex);
    }
  return point;
}

std::vector <int> Point_set::in_half_space(const V_3D& n,const V_3D& x0) const
{
  std::vector <int> point;
  int k_vertex=0,N_vertex=size();
  V_3D current;

  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    {
      current=get_vertex_v3d(k_vertex);
      if(current.distance_to_oriented_plane(n,x0)>0)
        point.push_back(k_vertex);
    }
  return point;
}

std::vector <int> Point_set::other_index(const std::vector <int>& index) const
{
  std::vector <int> point;
  int k_vertex=0,N_vertex=size();
  int k_vertex_2=0,N_vertex_2=index.size();
  int is_found=0;
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    {
      is_found=0;
      for(k_vertex_2=0;is_found==0 && k_vertex_2<N_vertex_2;k_vertex_2++)
        if(k_vertex==k_vertex_2)
          is_found=1;
      if(is_found==0)
        point.push_back(k_vertex);
    }
  return point;
}


const double* Point_set::get_vertex() const{return &vertex[0];}
std::vector <double> Point_set::get_vertex_vector() const{return vertex;}

V_3D Point_set::operator()(int index) const
{
  if(3*index+2>=int(vertex.size()) || index<0)
    {printf("Error in operator() in Point_set index(%d) is not correct in size(%d)\n",index,vertex.size());exit(-1);}

  V_3D v(vertex[3*index],vertex[3*index+1],vertex[3*index+2]);
  return v;
}
double Point_set::operator()(int index,int k_dim) const
{
  if(3*index+k_dim<0 || 3*index+k_dim>=int(vertex.size()))
    {printf("Error int const operator(index,k_dim) is not correct in operator() in Point_set -> (%d,%d) with size=%d\n",index,k_dim,vertex.size());exit(-1);}
  return vertex[3*index+k_dim];
}
double& Point_set::operator()(int index,int k_dim)
{
  if(3*index+k_dim<0 || 3*index+k_dim>=int(vertex.size()))
    {printf("Error in operator(index,k_dim) is not correct in operator() in Point_set -> (%d,%d) with size=%d\n",index,k_dim,vertex.size());exit(-1);}
  return vertex[3*index+k_dim];
}



int Point_set::keep_index(int k_start,int k_end)
{
  if(k_start>k_end || k_start<0 || k_end>=size())
    {printf("Error in keep_index in Point_set, (k_start,k_end)=(%d,%d) and size=%d\n",k_start,k_end,size());exit(-1);}

  vector <double> new_vertex;
  int k_vertex=0;
  int k_dim=0;
  for(k_vertex=k_start;k_vertex<=k_end;k_vertex++)
    for(k_dim=0;k_dim<3;k_dim++)
      new_vertex.push_back(get_vertex(k_vertex,k_dim));
  vertex=new_vertex;
  return 0;
}
int Point_set::delete_index(int k_start,int k_end)
{
  if(k_start>k_end || k_start<0 || k_end>=size())
    {printf("Error in delete_index in Point_set, (k_start,k_end)=(%d,%d) and size=%d\n",k_start,k_end,size());exit(-1);}

  vector <double> new_vertex;
  int k_vertex=0;
  int N_vertex=size();
  int k_dim=0;
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    if(k_vertex<k_start || k_vertex>k_end)
      for(k_dim=0;k_dim<3;k_dim++)
        new_vertex.push_back(get_vertex(k_vertex,k_dim));
  vertex=new_vertex;
  return 0;  
}









Point_set operator+(const Point_set& p, V_3D T)
{
  Point_set set;
  int N_vertex=p.size();
  int k_vertex=0;
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    set.set_vertex(k_vertex,p(k_vertex)+T);
  return set;
}
Point_set operator+(V_3D T,const Point_set& p)
{
  Point_set set;
  int N_vertex=p.size();
  int k_vertex=0;
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    set.set_vertex(k_vertex,p(k_vertex)+T);
  return set;
}

Point_set operator-(const Point_set& p, V_3D T)
{
  Point_set set;
  int N_vertex=p.size();
  int k_vertex=0;
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    set.set_vertex(k_vertex,p(k_vertex)-T);
  return set;
}
Point_set operator-(V_3D T,const Point_set& p)
{
  Point_set set;
  int N_vertex=p.size();
  int k_vertex=0;
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    set.set_vertex(k_vertex,T-p(k_vertex));
  return set;
}


Point_set operator*(const Matrix& M,const Point_set& p)
{
  Point_set set;
  int N_vertex=p.size();
  int k_vertex=0;
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    set.set_vertex(k_vertex,M*p(k_vertex));
  return set;
}

bool operator==(const Point_set& pa,const Point_set& pb)
{
  if(pa.size()==pb.size())
    {
      int k_vertex=0,N_vertex=pa.size();
      for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
        if(pa(k_vertex)!=pb(k_vertex))
          return false;
      return true;
    }
  return false;
}

bool operator!=(const Point_set& pa,const Point_set& pb)
{
  if(pa.size()==pb.size())
    {
      int k_vertex=0,N_vertex=pa.size();
      for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
        if(pa(k_vertex)!=pb(k_vertex))
          return true;
      return false;
    }
  return true;
}

ostream& operator << (ostream& flux, const Point_set& p)
{
  int k_vertex=0;
  int N_vertex=p.size();
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    flux<<"("<<p.get_vertex(k_vertex,0)<<","<<p.get_vertex(k_vertex,1)<<","<<p.get_vertex(k_vertex,2)<<")\n";
  return flux;
}

const Point_set& Point_set::operator=(const Point_set& p)
{
  vertex=p.get_vertex_vector();
  return (*this);
}





int Point_set::load_off_file(const char* filename)
{std::string file=filename;return load_off_file(file);}
int Point_set::load_off_file(const std::string& filename)
{
 
  destroy();

#define SIZE_BUFFER 2048
  char buffer[SIZE_BUFFER]={'\0'};
  
  FILE *fid=NULL;
  fid=fopen(filename.data(),"r");
  if(fid==NULL)
    {printf("ERROR loading %s in load_off_file in Point_set\n",filename.data());exit(-1);}



  //detect if OFF or NOFF
  int is_NOFF=0;
  while(strcmp(buffer,"OFF")!=0 && strcmp(buffer,"NOFF")!=0)
    {
      if(fscanf(fid,"%s",buffer)==EOF)
        {printf("ERROR file %s is not off file\n",filename.data());exit(-1);}
      if(buffer[0]=='#')//comments
        fgets(buffer,sizeof(buffer),fid);
    }
  if(strcmp(buffer,"NOFF")==0)
    is_NOFF=1;

  //pass comments
  fscanf(fid,"%s",buffer);
  while(buffer[0]=='#')//comments
    {
      fgets(buffer,sizeof(buffer),fid);
      fscanf(fid,"%s",buffer);
    }


  //read Number of vertices
  int N_vertex=0;
  int k_vertex=0;
  N_vertex=atoi(buffer);//N_vertex
  fgets(buffer,sizeof(buffer),fid);//read line

  float X[3]={0.0,0.0,0.0};
  if(is_NOFF==0)
    while(k_vertex<N_vertex)
      {
        if(fscanf(fid,"%f %f %f",X,X+1,X+2)!=3)
          {
            fscanf(fid,"%s",buffer);
            if(buffer[0]!='#')
              {printf("ERROR loading %s in Point_set\n",filename.data());exit(-1);}
            else//read line
              fgets(buffer,sizeof(buffer),fid);
          }
        else
          {
            add_vertex(X[0],X[1],X[2]);
            k_vertex++;
          }
      }
  else
    while(k_vertex<N_vertex)
      {
        if(fscanf(fid,"%f %f %f %*f %*f %*f",X,X+1,X+2)!=3)
          {
            fscanf(fid,"%s",buffer);
            if(buffer[0]!='#')
              {printf("ERROR loading %s in Point_set\n",filename.data());exit(-1);}
            else//read line
              fgets(buffer,sizeof(buffer),fid);
          }
        else
          {
            add_vertex(X[0],X[1],X[2]);
            k_vertex++;
          }
      }


  if(fclose(fid)!=0)
    {printf("ERROR closing %s in load_off_file in Point_set\n",filename.data());exit(-1);}

  return 0;
}


int Point_set::write_off_type(const char* filename) const
{std::string file=filename;return write_off_type(file);}
int Point_set::write_off_type(const std::string& filename) const
{
  FILE *fid=NULL;
  fid=fopen(filename.data(),"a");
  if(fid==NULL)
    {printf("ERROR loading %s in write_off_type in Point_set\n",filename.data());exit(-1);}

  int k_vertex=0,N_vertex=size();
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    fprintf(fid,"%9.9f %9.9f %9.9f\n",(*this)(k_vertex,0),(*this)(k_vertex,1),(*this)(k_vertex,2));

  if(fclose(fid)!=0)
    {printf("ERROR closing %s in write_off_type in Point_set\n",filename.data());exit(-1);}

  return 0;
}

int Point_set::load_obj_file(const char* filename)
{std::string file=filename;return load_obj_file(file);}
int Point_set::load_obj_file(const std::string& filename)
{
  destroy();

#define SIZE_BUFFER 2048
  char buffer[SIZE_BUFFER]={'\0'};
  
  FILE *fid=NULL;
  fid=fopen(filename.data(),"r");
  if(fid==NULL)
    {printf("ERROR loading %s in load_obj_file in Point_set\n",filename.data());exit(-1);}

  float X[3];
  while(fscanf(fid,"%s",buffer)!=EOF)
    {
      if(strcmp(buffer,"v")==0)
        {
          if(fscanf(fid,"%f %f %f\n",X,X+1,X+2)!=3)
            {printf("Error reading vertices in load_obj_file in Point_set\n");exit(-1);}
          add_vertex(X[0],X[1],X[2]);
        }
      else //read the whole line
        fgets(buffer,sizeof(buffer),fid);
    }


  if(fclose(fid)!=0)
    {printf("ERROR closing %s in load_obj_file in Point_set\n",filename.data());exit(-1);}
  return 0;
}


int Point_set::write_obj_type(const char* filename) const
{std::string file=filename;return write_obj_type(file);}
int Point_set::write_obj_type(const std::string& filename) const
{
  FILE *fid=NULL;
  fid=fopen(filename.data(),"a");
  if(fid==NULL)
    {printf("ERROR loading %s in write_obj_type in Point_set\n",filename.data());exit(-1);}

  int k_vertex=0,N_vertex=size();
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    fprintf(fid,"v %9.9f %9.9f %9.9f\n",(*this)(k_vertex,0),(*this)(k_vertex,1),(*this)(k_vertex,2));

  if(fclose(fid)!=0)
    {printf("ERROR closing %s in write_obj_type in Point_set\n",filename.data());exit(-1);}

  return 0;
}




//draw little cube for every vertex
int Point_set::export_visible_off(const char* filename,double s_cube_size) const
{std::string file=filename;return export_visible_off(file,s_cube_size);}
int Point_set::export_visible_off(const std::string& filename,double s_cube_size) const
{
  V_3D D=get_size_set();
  double d_max=D.get_max_of_coeff();

  double s_size=d_max/s_cube_size;
  


  FILE *fid=NULL;
  fid=fopen(filename.data(),"w");
  if(fid==NULL)
    {printf("ERROR loading %s in export_visible_off in Point_set\n",filename.data());exit(-1);}


  fprintf(fid,"#VISIBLE OFF SCENE %s\n",filename.data());
  fprintf(fid,"#N_vertex=%d\n",size());
  fprintf(fid,"OFF\n");
  fprintf(fid,"%d %d 0\n",8*size(),6*size());

  int k_vertex=0,N_vertex=size();
  V_3D current;
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    {
      current=get_vertex(k_vertex);
      fprintf(fid,"%9.9f %9.9f %9.9f\n",current[0]-s_size,current[1]-s_size,current[2]-s_size);
      fprintf(fid,"%9.9f %9.9f %9.9f\n",current[0]+s_size,current[1]-s_size,current[2]-s_size);
      fprintf(fid,"%9.9f %9.9f %9.9f\n",current[0]+s_size,current[1]+s_size,current[2]-s_size);
      fprintf(fid,"%9.9f %9.9f %9.9f\n",current[0]-s_size,current[1]+s_size,current[2]-s_size);

      fprintf(fid,"%9.9f %9.9f %9.9f\n",current[0]-s_size,current[1]-s_size,current[2]+s_size);
      fprintf(fid,"%9.9f %9.9f %9.9f\n",current[0]+s_size,current[1]-s_size,current[2]+s_size);
      fprintf(fid,"%9.9f %9.9f %9.9f\n",current[0]+s_size,current[1]+s_size,current[2]+s_size);
      fprintf(fid,"%9.9f %9.9f %9.9f\n",current[0]-s_size,current[1]+s_size,current[2]+s_size);
    }
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    {
      fprintf(fid,"4 %d %d %d %d\n",8*k_vertex+0,8*k_vertex+1,8*k_vertex+2,8*k_vertex+3);
      fprintf(fid,"4 %d %d %d %d\n",8*k_vertex+1,8*k_vertex+5,8*k_vertex+6,8*k_vertex+2);
      fprintf(fid,"4 %d %d %d %d\n",8*k_vertex+5,8*k_vertex+4,8*k_vertex+7,8*k_vertex+6);
      fprintf(fid,"4 %d %d %d %d\n",8*k_vertex+4,8*k_vertex+0,8*k_vertex+3,8*k_vertex+7);
      fprintf(fid,"4 %d %d %d %d\n",8*k_vertex+2,8*k_vertex+6,8*k_vertex+7,8*k_vertex+3);
      fprintf(fid,"4 %d %d %d %d\n",8*k_vertex+0,8*k_vertex+4,8*k_vertex+5,8*k_vertex+1);
    }
  
  fprintf(fid,"#EOF [OK] %s\n",filename.data());
  if(fclose(fid)!=0)
    {printf("ERROR closing %s in export_visible_off in Point_set\n",filename.data());exit(-1);}


  return 0;
}



int Point_set::load_g_file(const char* filename)
{std::string file=filename;return load_g_file(file);}
int Point_set::load_g_file(const std::string& filename)
{
   destroy();

#define SIZE_BUFFER 2048
  char buffer[SIZE_BUFFER]={'\0'};
  
  FILE *fid=NULL;
  fid=fopen(filename.data(),"r");
  if(fid==NULL)
    {printf("ERROR loading %s in load_g_file in Point_set\n",filename.data());exit(-1);}





  //pass comments
  fscanf(fid,"%s",buffer);
  while(buffer[0]=='#')//comments
    {
      fgets(buffer,sizeof(buffer),fid);
      fscanf(fid,"%s",buffer);
    }


  //read Number of vertices
  int N_vertex=0;
  int k_vertex=0;
  N_vertex=atoi(buffer);//N_vertex
  fgets(buffer,sizeof(buffer),fid);//read line

  float X[3]={0.0,0.0,0.0};
  while(k_vertex<N_vertex)
    {
      if(fscanf(fid,"%f %f %f",X,X+1,X+2)!=3)
        {
          fscanf(fid,"%s",buffer);
          if(buffer[0]!='#')
            {printf("ERROR loading %s in Point_set\n",filename.data());exit(-1);}
          else//read line
            fgets(buffer,sizeof(buffer),fid);
        }
      else
        {
          add_vertex(X[0],X[1],X[2]);
          k_vertex++;
        }
    }


  if(fclose(fid)!=0)
    {printf("ERROR closing %s in load_obj_file in Point_set\n",filename.data());exit(-1);}
 
  return 0;
}


int Point_set::load_collada_file(const char* filename)
{std::string file=filename;return load_collada_file(file);}
int Point_set::load_collada_file(const std::string& filename)
{return load_collada_file(filename,"_ALL_");}

// Load vertices starting with node positionned at geometry entry
int Point_set::load_collada_mesh_vertices(TiXmlNode* geometry_node,const std::string& filename)
{

  TiXmlNode *mesh_node=NULL;
  mesh_node=geometry_node->FirstChild("mesh");
  
  //need to find mesh
  if(mesh_node==NULL)
    {printf("Error in Point_set in load_collada_mesh_connectivity for file %s\n",filename.data());exit(-1);}

  
  //then try to find position array
  TiXmlElement *mesh_source_root=NULL;
  TiXmlElement *mesh_source=NULL;
  mesh_source=mesh_node->FirstChild("source")->ToElement();
  mesh_source_root=mesh_source;
  if(mesh_source==NULL)
    {printf("Error, no <source> entry in <mesh> in load_collada_mesh_vertices in Point_set in file %s\n",filename.data());exit(-1);}

  std::string name_attribute;
  do
    {
      if(mesh_source->Attribute("name")!=NULL)
        name_attribute = mesh_source->Attribute("name");
      if(name_attribute!="position")
        mesh_source = mesh_source->NextSiblingElement("source");
    }while(mesh_source!=NULL && name_attribute!="position");

  if(name_attribute!="position")
    {
      //might try to find [name]-positions in id
      std::string extension_to_find="positions";
      mesh_source=mesh_source_root;
      do
        {
          if(mesh_source->Attribute("id")!=NULL)
            name_attribute = mesh_source->Attribute("id");
          if(name_attribute.find(extension_to_find)!=std::string::npos)
            mesh_source->NextSiblingElement("source");    
        }while(mesh_source!=NULL && name_attribute.find(extension_to_find)==std::string::npos);


      if(name_attribute.find(extension_to_find)==std::string::npos)
        {printf("Error cannot find positions in <mesh><source> in load_collada_mesh_vertices in Point_set in file %s\n",filename.data());exit(-1);}
      //else find [something]positions
    }

  //load_float_array
  std::vector <double> position;
  position = load_collada_float_array(mesh_source,filename);

  //add the vertex
  return add_vertex(position);
}

int Point_set::load_collada_file(const char* filename,const char* name)
{std::string file=filename;std::string mesh_name=name;return load_collada_file(file,mesh_name);}
int Point_set::load_collada_file(const std::string& filename,const std::string& name)
{
  TiXmlDocument doc(filename);
  if(!doc.LoadFile())
    {
      printf("ERROR opening %s\n",filename.data());
      cerr << "error #" << doc.ErrorId() << " : " << doc.ErrorDesc() << endl;
      exit(-1);
    }


  //find library_geometries
  TiXmlHandle root(doc.RootElement());
  TiXmlNode *lib_geometry=NULL;lib_geometry=root.ToNode();
  if(lib_geometry==NULL)
    {printf("Error reading file %s in load_collada_file in Point_set\n",filename.data());exit(-1);}


  //go to library_geometries entry
  lib_geometry = lib_geometry->FirstChild("library_geometries");

  if(lib_geometry==NULL)
    {printf("Error in file %s, cannot find library_geometries in load_collada_file in Point_set\n",filename.data());exit(-1);}
  
  
  //load all geometries
  TiXmlNode *geometry=NULL;
  geometry=lib_geometry->FirstChild("geometry");
  if(geometry==NULL)
    {printf("Error cannot find geometry in file %s in load_collada_file in Point_set\n",filename.data());exit(-1);}
  

  std::string geometry_id;
  int is_find=0;
  do
    {
      if(geometry->ToElement()->Attribute("id")==NULL)
        {printf("Error in file %s, geometry does not have id \n",filename.data());exit(-1);}
      geometry_id = geometry->ToElement()->Attribute("id");
      if(geometry_id==name || name=="_ALL_")
        {
          load_collada_mesh_vertices(geometry,filename);
          if(name!="_ALL_")
            is_find=1;
        }
      geometry=geometry->NextSiblingElement("geometry");
    }while(geometry!=NULL && is_find==0);
  


  if(is_find==0 && name!="_ALL_")
    {printf("Error in file %s, no geometry found to load Point_set with id=\"%s\"\n",filename.data(),name.data());exit(-1);}

  return 0; 
}




std::vector <double> Point_set::load_collada_float_array(TiXmlNode *mesh_source,const std::string& filename)
{
  TiXmlElement *float_array_node=NULL;
  float_array_node = mesh_source->FirstChild("float_array")->ToElement();
  if(float_array_node==NULL)
    {printf("Error cannot find float_array in load_collada_float_array in Point_set in file %s\n",filename.data());exit(-1);}
  
  int count=0;
  float_array_node->QueryIntAttribute("count",&count);
  if(float_array_node->FirstChild()==NULL)
    {printf("Error in load_collada_float_array in Point_set in file %s\n",filename.data());exit(-1);}
  std::string full_array = float_array_node->FirstChild()->ToText()->ValueStr();

  //fill vector array
  std::vector <double> array;
  int c=0,k1=0,k0=0;
  string X;
  for(c=0;c<count;c++)
    {
      k1= full_array.find(" ",k1+1);
      X = full_array.substr(k0,k1-k0);
      array.push_back(atof(X.data())); //add vertex
      k0=k1+1;
    }

  return array;
}

int Point_set::unitize_size()
{
  double max_size = get_size_set().get_max_of_coeff();
  if(fabs(max_size)<0.00001)
    {printf("Error in unitize size in Point_set\n");exit(-1);}
  return scale(1.0/max_size);
}

V_3D Point_set::get_closest_vertex(const V_3D& center) const
{return get_vertex_v3d(N_closest(center,1)[0]);}
V_3D Point_set::get_closest_vertex(double x,double y,double z) const
{return get_closest_vertex(V_3D(x,y,z));}
V_3D Point_set::get_closest_vertex(const double *x) const
{return get_closest_vertex(V_3D(x[0],x[1],x[2]));}



int Point_set::write_g_type(const char* filename) const
{return write_g_type(std::string(filename));}
int Point_set::write_g_type(const std::string& filename) const
{
  return write_off_type(filename);
}