Skeleton.cpp

Go to the documentation of this file.

#include <Skeleton.h>
#define PI 3.14259



Skeleton::Skeleton()
{
  N_joint = 0;
  N_list_bones = 0;
  joint_root=NULL;
  joint_root=new Joint;

  if(joint_root==NULL)
    {
      cout<<"error allocation joint in Skeleton"<<endl;
      exit(-1);
    }

  joint_access.resize(0);

}



Skeleton::~Skeleton()
{
  reccursive_destroy();
  destroy();
}



int Skeleton::destroy()
{
  int ok=0;


  if(joint_root!=NULL)
    delete joint_root;


  return ok;
}



int Skeleton::get_N_joint()
{
  if(N_joint==0)
    read_skeleton();
  return N_joint;
}

Joint *Skeleton::get_root()
{
  if(joint_root==NULL)
    joint_root=new Joint;
  return joint_root;
}




int Skeleton::add_joint_number()
{
  N_joint++;
  return 0;
}




int Skeleton::read_reccursive_joint(Joint *current,int *k_joint,Joint **T_joint)
{
  (*k_joint)++;

  //get tree
  if(current->get_N_son()>0)
    {
      T_joint[*k_joint]=current;
      for(int k=0;k<current->get_N_son();k++)
        read_reccursive_joint(current->get_son(k),k_joint,T_joint);
    }
  //get leaf
  if(current->get_N_son()==0)
    T_joint[*k_joint]=current;

  
  return 0;
}






int Skeleton::set_size_time(int k_bone,int size)
{
  int ok=0;
  if(anim.size()<=(unsigned int)k_bone)
      anim.resize(k_bone+1);

  ok += anim[k_bone].set_size_time(size);
  ok += anim[k_bone].set_size_angle(size);

  return ok;
}

int Skeleton::get_size_time(int k_bone)
{

  return anim[k_bone].get_size();
}

int Skeleton::set_time(int k_bone,int k_time,double val)
{
  int ok=0;
  ok += anim[k_bone].set_time(k_time,val);
  return ok;
}

int Skeleton::set_angle(int k_bone,int k_time,int k_axis,double val)
{
  int ok=0;
  ok += anim[k_bone].set_angle(k_time,k_axis,val);
  return ok;
}

int Skeleton::set_translate(int k_bone,int k_time,double tx,double ty,double tz)
{
  int ok=0;
  ok += anim[k_bone].set_translation(k_time,tx,ty,tz);
  return ok;
}
int Skeleton::set_anim_matrix(int k_bone,int k_time,int k_dim,double value)
{
  int ok=0;
  ok += anim[k_bone].set_anim_matrix(k_time,k_dim,value);
  return 0;
}








int Skeleton::read_anim()
{
  int k;
  cout<<"Size anim :"<<anim.size()<<endl;
  for(k=0;(unsigned int)k<anim.size();k++)
    {
      cout<<"anim "<<k<<":"<<" size= "<<anim[k].get_size()<<endl;
    }
  return 0;
}

Skeleton& Skeleton::operator=(const Skeleton& skeleton)
{


  
  reccursive_destroy(&joint_root);
  joint_root = new Joint();

  //  joint_root = new Joint();
  N_joint = skeleton.N_joint;

  int k=0;


  N_list_bones = skeleton.N_list_bones;
  
  Joint *to_copy=NULL;
  to_copy= skeleton.joint_root;
  copy_reccursive_joint(joint_root,to_copy);


  anim.resize(skeleton.anim.size());
  for(k=0;k<(int)skeleton.anim.size();k++)
    anim[k] = skeleton.anim[k];

  joint_access.resize(skeleton.joint_access.size());
  for(k=0;k<(int)skeleton.joint_access.size();k++)
    joint_access[k] = skeleton.joint_access[k];


  read_skeleton();

  return *this;
}

