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