Navigator_tool.cpp

Go to the documentation of this file.

#include <Navigator_tool.h>

#define PI 3.14259


Navigator_tool::Navigator_tool()
{
  kt=0;
  N_speed = 260;
  start_t=0.0;
  end_t=1.0;
  X_screen = 640;
  Y_screen = 480;
  percent_x = 0.65;
  percent_y = 0.65;

  dist_obs  = -84;
  dist_obs2 = -1;
  dist_obs3 = -1;
  dist_obs4 = -1;

  current_view = 1;
  selection_mode = 0;
  selected_joint = -1;
  view_selection_mode = 0;

  skeleton_shown=0;
}



Navigator_tool::~Navigator_tool()
{
}

int Navigator_tool::set_tr_x(double _tr_x)
{tr_x = _tr_x;return 0;}
int Navigator_tool::set_tr_y(double _tr_y)
{tr_y = _tr_y;return 0;}
int Navigator_tool::set_tr_z(double _tr_z)
{tr_z = _tr_z;return 0;}
double Navigator_tool::get_tr_x()
{return tr_x;}
double Navigator_tool::get_tr_y()
{return tr_y;}
double Navigator_tool::get_tr_z()
{return tr_z;}

int Navigator_tool::set_tr2_x(double _tr2_x)
{tr2_x = _tr2_x;return 0;}
int Navigator_tool::set_tr2_y(double _tr2_y)
{tr2_y = _tr2_y;return 0;}
int Navigator_tool::set_tr2_z(double _tr2_z)
{tr2_z = _tr2_z;return 0;}
double Navigator_tool::get_tr2_x()
{return tr2_x;}
double Navigator_tool::get_tr2_y()
{return tr2_y;}
double Navigator_tool::get_tr2_z()
{return tr2_z;}


int Navigator_tool::set_tr3_x(double _tr3_x)
{tr3_x = _tr3_x;return 0;}
int Navigator_tool::set_tr3_y(double _tr3_y)
{tr3_y = _tr3_y;return 0;}
int Navigator_tool::set_tr3_z(double _tr3_z)
{tr3_z = _tr3_z;return 0;}
double Navigator_tool::get_tr3_x()
{return tr3_x;}
double Navigator_tool::get_tr3_y()
{return tr3_y;}
double Navigator_tool::get_tr3_z()
{return tr3_z;}


int Navigator_tool::set_tr4_x(double _tr4_x)
{tr4_x = _tr4_x;return 0;}
int Navigator_tool::set_tr4_y(double _tr4_y)
{tr4_y = _tr4_y;return 0;}
int Navigator_tool::set_tr4_z(double _tr4_z)
{tr4_z = _tr4_z;return 0;}
double Navigator_tool::get_tr4_x()
{return tr4_x;}
double Navigator_tool::get_tr4_y()
{return tr4_y;}
double Navigator_tool::get_tr4_z()
{return tr4_z;}




int Navigator_tool::set_theta_1(double _theta_1)
{theta_1=_theta_1;return 0;}
int Navigator_tool::set_theta_2(double _theta_2)
{theta_2=_theta_2;return 0;}
double Navigator_tool::get_theta_1()
{return theta_1;}
double Navigator_tool::get_theta_2()
{return theta_2;}

int Navigator_tool::set_width(double _width)
{width=_width;return 0;}
int Navigator_tool::set_height(double _height)
{height=_height;return 0;}
double Navigator_tool::get_width()
{return width;}
double Navigator_tool::get_height()
{return height;}

int Navigator_tool::set_dist_obs(double _dist_obs)
{dist_obs=_dist_obs;return 0;};
double Navigator_tool::get_dist_obs()
{return dist_obs;}

int Navigator_tool::set_dist_obs2(double _dist_obs2)
{dist_obs2=_dist_obs2;return 0;};
double Navigator_tool::get_dist_obs2()
{return dist_obs2;}

