Joint.cpp

Go to the documentation of this file.

#include <Joint.h>

#define PI 3.14259


Joint::Joint()
{
  N_son=0;
  father = this;
  strcpy(name,"NO_NAME");
  for(int k=0;k<3;orientation[k]=0,k++);

  access_number = -1;
  level=0;
}

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



int Joint::destroy()
{
  access_number = -1;
  return 0;
}

int Joint::set_name(const char* _name)
{
  strcpy(name,_name);
  return 0;
}

int Joint::set_name(const std::string& _name)
{
  strcpy(name,_name.data());
  return 0;
}

char *Joint::get_name()
{
  return name;
}

int Joint::set_position(double x,double y,double z)
{
  M.set_translation(x,y,z);
  return 0;
}

int Joint::add_position(double x,double y,double z)
{
  M.add_translation(x,y,z);
  return 0;
}
int Joint::add_position(double *translation)
{
  M.add_translation(translation);
  return 0;
}

int Joint::get_position(double *_position)
{
  for(int k_dim=0;k_dim<3;k_dim++)
    _position[k_dim]=M.value(3,k_dim);
  return 0;
}

double* Joint::get_position()
{

  double *position=NULL;
  position = new double[3];
  if(position==NULL)
    {
      cout<<"ERROR allocation position in joint get_position\n"<<endl;
      exit(-1);
    }

  get_position(position);


  return position;
}

int Joint::set_orientation(double t1,double t2,double t3)
{
  double t[3]={t1,t2,t3};
  
  M.set_euler_angle(t);
  orientation[0]=t1;
  orientation[1]=t2;
  orientation[2]=t3;

  return 0;
}

int Joint::set_orientation(double *t)
{
  M.set_euler_angle(t);
  for(int k_dim=0;k_dim<3;k_dim++)
    orientation[k_dim]=t[k_dim];
  return 0;
}

int Joint::add_rotation(double *v,double theta)
{
  if(theta>2*PI)
    printf("\n\n<<WARNING in add rotation, theta >2*PI, are you sure that you are sending radians angles ?>>\n\n");

  return M.multiply_vector_rotation(v,theta);
}

int Joint::add_rotation(double v0,double v1,double v2,double theta)
{
  double v[3]={v0,v1,v2};
  return M.multiply_vector_rotation(v,theta);
}

int Joint::get_orientation(double *_orientation)
{
  for(int k_dim=0;k_dim<3;_orientation[k_dim]=orientation[k_dim],k_dim++);
  return 0;
}
double* Joint::get_orientation()
{
  return orientation;
}

int Joint::set_father(Joint *_father)
{
  father = _father;
  return 0;
}

Joint* Joint::get_father()
{
  return father;
}

int Joint::set_son(int k,Joint *new_joint)
{
  int count=0;
  if(k>=0 && k<N_son)
    {
      std::list <Joint*>::iterator it_joint;
      for(it_joint=son.begin(),count=0;count<k;count++,it_joint++);
      *it_joint = new_joint;
      return 0;
    }
  
  cout<<"WARNING TRY TO SET INCORRECT SON n."<<k<<" with only "<<N_son<<" in memory for "<<name<<"("<<access_number<<")"<<endl;

  return -1;
}

Joint* Joint::get_son(int k)
{
  int count=0;
  if(k>=0 && k<N_son)
    {
      std::list <Joint*>::iterator it_joint;
      for(it_joint=son.begin(),count=0;count<k;count++,it_joint++);
      return *it_joint;
    }
  
  cout<<"WARNING TRY TO GET INCORRECT SON n."<<k<<" with only "<<N_son<<" in memory for "<<name<<"("<<access_number<<")"<<endl;
  return NULL;
}


int Joint::add_son()
{
  int ok=0;

  N_son++;
  Joint *new_son = new Joint();
  new_son->set_father(this);
  son.push_front(new_son);

  return ok;
}

