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