From 8e60c68637e79580cdefa53edf79a4804849997d Mon Sep 17 00:00:00 2001 From: Francois Fleuret Date: Fri, 7 Oct 2016 12:03:24 +0200 Subject: [PATCH] Initial commit --- Makefile | 51 +++++ canvas.cc | 25 +++ canvas.h | 37 ++++ canvas_cairo.cc | 91 +++++++++ canvas_cairo.h | 48 +++++ flatland.cc | 283 +++++++++++++++++++++++++ misc.cc | 31 +++ misc.h | 53 +++++ polygon.cc | 533 ++++++++++++++++++++++++++++++++++++++++++++++++ polygon.h | 128 ++++++++++++ run.sh | 68 ++++++ universe.cc | 196 ++++++++++++++++++ universe.h | 78 +++++++ 13 files changed, 1622 insertions(+) create mode 100644 Makefile create mode 100644 canvas.cc create mode 100644 canvas.h create mode 100644 canvas_cairo.cc create mode 100644 canvas_cairo.h create mode 100644 flatland.cc create mode 100644 misc.cc create mode 100644 misc.h create mode 100644 polygon.cc create mode 100644 polygon.h create mode 100755 run.sh create mode 100644 universe.cc create mode 100644 universe.h diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..304f403 --- /dev/null +++ b/Makefile @@ -0,0 +1,51 @@ + +# dyncnn is a deep-learning algorithm for the prediction of +# interacting object dynamics +# +# Copyright (c) 2016 Idiap Research Institute, http://www.idiap.ch/ +# Written by Francois Fleuret +# +# This file is part of dyncnn. +# +# dyncnn is free software: you can redistribute it and/or modify it +# under the terms of the GNU General Public License version 3 as +# published by the Free Software Foundation. +# +# dyncnn is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +# General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with dyncnn. If not, see . + +ifeq ($(DEBUG),yes) + CXXFLAGS = -fPIC -Wall -g -DDEBUG +else + CXXFLAGS = -fPIC -Wall -g -O3 +endif + +LDFLAGS = -lz -ldl + +CXXFLAGS += -I/usr/include/cairo -DCAIRO_SUPPORT + +LDFLAGS += -lcairo + +all: flatland TAGS + +TAGS: *.cc *.h + etags *.cc *.h + +flatland: flatland.o misc.o \ + universe.o \ + polygon.o \ + canvas.o canvas_cairo.o + $(CXX) $(CXXFLAGS) $(LDFLAGS) -lX11 -o $@ $^ + +Makefile.depend: *.h *.cc Makefile + $(CC) $(CXXFLAGS) -M *.cc > Makefile.depend + +clean: + \rm -f flatland *.o *.so Makefile.depend + +-include Makefile.depend diff --git a/canvas.cc b/canvas.cc new file mode 100644 index 0000000..358bd7e --- /dev/null +++ b/canvas.cc @@ -0,0 +1,25 @@ + +/* + * dyncnn is a deep-learning algorithm for the prediction of + * interacting object dynamics + * + * Copyright (c) 2016 Idiap Research Institute, http://www.idiap.ch/ + * Written by Francois Fleuret + * + * This file is part of dyncnn. + * + * dyncnn is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 3 as + * published by the Free Software Foundation. + * + * dyncnn is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with dyncnn. If not, see . + * + */ + +#include "canvas.h" diff --git a/canvas.h b/canvas.h new file mode 100644 index 0000000..f54f6d0 --- /dev/null +++ b/canvas.h @@ -0,0 +1,37 @@ + +/* + * dyncnn is a deep-learning algorithm for the prediction of + * interacting object dynamics + * + * Copyright (c) 2016 Idiap Research Institute, http://www.idiap.ch/ + * Written by Francois Fleuret + * + * This file is part of dyncnn. + * + * dyncnn is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 3 as + * published by the Free Software Foundation. + * + * dyncnn is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with dyncnn. If not, see . + * + */ + +#ifndef CANVAS_H +#define CANVAS_H + +#include "misc.h" + +class Canvas { +public: + virtual void set_line_width(scalar_t w) = 0; + virtual void set_drawing_color(scalar_t r, scalar_t g, scalar_t b) = 0; + virtual void draw_polygon(int filled, int nb, scalar_t *x, scalar_t *y) = 0; +}; + +#endif diff --git a/canvas_cairo.cc b/canvas_cairo.cc new file mode 100644 index 0000000..ec26f60 --- /dev/null +++ b/canvas_cairo.cc @@ -0,0 +1,91 @@ + +/* + * dyncnn is a deep-learning algorithm for the prediction of + * interacting object dynamics + * + * Copyright (c) 2016 Idiap Research Institute, http://www.idiap.ch/ + * Written by Francois Fleuret + * + * This file is part of dyncnn. + * + * dyncnn is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 3 as + * published by the Free Software Foundation. + * + * dyncnn is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with dyncnn. If not, see . + * + */ + +#include "canvas_cairo.h" + +CanvasCairo::CanvasCairo(scalar_t scale, int width, int height) { + const int actual_width = int(width * scale); + const int actual_height = int(height * scale); + const int depth = 4; + + _data = new unsigned char [actual_width * actual_height * depth]; + + _image = cairo_image_surface_create_for_data(_data, + CAIRO_FORMAT_RGB24, + actual_width, + actual_height, + actual_width * depth); + + _context_resource = cairo_create(_image); + + cairo_scale(_context_resource, scale, scale); + + cairo_set_source_rgb(_context_resource, 1.0, 1.0, 1.0); + cairo_set_line_width (_context_resource, 1.0); + + cairo_rectangle(_context_resource, 0, 0, width, height); + + cairo_fill(_context_resource); +} + +CanvasCairo::~CanvasCairo() { + cairo_destroy(_context_resource); + cairo_surface_destroy(_image); + delete[] _data; +} + + +void CanvasCairo::set_line_width(scalar_t w) { + cairo_set_line_width (_context_resource, w); +} + +void CanvasCairo::set_drawing_color(scalar_t r, scalar_t g, scalar_t b) { + cairo_set_source_rgb(_context_resource, r, g, b); +} + +void CanvasCairo::draw_polygon(int filled, int nb, scalar_t *x, scalar_t *y) { + cairo_move_to(_context_resource, x[0], y[0]); + for(int n = 0; n < nb; n++) { + cairo_line_to(_context_resource, x[n], y[n]); + } + cairo_close_path(_context_resource); + + if(filled) { + cairo_stroke_preserve(_context_resource); + cairo_fill(_context_resource); + } else { + cairo_stroke(_context_resource); + } +} + +static cairo_status_t write_cairo_to_file(void *closure, + const unsigned char *data, + unsigned int length) { + fwrite(data, 1, length, (FILE *) closure); + return CAIRO_STATUS_SUCCESS; +} + +void CanvasCairo::write_png(FILE *file) { + cairo_surface_write_to_png_stream(_image, write_cairo_to_file, file); +} diff --git a/canvas_cairo.h b/canvas_cairo.h new file mode 100644 index 0000000..3b8f06b --- /dev/null +++ b/canvas_cairo.h @@ -0,0 +1,48 @@ + +/* + * dyncnn is a deep-learning algorithm for the prediction of + * interacting object dynamics + * + * Copyright (c) 2016 Idiap Research Institute, http://www.idiap.ch/ + * Written by Francois Fleuret + * + * This file is part of dyncnn. + * + * dyncnn is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 3 as + * published by the Free Software Foundation. + * + * dyncnn is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with dyncnn. If not, see . + * + */ + +#ifndef CANVAS_CAIRO_H +#define CANVAS_CAIRO_H + +#include "canvas.h" + +#include +#include + +class CanvasCairo : public Canvas { + unsigned char *_data; + cairo_surface_t *_image; + cairo_t *_context_resource; + +public: + CanvasCairo(scalar_t scale, int width, int height); + ~CanvasCairo(); + + virtual void set_line_width(scalar_t w); + virtual void set_drawing_color(scalar_t r, scalar_t g, scalar_t b); + virtual void draw_polygon(int filled, int nb, scalar_t *x, scalar_t *y); + virtual void write_png(FILE *file); +}; + +#endif diff --git a/flatland.cc b/flatland.cc new file mode 100644 index 0000000..c27bd97 --- /dev/null +++ b/flatland.cc @@ -0,0 +1,283 @@ + +/* + * dyncnn is a deep-learning algorithm for the prediction of + * interacting object dynamics + * + * Copyright (c) 2016 Idiap Research Institute, http://www.idiap.ch/ + * Written by Francois Fleuret + * + * This file is part of dyncnn. + * + * dyncnn is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 3 as + * published by the Free Software Foundation. + * + * dyncnn is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with dyncnn. If not, see . + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; + +#include "misc.h" +#include "universe.h" +#include "canvas_cairo.h" + +void generate_png(Universe *universe, scalar_t scale, FILE *file) { + CanvasCairo canvas(scale, universe->width(), universe->height()); + canvas.set_line_width(1.0 / scale); + universe->draw(&canvas); + canvas.write_png(file); +} + +FILE *safe_fopen(const char *name, const char *mode) { + FILE *file = fopen(name, mode); + if(!file) { + cerr << "Cannot open " << name << endl; + exit(1); + } + return file; +} + +void print_help(const char *command) { + cerr << command << " [--dir ] [--seed ]]" << endl; + exit(1); +} + +int main(int argc, char **argv) { + const scalar_t world_width = 400; + const scalar_t world_height = 400; + const scalar_t block_size = 80; + + const scalar_t dt = 0.1; + const int nb_iterations_per_steps = 20; + + ////////////////////////////////////////////////////////////////////// + + // We will generate images { 0, every_nth, 2 * every_nth, ..., nb_frames - 1 } + + // The framerate every_nth may be set to smaller value to generate + // nice materials for presentations or papers. + + int every_nth = 4; + + int nb_frames = 5; + + int multi_grasp = 0; + int nb_shapes = 1; + char data_dir[1024] = "/tmp/"; + + ////////////////////////////////////////////////////////////////////// + + Universe *universe; + Polygon *grabbed_polygon; + + if(argc < 2) { + print_help(argv[0]); + } + + int nb_sequences = atoi(argv[1]); + + int i = 2; + while(i < argc) { + if(strcmp(argv[i], "--dir") == 0) { + i++; + if(i == argc) { print_help(argv[0]);} + strncpy(data_dir, argv[i], sizeof(data_dir) / sizeof(char) - 1); + i++; + } + + else if(strcmp(argv[i], "--seed") == 0) { + i++; + if(i == argc) { print_help(argv[0]); } + srand48(atoi(argv[i])); + i++; + } + + else if(strcmp(argv[i], "--nb_shapes") == 0) { + i++; + if(i == argc) { print_help(argv[0]);} + nb_shapes = atoi(argv[i]); + i++; + } + + else if(strcmp(argv[i], "--multi_grasp") == 0) { + multi_grasp = 1; + i++; + } + + else if(strcmp(argv[i], "--every_nth") == 0) { + i++; + if(i == argc) { print_help(argv[0]);} + every_nth = atoi(argv[i]); + i++; + } + + else if(strcmp(argv[i], "--nb_frames") == 0) { + i++; + if(i == argc) { print_help(argv[0]);} + nb_frames = atoi(argv[i]); + i++; + } + + else { + cerr << "Unknown option " << argv[i] << "." << endl; + abort(); + } + } + + if(nb_shapes < 1 || nb_shapes > 10) { + cerr << "nb_shapes has to be in {1, ..., 10}" << endl; + abort(); + } + + if(nb_frames < 0 || nb_frames > 50) { + cerr << "nb_frames has to be in {0, ..., 50}" << endl; + abort(); + } + + universe = new Universe(nb_shapes, world_width, world_height); + + for(int n = 0; n < nb_sequences; n++) { + + scalar_t grab_start_x = world_width * 0.5; + scalar_t grab_start_y = world_height * 0.75; + + if(multi_grasp) { + grab_start_x = world_width * (0.1 + 0.8 * drand48()); + grab_start_y = world_height * (0.1 + 0.8 * drand48()); + } + + if((n+1)%100 == 0) { + cout << "Created " + << n+1 << "/" << nb_sequences << " sequences in " + << data_dir + << "." << endl; + } + + do { + universe->clear(); + + const int nb_attempts_max = 100; + int nb_attempts = 0; + + for(int u = 0; u < nb_shapes; u++) { + Polygon *pol = 0; + + nb_attempts = 0; + + do { + scalar_t x[] = { + - block_size * 0.4, + + block_size * 0.4, + + block_size * 0.4, + - block_size * 0.4, + }; + + scalar_t y[] = { + - block_size * 0.6, + - block_size * 0.6, + + block_size * 0.6, + + block_size * 0.6, + }; + + scalar_t delta = block_size / sqrt(2.0); + scalar_t object_center_x = delta + (world_width - 2 * delta) * drand48(); + scalar_t object_center_y = delta + (world_height - 2 * delta) * drand48(); + scalar_t red, green, blue; + red = 1.00; + green = red; + blue = red; + delete pol; + pol = new Polygon(0.5, + red, green, blue, + x, y, sizeof(x)/sizeof(scalar_t)); + pol->set_position(object_center_x, object_center_y, M_PI * 2 * drand48()); + pol->set_speed(0, 0, 0); + universe->initialize_polygon(pol); + nb_attempts++; + } while(nb_attempts < nb_attempts_max && universe->collide(pol)); + + if(nb_attempts == nb_attempts_max) { + delete pol; + u = 0; + universe->clear(); + nb_attempts = 0; + } else { + universe->add_polygon(pol); + } + } + + grabbed_polygon = universe->pick_polygon(grab_start_x, grab_start_y); + } while(!grabbed_polygon); + + const scalar_t scaling = 0.16; + + CanvasCairo grab_trace(scaling, world_width, world_height); + + { + char buffer[1024]; + sprintf(buffer, "%s/%03d/", data_dir, n/1000); + mkdir(buffer, 0777); + } + + scalar_t grab_relative_x = grabbed_polygon->relative_x(grab_start_x, grab_start_y); + scalar_t grab_relative_y = grabbed_polygon->relative_y(grab_start_x, grab_start_y); + + { + int n = 36; + scalar_t xp[n], yp[n]; + for(int k = 0; k < n; k++) { + scalar_t radius = 1/scaling; + scalar_t alpha = 2 * M_PI * scalar_t(k) / scalar_t(n); + xp[k] = grab_start_x + radius * cos(alpha); + yp[k] = grab_start_y + radius * sin(alpha); + } + grab_trace.set_drawing_color(0.0, 0.0, 0.0); + grab_trace.set_line_width(2.0); + grab_trace.draw_polygon(1, n, xp, yp); + } + + for(int s = 0; s < nb_frames; s++) { + if(s % every_nth == 0) { + char buffer[1024]; + sprintf(buffer, "%s/%03d/dyn_%06d_world_%03d.png", data_dir, n/1000, n, s); + FILE *file = safe_fopen(buffer, "w"); + generate_png(universe, scaling, file); + fclose(file); + } + + for(int i = 0; i < nb_iterations_per_steps; i++) { + scalar_t xf = grabbed_polygon->absolute_x(grab_relative_x, grab_relative_y); + scalar_t yf = grabbed_polygon->absolute_y(grab_relative_x, grab_relative_y); + grabbed_polygon->apply_force(dt, xf, yf, 0.0, -1.0); + universe->update(dt); + } + } + + { + char buffer[1024]; + sprintf(buffer, "%s/%03d/dyn_%06d_grab.png", data_dir, n/1000, n); + FILE *file = safe_fopen(buffer, "w"); + grab_trace.write_png(file); + fclose(file); + } + } + + delete universe; +} diff --git a/misc.cc b/misc.cc new file mode 100644 index 0000000..84c010a --- /dev/null +++ b/misc.cc @@ -0,0 +1,31 @@ + +/* + * dyncnn is a deep-learning algorithm for the prediction of + * interacting object dynamics + * + * Copyright (c) 2016 Idiap Research Institute, http://www.idiap.ch/ + * Written by Francois Fleuret + * + * This file is part of dyncnn. + * + * dyncnn is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 3 as + * published by the Free Software Foundation. + * + * dyncnn is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with dyncnn. If not, see . + * + */ + +#include "misc.h" + +int compare_couple(const void *a, const void *b) { + if(((Couple *) a)->value < ((Couple *) b)->value) return -1; + else if(((Couple *) a)->value > ((Couple *) b)->value) return 1; + else return 0; +} diff --git a/misc.h b/misc.h new file mode 100644 index 0000000..0595f77 --- /dev/null +++ b/misc.h @@ -0,0 +1,53 @@ + +/* + * dyncnn is a deep-learning algorithm for the prediction of + * interacting object dynamics + * + * Copyright (c) 2016 Idiap Research Institute, http://www.idiap.ch/ + * Written by Francois Fleuret + * + * This file is part of dyncnn. + * + * dyncnn is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 3 as + * published by the Free Software Foundation. + * + * dyncnn is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with dyncnn. If not, see . + * + */ + +#ifndef MISC_H +#define MISC_H + +#include +#include + +#ifdef DEBUG +#define ASSERT(x, s) if(!(x)) { std::cerr << "ASSERT FAILED IN " << __FILE__ << ":" << __LINE__ << " [" << (s) << "]\n"; abort(); } +#else +#define ASSERT(x, s) +#endif + +// typedef float scalar_t; +typedef double scalar_t; + +inline scalar_t sq(scalar_t x) { return x*x; } + +inline scalar_t prod_vect(scalar_t x1, scalar_t y1, scalar_t x2, scalar_t y2) { + return x1 * y2 - x2 * y1; +} + +struct Couple { + int index; + scalar_t value; +}; + +int compare_couple(const void *a, const void *b); + +#endif diff --git a/polygon.cc b/polygon.cc new file mode 100644 index 0000000..69b125e --- /dev/null +++ b/polygon.cc @@ -0,0 +1,533 @@ + +/* + * dyncnn is a deep-learning algorithm for the prediction of + * interacting object dynamics + * + * Copyright (c) 2016 Idiap Research Institute, http://www.idiap.ch/ + * Written by Francois Fleuret + * + * This file is part of dyncnn. + * + * dyncnn is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 3 as + * published by the Free Software Foundation. + * + * dyncnn is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with dyncnn. If not, see . + * + */ + +#include + +using namespace std; + +#include +#include "polygon.h" + +static const scalar_t dl = 20.0; +static const scalar_t repulsion_constant = 0.2; +static const scalar_t dissipation = 0.5; + +Polygon::Polygon(scalar_t mass, + scalar_t red, scalar_t green, scalar_t blue, + scalar_t *x, scalar_t *y, + int nv) : _mass(mass), + _moment_of_inertia(0), _radius(0), + _relative_x(new scalar_t[nv]), _relative_y(new scalar_t[nv]), + _total_nb_dots(0), + _nb_dots(new int[nv]), + _effecting_edge(0), + _length(new scalar_t[nv]), + _triangles(new Triangle[nv-2]), + _initialized(false), _nailed(false), + _nb_vertices(nv), + _x(new scalar_t[nv]), _y(new scalar_t[nv]), + _red(red), _green(green), _blue(blue) { + _last_center_x = 0; + _last_center_y = 0; + _last_theta = 0; + + if(x) for(int i = 0; i < nv; i++) _relative_x[i] = x[i]; + if(y) for(int i = 0; i < nv; i++) _relative_y[i] = y[i]; +} + +Polygon::~Polygon() { + delete[] _relative_x; + delete[] _relative_y; + delete[] _x; + delete[] _y; + delete[] _nb_dots; + delete[] _length; + delete[] _triangles; + delete[] _effecting_edge; +} + +Polygon *Polygon::clone() { + return new Polygon(_mass, _red, _green, _blue, _relative_x, _relative_y, _nb_vertices); +} + +#ifdef XFIG_SUPPORT +void Polygon::color_xfig(XFigTracer *tracer) { + tracer->add_color(int(255 * _red), int(255 * _green), int(255 * _blue)); +} + +void Polygon::print_xfig(XFigTracer *tracer) { + tracer->draw_polygon(int(255 * _red), int(255 * _green), int(255 * _blue), + _nb_vertices, _x, _y); +} +#endif + +#ifdef X11_SUPPORT +void Polygon::draw(SimpleWindow *window) { + window->color(_red, _green, _blue); + int x[_nb_vertices], y[_nb_vertices]; + for(int n = 0; n < _nb_vertices; n++) { + x[n] = int(_x[n]); + y[n] = int(_y[n]); + } + window->fill_polygon(_nb_vertices, x, y); +} + +void Polygon::draw_contours(SimpleWindow *window) { + int x[_nb_vertices], y[_nb_vertices]; + for(int n = 0; n < _nb_vertices; n++) { + x[n] = int(_x[n]); + y[n] = int(_y[n]); + } + window->color(0.0, 0.0, 0.0); + // window->color(1.0, 1.0, 1.0); + for(int n = 0; n < _nb_vertices; n++) { + window->draw_line(x[n], y[n], x[(n+1)%_nb_vertices], y[(n+1)%_nb_vertices]); + } +} +#endif + +void Polygon::draw(Canvas *canvas) { + canvas->set_drawing_color(_red, _green, _blue); + canvas->draw_polygon(1, _nb_vertices, _x, _y); +} + +void Polygon::draw_contours(Canvas *canvas) { + canvas->set_drawing_color(0.0, 0.0, 0.0); + canvas->draw_polygon(0, _nb_vertices, _x, _y); +} + +void Polygon::set_vertex(int k, scalar_t x, scalar_t y) { + _relative_x[k] = x; + _relative_y[k] = y; +} + +void Polygon::set_position(scalar_t center_x, scalar_t center_y, scalar_t theta) { + _center_x = center_x; + _center_y = center_y; + _theta = theta; +} + +void Polygon::set_speed(scalar_t dcenter_x, scalar_t dcenter_y, scalar_t dtheta) { + _dcenter_x = dcenter_x; + _dcenter_y = dcenter_y; + _dtheta = dtheta; +} + +bool Polygon::contain(scalar_t x, scalar_t y) { + for(int t = 0; t < _nb_vertices-2; t++) { + scalar_t xa = _x[_triangles[t].a], ya = _y[_triangles[t].a]; + scalar_t xb = _x[_triangles[t].b], yb = _y[_triangles[t].b]; + scalar_t xc = _x[_triangles[t].c], yc = _y[_triangles[t].c]; + if(prod_vect(x - xa, y - ya, xb - xa, yb - ya) <= 0 && + prod_vect(x - xb, y - yb, xc - xb, yc - yb) <= 0 && + prod_vect(x - xc, y - yc, xa - xc, ya - yc) <= 0) return true; + } + return false; +} + +void Polygon::triangularize(int &nt, int nb, int *index) { + if(nb == 3) { + + if(nt >= _nb_vertices-2) { + cerr << "Error type #1 in triangularization." << endl; + exit(1); + } + + _triangles[nt].a = index[0]; + _triangles[nt].b = index[1]; + _triangles[nt].c = index[2]; + nt++; + + } else { + int best_m = -1, best_n = -1; + scalar_t best_split = -1, det, s = -1, t = -1; + + for(int n = 0; n < nb; n++) for(int m = 0; m < n; m++) if(n > m+1 && m+nb > n+1) { + bool no_intersection = true; + for(int k = 0; no_intersection & (k < nb); k++) + if(k != n && k != m && (k+1)%nb != n && (k+1)%nb != m) { + intersection(_relative_x[index[n]], _relative_y[index[n]], + _relative_x[index[m]], _relative_y[index[m]], + _relative_x[index[k]], _relative_y[index[k]], + _relative_x[index[(k+1)%nb]], _relative_y[index[(k+1)%nb]], det, s, t); + no_intersection = det == 0 || s < 0 || s > 1 || t < 0 || t > 1; + } + + if(no_intersection) { + scalar_t a1 = 0, a2 = 0; + for(int k = 0; k < nb; k++) if(k >= m && k < n) + a1 += prod_vect(_relative_x[index[k]] - _relative_x[index[m]], + _relative_y[index[k]] - _relative_y[index[m]], + _relative_x[index[k+1]] - _relative_x[index[m]], + _relative_y[index[k+1]] - _relative_y[index[m]]); + else + a2 += prod_vect(_relative_x[index[k]] - _relative_x[index[m]], + _relative_y[index[k]] - _relative_y[index[m]], + _relative_x[index[(k+1)%nb]] - _relative_x[index[m]], + _relative_y[index[(k+1)%nb]] - _relative_y[index[m]]); + + if((a1 * a2 > 0 && best_split < 0) || (abs(a1 - a2) < best_split)) { + best_n = n; best_m = m; + best_split = abs(a1 - a2); + } + } + } + + if(best_n >= 0 && best_m >= 0) { + int index_neg[nb], index_pos[nb]; + int neg = 0, pos = 0; + for(int k = 0; k < nb; k++) { + if(k >= best_m && k <= best_n) index_pos[pos++] = index[k]; + if(k <= best_m || k >= best_n) index_neg[neg++] = index[k]; + } + if(pos < 3 || neg < 3) { + cerr << "Error type #2 in triangularization." << endl; + exit(1); + } + triangularize(nt, pos, index_pos); + triangularize(nt, neg, index_neg); + } else { + cerr << "Error type #3 in triangularization." << endl; + exit(1); + } + } +} + +void Polygon::initialize(int nb_polygons) { + scalar_t a; + + _nb_polygons = nb_polygons; + + a = _relative_x[_nb_vertices - 1] * _relative_y[0] + - _relative_x[0] * _relative_y[_nb_vertices - 1]; + + for(int n = 0; n < _nb_vertices - 1; n++) + a += _relative_x[n] * _relative_y[n+1] - _relative_x[n+1] * _relative_y[n]; + a *= 0.5; + + // Reorder the vertices + + if(a < 0) { + a = -a; + scalar_t x, y; + for(int n = 0; n < _nb_vertices / 2; n++) { + x = _relative_x[n]; + y = _relative_y[n]; + _relative_x[n] = _relative_x[_nb_vertices - 1 - n]; + _relative_y[n] = _relative_y[_nb_vertices - 1 - n]; + _relative_x[_nb_vertices - 1 - n] = x; + _relative_y[_nb_vertices - 1 - n] = y; + } + } + + // Compute the center of mass and moment of inertia + + scalar_t sx, sy, w; + w = 0; + sx = 0; + sy = 0; + for(int n = 0; n < _nb_vertices; n++) { + int np = (n+1)%_nb_vertices; + w =_relative_x[n] * _relative_y[np] - _relative_x[np] * _relative_y[n]; + sx += (_relative_x[n] + _relative_x[np]) * w; + sy += (_relative_y[n] + _relative_y[np]) * w; + } + sx /= 6 * a; + sy /= 6 * a; + + _radius = 0; + for(int n = 0; n < _nb_vertices; n++) { + _relative_x[n] -= sx; + _relative_y[n] -= sy; + scalar_t r = sqrt(sq(_relative_x[n]) + sq(_relative_y[n])); + if(r > _radius) _radius = r; + } + + scalar_t num = 0, den = 0; + for(int n = 0; n < _nb_vertices; n++) { + int np = (n+1)%_nb_vertices; + den += abs(prod_vect(_relative_x[np], _relative_y[np], _relative_x[n], _relative_y[n])); + num += abs(prod_vect(_relative_x[np], _relative_y[np], _relative_x[n], _relative_y[n])) * + (_relative_x[np] * _relative_x[np] + _relative_y[np] * _relative_y[np] + + _relative_x[np] * _relative_x[n] + _relative_y[np] * _relative_y[n] + + _relative_x[n] * _relative_x[n] + _relative_y[n] * _relative_y[n]); + } + + _moment_of_inertia = num / (6 * den); + + scalar_t vx = cos(_theta), vy = sin(_theta); + + for(int n = 0; n < _nb_vertices; n++) { + _x[n] = _center_x + _relative_x[n] * vx + _relative_y[n] * vy; + _y[n] = _center_y - _relative_x[n] * vy + _relative_y[n] * vx; + } + + scalar_t length; + _total_nb_dots = 0; + for(int n = 0; n < _nb_vertices; n++) { + length = sqrt(sq(_relative_x[n] - _relative_x[(n+1)%_nb_vertices]) + + sq(_relative_y[n] - _relative_y[(n+1)%_nb_vertices])); + _length[n] = length; + _nb_dots[n] = int(length / dl + 1); + _total_nb_dots += _nb_dots[n]; + } + + delete[] _effecting_edge; + _effecting_edge = new int[_nb_polygons * _total_nb_dots]; + for(int p = 0; p < _nb_polygons * _total_nb_dots; p++) _effecting_edge[p] = -1; + + int nt = 0; + int index[_nb_vertices]; + for(int n = 0; n < _nb_vertices; n++) index[n] = n; + triangularize(nt, _nb_vertices, index); + + _initialized = true; +} + +bool Polygon::update(scalar_t dt) { + if(!_nailed) { + _center_x += _dcenter_x * dt; + _center_y += _dcenter_y * dt; + _theta += _dtheta * dt; + } + + scalar_t d = exp(log(dissipation) * dt); + _dcenter_x *= d; + _dcenter_y *= d; + _dtheta *= d; + + scalar_t vx = cos(_theta), vy = sin(_theta); + + for(int n = 0; n < _nb_vertices; n++) { + _x[n] = _center_x + _relative_x[n] * vx + _relative_y[n] * vy; + _y[n] = _center_y - _relative_x[n] * vy + _relative_y[n] * vx; + } + + if(abs(_center_x - _last_center_x) + + abs(_center_y - _last_center_y) + + abs(_theta - _last_theta) * _radius > 0.1) { + _last_center_x = _center_x; + _last_center_y = _center_y; + _last_theta = _theta; + return true; + } else return false; +} + +scalar_t Polygon::relative_x(scalar_t ax, scalar_t ay) { + return (ax - _center_x) * cos(_theta) - (ay - _center_y) * sin(_theta); +} + +scalar_t Polygon::relative_y(scalar_t ax, scalar_t ay) { + return (ax - _center_x) * sin(_theta) + (ay - _center_y) * cos(_theta); +} + +scalar_t Polygon::absolute_x(scalar_t rx, scalar_t ry) { + return _center_x + rx * cos(_theta) + ry * sin(_theta); +} + +scalar_t Polygon::absolute_y(scalar_t rx, scalar_t ry) { + return _center_y - rx * sin(_theta) + ry * cos(_theta); +} + +void Polygon::apply_force(scalar_t dt, scalar_t x, scalar_t y, scalar_t fx, scalar_t fy) { + _dcenter_x += fx / _mass * dt; + _dcenter_y += fy / _mass * dt; + _dtheta -= prod_vect(x - _center_x, y - _center_y, fx, fy) / (_mass * _moment_of_inertia) * dt; +} + +void Polygon::apply_border_forces(scalar_t dt, scalar_t xmax, scalar_t ymax) { + for(int v = 0; v < _nb_vertices; v++) { + int vp = (v+1)%_nb_vertices; + for(int d = 0; d < _nb_dots[v]; d++) { + scalar_t s = scalar_t(d * dl)/_length[v]; + scalar_t x = _x[v] * (1 - s) + _x[vp] * s; + scalar_t y = _y[v] * (1 - s) + _y[vp] * s; + scalar_t vx = 0, vy = 0; + if(x < 0) vx = x; + else if(x > xmax) vx = x - xmax; + if(y < 0) vy = y; + else if(y > ymax) vy = y - ymax; + apply_force(dt, x, y, - dl * vx * repulsion_constant, - dl * vy * repulsion_constant); + } + } +} + +void Polygon::apply_collision_forces(scalar_t dt, int n_polygon, Polygon *p) { + scalar_t closest_x[_total_nb_dots], closest_y[_total_nb_dots]; + bool inside[_total_nb_dots]; + scalar_t distance[_total_nb_dots]; + int _new_effecting_edge[_total_nb_dots]; + + int first_dot = 0; + + for(int v = 0; v < _nb_vertices; v++) { + int vp = (v+1)%_nb_vertices; + scalar_t x = _x[v], y = _y[v], xp = _x[vp], yp = _y[vp]; + + for(int d = 0; d < _nb_dots[v]; d++) { + inside[d] = false; + distance[d] = FLT_MAX; + } + + // First, we tag the dots located inside the polygon p + + for(int t = 0; t < p->_nb_vertices-2; t++) { + scalar_t min = 0, max = 1; + scalar_t xa = p->_x[p->_triangles[t].a], ya = p->_y[p->_triangles[t].a]; + scalar_t xb = p->_x[p->_triangles[t].b], yb = p->_y[p->_triangles[t].b]; + scalar_t xc = p->_x[p->_triangles[t].c], yc = p->_y[p->_triangles[t].c]; + scalar_t den, num; + + const scalar_t eps = 1e-6; + + den = prod_vect(xp - x, yp - y, xb - xa, yb - ya); + num = prod_vect(xa - x, ya - y, xb - xa, yb - ya); + if(den > eps) { + if(num / den < max) max = num / den; + } else if(den < -eps) { + if(num / den > min) min = num / den; + } else { + if(num < 0) { min = 1; max = 0; } + } + + den = prod_vect(xp - x, yp - y, xc - xb, yc - yb); + num = prod_vect(xb - x, yb - y, xc - xb, yc - yb); + if(den > eps) { + if(num / den < max) max = num / den; + } else if(den < -eps) { + if(num / den > min) min = num / den; + } else { + if(num < 0) { min = 1; max = 0; } + } + + den = prod_vect(xp - x, yp - y, xa - xc, ya - yc); + num = prod_vect(xc - x, yc - y, xa - xc, ya - yc); + if(den > eps) { + if(num / den < max) max = num / den; + } else if(den < -eps) { + if(num / den > min) min = num / den; + } else { + if(num < 0) { min = 1; max = 0; } + } + + for(int d = 0; d < _nb_dots[v]; d++) { + scalar_t s = scalar_t(d * dl)/_length[v]; + if(s >= min && s <= max) inside[d] = true; + } + } + + // Then, we compute for each dot what is the closest point on + // the boundary of p + + for(int m = 0; m < p->_nb_vertices; m++) { + int mp = (m+1)%p->_nb_vertices; + scalar_t xa = p->_x[m], ya = p->_y[m]; + scalar_t xb = p->_x[mp], yb = p->_y[mp]; + scalar_t gamma0 = ((x - xa) * (xb - xa) + (y - ya) * (yb - ya)) / sq(p->_length[m]); + scalar_t gamma1 = ((xp - x) * (xb - xa) + (yp - y) * (yb - ya)) / sq(p->_length[m]); + scalar_t delta0 = (prod_vect(xb - xa, yb - ya, x - xa, y - ya)) / p->_length[m]; + scalar_t delta1 = (prod_vect(xb - xa, yb - ya, xp - x, yp - y)) / p->_length[m]; + + for(int d = 0; d < _nb_dots[v]; d++) if(inside[d]) { + int r = _effecting_edge[(first_dot + d) * _nb_polygons + n_polygon]; + + // If there is already a spring, we look only at the + // vertices next to the current one + + if(r < 0 || m == r || m == (r+1)%p->_nb_vertices || (m+1)%p->_nb_vertices == r) { + + scalar_t s = scalar_t(d * dl)/_length[v]; + scalar_t delta = abs(s * delta1 + delta0); + if(delta < distance[d]) { + scalar_t gamma = s * gamma1 + gamma0; + if(gamma < 0) { + scalar_t l = sqrt(sq(x * (1 - s) + xp * s - xa) + sq(y * (1 - s) + yp * s - ya)); + if(l < distance[d]) { + distance[d] = l; + closest_x[d] = xa; + closest_y[d] = ya; + _new_effecting_edge[first_dot + d] = m; + } + } else if(gamma > 1) { + scalar_t l = sqrt(sq(x * (1 - s) + xp * s - xb) + sq(y * (1 - s) + yp * s - yb)); + if(l < distance[d]) { + distance[d] = l; + closest_x[d] = xb; + closest_y[d] = yb; + _new_effecting_edge[first_dot + d] = m; + } + } else { + distance[d] = delta; + closest_x[d] = xa * (1 - gamma) + xb * gamma; + closest_y[d] = ya * (1 - gamma) + yb * gamma; + _new_effecting_edge[first_dot + d] = m; + } + } + } + } else _new_effecting_edge[first_dot + d] = -1; + } + + // Apply forces + + for(int d = 0; d < _nb_dots[v]; d++) if(inside[d]) { + scalar_t s = scalar_t(d * dl)/_length[v]; + scalar_t x = _x[v] * (1 - s) + _x[vp] * s; + scalar_t y = _y[v] * (1 - s) + _y[vp] * s; + scalar_t vx = x - closest_x[d]; + scalar_t vy = y - closest_y[d]; + + p->apply_force(dt, + closest_x[d], closest_y[d], + dl * vx * repulsion_constant, dl * vy * repulsion_constant); + + apply_force(dt, + x, y, + - dl * vx * repulsion_constant, - dl * vy * repulsion_constant); + } + + first_dot += _nb_dots[v]; + } + + for(int d = 0; d < _total_nb_dots; d++) + _effecting_edge[d * _nb_polygons + n_polygon] = _new_effecting_edge[d]; + +} + +bool Polygon::collide(Polygon *p) { + for(int n = 0; n < _nb_vertices; n++) { + int np = (n+1)%_nb_vertices; + for(int m = 0; m < p->_nb_vertices; m++) { + int mp = (m+1)%p->_nb_vertices; + scalar_t det, s = -1, t = -1; + intersection(_x[n], _y[n], _x[np], _y[np], + p->_x[m], p->_y[m], p->_x[mp], p->_y[mp], det, s, t); + if(det != 0 && s>= 0 && s <= 1&& t >= 0 && t <= 1) return true; + } + } + + for(int n = 0; n < _nb_vertices; n++) if(p->contain(_x[n], _y[n])) return true; + for(int n = 0; n < p->_nb_vertices; n++) if(contain(p->_x[n], p->_y[n])) return true; + + return false; +} diff --git a/polygon.h b/polygon.h new file mode 100644 index 0000000..8e23a70 --- /dev/null +++ b/polygon.h @@ -0,0 +1,128 @@ + +/* + * dyncnn is a deep-learning algorithm for the prediction of + * interacting object dynamics + * + * Copyright (c) 2016 Idiap Research Institute, http://www.idiap.ch/ + * Written by Francois Fleuret + * + * This file is part of dyncnn. + * + * dyncnn is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 3 as + * published by the Free Software Foundation. + * + * dyncnn is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with dyncnn. If not, see . + * + */ + +#ifndef POLYGON_H +#define POLYGON_H + +#include "misc.h" +#include "canvas.h" + +#ifdef XFIG_SUPPORT +#include "xfig_tracer.h" +#endif + +#ifdef X11_SUPPORT +#include "simple_window.h" +#endif + +class Polygon { + struct Triangle { + int a, b, c; + }; + + int _nb_max_polygons; + int _nb_polygons; // We need to know the total number of polygons in + // the universe + + scalar_t _mass, _moment_of_inertia, _radius; + + scalar_t *_relative_x, *_relative_y; + scalar_t _last_center_x, _last_center_y, _last_theta; + + int _total_nb_dots; + int *_nb_dots; + int *_effecting_edge; + + scalar_t *_length; + Triangle *_triangles; + + inline void intersection(scalar_t xa2, scalar_t ya2, + scalar_t xa1, scalar_t ya1, + scalar_t xb2, scalar_t yb2, + scalar_t xb1, scalar_t yb1, + scalar_t &det, scalar_t &s1, scalar_t &s2) { + scalar_t m11, m12, m21, m22, n11, n12, n21, n22, v1, v2; + m11 = xa1 - xa2; m12 = xb2 - xb1; m21 = ya1 - ya2; m22 = yb2 - yb1; + det = m11 * m22 - m12 * m21; + if(det != 0) { + n11 = + m22 / det; n12 = - m12 / det; n21 = - m21 / det; n22 = + m11 / det; + v1 = xb2 - xa2; v2 = yb2 - ya2; + s1 = n11 * v1 + n12 * v2; s2 = n21 * v1 + n22 * v2; + } + } + +public: + bool _initialized; + bool _nailed; + int _nb_vertices; + scalar_t *_x, *_y; + scalar_t _red, _green, _blue; + scalar_t _center_x, _center_y, _theta; + scalar_t _dcenter_x, _dcenter_y, _dtheta; + + // The x and y parameters are ignored if null. Useful to initialize + // directly. + + Polygon(scalar_t mass, + scalar_t red, scalar_t green, scalar_t blue, + scalar_t *x, scalar_t *y, + int nv); + + ~Polygon(); + + Polygon *clone(); + +#ifdef XFIG_SUPPORT + void color_xfig(XFigTracer *tracer); + void print_xfig(XFigTracer *tracer); +#endif + +#ifdef X11_SUPPORT + void draw(SimpleWindow *window); + void draw_contours(SimpleWindow *window); +#endif + + void draw(Canvas *canvas); + void draw_contours(Canvas *canvas); + + void set_vertex(int k, scalar_t x, scalar_t y); + void set_position(scalar_t center_x, scalar_t center_y, scalar_t theta); + void set_speed(scalar_t dcenter_x, scalar_t dcenter_y, scalar_t dtheta); + bool contain(scalar_t x, scalar_t y); + void triangularize(int &nt, int nb, int *index); + void initialize(int nb_polygons); + bool update(scalar_t dt); + scalar_t relative_x(scalar_t ax, scalar_t ay); + scalar_t relative_y(scalar_t ax, scalar_t ay); + scalar_t absolute_x(scalar_t rx, scalar_t ry); + scalar_t absolute_y(scalar_t rx, scalar_t ry); + + void apply_force(scalar_t dt, scalar_t x, scalar_t y, scalar_t fx, scalar_t fy); + void apply_border_forces(scalar_t dt, scalar_t xmax, scalar_t ymax); + void apply_collision_forces(scalar_t dt, int n_polygon, Polygon *p); + + bool collide(Polygon *p); +}; + +#endif diff --git a/run.sh b/run.sh new file mode 100755 index 0000000..d905ef9 --- /dev/null +++ b/run.sh @@ -0,0 +1,68 @@ +#!/bin/bash + +# dyncnn is a deep-learning algorithm for the prediction of +# interacting object dynamics +# +# Copyright (c) 2016 Idiap Research Institute, http://www.idiap.ch/ +# Written by Francois Fleuret +# +# This file is part of dyncnn. +# +# dyncnn is free software: you can redistribute it and/or modify it +# under the terms of the GNU General Public License version 3 as +# published by the Free Software Foundation. +# +# dyncnn is distributed in the hope that it will be useful, but +# WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +# General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with dyncnn. If not, see . + +# This script creates the synthetic data-set for shape collision + +set -e +set -o pipefail + +[[ "${TORCH_NB_THREADS}" ]] || echo "You can set \$TORCH_NB_THREADS to the proper value (default 1)." +[[ "${TORCH_USE_GPU}" ]] || echo "You can set \$TORCH_USE_GPU to 'yes' or 'no' (default 'no')." +[[ "${DYNCNN_DATA_DIR}" ]] || DYNCNN_DATA_DIR="./data/10p-mg" +[[ "${DYNCNN_RESULT_DIR}" ]] || DYNCNN_RESULT_DIR="./results" + +if [[ -d "${DYNCNN_DATA_DIR}" ]]; then + echo "Found ${DYNCNN_DATA_DIR}, checking the number of images in there." + if [[ $(find "${DYNCNN_DATA_DIR}" -name "dyn_*.png" | wc -l) == 150000 ]]; then + echo "Looks good !" + else + echo "I do not find the proper number of images. Please remove the dir and re-run this scripts, or fix manually." + exit 1 + fi +else + # Creating the data-base + make -j -k + mkdir -p "${DYNCNN_DATA_DIR}" + ./flatland 50000 \ + --every_nth 4 --nb_frames 5 \ + --multi_grasp --nb_shapes 10 \ + --dir "${DYNCNN_DATA_DIR}" +fi + +# Train the model (takes days) + +if [[ ! -f "${DYNCNN_RESULT_DIR}"/epoch_01000_model ]]; then + ./dyncnn.lua --heavy --dataDir="${DYNCNN_DATA_DIR}" \ + --resultFreq=100 \ + --resultDir "${DYNCNN_RESULT_DIR}" \ + --nbEpochs 1000 +fi + +# Create the images of internal activations + +for n in 2 12; do + ./dyncnn.lua --heavy --dataDir=./data/10p-mg/ \ + --learningStateFile="${DYNCNN_RESULT_DIR}"/epoch_01000_model \ + --resultDir="${DYNCNN_RESULT_DIR}" \ + --noLog \ + --exampleInternals=${n} +done diff --git a/universe.cc b/universe.cc new file mode 100644 index 0000000..658c32c --- /dev/null +++ b/universe.cc @@ -0,0 +1,196 @@ + +/* + * dyncnn is a deep-learning algorithm for the prediction of + * interacting object dynamics + * + * Copyright (c) 2016 Idiap Research Institute, http://www.idiap.ch/ + * Written by Francois Fleuret + * + * This file is part of dyncnn. + * + * dyncnn is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 3 as + * published by the Free Software Foundation. + * + * dyncnn is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with dyncnn. If not, see . + * + */ + +#include + +#include "universe.h" + +Universe::Universe(int nb_max_polygons, + scalar_t width, scalar_t height) : _width(width), + _height(height), + _nb_max_polygons(nb_max_polygons), + _nb_polygons(0) { + _polygons = new Polygon *[_nb_max_polygons]; + for(int n = 0; n < _nb_max_polygons; n++) _polygons[n] = 0; +} + +Universe::~Universe() { + clear(); + delete[] _polygons; +} + +void Universe::initialize_polygon(Polygon *p) { + p->initialize(_nb_max_polygons); +} + +void Universe::clear() { + for(int n = 0; n < _nb_polygons; n++) if(_polygons[n]) delete _polygons[n]; + _nb_polygons = 0; +} + +void Universe::add_polygon(Polygon *p) { + if(_nb_polygons < _nb_max_polygons) { + if(!p->_initialized) { + cerr << "You can not add a non-initialized polygon." << endl; + abort(); + } + _polygons[_nb_polygons++] = p; + } else { + cerr << "Too many polygons!" << endl; + exit(1); + } +} + +bool Universe::collide(Polygon *p) { + for(int n = 0; n < _nb_polygons; n++) + if(_polygons[n] && _polygons[n]->collide(p)) return true; + + return false; +} + +void Universe::compute_pseudo_collisions(int nb_axis, int *nb_colliding_axis) { + Couple couples[_nb_polygons * 2]; + int in[_nb_polygons]; + memset((void *) nb_colliding_axis, 0, _nb_polygons * _nb_polygons * sizeof(int)); + + for(int a = 0; a < nb_axis; a++) { + scalar_t alpha = M_PI * scalar_t(a) / scalar_t(nb_axis); + scalar_t vx = cos(alpha), vy = sin(alpha); + + for(int n = 0; n < _nb_polygons; n++) { + scalar_t *x = _polygons[n]->_x, *y = _polygons[n]->_y; + scalar_t min = x[0] * vx + y[0] * vy, max = min; + + for(int v = 1; v < _polygons[n]->_nb_vertices; v++) { + scalar_t s = x[v] * vx + y[v] * vy; + if(s > max) max = s; + if(s < min) min = s; + } + + couples[2 * n + 0].value = min; + couples[2 * n + 0].index = n; + couples[2 * n + 1].value = max; + couples[2 * n + 1].index = n; + } + + qsort((void *) couples, 2 * _nb_polygons, sizeof(Couple), compare_couple); + + int nb_in = 0; + memset((void *) in, 0, _nb_polygons * sizeof(int)); + for(int k = 0; k < 2 * _nb_polygons; k++) { + int i = couples[k].index; + in[i] = !in[i]; + if(in[i]) { + nb_in++; + if(nb_in > 1) { + for(int j = 0; j < i; j++) + if(j != i && in[j]) nb_colliding_axis[j + i * _nb_polygons]++; + for(int j = i+1; j < _nb_polygons; j++) + if(j != i && in[j]) nb_colliding_axis[i + j * _nb_polygons]++; + } + } else nb_in--; + } + } + + for(int i = 0; i < _nb_polygons; i++) { + for(int j = 0; j < i; j++) { + if(nb_colliding_axis[j + i * _nb_polygons] > nb_colliding_axis[i + i * _nb_polygons]) + nb_colliding_axis[i + i * _nb_polygons] = nb_colliding_axis[j + i * _nb_polygons]; + nb_colliding_axis[i + j * _nb_polygons] = nb_colliding_axis[j + i * _nb_polygons]; + } + } +} + +bool Universe::update(scalar_t dt) { + bool result = false; + apply_collision_forces(dt); + for(int n = 0; n < _nb_polygons; n++) if(_polygons[n]) { + _polygons[n]->apply_border_forces(dt, _width, _height); + result |= _polygons[n]->update(dt); + } + return result; +} + +Polygon *Universe::pick_polygon(scalar_t x, scalar_t y) { + for(int n = 0; n < _nb_polygons; n++) + if(_polygons[n] && _polygons[n]->contain(x, y)) return _polygons[n]; + return 0; +} + +#ifdef XFIG_SUPPORT +void Universe::print_xfig(XFigTracer *tracer) { + for(int n = 0; n < _nb_polygons; n++) { + if(_polygons[n]) { + _polygons[n]->color_xfig(tracer); + } + } + for(int n = 0; n < _nb_polygons; n++) { + if(_polygons[n]) { + _polygons[n]->print_xfig(tracer); + } + } +} +#endif + +#ifdef X11_SUPPORT +void Universe::draw(SimpleWindow *window) { + for(int n = 0; n < _nb_polygons; n++) { + if(_polygons[n]) { + _polygons[n]->draw(window); + } + } + + for(int n = 0; n < _nb_polygons; n++) { + if(_polygons[n]) { + _polygons[n]->draw_contours(window); + } + } +} +#endif + +void Universe::draw(Canvas *canvas) { + for(int n = 0; n < _nb_polygons; n++) { + if(_polygons[n]) { + _polygons[n]->draw(canvas); + } + } + + for(int n = 0; n < _nb_polygons; n++) { + if(_polygons[n]) { + _polygons[n]->draw_contours(canvas); + } + } +} + +void Universe::apply_collision_forces(scalar_t dt) { + const int nb_axis = 2; + int nb_collision[_nb_polygons * _nb_polygons]; + + compute_pseudo_collisions(nb_axis, nb_collision); + + for(int n = 0; n < _nb_polygons; n++) if(_polygons[n]) + for(int m = 0; m < _nb_polygons; m++) + if(m != n && _polygons[m] && nb_collision[n + _nb_polygons * m] == nb_axis) + _polygons[n]->apply_collision_forces(dt, m, _polygons[m]); +} diff --git a/universe.h b/universe.h new file mode 100644 index 0000000..6cb4193 --- /dev/null +++ b/universe.h @@ -0,0 +1,78 @@ + +/* + * dyncnn is a deep-learning algorithm for the prediction of + * interacting object dynamics + * + * Copyright (c) 2016 Idiap Research Institute, http://www.idiap.ch/ + * Written by Francois Fleuret + * + * This file is part of dyncnn. + * + * dyncnn is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License version 3 as + * published by the Free Software Foundation. + * + * dyncnn is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with dyncnn. If not, see . + * + */ + +#ifndef UNIVERSE_H +#define UNIVERSE_H + +#include +#include + +#include "misc.h" +#include "canvas.h" +#include "polygon.h" + +#ifdef X11_SUPPORT +#include "simple_window.h" +#endif + +using namespace std; + +class Universe { + scalar_t _width, _height; +public: + int _nb_max_polygons, _nb_polygons; + Polygon **_polygons; + + inline scalar_t width() { return _width; } + inline scalar_t height() { return _height; } + + Universe(int nb_max_polygons, scalar_t width, scalar_t height); + // The destructor deletes all the added polygons + ~Universe(); + + void initialize_polygon(Polygon *p); + void clear(); + void add_polygon(Polygon *p); + bool collide(Polygon *p); + + // Compute collisions between projections of the polygons on a few + // axis to speed up the computation + void compute_pseudo_collisions(int nb_axis, int *nb_colliding_axis); + void apply_collision_forces(scalar_t dt); + bool update(scalar_t dt); + + Polygon *pick_polygon(scalar_t x, scalar_t y); + +#ifdef XFIG_SUPPORT + void print_xfig(XFigTracer *tracer); +#endif + +#ifdef X11_SUPPORT + void draw(SimpleWindow *window); +#endif + + void draw(Canvas *canvas); +}; + +#endif -- 2.20.1