Update.
[flatland.git] / universe.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 <string.h>
26
27 #include "universe.h"
28
29 Universe::Universe(int nb_max_polygons,
30                    scalar_t width, scalar_t height) : _width(width),
31                                                       _height(height),
32                                                       _nb_max_polygons(nb_max_polygons),
33                                                       _nb_polygons(0) {
34   _polygons = new Polygon *[_nb_max_polygons];
35   for(int n = 0; n < _nb_max_polygons; n++) _polygons[n] = 0;
36 }
37
38 Universe::~Universe() {
39   clear();
40   delete[] _polygons;
41 }
42
43 void Universe::initialize_polygon(Polygon *p) {
44   p->initialize(_nb_max_polygons);
45 }
46
47 void Universe::clear() {
48   for(int n = 0; n < _nb_polygons; n++) if(_polygons[n]) delete _polygons[n];
49   _nb_polygons = 0;
50 }
51
52 void Universe::add_polygon(Polygon *p) {
53   if(_nb_polygons < _nb_max_polygons) {
54     if(!p->_initialized) {
55       cerr << "You can not add a non-initialized polygon." << endl;
56       abort();
57     }
58     _polygons[_nb_polygons++] = p;
59   } else {
60     cerr << "Too many polygons!" << endl;
61     exit(1);
62   }
63 }
64
65 bool Universe::collide(Polygon *p) {
66   for(int n = 0; n < _nb_polygons; n++)
67     if(_polygons[n] && _polygons[n]->collide(p)) return true;
68
69   return false;
70 }
71
72 void Universe::compute_pseudo_collisions(int nb_axis, int *nb_colliding_axis) {
73   Couple couples[_nb_polygons * 2];
74   int in[_nb_polygons];
75   memset((void *) nb_colliding_axis, 0, _nb_polygons * _nb_polygons * sizeof(int));
76
77   for(int a = 0; a < nb_axis; a++) {
78     scalar_t alpha = M_PI * scalar_t(a) / scalar_t(nb_axis);
79     scalar_t vx = cos(alpha), vy = sin(alpha);
80
81     for(int n = 0; n < _nb_polygons; n++) {
82       scalar_t *x = _polygons[n]->_x, *y = _polygons[n]->_y;
83       scalar_t min = x[0] * vx + y[0] * vy, max = min;
84
85       for(int v = 1; v < _polygons[n]->_nb_vertices; v++) {
86         scalar_t s = x[v] * vx + y[v] * vy;
87         if(s > max) max = s;
88         if(s < min) min = s;
89       }
90
91       couples[2 * n + 0].value = min;
92       couples[2 * n + 0].index = n;
93       couples[2 * n + 1].value = max;
94       couples[2 * n + 1].index = n;
95     }
96
97     qsort((void *) couples, 2 * _nb_polygons, sizeof(Couple), compare_couple);
98
99     int nb_in = 0;
100     memset((void *) in, 0, _nb_polygons * sizeof(int));
101     for(int k = 0; k < 2 * _nb_polygons; k++) {
102       int i = couples[k].index;
103       in[i] = !in[i];
104       if(in[i]) {
105         nb_in++;
106         if(nb_in > 1) {
107           for(int j = 0; j < i; j++)
108             if(j != i && in[j]) nb_colliding_axis[j + i * _nb_polygons]++;
109           for(int j = i+1; j < _nb_polygons; j++)
110             if(j != i && in[j]) nb_colliding_axis[i + j * _nb_polygons]++;
111         }
112       } else nb_in--;
113     }
114   }
115
116   for(int i = 0; i < _nb_polygons; i++) {
117     for(int j = 0; j < i; j++) {
118       if(nb_colliding_axis[j + i * _nb_polygons] > nb_colliding_axis[i + i * _nb_polygons])
119         nb_colliding_axis[i + i * _nb_polygons] = nb_colliding_axis[j + i * _nb_polygons];
120       nb_colliding_axis[i + j * _nb_polygons] = nb_colliding_axis[j + i * _nb_polygons];
121     }
122   }
123 }
124
125 bool Universe::update(scalar_t dt, scalar_t padding) {
126   bool result = false;
127   apply_collision_forces(dt);
128   for(int n = 0; n < _nb_polygons; n++) if(_polygons[n]) {
129       _polygons[n]->apply_border_forces(dt,
130                                         padding, padding,
131                                         _width - padding, _height - padding);
132     result |= _polygons[n]->update(dt);
133   }
134   return result;
135 }
136
137 Polygon *Universe::pick_polygon(scalar_t x, scalar_t y) {
138   for(int n = 0; n < _nb_polygons; n++)
139     if(_polygons[n] && _polygons[n]->contain(x, y)) return _polygons[n];
140   return 0;
141 }
142
143 void Universe::draw(Canvas *canvas) {
144   for(int n = 0; n < _nb_polygons; n++) {
145     if(_polygons[n]) {
146       _polygons[n]->draw(canvas);
147     }
148   }
149
150   for(int n = 0; n < _nb_polygons; n++) {
151     if(_polygons[n]) {
152       _polygons[n]->draw_contours(canvas);
153     }
154   }
155 }
156
157 void Universe::apply_collision_forces(scalar_t dt) {
158   const int nb_axis = 2;
159   int nb_collision[_nb_polygons * _nb_polygons];
160
161   compute_pseudo_collisions(nb_axis, nb_collision);
162
163   for(int n = 0; n < _nb_polygons; n++) if(_polygons[n])
164     for(int m = 0; m < _nb_polygons; m++)
165       if(m != n && _polygons[m] && nb_collision[n + _nb_polygons * m] == nb_axis)
166         _polygons[n]->apply_collision_forces(dt, m, _polygons[m]);
167 }