int Skeleton::copy_reccursive_joint(Joint *current,Joint *to_copy)
{
  int ok=0;

  current->set_name(to_copy->get_name());
  double orientation[3];
  to_copy->get_orientation(orientation);
  current->set_orientation(orientation);
  current->set_bind_pose(to_copy->get_bind_pose());
  current->set_matrix(to_copy->get_matrix());
  current->set_access_number(to_copy->get_access_number());

  //  cout<<current->get_matrix()<<endl;

  Joint *father = current;
  int k_son=0;

  int N_son = to_copy->get_N_son();
  if(N_son>0)
    {
      for(k_son=0;k_son<N_son;k_son++)
        {

          current->add_son();
          current = current->get_son(0);

          //      cout<<N_son-k_son-1<<", "<<N_son<<endl;

          //to_copy = to_copy->get_son(k_son);
          current->set_father(father);
          ok += copy_reccursive_joint(current,to_copy->get_son(N_son-k_son-1));
          //ok += copy_reccursive_joint(current,to_copy->get_son(k_son));
          current = current->get_father();
        }
    }

  return ok;
}


int Skeleton::read_skeleton()
{
  Joint *current=joint_root;
  Joint *father=joint_root;
  int k=0;

  N_joint=0;
  int access_number=0;
  read_reccursive_joint_cout(current,father,&k,&access_number);

  return 0;
}

int Skeleton::read_reccursive_joint_cout(Joint *current,Joint *father,int *level,int *access_number)
{
  //cout<<"l."<<*level<<" joint :"<<current->get_name()<<"("<<current->get_access_number()<<") father: "<<father->get_name()<<"("<<father->get_access_number()<<") number of son: "<<current->get_N_son()<<endl;

  current->set_access_number(*access_number);
  current->set_level(*level);
  *access_number=*access_number+1;

  father = current;
  for(int k=0;k<current->get_N_son();k++)
    {
      //      current = current->get_son(k);
      *level=*level+1;
      read_reccursive_joint_cout(current->get_son(current->get_N_son()-k-1),father,level,access_number);
      *level=*level-1;
    }
  N_joint++;
  return 0;
}


Matrix Skeleton::get_rotation_animation(int k_bone,double t)
{
  return anim[k_bone].get_rotation(t);
}


int Skeleton::fill_bones_number()
{
  int ok=0;
  Joint *current = joint_root;

  int count=0;
  ok += reccursive_fill_bones_access_number(current,&count);

  return ok;
}

int Skeleton::reccursive_fill_bones_access_number(Joint *current,int *count)
{
  int ok=0;
  //ok += current->set_access_number(look_for_bone_index(current->get_name()));
  
  *count = *count+1;
  int k_son=0;
  for(k_son=0;k_son<current->get_N_son();k_son++)
    reccursive_fill_bones_access_number(current->get_son(k_son),count);

  return ok;
}

int Skeleton::translate(double *t)
{
  int k_dim=0;
  Matrix D=joint_root->get_matrix();
  for(k_dim=0;k_dim<3;k_dim++)
    D.set_value(D.value(3,k_dim)+t[k_dim],3,k_dim);
  joint_root->set_matrix(D);


  fix_init_bind_pose();
  precalculate_matrix();

  return 0;
}

int Skeleton::translate(double tx,double ty,double tz)
{
  double t[3]={tx,ty,tz};
  return translate(t);
}

// int Skeleton::reccursive_translate(Joint *current,double *t)
// {
//   int ok=0;

//   int k_dim=0;
//   Matrix D=joint_root->get_matrix();
//   for(k_dim=0;k_dim<3;k_dim++)
//     D.set_value(D.value(3,k_dim)+t[k_dim],3,k_dim);
//   joint_root->set_matrix(D);
    
//   int k_son=0;
//   for(k_son=0;k_son<current->get_N_son();k_son++)
//     reccursive_translate(current->get_son(k_son),t);

//   return ok;
// }

int Skeleton::scale(double *s)
{


  reccursive_scale(joint_root,s);


  fix_init_bind_pose();
  read_skeleton();
  precalculate_matrix();

//   Matrix D=joint_root->get_matrix();
//   for(k_dim=0;k_dim<3;k_dim++)
//     D.set_value(D.value(3,k_dim)*s[k_dim],3,k_dim);
//   joint_root->set_matrix(D);

  return 0;
}