int Joint::get_N_son()
{
  return N_son;
}

Matrix Joint::get_matrix()
{
  return M;
}

int Joint::scale(double *s)
{
  return M.scale(s);
}
int Joint::scale(double s0,double s1,double s2)
{
  double s[3]={s0,s1,s2};
  return scale(s);
}

int Joint::set_bind_pose(Matrix D)
{
  bind_pose = D;
  return 0;
}

Matrix Joint::get_bind_pose()
{
  return bind_pose;
}


int Joint::multiply_matrix(Matrix R)
{
  M = M*R;
  return 0;
}

int Joint::set_matrix(Matrix _M)
{
  M = _M;
  return 0;
}


int Joint::set_position_bind(double x,double y,double z)
{
  double t[3]={x,y,z};
  return set_position_bind(t);
}

int Joint::set_position_bind(double *t)
{
  for(int k_dim=0;k_dim<3;k_dim++)
    bind_pose.set_value(t[k_dim],3,k_dim);
  return 0;
}

int Joint::set_access_number(int _access_number)
{
  access_number = _access_number;
  return 0;
}

int Joint::get_access_number()
{
  return access_number;
}

int Joint::get_matrix(Matrix *_M)
{
  int k_1=0;
  int k_2=0;
  for(k_1=0;k_1<4;k_1++)
    for(k_2=0;k_2<4;k_2++)
      _M->set_value(M.value(k_1,k_2),k_1,k_2);

  return 0;
}


int Joint::set_world_matrix(Matrix _world_matrix)
{
  world_matrix = _world_matrix;
  return 0;
}
Matrix Joint::get_world_matrix()
{
  return world_matrix;
}
int Joint::set_T(Matrix _T)
{
  T = _T;
  return 0;
}
Matrix Joint::get_T()
{
  return T;
}

int Joint::get_bind_pose(Matrix *_M)
{
  *_M = bind_pose;
  return 0;
}

Joint& Joint::operator=(const Joint& J)
{
  N_son = 0;
  strcpy(name,J.name);
  M = J.M;
  for(int k=0;k<3;k++)
    orientation[k] = J.orientation[k];
  bind_pose = J.bind_pose;
  world_matrix = J.world_matrix;
  T = J.T;
  access_number = J.access_number;
  level = J.level;

  return *this;
}



double Joint::get_min_distance_to_bone(V_3D vertex)
{
  double distance_min=999.9f;
  double current_distance = 0.0f;

  V_3D P1 = world_matrix.get_position();
  V_3D P2;
  V_3D closest;

  V_3D u1,u2;
  double projection=0.0f;

  Joint *_son=0;
  if(get_N_son()==0) //case no son
    {
      distance_min = 9999.99f;
    }
  else
    {
      int k_son=0;
      for(k_son=0;k_son<get_N_son();k_son++)
        {
          _son = get_son(k_son);
          P2 = _son->get_world_matrix().get_position();
          
          u1 = vertex-P1;
          u2 = P2-P1;

          projection = u1.dot(u2)/u2.dot(u2); // (u1.u2)/||u2||^2
          if(projection<0)
            closest = P1;
          else if(projection>1)
            closest = P2;
          else
            closest = P1 + projection*u2;

          current_distance = (vertex-closest).norm(); //distance to
                                                      //closest bone
          if(current_distance<distance_min)
            distance_min=current_distance;
        }
    }

  return distance_min;
}


int Joint::get_level()
{return level;}

int Joint::set_level(int _level)
{level = _level;return 0;}

Joint::Joint(const Joint& _j)
{
  N_son=0;
  father = this;
  strcpy(name,"NO_NAME");
  for(int k=0;k<3;orientation[k]=0,k++);

  access_number = -1;
  level=_j.level;
}

int Joint::set_weight_name(const char* _weight_name)
{weight_name=_weight_name;return 0;}
const char* Joint::get_weight_name() const
{return weight_name.data();}