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