manipulator.cc

   1
   2 ////////////////////////////////////////////////////////////////////////////////
   3 // This program is free software; you can redistribute it and/or              //
   4 // modify it under the terms of the GNU General Public License                //
   5 // version 2 as published by the Free Software Foundation.                    //
   6 //                                                                            //
   7 // This program is distributed in the hope that it will be useful, but        //
   8 // WITHOUT ANY WARRANTY; without even the implied warranty of                 //
   9 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU          //
  10 // General Public License for more details.                                   //
  11 //                                                                            //
  12 // Written and (C) by François Fleuret                                        //
  13 // Contact <francois.fleuret@epfl.ch> for comments & bug reports              //
  14 ////////////////////////////////////////////////////////////////////////////////
  15
  16 #include "manipulator.h"
  17
  18 static const scalar_t force_break = 200;
  19 static const scalar_t force_max = 50;
  20 static const scalar_t hand_speed = 5;
  21
  22 Manipulator::Manipulator(Task *task) : _touching_polygon(0), _current_polygon(0),
  23                                        _grab_relative_x(0), _grab_relative_y(0),
  24                                        _xmax(0), _ymax(0),
  25                                        _hand_x(0), _hand_y(0),
  26                                        _force_x(0), _force_y(0),
  27                                        _grab(false), _release(false),
  28                                        _current_action(ACTION_GRAB) {
  29   Map::init(NB_DISCRETE_PARAMETERS + NB_CONTINUOUS_MAPS * continuous_map_size);
  30   _xmax = task->width();
  31   _ymax = task->height();
  32   for(int m = - continuous_map_size; m <= continuous_map_size; m++)
  33     _kernel_continuous_maps[m + continuous_map_size] = 2 * exp(-sq(m)/sq(continuous_map_size/9)) - 1;
  34 }
  35
  36 int Manipulator::nb_actions() { return NB_ACTIONS; }
  37
  38 void Manipulator::do_action(int action) {
  39   scalar_t prev_hand_x  =_hand_x, prev_hand_y = _hand_y;
  40   switch(action) {
  41   case ACTION_IDLE:
  42     break;
  43   case ACTION_MOVE_RIGHT:
  44     _hand_x += hand_speed;
  45     if(_hand_x >= _xmax) {
  46       _hand_x = prev_hand_x - hand_speed;
  47       _current_action = ACTION_MOVE_LEFT;
  48     }
  49     break;
  50   case ACTION_MOVE_LEFT:
  51     _hand_x -= hand_speed;
  52     if(_hand_x < 0) {
  53       _hand_x = prev_hand_x + hand_speed;
  54       _current_action = ACTION_MOVE_RIGHT;
  55     }
  56     break;
  57   case ACTION_MOVE_DOWN:
  58     _hand_y += hand_speed;
  59     if(_hand_y >= _ymax) {
  60       _hand_y = prev_hand_y - hand_speed;
  61       _current_action = ACTION_MOVE_UP;
  62     }
  63     break;
  64   case ACTION_MOVE_UP:
  65     _hand_y -= hand_speed;
  66     if(_hand_y < 0) {
  67       _hand_y = prev_hand_y + hand_speed;
  68       _current_action = ACTION_MOVE_DOWN;
  69     }
  70     break;
  71   case ACTION_GRAB:
  72     _grab = true;
  73     break;
  74   case ACTION_RELEASE:
  75     _release = true;
  76     break;
  77   default:
  78     cerr << "Unknown action number (" << action << ")" << endl;
  79     abort();
  80   }
  81 }
  82
  83 void Manipulator::update_map() {
  84   parameters[FEEL_TOUCHING_OBJECT] = (_touching_polygon ? +1 : -1);
  85   parameters[FEEL_GRABBING_OBJECT] = (_current_polygon ? +1 : -1);
  86
  87   int px = int(_hand_x * continuous_map_size / _xmax),
  88     py = int(_hand_y * continuous_map_size / _ymax),
  89     fx = int((continuous_map_size - 1) * (0.5 + 0.5 * _force_x / force_max)),
  90     fy = int((continuous_map_size - 1) * (0.5 + 0.5 * _force_y / force_max));
  91
  92   ASSERT(fx >=0 && fx < continuous_map_size && fy >= 0 && fy < continuous_map_size,
  93          "Discrete force sensations out of bounds");
  94
  95   for(int m = 0; m < continuous_map_size; m++) {
  96     parameters[NB_DISCRETE_PARAMETERS + MAP_LOCATION_X * continuous_map_size + m] =
  97       _kernel_continuous_maps[px - m + continuous_map_size];
  98     parameters[NB_DISCRETE_PARAMETERS + MAP_LOCATION_Y * continuous_map_size + m] =
  99       _kernel_continuous_maps[py - m + continuous_map_size];
 100     parameters[NB_DISCRETE_PARAMETERS + MAP_RESISTANCE_X * continuous_map_size + m] =
 101       _kernel_continuous_maps[fx - m + continuous_map_size];
 102     parameters[NB_DISCRETE_PARAMETERS + MAP_RESISTANCE_Y * continuous_map_size + m] =
 103       _kernel_continuous_maps[fy - m + continuous_map_size];
 104   }
 105 }
 106
 107 void Manipulator::update(scalar_t dt, Universe *universe) {
 108   _touching_polygon = universe->pick_polygon(_hand_x, _hand_y);
 109
 110   if(!_current_polygon && _grab) {
 111     _current_polygon = _touching_polygon;
 112     if(_current_polygon) {
 113       _grab_relative_x = _current_polygon->relative_x(_hand_x, _hand_y);
 114       _grab_relative_y = _current_polygon->relative_y(_hand_x, _hand_y);
 115     }
 116   }
 117
 118   if(_release) _current_polygon = 0;
 119
 120   _grab = false; _release = false;
 121
 122   if(_current_polygon) {
 123     scalar_t xf = _current_polygon->absolute_x(_grab_relative_x, _grab_relative_y);
 124     scalar_t yf = _current_polygon->absolute_y(_grab_relative_x, _grab_relative_y);
 125     _force_x = (_hand_x - xf) * 10;
 126     _force_y = (_hand_y - yf) * 10;
 127
 128     scalar_t f = sqrt(sq(_force_x) + sq(_force_y));
 129
 130     if(f >= force_break) {
 131       _current_polygon = false;
 132       _force_x = 0;
 133       _force_y = 0;
 134     } else {
 135       if(f >= force_max) {
 136         _force_x *= (force_max/f);
 137         _force_y *= (force_max/f);
 138       }
 139       _current_polygon->set_speed(0, 0, 0);
 140       _current_polygon->apply_force(dt, xf, yf, _force_x, _force_y);
 141     }
 142
 143   } else {
 144     _force_x = 0;
 145     _force_y = 0;
 146   }
 147 }
 148
 149 int Manipulator::random_action() {
 150   const scalar_t proba_to_grab_when_touching = 0.1;
 151   const scalar_t proba_to_change_motion = 0.025;
 152
 153   if(_current_polygon) {
 154     if(drand48() < 0.01) _current_action = ACTION_RELEASE;
 155     else if(_current_action == ACTION_RELEASE || _current_action == ACTION_GRAB ||
 156             drand48() < proba_to_change_motion) {
 157       switch(int(drand48() * 4)) {
 158       case 0:
 159         _current_action = ACTION_MOVE_UP;
 160         break;
 161       case 1:
 162         _current_action = ACTION_MOVE_RIGHT;
 163         break;
 164       case 2:
 165         _current_action = ACTION_MOVE_DOWN;
 166         break;
 167       case 3:
 168         _current_action = ACTION_MOVE_LEFT;
 169         break;
 170       default:
 171         abort();
 172       }
 173     }
 174   } else {
 175     if(_touching_polygon && drand48() < proba_to_grab_when_touching) _current_action = ACTION_GRAB;
 176     else if(_current_action == ACTION_RELEASE || _current_action == ACTION_GRAB ||
 177             drand48() < proba_to_change_motion) {
 178       switch(int(drand48() * 4)) {
 179       case 0:
 180         _current_action = ACTION_MOVE_UP;
 181         break;
 182       case 1:
 183         _current_action = ACTION_MOVE_RIGHT;
 184         break;
 185       case 2:
 186         _current_action = ACTION_MOVE_DOWN;
 187         break;
 188       case 3:
 189         _current_action = ACTION_MOVE_LEFT;
 190         break;
 191       default:
 192         abort();
 193       }
 194     }
 195   }
 196
 197   return _current_action;
 198 }
 199
 200 int Manipulator::parameter_width() {
 201   return 80 + continuous_map_size * sqsize + 1;
 202 }
 203
 204 int Manipulator::parameter_height() {
 205   return 6 * sqsize + 2;
 206 }
 207
 208 void Manipulator::draw_parameters(int x0, int y0, SimpleWindow *window) {
 209   scalar_t s;
 210
 211   int wtext = parameter_width() - continuous_map_size * sqsize - 1;
 212
 213   window->color(0.8, 0.8, 0.8);
 214   window->fill_rectangle(x0 + wtext, y0, continuous_map_size * sqsize + 1, 4 * sqsize + 1);
 215   window->draw_text("x-position", x0 + 8, y0 + 0 * sqsize + sqsize - 4);
 216   window->draw_text("y-position", x0 + 8, y0 + 1 * sqsize + sqsize - 4);
 217   window->draw_text(   "x-force", x0 + 8, y0 + 2 * sqsize + sqsize - 4);
 218   window->draw_text(   "y-force", x0 + 8, y0 + 3 * sqsize + sqsize - 4);
 219
 220   window->fill_rectangle(x0 + wtext + 0, y0 + 4 * sqsize, sqsize + 1, sqsize + 1);
 221   window->draw_text("grabbing", x0 + 8, y0 + 4 * sqsize + sqsize - 4);
 222
 223   window->fill_rectangle(x0 + wtext + 0, y0 + 5 * sqsize, sqsize + 1, sqsize + 1);
 224   window->draw_text("touching", x0 + 8, y0 + 5 * sqsize + sqsize - 4);
 225
 226   s = parameters[FEEL_GRABBING_OBJECT];
 227   window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s));
 228   window->fill_rectangle(x0 + wtext + 1, y0 + 4 * sqsize + 1, sqsize - 1, sqsize - 1);
 229
 230   s = parameters[FEEL_TOUCHING_OBJECT];
 231   window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s));
 232   window->fill_rectangle(x0 + wtext + 1, y0 + 5 * sqsize + 1, sqsize - 1, sqsize - 1);
 233
 234   for(int m = 0; m < continuous_map_size; m++) {
 235     s = parameters[NB_DISCRETE_PARAMETERS + MAP_LOCATION_X * continuous_map_size + m];
 236     window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s));
 237     window->fill_rectangle(x0 + wtext + m * sqsize + 1, y0 + 0 * sqsize + 1,
 238                            sqsize-1, sqsize-1);
 239
 240     s = parameters[NB_DISCRETE_PARAMETERS + MAP_LOCATION_Y * continuous_map_size + m];
 241     window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s));
 242     window->fill_rectangle(x0 + wtext + m * sqsize + 1, y0 + 1 * sqsize + 1,
 243                            sqsize-1, sqsize-1);
 244
 245     s = parameters[NB_DISCRETE_PARAMETERS + MAP_RESISTANCE_X * continuous_map_size + m];
 246     window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s));
 247     window->fill_rectangle(x0 + wtext + m * sqsize + 1, y0 + 2 * sqsize + 1,
 248                            sqsize-1, sqsize-1);
 249
 250     s = parameters[NB_DISCRETE_PARAMETERS + MAP_RESISTANCE_Y * continuous_map_size + m];
 251     window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s));
 252     window->fill_rectangle(x0 + wtext + m * sqsize + 1, y0 + 3 * sqsize + 1,
 253                            sqsize-1, sqsize-1);
 254   }
 255 }
 256
 257 void Manipulator::draw_on_universe(SimpleWindow *window) {
 258   int xc = int(_hand_x), yc = int(_hand_y);
 259   const int delta = 5;
 260
 261   window->color(1.0, 1.0, 1.0);
 262
 263   window->draw_line(xc - delta, yc, xc + delta, yc);
 264   window->draw_line(xc, yc - delta, xc, yc + delta);
 265
 266   if(_current_polygon) {
 267     window->draw_line(xc - delta, yc - delta, xc + delta, yc - delta);
 268     window->draw_line(xc + delta, yc - delta, xc + delta, yc + delta);
 269     window->draw_line(xc + delta, yc + delta, xc - delta, yc + delta);
 270     window->draw_line(xc - delta, yc + delta, xc - delta, yc - delta);
 271   }
 272
 273   if(_touching_polygon) {
 274     window->draw_line(xc - (delta + 2), yc - (delta + 2), xc + (delta + 2), yc - (delta + 2));
 275     window->draw_line(xc + (delta + 2), yc - (delta + 2), xc + (delta + 2), yc + (delta + 2));
 276     window->draw_line(xc + (delta + 2), yc + (delta + 2), xc - (delta + 2), yc + (delta + 2));
 277     window->draw_line(xc - (delta + 2), yc + (delta + 2), xc - (delta + 2), yc - (delta + 2));
 278   }
 279 }
 280
 281 // void Manipulator::force_grab(Universe *universe, scalar_t x, scalar_t y) {
 282 //   _hand_x = x;
 283 //   _hand_y = y;
 284 //   _current_polygon = universe->pick_polygon(_hand_x, _hand_y);
 285 //   if(_current_polygon) {
 286 //     _grab_relative_x = (_hand_x - _current_polygon->_center_x) * cos(_current_polygon->_theta)
 287 //       - (_hand_y - _current_polygon->_center_y) * sin(_current_polygon->_theta);
 288 //     _grab_relative_y = + (_hand_x - _current_polygon->_center_x) * sin(_current_polygon->_theta)
 289 //       + (_hand_y - _current_polygon->_center_y) * cos(_current_polygon->_theta);
 290 //   }
 291 // }
 292
 293 void Manipulator::force_move(scalar_t x, scalar_t y) {
 294   scalar_t prev_hand_x  =_hand_x, prev_hand_y = _hand_y;
 295   _hand_x = x;
 296   _hand_y = y;
 297   if(_hand_x < 0 || _hand_x >= _xmax) _hand_x = prev_hand_x;
 298   if(_hand_y < 0 || _hand_y >= _ymax) _hand_y = prev_hand_y;
 299 }
 300
 301 void Manipulator::force_release() {
 302   _current_polygon = 0;
 303   _force_x = 0.0;
 304   _force_y = 0.0;
 305 }