Curve_3D.cpp

Go to the documentation of this file.

#include <Curve_3D.h>

Curve_3D::Curve_3D():data()
{
  data.resize(0);
}

Curve_3D::~Curve_3D()
{
  destroy();
}

Curve_3D::Curve_3D(const Curve_3D& _curve):data()
{data = _curve.data;}

int Curve_3D::destroy()
{data.resize(0);return 0;}

int Curve_3D::size() const
{return data.size();}

int Curve_3D::add(const V_3D& point)
{data.push_back(point);return 0;}
int Curve_3D::add_point(const V_3D& point)
{data.push_back(point);return 0;}

V_3D& Curve_3D::get(int k_index)
{
  if(k_index<0 || k_index>=size())
    {printf("Error k_index [%d] not in bounds in get in Curve_3D (size=%d)\n",k_index,size());exit(-1);}
  return data[k_index];
}
V_3D Curve_3D::get(int k_index) const
{
  if(k_index<0 || k_index>=size())
    {printf("Error k_index [%d] not in bounds in get in Curve_3D (size=%d)\n",k_index,size());exit(-1);}
  return data[k_index];
}
V_3D& Curve_3D::operator[](int k_index)
{
  if(k_index<0 || k_index>=size())
    {printf("Error k_index [%d] not in bounds in operator[] in Curve_3D (size=%d)\n",k_index,size());exit(-1);}
  return data[k_index];
}
V_3D  Curve_3D::operator[](int k_index) const
{
  if(k_index<0 || k_index>=size())
    {printf("Error k_index [%d] not in bounds in operator[] in Curve_3D (size=%d)\n",k_index,size());exit(-1);}
  return data[k_index];
}
  
Curve_3D& Curve_3D::operator=(const Curve_3D& _curve)
{
  int N_points=_curve.size();
  data.resize(N_points);
  for(int k=0;k<N_points;k++)
    data[k] = _curve.data[k];
  return *this;
}


int Curve_3D::resample_closed(int N)
{
  int k=0;

  std::vector <V_3D> data2;
  double rho=0.5;
  double n20=1.0,n31=1.0;
  
//   int size_1=size();
//   double u=0.0;
//   int u0=0,u1=0;
//   double s=0.0;
//   for(k=0;k<N;k++)
//     {
//       u=double(k/double(N-1)*(size()));
//       u0=int(u);
//       s=u-u0;
//       u1=u0+1;
//       //for close curve
//       if(u1==size())
//      u1=0;
//       if(u0==size())
//      {u1=1;u0=0;}
//       data2.push_back(
//                   (1.0-s)*data[u0] + s*data[u1]);
//     }
//   data2.push_back(data[size_1-1]);
//   data = data2;









  //int size_1=size();
  double u=0.0;
  int u0=0,u1=0,u2=0,u3=0;
  double s=0.0;

  for(k=0;k<N;k++)
    {
      u=double(k/double(N)*(size()));
      u1=int(u);
      u2=u1+1;
      u3=u1+2;
      u0=u1-1;
      s=(u-u1)/(u2-u1);
      u1=u0+1;

      if(u0==-2)
        {u0=size()-2;u1=size()-1;}
      if(u0==-1)
        {u0=size()-1;}
      if(u3==size())
        {u3=0;}
      if(u3==size()+1)
        {u3=1;u2=0;}
      if(u3==size()+2)
        {u3=2;u2=1;u1=0;}

      //      data2.push_back((1-s)*data[u1]+s*data[u2]);
//       data2.push_back(
//                    data[u1]
//                    +
//                    s*(data[u2]-data[u0])
//                    +
//                    s*s*(2*(data[u0]-data[u1])+data[u2]-data[u3])
//                    +
//                    s*s*s*(data[u3]-data[u2]-data[u0]+data[u1])
//                    );
      


      //n20=(data[u2]-data[u0]).norm();
      //n31=(data[u3]-data[u1]).norm();
      data2.push_back(
                      data[u1]
                      +
                      s*(data[u2]-data[u0])/n20*rho
                      +
                      s*s*(3*(data[u2]-data[u1])-2*(data[u2]-data[u0])/n20*rho-(data[u3]-data[u1])/n31*rho)
                      +
                      s*s*s*((data[u3]-data[u1])/n31*rho+(data[u2]-data[u0])/n20*rho+2*(data[u1]-data[u2]))
                      );


    }
  //data2.push_back(data[size_1-1]);
  data = data2;


  //Le code C permettant d'interpoler cubiquement quatre valeurs dans la plage mu [0...1] est le suivant :
//     double interpolationCubique(double y0,double y1,double y2,double y3,double mu)
//   {
//      double a0,a1,a2,a3,mu2;
  
//      mu2 = mu*mu;
//      a0 = y3 - y2 - y0 + y1;
//      a1 = y0 - y1 - a0;
//      a2 = y2 - y0;
//      a3 = y1;
  
//      return (a0*mu*mu2+a1*mu2+a2*mu+a3);
//   }



  return 0;
}