int Skeleton::scale(double sx,double sy,double sz)
{
  double s[3]={sx,sy,sz};
  return scale(s);
}

int Skeleton::scale(double s)
{
  double _s[3]={s,s,s};
  return scale(_s);
}

int Skeleton::reccursive_scale(Joint *current,double *s)
{
  int ok=0;

  int k_dim=0;
  Matrix D=current->get_matrix();
  for(k_dim=0;k_dim<3;k_dim++)
    D.set_value(D.value(3,k_dim)*s[k_dim],3,k_dim);
  current->set_matrix(D);
    
  int k_son=0;
  for(k_son=0;k_son<current->get_N_son();k_son++)
    reccursive_scale(current->get_son(current->get_N_son()-k_son-1),s);

  return ok;
}



double Skeleton::get_angle(int k_bone,int k_time,int k_axis)
{
  return anim[k_bone].get_angle(k_time,k_axis);
}

Joint* Skeleton::get_joint(int k_bone)
{

  if(k_bone<0 || k_bone>=(int)joint_access.size())
    {printf("Error in get_joint in Skeleton, k_bone[%d] (size=%d) is not correct\n",k_bone,joint_access.size());exit(-1);}
  return joint_access[k_bone];

//   if(k_bone<0 || k_bone>N_joint){printf("ERROR in get_joint in Skeleton, this joint does not exists [%d]\n",k_bone);exit(-1);}

//   Joint *current = joint_root;
//   Joint *joint=NULL;
//   int counter=0;

//   reccursive_bone_looking_for(current,&counter,&joint,k_bone);
//   return joint;
}

int Skeleton::reccursive_bone_looking_for(Joint *current,int *counter,Joint **joint,int k_bone)
{
  int N_son=current->get_N_son();

  if(*counter==k_bone)
    *joint = current;


  *counter = *counter+1;
  for(int k_son=0;*counter<=k_bone && k_son<N_son;k_son++)
    reccursive_bone_looking_for(current->get_son(N_son-k_son-1),counter,joint,k_bone);


  return 0;


}




Matrix Skeleton::get_position(int k_bone)
{

  Matrix P;

  Joint *current = joint_root;
  int counter=0;
  Matrix M;
  reccursive_bone_looking_for_position(current,M,&counter,k_bone,&P);
  return P;
}

int Skeleton::reccursive_bone_looking_for_position(Joint *current,Matrix M,int *counter,int k_bone,Matrix *P)
{
  M = M.multiply(current->get_matrix());
  if(*counter==k_bone)
    *P=M;

  *counter = *counter+1;
  if(*counter<=k_bone)
    for(int k_son=0;*counter<=k_bone && k_son<current->get_N_son();k_son++)
      {
        reccursive_bone_looking_for_position(current->get_son(k_son),M,counter,k_bone,P);
      }
  return 0;

}




int Skeleton::deform_skeleton(double t,Skeleton *output)
{
  int ok=0;
  
  Joint *current=joint_root;
  Joint *to_deform = output->get_root();

  deform_reccursive_joint(current,t,to_deform);

  ok += output->precalculate_matrix();

  return ok;
}


int Skeleton::deform_reccursive_joint(Joint *current,double t,Joint *to_deform)
{
  int index_bone = current->get_access_number();
  Matrix R;
  Matrix T;
  Matrix M;

  Matrix R0 = anim[index_bone].get_rotation(0);
  Matrix R0m1 = R0.invert();


  R = anim[index_bone].get_rotation(t);
  T = anim[index_bone].get_translation(t);
  M = anim[index_bone].get_matrix(t);




  to_deform->set_matrix(current->get_matrix()*(T*R));
  //to_deform -> set_matrix(M*current->get_matrix());
  //to_deform -> set_matrix(M);


  int N_son=current->get_N_son();
  if(N_son>0)
    for(int k=0;k<N_son;k++)
      deform_reccursive_joint(current->get_son(N_son-k-1),t,to_deform->get_son(N_son-k-1));

  return 0;

}

