4 flatland is a simple 2d physical simulator
6 Copyright (c) 2016 Idiap Research Institute, http://www.idiap.ch/
7 Written by Francois Fleuret <francois.fleuret@idiap.ch>
9 This file is part of flatland
11 flatland 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.
15 flatland 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.
20 You should have received a copy of the GNU General Public License
21 along with flatland. If not, see <http://www.gnu.org/licenses/>.
32 static const scalar_t dl = 20.0;
33 static const scalar_t repulsion_constant = 0.2;
34 static const scalar_t speed_max = 1e2;
35 static const scalar_t angular_speed_max = M_PI / 10;
37 Polygon::Polygon(scalar_t mass,
38 scalar_t red, scalar_t green, scalar_t blue,
39 scalar_t *x, scalar_t *y,
40 int nv) : _mass(mass),
41 _moment_of_inertia(0), _radius(0),
42 _relative_x(new scalar_t[nv]), _relative_y(new scalar_t[nv]),
44 _nb_dots(new int[nv]),
46 _length(new scalar_t[nv]),
47 _triangles(new Triangle[nv-2]),
48 _initialized(false), _nailed(false),
50 _x(new scalar_t[nv]), _y(new scalar_t[nv]),
51 _red(red), _green(green), _blue(blue) {
56 if(x) for(int i = 0; i < nv; i++) _relative_x[i] = x[i];
57 if(y) for(int i = 0; i < nv; i++) _relative_y[i] = y[i];
68 delete[] _effecting_edge;
71 Polygon *Polygon::clone() {
72 return new Polygon(_mass, _red, _green, _blue, _relative_x, _relative_y, _nb_vertices);
76 void Polygon::color_xfig(XFigTracer *tracer) {
77 tracer->add_color(int(255 * _red), int(255 * _green), int(255 * _blue));
80 void Polygon::print_xfig(XFigTracer *tracer) {
81 tracer->draw_polygon(int(255 * _red), int(255 * _green), int(255 * _blue),
82 _nb_vertices, _x, _y);
87 void Polygon::draw(SimpleWindow *window) {
88 window->color(_red, _green, _blue);
89 int x[_nb_vertices], y[_nb_vertices];
90 for(int n = 0; n < _nb_vertices; n++) {
94 window->fill_polygon(_nb_vertices, x, y);
97 void Polygon::draw_contours(SimpleWindow *window) {
98 int x[_nb_vertices], y[_nb_vertices];
99 for(int n = 0; n < _nb_vertices; n++) {
103 window->color(0.0, 0.0, 0.0);
104 // window->color(1.0, 1.0, 1.0);
105 for(int n = 0; n < _nb_vertices; n++) {
106 window->draw_line(x[n], y[n], x[(n+1)%_nb_vertices], y[(n+1)%_nb_vertices]);
111 void Polygon::draw(Canvas *canvas) {
112 canvas->set_drawing_color(_red, _green, _blue);
113 canvas->draw_polygon(1, _nb_vertices, _x, _y);
116 void Polygon::draw_contours(Canvas *canvas) {
117 canvas->set_drawing_color(0.0, 0.0, 0.0);
118 canvas->draw_polygon(0, _nb_vertices, _x, _y);
121 void Polygon::set_vertex(int k, scalar_t x, scalar_t y) {
126 void Polygon::set_position(scalar_t center_x, scalar_t center_y, scalar_t theta) {
127 _center_x = center_x;
128 _center_y = center_y;
132 void Polygon::set_speed(scalar_t dcenter_x, scalar_t dcenter_y, scalar_t dtheta) {
133 _dcenter_x = dcenter_x;
134 _dcenter_y = dcenter_y;
138 bool Polygon::contain(scalar_t x, scalar_t y) {
139 for(int t = 0; t < _nb_vertices-2; t++) {
140 scalar_t xa = _x[_triangles[t].a], ya = _y[_triangles[t].a];
141 scalar_t xb = _x[_triangles[t].b], yb = _y[_triangles[t].b];
142 scalar_t xc = _x[_triangles[t].c], yc = _y[_triangles[t].c];
143 if(prod_vect(x - xa, y - ya, xb - xa, yb - ya) <= 0 &&
144 prod_vect(x - xb, y - yb, xc - xb, yc - yb) <= 0 &&
145 prod_vect(x - xc, y - yc, xa - xc, ya - yc) <= 0) return true;
150 void Polygon::triangularize(int &nt, int nb, int *index) {
153 if(nt >= _nb_vertices-2) {
154 cerr << "Error type #1 in triangularization." << endl;
158 _triangles[nt].a = index[0];
159 _triangles[nt].b = index[1];
160 _triangles[nt].c = index[2];
164 int best_m = -1, best_n = -1;
165 scalar_t best_split = -1, det, s = -1, t = -1;
167 for(int n = 0; n < nb; n++) for(int m = 0; m < n; m++) if(n > m+1 && m+nb > n+1) {
168 bool no_intersection = true;
169 for(int k = 0; no_intersection & (k < nb); k++)
170 if(k != n && k != m && (k+1)%nb != n && (k+1)%nb != m) {
171 intersection(_relative_x[index[n]], _relative_y[index[n]],
172 _relative_x[index[m]], _relative_y[index[m]],
173 _relative_x[index[k]], _relative_y[index[k]],
174 _relative_x[index[(k+1)%nb]], _relative_y[index[(k+1)%nb]], det, s, t);
175 no_intersection = det == 0 || s < 0 || s > 1 || t < 0 || t > 1;
178 if(no_intersection) {
179 scalar_t a1 = 0, a2 = 0;
180 for(int k = 0; k < nb; k++) if(k >= m && k < n)
181 a1 += prod_vect(_relative_x[index[k]] - _relative_x[index[m]],
182 _relative_y[index[k]] - _relative_y[index[m]],
183 _relative_x[index[k+1]] - _relative_x[index[m]],
184 _relative_y[index[k+1]] - _relative_y[index[m]]);
186 a2 += prod_vect(_relative_x[index[k]] - _relative_x[index[m]],
187 _relative_y[index[k]] - _relative_y[index[m]],
188 _relative_x[index[(k+1)%nb]] - _relative_x[index[m]],
189 _relative_y[index[(k+1)%nb]] - _relative_y[index[m]]);
191 if((a1 * a2 > 0 && best_split < 0) || (abs(a1 - a2) < best_split)) {
192 best_n = n; best_m = m;
193 best_split = abs(a1 - a2);
198 if(best_n >= 0 && best_m >= 0) {
199 int index_neg[nb], index_pos[nb];
200 int neg = 0, pos = 0;
201 for(int k = 0; k < nb; k++) {
202 if(k >= best_m && k <= best_n) index_pos[pos++] = index[k];
203 if(k <= best_m || k >= best_n) index_neg[neg++] = index[k];
205 if(pos < 3 || neg < 3) {
206 cerr << "Error type #2 in triangularization." << endl;
209 triangularize(nt, pos, index_pos);
210 triangularize(nt, neg, index_neg);
212 cerr << "Error type #3 in triangularization." << endl;
218 void Polygon::initialize(int nb_polygons) {
221 _nb_polygons = nb_polygons;
223 a = _relative_x[_nb_vertices - 1] * _relative_y[0]
224 - _relative_x[0] * _relative_y[_nb_vertices - 1];
226 for(int n = 0; n < _nb_vertices - 1; n++)
227 a += _relative_x[n] * _relative_y[n+1] - _relative_x[n+1] * _relative_y[n];
230 // Reorder the vertices
235 for(int n = 0; n < _nb_vertices / 2; n++) {
238 _relative_x[n] = _relative_x[_nb_vertices - 1 - n];
239 _relative_y[n] = _relative_y[_nb_vertices - 1 - n];
240 _relative_x[_nb_vertices - 1 - n] = x;
241 _relative_y[_nb_vertices - 1 - n] = y;
245 // Compute the center of mass and moment of inertia
251 for(int n = 0; n < _nb_vertices; n++) {
252 int np = (n+1)%_nb_vertices;
253 w =_relative_x[n] * _relative_y[np] - _relative_x[np] * _relative_y[n];
254 sx += (_relative_x[n] + _relative_x[np]) * w;
255 sy += (_relative_y[n] + _relative_y[np]) * w;
261 for(int n = 0; n < _nb_vertices; n++) {
262 _relative_x[n] -= sx;
263 _relative_y[n] -= sy;
264 scalar_t r = sqrt(sq(_relative_x[n]) + sq(_relative_y[n]));
265 if(r > _radius) _radius = r;
268 scalar_t num = 0, den = 0;
269 for(int n = 0; n < _nb_vertices; n++) {
270 int np = (n+1)%_nb_vertices;
271 den += abs(prod_vect(_relative_x[np], _relative_y[np], _relative_x[n], _relative_y[n]));
272 num += abs(prod_vect(_relative_x[np], _relative_y[np], _relative_x[n], _relative_y[n])) *
273 (_relative_x[np] * _relative_x[np] + _relative_y[np] * _relative_y[np] +
274 _relative_x[np] * _relative_x[n] + _relative_y[np] * _relative_y[n] +
275 _relative_x[n] * _relative_x[n] + _relative_y[n] * _relative_y[n]);
278 _moment_of_inertia = num / (6 * den);
280 scalar_t vx = cos(_theta), vy = sin(_theta);
282 for(int n = 0; n < _nb_vertices; n++) {
283 _x[n] = _center_x + _relative_x[n] * vx + _relative_y[n] * vy;
284 _y[n] = _center_y - _relative_x[n] * vy + _relative_y[n] * vx;
289 for(int n = 0; n < _nb_vertices; n++) {
290 length = sqrt(sq(_relative_x[n] - _relative_x[(n+1)%_nb_vertices]) +
291 sq(_relative_y[n] - _relative_y[(n+1)%_nb_vertices]));
293 _nb_dots[n] = int(length / dl + 1);
294 _total_nb_dots += _nb_dots[n];
297 delete[] _effecting_edge;
298 _effecting_edge = new int[_nb_polygons * _total_nb_dots];
299 for(int p = 0; p < _nb_polygons * _total_nb_dots; p++) _effecting_edge[p] = -1;
302 int index[_nb_vertices];
303 for(int n = 0; n < _nb_vertices; n++) index[n] = n;
304 triangularize(nt, _nb_vertices, index);
309 bool Polygon::update(scalar_t dt) {
310 scalar_t speed = sqrt(_dcenter_x * _dcenter_x + _dcenter_y * _dcenter_y);
313 scalar_t speed_target = speed_max - exp(-speed / speed_max) * speed_max;
314 _dcenter_x = speed_target * _dcenter_x / speed;
315 _dcenter_y = speed_target * _dcenter_y / speed;
318 scalar_t angular_speed = abs(_dtheta);
320 if(angular_speed > 0) {
321 scalar_t angular_speed_target = angular_speed_max - exp(-angular_speed / angular_speed_max) * angular_speed_max;
322 _dtheta = angular_speed_target * _dtheta / angular_speed;
326 _center_x += _dcenter_x * dt;
327 _center_y += _dcenter_y * dt;
328 _theta += _dtheta * dt;
331 scalar_t vx = cos(_theta), vy = sin(_theta);
333 for(int n = 0; n < _nb_vertices; n++) {
334 _x[n] = _center_x + _relative_x[n] * vx + _relative_y[n] * vy;
335 _y[n] = _center_y - _relative_x[n] * vy + _relative_y[n] * vx;
338 if(abs(_center_x - _last_center_x) +
339 abs(_center_y - _last_center_y) +
340 abs(_theta - _last_theta) * _radius > 0.1) {
341 _last_center_x = _center_x;
342 _last_center_y = _center_y;
343 _last_theta = _theta;
348 scalar_t Polygon::relative_x(scalar_t ax, scalar_t ay) {
349 return (ax - _center_x) * cos(_theta) - (ay - _center_y) * sin(_theta);
352 scalar_t Polygon::relative_y(scalar_t ax, scalar_t ay) {
353 return (ax - _center_x) * sin(_theta) + (ay - _center_y) * cos(_theta);
356 scalar_t Polygon::absolute_x(scalar_t rx, scalar_t ry) {
357 return _center_x + rx * cos(_theta) + ry * sin(_theta);
360 scalar_t Polygon::absolute_y(scalar_t rx, scalar_t ry) {
361 return _center_y - rx * sin(_theta) + ry * cos(_theta);
364 void Polygon::apply_force(scalar_t dt, scalar_t x, scalar_t y, scalar_t fx, scalar_t fy) {
365 _dcenter_x += fx / _mass * dt;
366 _dcenter_y += fy / _mass * dt;
367 _dtheta -= prod_vect(x - _center_x, y - _center_y, fx, fy) / (_mass * _moment_of_inertia) * dt;
370 void Polygon::apply_border_forces(scalar_t dt,
371 scalar_t xmin, scalar_t ymin,
372 scalar_t xmax, scalar_t ymax) {
373 for(int v = 0; v < _nb_vertices; v++) {
374 int vp = (v+1)%_nb_vertices;
375 for(int d = 0; d < _nb_dots[v]; d++) {
376 scalar_t s = scalar_t(d * dl)/_length[v];
377 scalar_t x = _x[v] * (1 - s) + _x[vp] * s;
378 scalar_t y = _y[v] * (1 - s) + _y[vp] * s;
379 scalar_t vx = 0, vy = 0;
380 if(x < xmin) vx = x - xmin;
381 else if(x > xmax) vx = x - xmax;
382 if(y < ymin) vy = y - ymin;
383 else if(y > ymax) vy = y - ymax;
384 if(vx != 0 || vy != 0) {
385 // cerr << "apply_border_forces vx = " << vx << " vy = " << vy << endl;
386 apply_force(dt, x, y,
387 - dl * vx * repulsion_constant, - dl * vy * repulsion_constant);
393 void Polygon::apply_collision_forces(scalar_t dt, int n_polygon, Polygon *p) {
394 scalar_t closest_x[_total_nb_dots], closest_y[_total_nb_dots];
395 bool inside[_total_nb_dots];
396 scalar_t distance[_total_nb_dots];
397 int _new_effecting_edge[_total_nb_dots];
401 for(int v = 0; v < _nb_vertices; v++) {
402 int vp = (v+1)%_nb_vertices;
403 scalar_t x = _x[v], y = _y[v], xp = _x[vp], yp = _y[vp];
405 for(int d = 0; d < _nb_dots[v]; d++) {
407 distance[d] = FLT_MAX;
410 // First, we tag the dots located inside the polygon p by looping
411 // through the _nb_vertices - 2 triangles from the decomposition
413 for(int t = 0; t < p->_nb_vertices - 2; t++) {
414 scalar_t min = 0, max = 1;
415 scalar_t xa = p->_x[p->_triangles[t].a], ya = p->_y[p->_triangles[t].a];
416 scalar_t xb = p->_x[p->_triangles[t].b], yb = p->_y[p->_triangles[t].b];
417 scalar_t xc = p->_x[p->_triangles[t].c], yc = p->_y[p->_triangles[t].c];
420 const scalar_t eps = 1e-6;
422 den = prod_vect(xp - x, yp - y, xb - xa, yb - ya);
423 num = prod_vect(xa - x, ya - y, xb - xa, yb - ya);
425 if(num / den < max) max = num / den;
426 } else if(den < -eps) {
427 if(num / den > min) min = num / den;
429 if(num < 0) { min = 1; max = 0; }
432 den = prod_vect(xp - x, yp - y, xc - xb, yc - yb);
433 num = prod_vect(xb - x, yb - y, xc - xb, yc - yb);
435 if(num / den < max) max = num / den;
436 } else if(den < -eps) {
437 if(num / den > min) min = num / den;
439 if(num < 0) { min = 1; max = 0; }
442 den = prod_vect(xp - x, yp - y, xa - xc, ya - yc);
443 num = prod_vect(xc - x, yc - y, xa - xc, ya - yc);
445 if(num / den < max) max = num / den;
446 } else if(den < -eps) {
447 if(num / den > min) min = num / den;
449 if(num < 0) { min = 1; max = 0; }
452 for(int d = 0; d < _nb_dots[v]; d++) {
453 scalar_t s = scalar_t(d * dl)/_length[v];
454 if(s >= min && s <= max) inside[d] = true;
458 // Then, we compute for each dot what is the closest point on
461 for(int m = 0; m < p->_nb_vertices; m++) {
462 int mp = (m+1)%p->_nb_vertices;
463 scalar_t xa = p->_x[m], ya = p->_y[m];
464 scalar_t xb = p->_x[mp], yb = p->_y[mp];
465 scalar_t gamma0 = ((x - xa) * (xb - xa) + (y - ya) * (yb - ya)) / sq(p->_length[m]);
466 scalar_t gamma1 = ((xp - x) * (xb - xa) + (yp - y) * (yb - ya)) / sq(p->_length[m]);
467 scalar_t delta0 = (prod_vect(xb - xa, yb - ya, x - xa, y - ya)) / p->_length[m];
468 scalar_t delta1 = (prod_vect(xb - xa, yb - ya, xp - x, yp - y)) / p->_length[m];
470 for(int d = 0; d < _nb_dots[v]; d++) if(inside[d]) {
471 int r = _effecting_edge[(first_dot + d) * _nb_polygons + n_polygon];
473 // If there is already a spring, we look only at the
474 // vertices next to the current one
476 if(r < 0 || m == r || m == (r+1)%p->_nb_vertices || (m+1)%p->_nb_vertices == r) {
478 scalar_t s = scalar_t(d * dl)/_length[v];
479 scalar_t delta = abs(s * delta1 + delta0);
480 if(delta < distance[d]) {
481 scalar_t gamma = s * gamma1 + gamma0;
483 scalar_t l = sqrt(sq(x * (1 - s) + xp * s - xa) + sq(y * (1 - s) + yp * s - ya));
484 if(l < distance[d]) {
488 _new_effecting_edge[first_dot + d] = m;
490 } else if(gamma > 1) {
491 scalar_t l = sqrt(sq(x * (1 - s) + xp * s - xb) + sq(y * (1 - s) + yp * s - yb));
492 if(l < distance[d]) {
496 _new_effecting_edge[first_dot + d] = m;
500 closest_x[d] = xa * (1 - gamma) + xb * gamma;
501 closest_y[d] = ya * (1 - gamma) + yb * gamma;
502 _new_effecting_edge[first_dot + d] = m;
506 } else _new_effecting_edge[first_dot + d] = -1;
511 for(int d = 0; d < _nb_dots[v]; d++) if(inside[d]) {
512 scalar_t s = scalar_t(d * dl)/_length[v];
513 scalar_t x = _x[v] * (1 - s) + _x[vp] * s;
514 scalar_t y = _y[v] * (1 - s) + _y[vp] * s;
515 scalar_t vx = x - closest_x[d];
516 scalar_t vy = y - closest_y[d];
519 closest_x[d], closest_y[d],
520 dl * vx * repulsion_constant, dl * vy * repulsion_constant);
524 - dl * vx * repulsion_constant, - dl * vy * repulsion_constant);
527 first_dot += _nb_dots[v];
530 for(int d = 0; d < _total_nb_dots; d++)
531 _effecting_edge[d * _nb_polygons + n_polygon] = _new_effecting_edge[d];
535 bool Polygon::collide_with_borders(scalar_t xmin, scalar_t ymin,
536 scalar_t xmax, scalar_t ymax) {
537 for(int n = 0; n < _nb_vertices; n++) {
538 if(_x[n] <= xmin || _x[n] >= xmax || _y[n] <= ymin || _y[n] >= ymax) return true;
543 bool Polygon::collide(Polygon *p) {
544 for(int n = 0; n < _nb_vertices; n++) {
545 int np = (n+1)%_nb_vertices;
546 for(int m = 0; m < p->_nb_vertices; m++) {
547 int mp = (m+1)%p->_nb_vertices;
548 scalar_t det, s = -1, t = -1;
549 intersection(_x[n], _y[n], _x[np], _y[np],
550 p->_x[m], p->_y[m], p->_x[mp], p->_y[mp], det, s, t);
551 if(det != 0 && s>= 0 && s <= 1&& t >= 0 && t <= 1) return true;
555 for(int n = 0; n < _nb_vertices; n++) if(p->contain(_x[n], _y[n])) return true;
556 for(int n = 0; n < p->_nb_vertices; n++) if(contain(p->_x[n], p->_y[n])) return true;