std::vector <V_3D> Curve_3D::get_diff_closed_curve() const
{
  std::vector <V_3D> diff;

  int k_vertex=0;
  int N_vertex=size();
  double L;
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    {
      if(k_vertex>=1 && k_vertex<N_vertex-1)
        {
          L = (get(k_vertex+1)-get(k_vertex)).norm()+(get(k_vertex)-get(k_vertex-1)).norm();
          diff.push_back(
                         1.0/L*
                         (get(k_vertex+1)-get(k_vertex-1)));
        }
      else if(k_vertex==0)
        {
          L = (get(1)-get(0)).norm()+(get(0)-get(N_vertex-1)).norm();
          diff.push_back(
                         1.0/L*
                         (get(1)-get(N_vertex-1)));
        }
      else if(k_vertex==N_vertex-1)
        {
          L = (get(0)-get(N_vertex-1)).norm()+(get(N_vertex-1)-get(N_vertex-2)).norm();
          diff.push_back(
                         1.0/L*
                         (get(0)-get(N_vertex-2)));
        }
    }
  return diff;
}


std::vector <V_3D> Curve_3D::get_diff2_closed_curve() const
{
  std::vector <V_3D> diff2;

  int k_vertex=0;
  int N_vertex=size();
  double L;
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    {
      if(k_vertex>=1 && k_vertex<N_vertex-1)
        {
          L = (get(k_vertex+1)-get(k_vertex)).norm()+(get(k_vertex)-get(k_vertex-1)).norm();
          diff2.push_back(
                          1.0/(L*L)*
                          (get(k_vertex+1)-2*get(k_vertex)+get(k_vertex-1)));
        }
      else if(k_vertex==0)
        {
          L = (get(1)-get(0)).norm()+(get(0)-get(N_vertex-1)).norm();
          diff2.push_back(
                         1.0/(L*L)*
                         (get(1)-2*get(0)+get(N_vertex-1)));
        }
      else if(k_vertex==N_vertex-1)
        {
          L = (get(0)-get(N_vertex-1)).norm()+(get(N_vertex-1)-get(N_vertex-2)).norm();
          diff2.push_back(
                         1.0/(L*L)*
                         (get(0)-2*get(N_vertex-1)+get(N_vertex-2)));
        }
    }
  return diff2;
}



std::vector <double> Curve_3D::get_curvature_closed_curve() const
{

   std::vector <V_3D> diff,diff2;
   std::vector <double> curvature;

  diff  = get_diff_closed_curve();
  diff2 = get_diff2_closed_curve();


   int k_vertex=0;
   int N_vertex=size();

  curvature.resize(N_vertex);
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    {
      //normalize vector
      curvature[k_vertex] = 
        (diff[k_vertex].vector_prod(diff2[k_vertex])).norm()
        /
        (diff[k_vertex].norm()*diff[k_vertex].norm()*diff[k_vertex].norm())
        ;
     }

  return curvature;



  

}