int Skeleton::init_time()
{
  if(N_joint==0)
    {
      cout<<"Warning N_joint=0 in Seleton::init_time !!"<<endl;
      exit(-1);
    }
  for(int k=0;k<N_joint;k++)
    {
      set_size_time(k,2);
      set_time(k,0,0.0);
      set_time(k,1,1.0);
    }
  return 0;
}

int Skeleton::fix_init_bind_pose()
{
  int ok=0;

  Joint *current = joint_root;
  Matrix D;

  ok += recursive_bind_pose_fixing(D,current);

  return ok;
}

int Skeleton::recursive_bind_pose_fixing(Matrix D,Joint *current)
{

  D = D*current->get_matrix();

  current->set_bind_pose(D.invert());
  for(int k=0;k<current->get_N_son();k++)
    recursive_bind_pose_fixing(D,current->get_son(current->get_N_son()-k-1));

  return 0;
}


int Skeleton::precalculate_matrix()
{
  Matrix D;
  Joint *current = joint_root;
  
  reccursive_matrix_precalculation(D,current);
  fill_joint_vector();

  return 0;

}

int Skeleton::reccursive_matrix_precalculation(Matrix D,Joint *current)
{
  D = D*current->get_matrix();
  current->set_world_matrix(D);
  current->set_T(D*current->get_bind_pose());

//   cout<<current->get_access_number()<<endl;
//   cout<<current->get_world_matrix()<<endl;

  int k=0;
  for(k=0;k<current->get_N_son();k++)
    reccursive_matrix_precalculation(D,current->get_son(current->get_N_son()-k-1));
  return 0;
}


int Skeleton::change_father(int k_bone)
{

  int ok=0;

  Joint *current = joint_root;
  Joint *new_root = NULL;
  Matrix D_init;
  Matrix temp_D;

  int is_good=0;
  ok += reccursive_set_name_access_number(joint_root);
  ok += reccursive_look_for_new_father(k_bone,current,&new_root,temp_D,&D_init,&is_good);




  Joint *futur_root = new Joint();
  *futur_root = *new_root;
  ok += add_reccursive_child(futur_root,new_root,-1);  
  //cout<<" ns :"<<futur_root->get_N_son()<<endl;

  Joint *futur_current = futur_root;
  current = new_root;
  Joint *save_current=NULL;
  int N_son=0;
  int k_son=0;

  int son_index=0;
  while(current->get_father()!=current)//old root node
    {

      son_index = current->get_access_number();
      save_current = current;

      current = current->get_father();

  
      //old father become new son
      futur_current->add_son();
      futur_current = futur_current->get_son(0);
      *futur_current=*current;
      futur_current->set_matrix(save_current->get_matrix().invert());

      //add son branch
      N_son=current->get_N_son();
      if(current->get_N_son()>1)
        //do not change the matrix comming from a father
        for(k_son=0;k_son<N_son;k_son++)
          if(current->get_son(N_son-k_son-1)->get_access_number()!=son_index)//this index must not be added
            {
              futur_current->add_son();
              ok += add_reccursive_child(futur_current->get_son(0),current->get_son(N_son-k_son-1),son_index);
            }

    }
  ok += reccursive_destroy(&joint_root);
  joint_root = futur_root;
  joint_root->set_matrix(D_init);

  read_skeleton();
  fix_init_bind_pose();

  Matrix Id;
  //futur_root->set_matrix(Id);

  ok += read_skeleton();
  ok += reccursive_set_name_access_number(joint_root);//name = access number
  ok += precalculate_matrix();

  return ok;
}

int Skeleton::reccursive_set_name_access_number(Joint *current)
{
  char value[10];
  sprintf(value,"%d",current->get_access_number());
  current->set_name(value);
  for(int k_son=0;k_son<current->get_N_son();k_son++)
    reccursive_set_name_access_number(current->get_son(current->get_N_son()-k_son-1));
  return 0;
}