int Navigator_tool::set_dist_obs3(double _dist_obs3)
{dist_obs3=_dist_obs3;return 0;};
double Navigator_tool::get_dist_obs3()
{return dist_obs3;}

int Navigator_tool::set_dist_obs4(double _dist_obs4)
{dist_obs4=_dist_obs4;return 0;};
double Navigator_tool::get_dist_obs4()
{return dist_obs4;}


int Navigator_tool::set_left_b(int _left_b)
{left_b=_left_b;return 0;}
int Navigator_tool::set_right_b(int _right_b)
{right_b=_right_b;return 0;}
int Navigator_tool::get_left_b()
{return left_b;}
int Navigator_tool::get_right_b()
{return right_b;}

int Navigator_tool::set_x_prec(int _x_prec)
{x_prec=_x_prec;return 0;}
int Navigator_tool::set_y_prec(int _y_prec)
{y_prec=_y_prec;return 0;}
int Navigator_tool::get_x_prec()
{return x_prec;}
int Navigator_tool::get_y_prec()
{return y_prec;}

int Navigator_tool::set_sel_x(double _sel_x)
{sel_x=_sel_x;return 0;}
int Navigator_tool::set_sel_y(double _sel_y)
{sel_y=_sel_y;return 0;}
int Navigator_tool::set_sel_z(double _sel_z)
{sel_z=_sel_z;return 0;}
double Navigator_tool::get_sel_x()
{return sel_x;}
double Navigator_tool::get_sel_y()
{return sel_y;}
double Navigator_tool::get_sel_z()
{return sel_z;}

int Navigator_tool::increase_sel_x(double d_x)
{sel_x+=d_x;return 0;}
int Navigator_tool::increase_sel_y(double d_y)
{sel_y+=d_y;return 0;}
int Navigator_tool::increase_sel_z(double d_z)
{sel_z+=d_z;return 0;}





int Navigator_tool::increase_theta_1(double d_theta)
{
  theta_1 += d_theta;
  return 0;
}

int Navigator_tool::increase_theta_2(double d_theta)
{
  theta_2 += d_theta;
  return 0;
}

int Navigator_tool::increase_dist_obs(double factor)
{
  dist_obs += (abs(dist_obs)+1)*factor;
  if(dist_obs>=0)
    dist_obs=0;
  return 0;
}
int Navigator_tool::increase_dist_obs2(double factor)
{
  dist_obs2 += (abs(dist_obs2)+1)*factor;
  if(dist_obs2>=0)
    dist_obs2=0;
  return 0;
}
int Navigator_tool::increase_dist_obs3(double factor)
{
  dist_obs3 += (abs(dist_obs3)+1)*factor;
  if(dist_obs3>=0)
    dist_obs3=0;
  return 0;
}
int Navigator_tool::increase_dist_obs4(double factor)
{
  dist_obs4 += (abs(dist_obs4)+1)*factor;
  if(dist_obs4>=0)
    dist_obs4=0;
  return 0;
}



int Navigator_tool::go_forward(double d_L)
{
  tr_x -= d_L*sin(theta_2*PI/180)*cos(theta_1*PI/180);
  tr_y += d_L*                    sin(theta_1*PI/180);
  tr_z += d_L*cos(theta_2*PI/180)*cos(theta_1*PI/180);

  return 0;
}

int Navigator_tool::go_backward(double d_L)
{
  tr_x += d_L*sin(theta_2*PI/180)*cos(theta_1*PI/180);
  tr_y -= d_L*                    sin(theta_1*PI/180);
  tr_z -= d_L*cos(theta_2*PI/180)*cos(theta_1*PI/180);
  
  return 0;
}

int Navigator_tool::go_right(double d_L)
{

  tr_x -= d_L*cos(theta_2*PI/180)*cos(theta_1*PI/180);
  tr_y -= d_L*0;
  tr_z -= d_L*sin(theta_2*PI/180)*cos(theta_1*PI/180);

  return 0;
}