double Curve_3D::length() const
{
  double L=0.0;
  int N_data=data.size();
  int k_data=0;
  for(k_data=0;k_data<N_data-1;k_data++)
    L += (get(k_data+1)-get(k_data)).norm();
  return L;
}



Curve_3D Curve_3D::part(int k_0,int k_1) const
{
  Curve_3D curve;
  
  if(k_0<0 || k_0>=size() || k_1<=0 || k_1>=size() || k_0>k_1)
    {printf("Error in part(%d,%d) in Curve_3D, parameters are not correct\n",k_0,k_1);exit(-1);}
    
  int k=0;
  for(k=k_0;k<=k_1;k++)
    curve.add_point(get(k));
  
  return curve;
}

Curve_3D Curve_3D::part_closed(int k_0,int k_1) const
{
  Curve_3D curve;

  if(k_0<0 || k_0>=size() || k_1<0 || k_1>=size())
    {printf("Error in part_closed(%d,%d) in Curve_3D, parameters are not correct\n",k_0,k_1);exit(-1);}
    
  int dist_1=0,dist_2=0;
  dist_1=k_1-k_0;
  dist_2=size()-abs(dist_1);



  int k=0;
  if(fabs(dist_1)<=dist_2)
    {
      if(k_0<=k_1)
        for(k=k_0;k<=k_1;k++)
          curve.add_point(get(k));
      else
        for(k=k_0;k!=k_1;k=(k-1<0?size()-1:k-1))
          curve.add_point(get(k));
    }
  else
    {
      if(k_0<=k_1)
        for(k=k_0;k!=k_1;k=(k-1<0?size()-1:k-1))
          curve.add_point(get(k));
      else
        for(k=k_0;k!=k_1;k=(k+1)%size())
          curve.add_point(get(k));      
    }

  
  return curve;
}

int Curve_3D::add(const Curve_3D& _curve)
{
  int k=0;
  for(k=0;k<_curve.size();k++)
    add_point(_curve.get(k));
  return 0;
}

int Curve_3D::add_first(const V_3D& point)
{
  int ok=0;
  Curve_3D temp=*this;
  ok += destroy();
  ok += add(point);
  ok += add(temp);
  return ok;
}


Curve_3D Curve_3D::resample_part_closed(int k_0,int k_1,int N)
{

  Curve_3D curve_part;
  curve_part = part_closed(k_0,k_1);
  curve_part.resample(N);


  
  int dist_1=0,dist_2=0;
  dist_1=k_1-k_0;
  dist_2=size()-abs(k_1-k_0);//pass through 0
  int k=0;
  if(dist_1<=dist_2)
    {
      if(k_0<=k_1)
        for(k=k_1;k!=k_0;k=(k+1)%size())
          curve_part.add_point(get(k));
      else
        for(k=k_1;k!=k_0;k=(k-1<0?size()-1:k-1))
          curve_part.add_point(get(k));
    }
  else
    {
      if(k_0<=k_1)
        for(k=k_1;k!=k_0;k=(k-1<0?size()-1:k-1))
          curve_part.add_point(get(k));
      else
        for(k=k_1;k!=k_0;k=(k+1)%size())
          curve_part.add_point(get(k)); 
    }

  return curve_part;
}