int Skeleton::reccursive_look_for_new_father(int k_bone,Joint *current,Joint **new_root,Matrix temp_D,Matrix *D_init,int *is_good)
{

  int k_son=0;


  if(*is_good==0)
    temp_D = temp_D*current->get_matrix();
  if(current->get_access_number()==k_bone)
  {
    *new_root = current;
    *is_good=1;
    (*D_init) = temp_D;
  }
  else
    for(k_son=0;*is_good==0 && k_son<current->get_N_son();k_son++)
      reccursive_look_for_new_father(k_bone,current->get_son(current->get_N_son()-k_son-1),new_root,temp_D,D_init,is_good);

  return 0;
}


int Skeleton::add_reccursive_child(Joint *new_joint,Joint *old_joint,int forbidden_index)
{
  int k_son=0;
  int N_son = old_joint->get_N_son();

  *new_joint = *old_joint;
  //cout<<"b"<<new_joint->get_access_number()<<endl;
  for(k_son=0;k_son<N_son;k_son++)
    {
      if(old_joint->get_son(N_son-k_son-1)->get_access_number()!=forbidden_index)//this index must not be added
        {
          new_joint->add_son();
          add_reccursive_child(new_joint->get_son(0),old_joint->get_son(N_son-k_son-1),forbidden_index);
        }
    }



  return 0;
  
}

int Skeleton::reccursive_destroy(Joint **current)
{
  Joint *child;
  for(int k_son=0;k_son<(*current)->get_N_son();k_son++)
    {
      child =  (*current)->get_son((*current)->get_N_son()-k_son-1);
      reccursive_destroy(&child);
    }

  delete *current;
  *current=NULL;
  return 0;
}

int Skeleton::reccursive_destroy()
{
  if(joint_root!=NULL)
    reccursive_destroy(&joint_root);
  return 0;
}



int Skeleton::cout_skeleton()
{
  Joint *current=joint_root;
  Joint *father=joint_root;
  int k=0;

  N_joint=0;
  int access_number=0;
  cout_reccursive_joint(current,father,&k,&access_number);

  return 0;
}

int Skeleton::cout_reccursive_joint(Joint *current,Joint *father,int *level,int *access_number)
{
  cout<<"lev."<<*level<<" joint :"<<current->get_name()<<"("<<current->get_access_number()<<") father: "<<current->get_father()->get_name()<<"("<<current->get_father()->get_access_number()<<") number of son: "<<current->get_N_son();
  cout<<" [";
  for(int k=0;k<current->get_N_son()-1;k++)
    printf("%d,",current->get_son(current->get_N_son()-k-1)->get_access_number());
  if(current->get_N_son()>0)
    printf("%d",current->get_son(0)->get_access_number());
  cout<<"]"<<endl;

  //current->set_access_number(*access_number);
  *access_number=*access_number+1;

  father = current;
  for(int k=0;k<current->get_N_son();k++)
    {
      //      current = current->get_son(k);
      *level=*level+1;
      cout_reccursive_joint(current->get_son(current->get_N_son()-k-1),father,level,access_number);
      *level=*level-1;
    }
  N_joint++;
  return 0;
}


int Skeleton::subsample_skeleton()
{
  int ok=0;
  int k_delete=0;
  Joint *current = joint_root;

  ok += read_skeleton();
  ok += precalculate_matrix();

  ok += reccursive_subsample_skeleton(k_delete,&current);

  ok += read_skeleton();
  ok += precalculate_matrix();

  return ok;
}