int Navigator_tool::go_left(double d_L)
{
  tr_x += d_L*cos(theta_2*PI/180)*cos(theta_1*PI/180);
  tr_y -= d_L*0;
  tr_z += d_L*sin(theta_2*PI/180)*cos(theta_1*PI/180);

  return 0;
}

int Navigator_tool::go_up(double d_L)
{
  tr_x -= d_L*sin(theta_2*PI/180)*sin(theta_1*PI/180);
  tr_y -= d_L*                    cos(theta_1*PI/180);
  tr_z += d_L*cos(theta_2*PI/180)*sin(theta_1*PI/180);
  
  return 0;
}

int Navigator_tool::go_down(double d_L)
{
  tr_x += d_L*sin(theta_2*PI/180)*sin(theta_1*PI/180);
  tr_y += d_L*                    cos(theta_1*PI/180);
  tr_z -= d_L*cos(theta_2*PI/180)*sin(theta_1*PI/180);

  return 0;
}

int Navigator_tool::increase_time()
{++kt;return 0;}

int Navigator_tool::get_counter()
{return kt;}


double Navigator_tool::get_time()
{
  //double start_t=0.28;
  //double end_t = 1.4;

  double y=(double)(kt%(int)N_speed)/(double)(N_speed+1);
  double u = start_t + y*(end_t-start_t);
  //double u = 0.001 + y*0.998;
  return u;
}


int Navigator_tool::set_screen_size(int _X_screen,int _Y_screen)
{
  X_screen = _X_screen;
  Y_screen = _Y_screen;
  return 0;
}
int Navigator_tool::get_X_screen() const
{return X_screen;}
int Navigator_tool::get_Y_screen() const
{return Y_screen;}

double Navigator_tool::get_screen_ratio() const
{
  return (double)X_screen/(double)Y_screen;
}

int Navigator_tool::set_percent_x(double _percent_x)
{percent_x = _percent_x;return 0;}
int Navigator_tool::set_percent_y(double _percent_y)
{percent_y = _percent_y;return 0;}
double Navigator_tool::get_percent_x() const
{return percent_x;}
double Navigator_tool::get_percent_y() const
{return percent_y;}


int Navigator_tool::set_current_view(int _current_view)
{current_view = _current_view;return 0;}
int Navigator_tool::get_current_view() const
{return current_view;}

int Navigator_tool::get_selection_mode() const
{return selection_mode;return 0;}
int Navigator_tool::set_selection_mode(int mode)
{selection_mode = mode;return 0;}

int Navigator_tool::set_selected_joint(int _selected_joint)
{selected_joint = _selected_joint;return 0;}
int Navigator_tool::get_selected_joint() const
{return selected_joint;}

int Navigator_tool::get_view_selection_mode() const
{return view_selection_mode;}
int Navigator_tool::set_view_selection_mode(int _view_selection_mode)
{view_selection_mode = _view_selection_mode;return 0;}

int Navigator_tool::get_rotation_mode() const
{return rotation_mode;}
int Navigator_tool::set_rotation_mode(int _rotation_mode)
{rotation_mode=_rotation_mode;return 0;}


int Navigator_tool::set_N_speed(int _N_speed)
{
  N_speed=_N_speed;
  return 0;
}

int Navigator_tool::get_N_speed() const
{return (int)N_speed;}

V_3D Navigator_tool::get_current_position() const
{
  V_3D current;

  current[0] = tr_x-dist_obs*sin(theta_2*PI/180)*cos(theta_1*PI/180);
  current[1] = tr_y+dist_obs                    *sin(theta_1*PI/180);
  current[2] = tr_z+dist_obs*cos(theta_2*PI/180)*cos(theta_1*PI/180);

  return current;
}

int Navigator_tool::is_skeleton_shown() const{return skeleton_shown;}
int Navigator_tool::set_skeleton_shown(int is_shown){skeleton_shown=is_shown;return 0;}