int Curve_3D::resample(int N)
{
  int k=0;

  std::vector <V_3D> data2;
  double rho=0.5;
  double n20=1.0,n31=1.0;


  V_3D A,B,C,D;
  

  //int size_1=size();
  double u=0.0;
  int u0=0,u1=0,u2=0,u3=0;
  double s=0.0;

  for(k=0;k<N;k++)
    {
      u=double(k/double(N-1)*(size()-1));
      u1=int(u);
      u2=u1+1;
      u3=u1+2;
      u0=u1-1;
      s=(u-u1)/(u2-u1);
      u1=u0+1;

      if(u0==-1)
        {A= data[u1];}
      if(u3==size())
        {D= data[u2];}
      if(u3==size()+1)
        {C= data[u1]; D=data[u1];}

      if(u0>=0)
        A=data[u0];
      B=data[u1];
      if(u2<size())
        C=data[u2];
      if(u3<size())
        D=data[u3];

      //n31=(data[u3]-data[u1]).norm();
      data2.push_back(
                      B
                      +
                      s*(C-A)/n20*rho
                      +
                      s*s*(3*(C-B)-2*(C-A)/n20*rho-(D-B)/n31*rho)
                      +
                      s*s*s*((D-B)/n31*rho+(C-A)/n20*rho+2*(B-C))
                      );


    }
  data = data2;


  return 0;
}


int Curve_3D::load_curve(const char* filename)
{
  
  #define SIZE_BUFFER 1024
  char buffer[SIZE_BUFFER]={'\0'};



  FILE *fid=NULL;
  fid=fopen(filename,"r");
  if(fid==NULL)
    {
      printf("ERROR opening file for loading %s\n",filename);
      exit(-1);
    }


  V_3D x;float x0=0.0,y0=0.0,z0=0.0;
  while(fscanf(fid,"%s",buffer)!=EOF)
    {
      switch(buffer[0])
        {
        case '#'://comments
          fgets(buffer,SIZE_BUFFER,fid);
          break;
        default:
          if(sscanf(buffer,"%f",&x0)!=1)
            {printf("Error reading curve file in %s [%s]\n",filename,buffer);exit(-1);}
          if(fscanf(fid,"%f",&y0)!=1 || fscanf(fid,"%f\n",&z0)!=1)
            {printf("Error reading curve file in %s at line [%s]\n",filename,buffer);exit(-1);}
          x.set(x0,y0,z0);
          add_point(x);
          break;

        }
    }
  

  


  if(fclose(fid)!=0)
    {printf("Closing file %s failed, error\n",filename);exit(-1);}



  return 0;
}



int Curve_3D::save_curve(const char* filename) const
{


  FILE *fid=NULL;
  fid=fopen(filename,"w");
  if(fid==NULL)
    {
      printf("ERROR opening file for saving %s\n",filename);
      exit(-1);
    }
  
  fprintf(fid,"# Curve Model %s\n",filename);
  fprintf(fid,"# N_vertex=%d \n\n",size());

  V_3D X;
  int k_vertex=0,N_vertex=size();
  for(k_vertex=0;k_vertex<N_vertex;k_vertex++)
    {
      X=(*this)[k_vertex];
      fprintf(fid,"%f %f %f\n",X[0],X[1],X[2]);
    }
  


  if(fclose(fid)!=0)
    {printf("Closing file %s failed, error\n",filename);exit(-1);}

  return 0;
}


std::vector <V_3D>& Curve_3D::to_vector(){return data;}
const std::vector <V_3D>& Curve_3D::to_vector() const{return data;}

int Curve_3D::set(const std::vector <V_3D>& _curve)
{
  destroy();
  for(int k=0;k<int(_curve.size());k++)
    add(_curve[k]);
  return 0;
}
int Curve_3D::set(const std::vector <double>& _curve)
{
  destroy();
  if( (_curve.size()%3) != 0)
    {printf("Error in set in Curve_3D, the curve positions is not a multiple of 3\n");exit(-1);}
  for(int k=0;k<int(_curve.size())/3;k++)
    add(V_3D(_curve[3*k+0],_curve[3*k+1],_curve[3*k+2]));
  return 0;
}

bool operator==(const Curve_3D& curve_1, const Curve_3D& curve_2)
{
  if(curve_1.size()!=curve_2.size())
    return 0;
  for(int k=0;k<curve_1.size();k++)
    if(curve_1[k]!=curve_2[k])
      return 0;
  return 1;
}
bool operator!=(const Curve_3D& curve_1, const Curve_3D& curve_2)
{
  if(curve_1==curve_2)
    return 0;
  return 1;
}