int Skeleton::reccursive_subsample_skeleton(int k_delete,Joint **current)
{
  int k_son=0;
  int N_son = (*current)->get_N_son();
  int is_deleted = 0;

  double L_max=0.15*5;//size max between two joints
  double angle_max = 20;

  if(N_son==1 && (*current)->get_father()->get_N_son()==1)
    {

      
      //calculate angle
      double X0[3],X1[3],X2[3];
      
      (*current)->get_father()->get_world_matrix().get_position(X0);
      (*current)->get_world_matrix().get_position(X1);
      (*current)->get_son(0)->get_world_matrix().get_position(X2);
      
      
      int k_dim=0;
      double L=0.0;
      for(k_dim=0;k_dim<3;k_dim++)
        L += (X2[k_dim]-X0[k_dim])*(X2[k_dim]-X0[k_dim]);
      L = powf(L,0.5);
      


      double V1[3],V2[3];
      double proj=0.0;
      double n_v1=0.0,n_v2=0.0;
      double angle=0.0;
      
      for(k_dim=0;k_dim<3;V1[k_dim]=X1[k_dim]-X0[k_dim],V2[k_dim]=X2[k_dim]-X1[k_dim],k_dim++);
      for(k_dim=0,proj=0.0,n_v1=0.0,n_v2=0.0;k_dim<3;k_dim++)
        {
          proj += V1[k_dim]*V2[k_dim];
          n_v1 += V1[k_dim]*V1[k_dim];
          n_v2 += V2[k_dim]*V2[k_dim];
        }
      n_v1 = powf(n_v1,0.5);
      n_v2 = powf(n_v2,0.5);
      
      
      angle = acos(proj/(n_v1*n_v2));
      
      if( (*current)->get_access_number()!=0 && (((angle*180/PI<angle_max) && L<L_max)  || L<L_max/10.0 ))//bellow size min L_max/10 the angle does not account
        {
          
          is_deleted = 1;
          (*current)->get_father()->set_son(0,(*current)->get_son(0));
          
          Joint *current_son=(*current)->get_son(0);
          
          current_son->set_father((*current)->get_father());
          current_son->set_matrix((*current)->get_matrix()*current_son->get_matrix());
          
          
        }
    }
  
  Joint *son=NULL;
  for(k_son=0;k_son<N_son;k_son++)
    {
      son = (*current)->get_son(N_son-k_son-1);
      reccursive_subsample_skeleton(k_delete, &son );
    }

  if(is_deleted==1)
    delete *current;

  



  return 0;
}

int Skeleton::find_closest_joint(double x,double y,double z,double radius)
{
  Joint *current=NULL;
  int k_joint=0;

  V_3D P1;P1.set(x,y,z);
  V_3D P2;double p[3];

  double distance=0.0;
  double distance_min=99.9;
  int k_closest=-1;

  for(k_joint=0;k_joint<N_joint;k_joint++)
    {
      current = get_joint(k_joint);
      current->get_world_matrix().get_position(p);
      P2.set(p);

      distance = (P1-P2).norm();
      //printf("%d : %f\n",k_joint,distance);
      if(distance_min>distance && distance<radius)
        {
          k_closest=k_joint;
          distance_min = distance;
        }
    }

  return k_closest;
}


int Skeleton::add_new_joint(double x,double y,double z,int father_number)
{
  int ok=0;

  Joint *father = get_joint(father_number);
  father->add_son();
  Joint *current = father->get_son(0);
  //current = new Joint();
  if(current==NULL)
    printf("Error allocating Joint in add_new_joint\n");

  Matrix current_world_matrix;
  current_world_matrix.set_translation(x,y,z);
  Matrix current_local;
  
  current_local = (father->get_world_matrix().invert())*current_world_matrix;

  current->set_matrix(current_local);


  

  ok += fix_init_bind_pose();
  ok += read_skeleton();
  ok += reccursive_set_name_access_number(joint_root);
  ok += precalculate_matrix();



//   if(father->get_N_son()>0)
//     {
//       int N_son = father->get_N_son();
//       int k_son = 0;

//       for(k_son=0;k_son<N_son;k_son++)
//      {
//        current->add_son();
//        current->get_son(0) = father->get_son(N_son-k+1);
//        current->set_father(father);
//      }

//     }

  return ok;
}




V_3D Skeleton::get_world_position_of_bone(int k_bone)
{
  return get_joint(k_bone)->get_world_matrix().get_position();
}

int Skeleton::fill_joint_vector()
{
  joint_access.resize(0);
  reccursive_fill_joint_vector(joint_root);

  return 0;
}

int Skeleton::reccursive_fill_joint_vector(Joint *current)
{
  int k_son=0;
  int N_son=current->get_N_son();

  joint_access.push_back(current);
  for(k_son=0;k_son<N_son;k_son++)
    reccursive_fill_joint_vector(current->get_son(N_son-k_son-1));

  return 0;
}


