polygon.cc

   1
   2 // Written and (C) by Francois Fleuret
   3 // Contact <francois.fleuret@idiap.ch> for comments & bug reports
   4
   5 #include <cmath>
   6 #include "polygon.h"
   7
   8 static const scalar_t dl = 20.0;
   9 static const scalar_t repulsion_constant = 0.2;
  10 static const scalar_t dissipation = 0.5;
  11
  12 Polygon::Polygon(scalar_t mass,
  13                  scalar_t red, scalar_t green, scalar_t blue,
  14                  scalar_t *x, scalar_t *y,
  15                  int nv) : _mass(mass),
  16                            _moment_of_inertia(0), _radius(0),
  17                            _relative_x(new scalar_t[nv]), _relative_y(new scalar_t[nv]),
  18                            _total_nb_dots(0),
  19                            _nb_dots(new int[nv]),
  20                            _effecting_edge(0),
  21                            _length(new scalar_t[nv]),
  22                            _triangles(new Triangle[nv-2]),
  23                            _initialized(false), _nailed(false),
  24                            _nb_vertices(nv),
  25                            _x(new scalar_t[nv]), _y(new scalar_t[nv]),
  26                            _red(red), _green(green), _blue(blue) {
  27   _last_center_x = 0;
  28   _last_center_y = 0;
  29   _last_theta = 0;
  30
  31   if(x) for(int i = 0; i < nv; i++) _relative_x[i] = x[i];
  32   if(y) for(int i = 0; i < nv; i++) _relative_y[i] = y[i];
  33 }
  34
  35 Polygon::~Polygon() {
  36   delete[] _relative_x;
  37   delete[] _relative_y;
  38   delete[] _x;
  39   delete[] _y;
  40   delete[] _nb_dots;
  41   delete[] _length;
  42   delete[] _triangles;
  43   delete[] _effecting_edge;
  44 }
  45
  46 Polygon *Polygon::clone() {
  47   return new Polygon(_mass, _red, _green, _blue, _relative_x, _relative_y, _nb_vertices);
  48 }
  49
  50 void Polygon::color_xfig(XFigTracer *tracer) {
  51   tracer->add_color(int(255 * _red), int(255 * _green), int(255 * _blue));
  52 }
  53
  54 void Polygon::print_xfig(XFigTracer *tracer) {
  55   tracer->draw_polygon(int(255 * _red), int(255 * _green), int(255 * _blue),
  56                        _nb_vertices, _x, _y);
  57 }
  58
  59 #ifdef X11_SUPPORT
  60 void Polygon::draw(SimpleWindow *window) {
  61   window->color(_red, _green, _blue);
  62   int x[_nb_vertices], y[_nb_vertices];
  63   for(int n = 0; n < _nb_vertices; n++) {
  64     x[n] = int(_x[n]);
  65     y[n] = int(_y[n]);
  66   }
  67   window->fill_polygon(_nb_vertices, x, y);
  68 }
  69
  70 void Polygon::draw_contours(SimpleWindow *window) {
  71   int x[_nb_vertices], y[_nb_vertices];
  72   for(int n = 0; n < _nb_vertices; n++) {
  73     x[n] = int(_x[n]);
  74     y[n] = int(_y[n]);
  75   }
  76   window->color(0.0, 0.0, 0.0);
  77   // window->color(1.0, 1.0, 1.0);
  78   for(int n = 0; n < _nb_vertices; n++) {
  79     window->draw_line(x[n], y[n], x[(n+1)%_nb_vertices], y[(n+1)%_nb_vertices]);
  80   }
  81 }
  82 #endif
  83
  84 void Polygon::draw(Canvas *canvas) {
  85   canvas->set_drawing_color(_red, _green, _blue);
  86   canvas->draw_polygon(1, _nb_vertices, _x, _y);
  87 }
  88
  89 void Polygon::draw_contours(Canvas *canvas) {
  90   canvas->set_drawing_color(0.0, 0.0, 0.0);
  91   canvas->draw_polygon(0, _nb_vertices, _x, _y);
  92 }
  93
  94 void Polygon::set_vertex(int k, scalar_t x, scalar_t y) {
  95   _relative_x[k] = x;
  96   _relative_y[k] = y;
  97 }
  98
  99 void Polygon::set_position(scalar_t center_x, scalar_t center_y, scalar_t theta) {
 100   _center_x = center_x;
 101   _center_y = center_y;
 102   _theta = theta;
 103 }
 104
 105 void Polygon::set_speed(scalar_t dcenter_x, scalar_t dcenter_y, scalar_t dtheta) {
 106   _dcenter_x = dcenter_x;
 107   _dcenter_y = dcenter_y;
 108   _dtheta = dtheta;
 109 }
 110
 111 bool Polygon::contain(scalar_t x, scalar_t y) {
 112   for(int t = 0; t < _nb_vertices-2; t++) {
 113     scalar_t xa = _x[_triangles[t].a], ya = _y[_triangles[t].a];
 114     scalar_t xb = _x[_triangles[t].b], yb = _y[_triangles[t].b];
 115     scalar_t xc = _x[_triangles[t].c], yc = _y[_triangles[t].c];
 116     if(prod_vect(x - xa, y - ya, xb - xa, yb - ya) <= 0 &&
 117        prod_vect(x - xb, y - yb, xc - xb, yc - yb) <= 0 &&
 118        prod_vect(x - xc, y - yc, xa - xc, ya - yc) <= 0) return true;
 119   }
 120   return false;
 121 }
 122
 123 void Polygon::triangularize(int &nt, int nb, int *index) {
 124   if(nb == 3) {
 125
 126     if(nt >= _nb_vertices-2) {
 127       cerr << "Error type #1 in triangularization." << endl;
 128       exit(1);
 129     }
 130
 131     _triangles[nt].a = index[0];
 132     _triangles[nt].b = index[1];
 133     _triangles[nt].c = index[2];
 134     nt++;
 135
 136   } else {
 137     int best_m = -1, best_n = -1;
 138     scalar_t best_split = -1, det, s = -1, t = -1;
 139
 140     for(int n = 0; n < nb; n++) for(int m = 0; m < n; m++) if(n > m+1 && m+nb > n+1) {
 141       bool no_intersection = true;
 142       for(int k = 0; no_intersection & (k < nb); k++)
 143         if(k != n && k != m && (k+1)%nb != n && (k+1)%nb != m) {
 144           intersection(_relative_x[index[n]], _relative_y[index[n]],
 145                        _relative_x[index[m]], _relative_y[index[m]],
 146                        _relative_x[index[k]], _relative_y[index[k]],
 147                        _relative_x[index[(k+1)%nb]], _relative_y[index[(k+1)%nb]], det, s, t);
 148           no_intersection = det == 0 || s < 0 || s > 1 || t < 0 || t > 1;
 149         }
 150
 151       if(no_intersection) {
 152         scalar_t a1 = 0, a2 = 0;
 153         for(int k = 0; k < nb; k++) if(k >= m && k < n)
 154           a1 += prod_vect(_relative_x[index[k]] - _relative_x[index[m]],
 155                           _relative_y[index[k]] - _relative_y[index[m]],
 156                           _relative_x[index[k+1]] - _relative_x[index[m]],
 157                           _relative_y[index[k+1]] - _relative_y[index[m]]);
 158         else
 159           a2 += prod_vect(_relative_x[index[k]] - _relative_x[index[m]],
 160                           _relative_y[index[k]] - _relative_y[index[m]],
 161                           _relative_x[index[(k+1)%nb]] - _relative_x[index[m]],
 162                           _relative_y[index[(k+1)%nb]] - _relative_y[index[m]]);
 163
 164         if((a1 * a2 > 0 && best_split < 0) || (abs(a1 - a2) < best_split)) {
 165           best_n = n; best_m = m;
 166           best_split = abs(a1 - a2);
 167         }
 168       }
 169     }
 170
 171     if(best_n >= 0 && best_m >= 0) {
 172       int index_neg[nb], index_pos[nb];
 173       int neg = 0, pos = 0;
 174       for(int k = 0; k < nb; k++) {
 175         if(k >= best_m && k <= best_n) index_pos[pos++] = index[k];
 176         if(k <= best_m || k >= best_n) index_neg[neg++] = index[k];
 177       }
 178       if(pos < 3 || neg < 3) {
 179         cerr << "Error type #2 in triangularization." << endl;
 180         exit(1);
 181       }
 182       triangularize(nt, pos, index_pos);
 183       triangularize(nt, neg, index_neg);
 184     } else {
 185       cerr << "Error type #3 in triangularization." << endl;
 186       exit(1);
 187     }
 188   }
 189 }
 190
 191 void Polygon::initialize(int nb_polygons) {
 192   scalar_t a;
 193
 194   _nb_polygons = nb_polygons;
 195
 196   a = _relative_x[_nb_vertices - 1] * _relative_y[0]
 197     - _relative_x[0] * _relative_y[_nb_vertices - 1];
 198
 199   for(int n = 0; n < _nb_vertices - 1; n++)
 200     a += _relative_x[n] * _relative_y[n+1] - _relative_x[n+1] * _relative_y[n];
 201   a *= 0.5;
 202
 203   // Reorder the vertices
 204
 205   if(a < 0) {
 206     a = -a;
 207     scalar_t x, y;
 208     for(int n = 0; n < _nb_vertices / 2; n++) {
 209       x = _relative_x[n];
 210       y = _relative_y[n];
 211       _relative_x[n] = _relative_x[_nb_vertices - 1 - n];
 212       _relative_y[n] = _relative_y[_nb_vertices - 1 - n];
 213       _relative_x[_nb_vertices - 1 - n] = x;
 214       _relative_y[_nb_vertices - 1 - n] = y;
 215     }
 216   }
 217
 218   // Compute the center of mass and moment of inertia
 219
 220   scalar_t sx, sy, w;
 221   w = 0;
 222   sx = 0;
 223   sy = 0;
 224   for(int n = 0; n < _nb_vertices; n++) {
 225     int np = (n+1)%_nb_vertices;
 226     w =_relative_x[n] * _relative_y[np] - _relative_x[np] * _relative_y[n];
 227     sx += (_relative_x[n] + _relative_x[np]) * w;
 228     sy += (_relative_y[n] + _relative_y[np]) * w;
 229   }
 230   sx /= 6 * a;
 231   sy /= 6 * a;
 232
 233   _radius = 0;
 234   for(int n = 0; n < _nb_vertices; n++) {
 235     _relative_x[n] -= sx;
 236     _relative_y[n] -= sy;
 237     scalar_t r = sqrt(sq(_relative_x[n]) + sq(_relative_y[n]));
 238     if(r > _radius) _radius = r;
 239   }
 240
 241   scalar_t num = 0, den = 0;
 242   for(int n = 0; n < _nb_vertices; n++) {
 243     int np = (n+1)%_nb_vertices;
 244     den += abs(prod_vect(_relative_x[np], _relative_y[np], _relative_x[n], _relative_y[n]));
 245     num += abs(prod_vect(_relative_x[np], _relative_y[np], _relative_x[n], _relative_y[n])) *
 246       (_relative_x[np] * _relative_x[np] + _relative_y[np] * _relative_y[np] +
 247        _relative_x[np] * _relative_x[n] + _relative_y[np] * _relative_y[n] +
 248        _relative_x[n] * _relative_x[n] + _relative_y[n] * _relative_y[n]);
 249   }
 250
 251   _moment_of_inertia = num / (6 * den);
 252
 253   scalar_t vx = cos(_theta), vy = sin(_theta);
 254
 255   for(int n = 0; n < _nb_vertices; n++) {
 256     _x[n] = _center_x + _relative_x[n] * vx + _relative_y[n] * vy;
 257     _y[n] = _center_y - _relative_x[n] * vy + _relative_y[n] * vx;
 258   }
 259
 260   scalar_t length;
 261   _total_nb_dots = 0;
 262   for(int n = 0; n < _nb_vertices; n++) {
 263     length = sqrt(sq(_relative_x[n] - _relative_x[(n+1)%_nb_vertices]) +
 264                   sq(_relative_y[n] - _relative_y[(n+1)%_nb_vertices]));
 265     _length[n] = length;
 266     _nb_dots[n] = int(length / dl + 1);
 267     _total_nb_dots += _nb_dots[n];
 268   }
 269
 270   delete[] _effecting_edge;
 271   _effecting_edge = new int[_nb_polygons * _total_nb_dots];
 272   for(int p = 0; p < _nb_polygons * _total_nb_dots; p++) _effecting_edge[p] = -1;
 273
 274   int nt = 0;
 275   int index[_nb_vertices];
 276   for(int n = 0; n < _nb_vertices; n++) index[n] = n;
 277   triangularize(nt, _nb_vertices, index);
 278
 279   _initialized = true;
 280 }
 281
 282 bool Polygon::update(scalar_t dt) {
 283   if(!_nailed) {
 284     _center_x += _dcenter_x * dt;
 285     _center_y += _dcenter_y * dt;
 286     _theta += _dtheta * dt;
 287   }
 288
 289   scalar_t d = exp(log(dissipation) * dt);
 290   _dcenter_x *= d;
 291   _dcenter_y *= d;
 292   _dtheta *= d;
 293
 294   scalar_t vx = cos(_theta), vy = sin(_theta);
 295
 296   for(int n = 0; n < _nb_vertices; n++) {
 297     _x[n] = _center_x + _relative_x[n] * vx + _relative_y[n] * vy;
 298     _y[n] = _center_y - _relative_x[n] * vy + _relative_y[n] * vx;
 299   }
 300
 301   if(abs(_center_x - _last_center_x) +
 302      abs(_center_y - _last_center_y) +
 303      abs(_theta - _last_theta) * _radius > 0.1) {
 304     _last_center_x = _center_x;
 305     _last_center_y = _center_y;
 306     _last_theta = _theta;
 307     return true;
 308   } else return false;
 309 }
 310
 311 scalar_t Polygon::relative_x(scalar_t ax, scalar_t ay) {
 312   return (ax - _center_x) * cos(_theta) - (ay - _center_y) * sin(_theta);
 313 }
 314
 315 scalar_t Polygon::relative_y(scalar_t ax, scalar_t ay) {
 316   return (ax - _center_x) * sin(_theta) + (ay - _center_y) * cos(_theta);
 317 }
 318
 319 scalar_t Polygon::absolute_x(scalar_t rx, scalar_t ry) {
 320   return _center_x + rx * cos(_theta) + ry * sin(_theta);
 321 }
 322
 323 scalar_t Polygon::absolute_y(scalar_t rx, scalar_t ry) {
 324   return _center_y - rx * sin(_theta) + ry * cos(_theta);
 325 }
 326
 327 void Polygon::apply_force(scalar_t dt, scalar_t x, scalar_t y, scalar_t fx, scalar_t fy) {
 328   _dcenter_x += fx / _mass * dt;
 329   _dcenter_y += fy / _mass * dt;
 330   _dtheta -= prod_vect(x - _center_x, y - _center_y, fx, fy) / (_mass * _moment_of_inertia) * dt;
 331 }
 332
 333 void Polygon::apply_border_forces(scalar_t dt, scalar_t xmax, scalar_t ymax) {
 334   for(int v = 0; v < _nb_vertices; v++) {
 335     int vp = (v+1)%_nb_vertices;
 336     for(int d = 0; d < _nb_dots[v]; d++) {
 337       scalar_t s = scalar_t(d * dl)/_length[v];
 338       scalar_t x = _x[v] * (1 - s) + _x[vp] * s;
 339       scalar_t y = _y[v] * (1 - s) + _y[vp] * s;
 340       scalar_t vx = 0, vy = 0;
 341       if(x < 0) vx = x;
 342       else if(x > xmax) vx = x - xmax;
 343       if(y < 0) vy = y;
 344       else if(y > ymax) vy = y - ymax;
 345       apply_force(dt, x, y, - dl * vx * repulsion_constant, - dl * vy * repulsion_constant);
 346     }
 347   }
 348 }
 349
 350 void Polygon::apply_collision_forces(scalar_t dt, int n_polygon, Polygon *p) {
 351   scalar_t closest_x[_total_nb_dots], closest_y[_total_nb_dots];
 352   bool inside[_total_nb_dots];
 353   scalar_t distance[_total_nb_dots];
 354   int _new_effecting_edge[_total_nb_dots];
 355
 356   int first_dot = 0;
 357
 358   for(int v = 0; v < _nb_vertices; v++) {
 359     int vp = (v+1)%_nb_vertices;
 360     scalar_t x = _x[v], y = _y[v], xp = _x[vp], yp = _y[vp];
 361
 362     for(int d = 0; d < _nb_dots[v]; d++) {
 363       inside[d] = false;
 364       distance[d] = FLT_MAX;
 365     }
 366
 367     // First, we tag the dots located inside the polygon p
 368
 369     for(int t = 0; t < p->_nb_vertices-2; t++) {
 370       scalar_t min = 0, max = 1;
 371       scalar_t xa = p->_x[p->_triangles[t].a], ya = p->_y[p->_triangles[t].a];
 372       scalar_t xb = p->_x[p->_triangles[t].b], yb = p->_y[p->_triangles[t].b];
 373       scalar_t xc = p->_x[p->_triangles[t].c], yc = p->_y[p->_triangles[t].c];
 374       scalar_t den, num;
 375
 376       const scalar_t eps = 1e-6;
 377
 378       den = prod_vect(xp - x, yp - y, xb - xa, yb - ya);
 379       num = prod_vect(xa - x, ya - y, xb - xa, yb - ya);
 380       if(den > eps) {
 381         if(num / den < max) max = num / den;
 382       } else if(den < -eps) {
 383         if(num / den > min) min = num / den;
 384       } else {
 385         if(num < 0) { min = 1; max = 0; }
 386       }
 387
 388       den = prod_vect(xp - x, yp - y, xc - xb, yc - yb);
 389       num = prod_vect(xb - x, yb - y, xc - xb, yc - yb);
 390       if(den > eps) {
 391         if(num / den < max) max = num / den;
 392       } else if(den < -eps) {
 393         if(num / den > min) min = num / den;
 394       } else {
 395         if(num < 0) { min = 1; max = 0; }
 396       }
 397
 398       den = prod_vect(xp - x, yp - y, xa - xc, ya - yc);
 399       num = prod_vect(xc - x, yc - y, xa - xc, ya - yc);
 400       if(den > eps) {
 401         if(num / den < max) max = num / den;
 402       } else if(den < -eps) {
 403         if(num / den > min) min = num / den;
 404       } else {
 405         if(num < 0) { min = 1; max = 0; }
 406       }
 407
 408       for(int d = 0; d < _nb_dots[v]; d++) {
 409         scalar_t s = scalar_t(d * dl)/_length[v];
 410         if(s >= min && s <= max) inside[d] = true;
 411       }
 412     }
 413
 414     // Then, we compute for each dot what is the closest point on
 415     // the boundary of p
 416
 417     for(int m = 0; m < p->_nb_vertices; m++) {
 418       int mp = (m+1)%p->_nb_vertices;
 419       scalar_t xa = p->_x[m], ya = p->_y[m];
 420       scalar_t xb = p->_x[mp], yb = p->_y[mp];
 421       scalar_t gamma0 = ((x - xa) * (xb - xa) + (y - ya) * (yb - ya)) / sq(p->_length[m]);
 422       scalar_t gamma1 = ((xp - x) * (xb - xa) + (yp - y) * (yb - ya)) / sq(p->_length[m]);
 423       scalar_t delta0 = (prod_vect(xb - xa, yb - ya, x - xa, y - ya)) / p->_length[m];
 424       scalar_t delta1 = (prod_vect(xb - xa, yb - ya, xp - x, yp - y)) / p->_length[m];
 425
 426       for(int d = 0; d < _nb_dots[v]; d++) if(inside[d]) {
 427         int r = _effecting_edge[(first_dot + d) * _nb_polygons + n_polygon];
 428
 429         // If there is already a spring, we look only at the
 430         // vertices next to the current one
 431
 432         if(r < 0 || m == r || m == (r+1)%p->_nb_vertices || (m+1)%p->_nb_vertices == r) {
 433
 434           scalar_t s = scalar_t(d * dl)/_length[v];
 435           scalar_t delta = abs(s * delta1 + delta0);
 436           if(delta < distance[d]) {
 437             scalar_t gamma = s * gamma1 + gamma0;
 438             if(gamma < 0) {
 439               scalar_t l = sqrt(sq(x * (1 - s) + xp * s - xa) + sq(y * (1 - s) + yp * s - ya));
 440               if(l < distance[d]) {
 441                 distance[d] = l;
 442                 closest_x[d] = xa;
 443                 closest_y[d] = ya;
 444                 _new_effecting_edge[first_dot + d] = m;
 445               }
 446             } else if(gamma > 1) {
 447               scalar_t l = sqrt(sq(x * (1 - s) + xp * s - xb) + sq(y * (1 - s) + yp * s - yb));
 448               if(l < distance[d]) {
 449                 distance[d] = l;
 450                 closest_x[d] = xb;
 451                 closest_y[d] = yb;
 452                 _new_effecting_edge[first_dot + d] = m;
 453               }
 454             } else {
 455               distance[d] = delta;
 456               closest_x[d] = xa * (1 - gamma) + xb * gamma;
 457               closest_y[d] = ya * (1 - gamma) + yb * gamma;
 458               _new_effecting_edge[first_dot + d] = m;
 459             }
 460           }
 461         }
 462       } else _new_effecting_edge[first_dot + d] = -1;
 463     }
 464
 465     // Apply forces
 466
 467     for(int d = 0; d < _nb_dots[v]; d++) if(inside[d]) {
 468       scalar_t s = scalar_t(d * dl)/_length[v];
 469       scalar_t x = _x[v] * (1 - s) + _x[vp] * s;
 470       scalar_t y = _y[v] * (1 - s) + _y[vp] * s;
 471       scalar_t vx = x - closest_x[d];
 472       scalar_t vy = y - closest_y[d];
 473
 474       p->apply_force(dt,
 475                      closest_x[d], closest_y[d],
 476                      dl * vx  * repulsion_constant, dl * vy * repulsion_constant);
 477
 478       apply_force(dt,
 479                   x, y,
 480                   - dl * vx * repulsion_constant, - dl * vy * repulsion_constant);
 481     }
 482
 483     first_dot += _nb_dots[v];
 484   }
 485
 486   for(int d = 0; d < _total_nb_dots; d++)
 487     _effecting_edge[d * _nb_polygons + n_polygon] = _new_effecting_edge[d];
 488
 489 }
 490
 491 bool Polygon::collide(Polygon *p) {
 492   for(int n = 0; n < _nb_vertices; n++) {
 493     int np = (n+1)%_nb_vertices;
 494     for(int m = 0; m < p->_nb_vertices; m++) {
 495       int mp = (m+1)%p->_nb_vertices;
 496       scalar_t det, s = -1, t = -1;
 497       intersection(_x[n], _y[n], _x[np], _y[np],
 498                    p->_x[m], p->_y[m], p->_x[mp], p->_y[mp], det, s, t);
 499       if(det != 0 && s>= 0 && s <= 1&& t >= 0 && t <= 1) return true;
 500     }
 501   }
 502
 503   for(int n = 0; n < _nb_vertices; n++) if(p->contain(_x[n], _y[n])) return true;
 504   for(int n = 0; n < p->_nb_vertices; n++) if(contain(p->_x[n], p->_y[n])) return true;
 505
 506   return false;
 507 }