int Skeleton::new_father(double x,double y,double z)
{
  destroy();

  N_joint=1;
  joint_root = new Joint;
  joint_root->set_position(x,y,z);


  read_skeleton();
  reccursive_set_name_access_number(joint_root);//name = access number
  precalculate_matrix();

  return 0;
}



int Skeleton::get_access_number_of_joint_named(std::string &name_joint)
{


  int count=0;
  Joint *current=joint_root;
  if(current==NULL)
    {printf("Error joint_root is null in get_access_number_of_joint_named in Skeleton\n");exit(-1);}

  
  for(count=0;count<get_N_joint()-1 && (name_joint != current->get_name()) && (name_joint != current->get_weight_name()) ;count++)
    current = get_joint(count+1);

  if(current==NULL || ((name_joint != current->get_name())&&(name_joint != current->get_weight_name())))
    {printf("WARNING cannot find joint [%s] in get_access_number_of_joint_named in skeleton\n",name_joint.data());return(-1);};//exit(-1);}

  return current->get_access_number();

}
int Skeleton::get_access_number_of_joint_named(const char* name_joint)
{
  std::string name;
  name = name_joint;
  return get_access_number_of_joint_named(name);
}

int Skeleton::init_time_animation()
{
  for(int k=0;k<get_N_joint();k++)
    {
      set_size_time(k,2);
      set_time(k,0,0.0);
      set_time(k,1,2000.0);
    }

  return 0;
}

int Skeleton::is_animated_section(int k_bone)
{
  if(k_bone >= (int)anim.size())
    {printf("error k_bone too large in is_animated_section\n");exit(-1);}

  if(anim[k_bone].get_size()==2)
    return 0;
  else
    return 1;

}

double Skeleton::get_first_anim_time(int k_bone)
{
  if(k_bone >= (int)anim.size())
    {printf("error k_bone too large in is_animated_section\n");exit(-1);}

  return anim[k_bone].get_time(0);
}
double Skeleton::get_last_anim_time(int k_bone)
{
  if(k_bone >= (int)anim.size())
    {printf("error k_bone too large in is_animated_section\n");exit(-1);}

  return anim[k_bone].get_time(anim[k_bone].get_size()-1);
}

int Skeleton::unitize_time_variation()
{
  int k_bone=0;
  int k_t=0;
  double current_t=0.0;
  double t_min=0.0,t_max=0.0;
  for(k_bone=0;k_bone<get_N_joint();k_bone++)
    {
      t_min = anim[k_bone].get_time(0);
      t_max = anim[k_bone].get_time(anim[k_bone].get_size()-1);

      for(k_t=0;k_t<anim[k_bone].get_size();k_t++)
        {
          current_t = anim[k_bone].get_time(k_t);
          anim[k_bone].set_time(k_t,(current_t-t_min)/(t_max-t_min));
        }
    }

  return 0;
}

int Skeleton::add_anim_angle(int index_bone,int axis,double offset_angle)
{
  int k_time;
  for(k_time=0;k_time<anim[index_bone].get_size();k_time++)
    anim[index_bone].set_angle(k_time,axis,offset_angle+anim[index_bone].get_angle(k_time,axis));

  return 0;
}

Skeleton::Skeleton(const Skeleton& skeleton)
{
  


  joint_root = new Joint();

  N_joint = skeleton.N_joint;

  int k=0;


  N_list_bones = skeleton.N_list_bones;
  
  Joint *to_copy;
  to_copy= skeleton.joint_root;
  copy_reccursive_joint(joint_root,to_copy);


  anim.resize(skeleton.anim.size());
  for(k=0;k<(int)skeleton.anim.size();k++)
    anim[k] = skeleton.anim[k];

//   joint_access.resize(skeleton.joint_access.size());
//   for(k=0;k<(int)skeleton.joint_access.size();k++)
//     joint_access[k] = skeleton.joint_access[k];


  read_skeleton();




}