From ba46b057f3e2613a86f2dea7fa8022990482c735 Mon Sep 17 00:00:00 2001 From: Francois Fleuret Date: Sat, 27 Oct 2007 21:51:01 +0200 Subject: [PATCH] Initial commit --- Makefile | 49 +++++ README.txt | 15 ++ approximer.cc | 180 +++++++++++++++++ approximer.h | 49 +++++ art.cc | 71 +++++++ dummy.cc | 114 +++++++++++ hit_shape.cc | 108 ++++++++++ intelligence.cc | 207 +++++++++++++++++++ intelligence.h | 41 ++++ main.cc | 482 +++++++++++++++++++++++++++++++++++++++++++++ manipulator.cc | 305 ++++++++++++++++++++++++++++ manipulator.h | 66 +++++++ map.cc | 126 ++++++++++++ map.h | 71 +++++++ misc.cc | 24 +++ misc.h | 43 ++++ move_square.cc | 87 ++++++++ polygon.cc | 502 +++++++++++++++++++++++++++++++++++++++++++++++ polygon.h | 101 ++++++++++ retina.cc | 319 ++++++++++++++++++++++++++++++ retina.h | 119 +++++++++++ simple_window.cc | 218 ++++++++++++++++++++ simple_window.h | 79 ++++++++ task.cc | 48 +++++ task.h | 51 +++++ test.sh | 44 +++++ universe.cc | 158 +++++++++++++++ universe.h | 52 +++++ variables.cc | 45 +++++ variables.h | 31 +++ 30 files changed, 3805 insertions(+) create mode 100644 Makefile create mode 100644 README.txt create mode 100644 approximer.cc create mode 100644 approximer.h create mode 100644 art.cc create mode 100644 dummy.cc create mode 100644 hit_shape.cc create mode 100644 intelligence.cc create mode 100644 intelligence.h create mode 100644 main.cc create mode 100644 manipulator.cc create mode 100644 manipulator.h create mode 100644 map.cc create mode 100644 map.h create mode 100644 misc.cc create mode 100644 misc.h create mode 100644 move_square.cc create mode 100644 polygon.cc create mode 100644 polygon.h create mode 100644 retina.cc create mode 100644 retina.h create mode 100644 simple_window.cc create mode 100644 simple_window.h create mode 100644 task.cc create mode 100644 task.h create mode 100755 test.sh create mode 100644 universe.cc create mode 100644 universe.h create mode 100644 variables.cc create mode 100644 variables.h diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..10669ed --- /dev/null +++ b/Makefile @@ -0,0 +1,49 @@ + +#============================================================================# +# This program is free software; you can redistribute it and/or # +# modify it under the terms of the GNU General Public License # +# version 2 as published by the Free Software Foundation. # +# # +# This program 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. # +# # +# Written and (C) by François Fleuret # +# Contact for comments & bug reports # +#============================================================================# + +# CXX=g++-3.3 + +ifeq ($(DEBUG),yes) + CXXFLAGS = -Wall -g -DDEBUG +else + # Optimized compilation +# CXXFLAGS = -Wall -g -O3 -pg --coverage + CXXFLAGS = -Wall -g -O3 +endif + +LDFLAGS = -L/usr/X11R6/lib/ + +TASK_SRC = dummy.cc move_square.cc hit_shape.cc +TASK_OBJ = $(TASK_SRC:.cc=.task) + +all: main TAGS $(TASK_OBJ) + +TAGS: *.cc *.h + etags *.cc *.h + +main: main.o misc.o simple_window.o universe.o polygon.o map.o \ + task.o retina.o manipulator.o approximer.o intelligence.o + $(CXX) -lX11 $(CXXFLAGS) -o $@ $^ $(LDFLAGS) + +%.task: %.cc misc.o universe.o polygon.o map.o task.o manipulator.o + $(CXX) $(CXXFLAGS) -shared -Wl,-soname,$@ -o $@ $^ + +Makefile.depend: *.h *.cc Makefile + $(CC) -M *.cc > Makefile.depend + +clean: + \rm main *.o *.task Makefile.depend + +-include Makefile.depend diff --git a/README.txt b/README.txt new file mode 100644 index 0000000..9bcc6d7 --- /dev/null +++ b/README.txt @@ -0,0 +1,15 @@ + + - move square, with obstacle, with a few of them, with a few an only + some bring rewards. + + - can move an object only indirectly (i.e. reward when blue square + reach a certain location, but penalty if blue square is touched) + + - copy an arrangements of objects on the other side of the world + + - move an arrangement of objects in the same order + + - avoid moving objects + + - reward if an object touch another one of a specified shape or + color, whatever its location diff --git a/approximer.cc b/approximer.cc new file mode 100644 index 0000000..c0d0577 --- /dev/null +++ b/approximer.cc @@ -0,0 +1,180 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +// $Id: approximer.cc,v 1.13 2006-12-22 23:13:46 fleuret Exp $ + +#include "approximer.h" + +MappingApproximer::MappingApproximer(int max_nb_weak_learners) : + _max_nb_weak_learners(max_nb_weak_learners), + _nb_weak_learners(0), + _indexes(new int[_max_nb_weak_learners]), + _thresholds(new scalar_t[_max_nb_weak_learners]), + _weights(new scalar_t[_max_nb_weak_learners]), + _nb_samples(-1), + _input_sorted_index(0), + _outputs_on_samples(0) { } + +MappingApproximer::~MappingApproximer() { + delete[] _indexes; + delete[] _thresholds; + delete[] _weights; + delete[] _outputs_on_samples; +} + +void MappingApproximer::load(istream &is) { + is.read((char *) &_nb_weak_learners, sizeof(_nb_weak_learners)); + if(_nb_weak_learners > _max_nb_weak_learners) { + cerr << "Number of weak learners missmatch." << endl; + exit(1); + } + is.read((char *) _indexes, _nb_weak_learners * sizeof(int)); + is.read((char *) _thresholds, _nb_weak_learners * sizeof(scalar_t)); + is.read((char *) _weights, _nb_weak_learners * sizeof(scalar_t)); +} + +void MappingApproximer::save(ostream &os) { + os.write((char *) &_nb_weak_learners, sizeof(_nb_weak_learners)); + os.write((char *) _indexes, _nb_weak_learners * sizeof(int)); + os.write((char *) _thresholds, _nb_weak_learners * sizeof(scalar_t)); + os.write((char *) _weights, _nb_weak_learners * sizeof(scalar_t)); +} + +void MappingApproximer::set_learning_input(int input_size, int nb_samples, + scalar_t *input, scalar_t *sample_weights) { + + _input_size = input_size; + _input = input; + _sample_weights = sample_weights; + if(_nb_samples != nb_samples ){ + _nb_samples = nb_samples; + delete[] _outputs_on_samples; + delete[] _input_sorted_index; + _outputs_on_samples = new scalar_t[_nb_samples]; + _input_sorted_index = new int[_input_size * _nb_samples]; + } + for(int t = 0; t < _nb_samples; t++) _outputs_on_samples[t] = 0.0; + + for(int n = 0; n < _input_size; n++) { + Couple couples[_nb_samples]; + for(int s = 0; s < _nb_samples; s++) { + couples[s].index = s; + couples[s].value = _input[s * _input_size + n]; + } + qsort(couples, _nb_samples, sizeof(Couple), compare_couple); + for(int s = 0; s < _nb_samples; s++) + _input_sorted_index[n * _nb_samples + s] = couples[s].index; + } + +} + +void MappingApproximer::learn_one_step(scalar_t *target) { + scalar_t delta[_nb_samples], s_delta = 0.0, s_weights = 0; + + for(int s = 0; s < _nb_samples; s++) { + delta[s] = _outputs_on_samples[s] - target[s]; + s_delta += _sample_weights[s] * delta[s]; + s_weights += _sample_weights[s]; + } + + scalar_t best_z = 0, z, prev, val; + int *i; + + for(int n = 0; n < _input_size; n++) { + z = s_delta; + i = _input_sorted_index + n * _nb_samples; + prev = _input[(*i) * _input_size + n]; + for(int s = 1; s < _nb_samples; s++) { + z -= 2 * _sample_weights[*i] * delta[*i]; + i++; + val = _input[(*i) * _input_size + n]; + if(val > prev && abs(z) > abs(best_z)) { + _thresholds[_nb_weak_learners] = (val + prev)/2; + _indexes[_nb_weak_learners] = n; + _weights[_nb_weak_learners] = - z / s_weights; + best_z = z; + } + prev = val; + } + } + + if(best_z == 0) return; + + // Update the responses on the samples + for(int s = 0; s < _nb_samples; s++) { + if(_input[s * _input_size + _indexes[_nb_weak_learners]] >= _thresholds[_nb_weak_learners]) + _outputs_on_samples[s] += _weights[_nb_weak_learners]; + else + _outputs_on_samples[s] -= _weights[_nb_weak_learners]; + } + + _nb_weak_learners++; +} + +scalar_t MappingApproximer::predict(scalar_t *input) { + scalar_t r = 0; + for(int w = 0; w < _nb_weak_learners; w++) + if(input[_indexes[w]] >= _thresholds[w]) + r += _weights[w]; + else + r -= _weights[w]; + return r; +} + +void test_approximer() { +// const int nb_samples = 1000, nb_weak_learners = 100; +// MappingApproximer approximer(nb_weak_learners); +// scalar_t input[nb_samples], output[nb_samples], weight[nb_samples]; +// for(int n = 0; n < nb_samples; n++) { +// input[n] = scalar_t(n * 2 * M_PI)/scalar_t(nb_samples); +// output[n] = sin(input[n]); +// weight[n] = (drand48() < 0.5) ? 1.0 : 0.0; +// } +// approximer.set_learning_input(1, nb_samples, input, weight); +// for(int w = 0; w < nb_weak_learners; w++) { +// approximer.learn_one_step(output); +// scalar_t e = 0; +// for(int n = 0; n < nb_samples; n++) +// e += weight[n] * sq(output[n] - approximer._outputs_on_samples[n]); +// cerr << w << " " << e << endl; +// } +// for(int n = 0; n < nb_samples; n++) { +// cout << input[n] << " " << approximer._outputs_on_samples[n] << endl; +// } + + const int dim = 5, nb_samples = 1000, nb_weak_learners = 100; + MappingApproximer approximer(nb_weak_learners); + scalar_t input[nb_samples * dim], output[nb_samples], weight[nb_samples]; + for(int n = 0; n < nb_samples; n++) { + scalar_t s = 0; + for(int d = 0; d < dim; d++) { + input[n * dim + d] = drand48(); + s += (d+1) * input[n * dim + d]; + } + output[n] = s; + weight[n] = (drand48() < 0.5) ? 1.0 : 0.0; + } + approximer.set_learning_input(dim, nb_samples, input, weight); + for(int w = 0; w < nb_weak_learners; w++) { + approximer.learn_one_step(output); + scalar_t e = 0; + for(int n = 0; n < nb_samples; n++) + e += weight[n] * sq(output[n] - approximer._outputs_on_samples[n]); + cerr << w << " " << e << endl; + } + for(int n = 0; n < nb_samples; n++) { + cout << output[n] << " " << approximer._outputs_on_samples[n] << endl; + } +} diff --git a/approximer.h b/approximer.h new file mode 100644 index 0000000..01d8a89 --- /dev/null +++ b/approximer.h @@ -0,0 +1,49 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +// $Id: approximer.h,v 1.6 2006-07-22 12:34:11 fleuret Exp $ + +#ifndef APPROXIMER_H +#define APPROXIMER_H + +#include +#include + +using namespace std; + +#include "misc.h" + +class MappingApproximer { + int _max_nb_weak_learners, _nb_weak_learners; + int *_indexes; + scalar_t *_thresholds; + scalar_t *_weights; + int _input_size, _nb_samples; + scalar_t *_input, *_sample_weights; + int *_input_sorted_index; +public: + scalar_t *_outputs_on_samples; + MappingApproximer(int size); + ~MappingApproximer(); + void load(istream &is); + void save(ostream &os); + void set_learning_input(int input_size, int nb_samples, scalar_t *input, scalar_t *sample_weights); + void learn_one_step(scalar_t *target); + scalar_t predict(scalar_t *input); +}; + +void test_approximer(); + +#endif diff --git a/art.cc b/art.cc new file mode 100644 index 0000000..d71f266 --- /dev/null +++ b/art.cc @@ -0,0 +1,71 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; + +#include "misc.h" +// #include "task.h" +// #include "simple_window.h" +#include "universe.h" +// #include "retina.h" +// #include "manipulator.h" + +int main(int argc, char **argv) { + + { + nice(19); + scalar_t xs[] = { -28, 28, 28, -28 }; + scalar_t ys[] = { -28, -28, 28, 28 }; + Universe universe(1000); + int w = 20, h = 20; + Polygon *p; + for(int x = 0; x < w; x++) for(int y = 0; y < h; y++) { + p = new Polygon(1.0, 1.0, 1.0, 0.0, xs, ys, 4); + p->set_position(30 + 60 * x, 30 + 60 * y, 0); + p->set_speed(0, 0, 0); + universe.add_polygon(p); + } + + const int nb_projectiles = 15; + Polygon *projectiles[nb_projectiles]; + + for(int b = 0; b < nb_projectiles; b++) { + + int ne = 100; + projectiles[b] = new Polygon(1.0, 0.0, 0.25, 1.0, 0, 0, ne); + for(int e = 0; e < ne; e++) { + scalar_t alpha = 2 * M_PI * scalar_t(e) / scalar_t(ne); + projectiles[b]->set_vertex(e, 200 + 20 * cos(alpha), 200 + 20 * sin(alpha)); + } + projectiles[b]->set_position(67 + 60 * b, -60, 0); + +// scalar_t xs[] = { -10, 10, 10, -10 }; +// scalar_t ys[] = { -80, -80, 80, 80 }; +// projectiles[b] = new Polygon(1.0, 0.0, 0.25, 1.0, xs, ys, 4); +// projectiles[b]->set_position(67 + 60 * 6, -60, 0); + + projectiles[b]->set_speed(0, 0, 0); + universe.add_polygon(projectiles[b]); + } + + for(int n = 0; n < 20000; n++) { + if(n%100 == 0) cout << n << endl; + for(int b = 0; b < nb_projectiles; b++) + projectiles[b]->apply_force(0.01, p->_center_x, p->_center_y, 1, 5); +// projectiles[b]->apply_force(0.01, p->_center_x, p->_center_y, 1, 10); + universe.update(0.01); + } + + for(int n = 0; n < 10000; n++) universe.update(0.01); + + ofstream os("/tmp/universe.fig"); + universe.print_fig(os); + exit(0); + } +} diff --git a/dummy.cc b/dummy.cc new file mode 100644 index 0000000..8173717 --- /dev/null +++ b/dummy.cc @@ -0,0 +1,114 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +// $Id: dummy.cc,v 1.16 2006-07-21 08:28:27 fleuret Exp $ + +#include "task.h" +#include "universe.h" +#include "manipulator.h" + +class Dummy : public Task { +public: + + virtual char *name() { return "DUMMY"; } + + virtual int nb_degrees() { return 1; } + + virtual int width() { return 600; } + + virtual int height() { return 700; } + + virtual void init(Universe *universe, int degree) { + Polygon *p; + scalar_t width = 80, height = 80; + scalar_t deltax = 100, deltay = 100; + + for(int k = 0; k < 4; k++) { + scalar_t x0 = k * deltax; + scalar_t y0 = 0; + p = new Polygon(1.0, 1.0, scalar_t(k)/scalar_t(4), 0.0, 0, 0, 4); + p->set_vertex(0, 0, 0); + p->set_vertex(1, width, 0);; + p->set_vertex(2, width, height); + p->set_vertex(3, 0, height); + p->set_position(x0 + deltax/2, y0 + deltay/2, 0); + p->set_speed(0, 0, 0); + universe->initialize(p); + universe->add_polygon(p); + } + + for(int k = 0; k < 2; k++) { + scalar_t x0 = deltax + k * deltax * 2; + scalar_t y0 = deltay; + p = new Polygon(1.0, scalar_t(k)/scalar_t(2), 1.0, 0.0, 0, 0, 4); + p->set_vertex(0, 0, 0); + p->set_vertex(1, width + deltax, 0);; + p->set_vertex(2, width + deltax, height); + p->set_vertex(3, 0, height); + p->set_position(x0, y0 + deltay/2, 0); + p->set_speed(0, 0, 0); + universe->initialize(p); + universe->add_polygon(p); + } + + scalar_t x0 = deltax; + scalar_t y0 = 2 * deltay; + p = new Polygon(1.0, 1.0, 1.0, 0.0, 0, 0, 4); + p->set_vertex(0, 0, 0); + p->set_vertex(1, width + 3 * deltax, 0);; + p->set_vertex(2, width + 3 * deltax, height); + p->set_vertex(3, 0, height); + p->set_position(x0 + deltax, y0 + deltay/2, 0); + p->set_speed(0, 0, 0); + universe->initialize(p); + universe->add_polygon(p); + + p = new Polygon(1.0, 0.8, 0.2, 0.8, 0, 0, 8); + p->set_vertex(0, 0.0, 0.0); + p->set_vertex(1, 180.0, 0.0); + p->set_vertex(2, 180.0, 220.0); + p->set_vertex(3, 0.0, 220.0); + p->set_vertex(4, 0.0, 180.0); + p->set_vertex(5, 140.0, 180.0); + p->set_vertex(6, 140.0, 40.0); + p->set_vertex(7, 0.0, 40.0); + p->set_position(120, 420, 0); + p->set_speed(0, 0, 0); + universe->initialize(p); + universe->add_polygon(p); + + int ne = 25; + p = new Polygon(1.0, 0.0, 0.25, 1.0, 0, 0, ne); + for(int e = 0; e < ne; e++) { + scalar_t alpha = 2 * M_PI * scalar_t(e) / scalar_t(ne); + p->set_vertex(e, 200 + 50 * cos(alpha), 200 + 70 * sin(alpha)); + } + p->set_position(400, 400, 0); + p->set_speed(0, 0, 0); + universe->initialize(p); + universe->add_polygon(p); + } + + virtual void update(scalar_t dt, Universe *universe, Manipulator *manipulator) { } + + virtual scalar_t reward(Universe *universe, Manipulator *manipulator) { return 0.0; } + + virtual void draw(SimpleWindow *window) { } + +}; + +extern "C" Task *new_task() { + return new Dummy(); +} diff --git a/hit_shape.cc b/hit_shape.cc new file mode 100644 index 0000000..66cf76d --- /dev/null +++ b/hit_shape.cc @@ -0,0 +1,108 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +// $Id: hit_shape.cc,v 1.5 2006-07-23 21:14:42 fleuret Exp $ + +#include "task.h" +#include "universe.h" +#include "manipulator.h" + +class MoveSquare : public Task { + + int _square_size; + int _nb_shapes; + Polygon **_shapes; + bool *_targets; + const static int world_width = 500; + const static int world_height = 500; + +public: + + virtual char *name() { return "HIT_SQUARE"; } + + virtual int nb_degrees() { return 1; } + + virtual int width() { return world_width; } + + virtual int height() { return world_height; } + + void scramble(Universe *universe) { + scalar_t margin = sqrt(2)/2 * _square_size; + scalar_t x[] = { -_square_size/2, _square_size/2, _square_size/2, -_square_size/2 }; + scalar_t y[] = { -_square_size/2, -_square_size/2, _square_size/2, _square_size/2 }; + + universe->clear(); + + for(int s = 0; s < _nb_shapes; s++) _shapes[s] = 0; + + int s = 0; + for(int k = 0; k < _nb_shapes * 10 && s < _nb_shapes; k++) { + Polygon *p; +// bool red = drand48() < 0.2; + bool red = 1-s%2; + + if(red) p = new Polygon(0.5, 1.0, 0.0, 0.0, x, y, 4); + else p = new Polygon(0.5, 1.0, 1.0, 0.0, x, y, 4); + + p->set_position(margin + (world_width - 2 * margin) * drand48(), + margin + (world_height - 2 * margin) * drand48(), + 2 * M_PI * drand48()); + p->set_speed(0, 0, 0); + universe->initialize(p); + p->_nailed = true; + if(universe->collide(p)) delete p; + else { + universe->add_polygon(p); + _shapes[s] = p; + _targets[s] = red; + s++; + } + } + } + + virtual void init(Universe *universe, int degree) { + _square_size = 150; + _nb_shapes = 2; + _shapes = new Polygon *[_nb_shapes]; + _targets = new bool[_nb_shapes]; + scramble(universe); + } + + virtual void update(scalar_t dt, Universe *universe, Manipulator *manipulator) { } + + virtual scalar_t reward(Universe *universe, Manipulator *manipulator) { + if(manipulator->grabbing()) { + for(int k = 0; k < _nb_shapes; k++) if(_shapes[k] == manipulator->grabbing()) { + bool hit = _targets[k]; + if(hit & drand48() < 0.25) { + manipulator->force_release(); + scramble(universe); + return 1.0; + } +// manipulator->force_release(); +// scramble(universe); +// if(hit) return 1.0; +// else return -1.0; + } + } + return 0.0; + } + + virtual void draw(SimpleWindow *window) { } +}; + +extern "C" Task *new_task() { + return new MoveSquare(); +} diff --git a/intelligence.cc b/intelligence.cc new file mode 100644 index 0000000..53bdab8 --- /dev/null +++ b/intelligence.cc @@ -0,0 +1,207 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +// $Id: intelligence.cc,v 1.34 2006-12-18 15:01:09 fleuret Exp $ + +#include "intelligence.h" + +Intelligence::Intelligence(Map *input, + Manipulator *manipulator, + int max_memory_tick, + int nb_weak_learners) : + _nb_actions(manipulator->nb_actions()), + _input(input), + _manipulator(manipulator), + _max_memory_tick(max_memory_tick), + _memory_tick(0), + _memory(new scalar_t[_max_memory_tick * _input->nb_parameters]), + _rewards(new scalar_t[_max_memory_tick]), + _actions(new int[_max_memory_tick]), + _q_predictors(new MappingApproximer *[manipulator->nb_actions()]), + _nb_weak_learners(nb_weak_learners) { + for(int a = 0; a < _nb_actions; a++) + _q_predictors[a] = new MappingApproximer(_nb_weak_learners); +} + +Intelligence::~Intelligence() { + for(int a = 0; a < _nb_actions; a++) delete _q_predictors[a]; + delete[] _q_predictors; + delete[] _memory; + delete[] _rewards; + delete[] _actions; +} + +void Intelligence::load(istream &is) { + int na, np; + + is.read((char *) &na, sizeof(int)); + is.read((char *) &np, sizeof(int)); + + if(na != _nb_actions || np != _input->nb_parameters) { + cerr << "Missmatch between the number of actions or input map size and the saved memory." << endl; + exit(1); + } + + is.read((char *) &_memory_tick, sizeof(int)); + + if(_memory_tick > _max_memory_tick) { + cerr << "Can not load, too large memory dump." << endl; + exit(1); + } + + is.read((char *) _actions, sizeof(int) * _memory_tick); + is.read((char *) _rewards, sizeof(scalar_t) * _memory_tick); + is.read((char *) _memory, sizeof(scalar_t) * _input->nb_parameters * _memory_tick); + + for(int a = 0; a < _nb_actions; a++) _q_predictors[a]->load(is); +} + +void Intelligence::save(ostream &os) { + os.write((char *) &_nb_actions, sizeof(_nb_actions)); + os.write((char *) &_input->nb_parameters, sizeof(int)); + os.write((char *) &_memory_tick, sizeof(int)); + os.write((char *) _actions, sizeof(int) * _memory_tick); + os.write((char *) _rewards, sizeof(scalar_t) * _memory_tick); + os.write((char *) _memory, sizeof(scalar_t) * _input->nb_parameters * _memory_tick); + + for(int a = 0; a < _nb_actions; a++) _q_predictors[a]->save(os); +} + +void Intelligence::update(int last_action, scalar_t last_reward) { + if(_memory_tick == _max_memory_tick) abort(); + ASSERT(last_action >= 0 && last_action < _nb_actions, "Action number out of bounds."); + _actions[_memory_tick] = last_action; + _rewards[_memory_tick] = last_reward; + int k = _memory_tick * _input->nb_parameters; + for(int p = 0; p < _input->nb_parameters; p++) _memory[k++] = _input->parameters[p]; + _memory_tick++; +} + +void Intelligence::save_memory(char *filename) { + ofstream out(filename); + + if(out.fail()) { + cerr << "Can not save to " << filename << "." << endl; + exit(1); + } + + out.write((char *) &_input->nb_parameters, sizeof(int)); + out.write((char *) &_memory_tick, sizeof(int)); + out.write((char *) _actions, sizeof(int) * _memory_tick); + out.write((char *) _rewards, sizeof(scalar_t) * _memory_tick); + out.write((char *) _memory, sizeof(scalar_t) * _input->nb_parameters * _memory_tick); + out.flush(); +} + +void Intelligence::load_memory(char *filename) { + ifstream in(filename); + + if(in.fail()) { + cerr << "Can not load from " << filename << "." << endl; + exit(1); + } + + int np; + in.read((char *) &np, sizeof(int)); + in.read((char *) &_memory_tick, sizeof(int)); + + if(np != _input->nb_parameters) { + cerr << "Missmatch between the input map size and the saved memory." << endl; + exit(1); + } + + if(_memory_tick > _max_memory_tick) { + cerr << "Can not load, too large memory dump." << endl; + exit(1); + } + + in.read((char *) _actions, sizeof(int) * _memory_tick); + in.read((char *) _rewards, sizeof(scalar_t) * _memory_tick); + in.read((char *) _memory, sizeof(scalar_t) * _input->nb_parameters * _memory_tick); +} + +void Intelligence::learn(scalar_t proportion_for_training) { + scalar_t **sample_weigths; + int nb_train_ticks = int(_memory_tick * proportion_for_training); + sample_weigths = new scalar_t *[_nb_actions]; + for(int a = 0; a < _nb_actions; a++) { + sample_weigths[a] = new scalar_t[_memory_tick]; + for(int t = 0; t < _memory_tick; t++) + if(_actions[t] == a && t < nb_train_ticks) sample_weigths[a][t] = 1.0; + else sample_weigths[a][t] = 0.0; + _q_predictors[a]->set_learning_input(_input->nb_parameters, + _memory_tick, + _memory, + sample_weigths[a]); + } + + scalar_t target[_memory_tick]; + for(int t = 0; t < _memory_tick - 1; t++) target[t] = _rewards[t]; + + for(int u = 0; u < _nb_weak_learners; u++) { + + for(int t = 0; t < _memory_tick - 1; t++) { + scalar_t s = 0, u; + for(int a = 0; a < _nb_actions; a++) { + u = _q_predictors[a]->_outputs_on_samples[t+1]; + if(u > s) s = u; + } + const scalar_t lambda = 0.0; + target[t] = lambda * s + _rewards[t]; + } + + for(int a = 0; a < _nb_actions; a++) _q_predictors[a]->learn_one_step(target); + + { + scalar_t e_train[_nb_actions]; + for(int a = 0; a < _nb_actions; a++) e_train[a] = 0; + for(int t = 0; t < nb_train_ticks; t++) + e_train[_actions[t]] += sq(_q_predictors[_actions[t]]->_outputs_on_samples[t] - target[t]); + cout << "ERROR_TRAIN " << u+1; + for(int a = 0; a < _nb_actions; a++) cout << " " << e_train[a]; + cout << endl; + } + + { + scalar_t e_test[_nb_actions]; + for(int a = 0; a < _nb_actions; a++) e_test[a] = 0; + for(int t = nb_train_ticks; t < _memory_tick; t++) + e_test[_actions[t]] += sq(_q_predictors[_actions[t]]->_outputs_on_samples[t] - target[t]); + cout << "ERROR_TEST " << u+1; + for(int a = 0; a < _nb_actions; a++) cout << " " << e_test[a]; + cout << endl; + } + } + + for(int a = 0; a < _nb_actions; a++) delete[] sample_weigths[a]; + delete[] sample_weigths; +} + +int Intelligence::best_action() { + int max_a = -1; + scalar_t q, max_q; + cout << "ACTION_SCORES"; + for(int a = 0; a < _nb_actions; a++) { + q = _q_predictors[a]->predict(_input->parameters); + cout << " " << q; + if(a == 0 || q > max_q) { + max_a = a; + max_q = q; + } + } + cout << endl; + return max_a; +} + diff --git a/intelligence.h b/intelligence.h new file mode 100644 index 0000000..0444e34 --- /dev/null +++ b/intelligence.h @@ -0,0 +1,41 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +// $Id: intelligence.h,v 1.20 2006-07-26 06:52:30 fleuret Exp $ + +#include "map.h" +#include "approximer.h" +#include "manipulator.h" + +class Intelligence { + int _nb_actions; + Map *_input; + Manipulator *_manipulator; + int _max_memory_tick, _memory_tick; + scalar_t *_memory, *_rewards; + int *_actions; + MappingApproximer **_q_predictors; + int _nb_weak_learners; +public: + Intelligence(Map *input, Manipulator *manipulator, int max_memory_tick, int nb_weak_learners); + ~Intelligence(); + void load(istream &is); + void save(ostream &os); + void update(int last_action, scalar_t last_reward); + void save_memory(char *filename); + void load_memory(char *filename); + void learn(scalar_t proportion_for_training); + int best_action(); +}; diff --git a/main.cc b/main.cc new file mode 100644 index 0000000..cb94b7f --- /dev/null +++ b/main.cc @@ -0,0 +1,482 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; + +#include "misc.h" +#include "task.h" +#include "simple_window.h" +#include "universe.h" +#include "retina.h" +#include "manipulator.h" +#include "intelligence.h" + +// To train +// ./main --task hit_shape.task 0 --action-mode=random --nb-ticks=5000 --proportion-for-training=0.5 --save-file=dump.mem --no-window + +// To test +// ./main --task hit_shape.task 0 --action-mode=intelligent --load-file=dump.mem + +////////////////////////////////////////////////////////////////////// + +void check_opt(int argc, char **argv, int n_opt, int n, char *help) { + if(n_opt + n >= argc) { + cerr << "Missing argument for " << argv[n_opt] << "." << endl; + cerr << "Expecting " << help << "." << endl; + exit(1); + } +} + +void print_help_and_exit(int e) { + cout << "$Id: main.cc,v 1.89 2007-06-16 13:51:53 fleuret Exp $" << endl; + cout << endl; + cout << "Arguments:" << endl; + cout << " --no-window" << endl; + cout << " --nb-ticks=" << endl; + cout << " --nb-training-iterations=" << endl; + cout << " --load-file=" << endl; + cout << " --save-file=" << endl; + cout << " --proportion-for-training=" << endl; + cout << " --action-mode=" << endl; + cout << " --task " << endl; + exit(e); +} + +////////////////////////////////////////////////////////////////////// + +int main(int argc, char **argv) { + + const int buffer_size = 256; + char intelligence_load_file[buffer_size] = "", intelligence_save_file[buffer_size] = ""; + + Task *task = 0; + int task_degree = 0; + + int nb_ticks = 10000; + int nb_training_iterations = 10; + scalar_t proportion_for_training = -1; + + Polygon *grabbed_polygon = 0; + scalar_t relative_grab_x = 0, relative_grab_y = 0; + + bool quit = false; + bool press_shift = false; + enum { IDLE, RANDOM, INTELLIGENT } action_mode = IDLE; + bool got_event = true; + int current_action = 0; + + scalar_t last_hand_x = 0, last_hand_y = 0; + Polygon *last_grabbing = 0; + bool no_window = false; + + ////////////////////////////////////////////////////////////////////// + // Parsing the shell arguments + ////////////////////////////////////////////////////////////////////// + + int i = 1; + while(i < argc) { + if(argc == 1 || strcmp(argv[i], "--help") == 0) print_help_and_exit(0); + + else if(strcmp(argv[i], "--test") == 0) { + test_approximer(); + exit(0); + } + + else if(strcmp(argv[i], "--task") == 0) { + check_opt(argc, argv, i, 2, " "); + if(task) { + cerr << "Can not load two tasks." << endl; + exit(1); + } + task = load_task(argv[i+1]); + task_degree = atoi(argv[i+2]); + i += 3; + + } else if(strncmp(argv[i], "--", 2) == 0) { + char variable_name[buffer_size] = "", variable_value[buffer_size] = ""; + char *o = argv[i]+2, *s = variable_name, *u = variable_value; + while(*o && *o != '=') *s++ = *o++; + if(*o) { + o++; + while(*o) *u++ = *o++; + } + + if(strcmp(variable_name, "nb-ticks") == 0) + nb_ticks = atoi(variable_value); + else if(strcmp(variable_name, "nb-training-iterations") == 0) + nb_training_iterations = atoi(variable_value); + else if(strcmp(variable_name, "proportion-for-training") == 0) + proportion_for_training = atof(variable_value); + else if(strcmp(variable_name, "no-window") == 0) + no_window = true; + else if(strcmp(variable_name, "save-file") == 0) + strcpy(intelligence_save_file, variable_value); + else if(strcmp(variable_name, "load-file") == 0) + strcpy(intelligence_load_file, variable_value); + else if(strcmp(variable_name, "action-mode") == 0) { + if(strcmp(variable_value, "idle") == 0) action_mode = IDLE; + else if(strcmp(variable_value, "random") == 0) action_mode = RANDOM; + else if(strcmp(variable_value, "intelligent") == 0) action_mode = INTELLIGENT; + else { + cerr << "The only known modes are idle, random and intelligent" << endl; + exit(1); + } + } else { + cerr << "Unknown option " << argv[i] << endl; + print_help_and_exit(1); + } + i++; + } else { + cerr << "Unknown option " << argv[i] << endl; + print_help_and_exit(1); + } + } + + cout << "FlatLand, a toy universe for goal-planning experiments." << endl; + cout << "$Id: main.cc,v 1.89 2007-06-16 13:51:53 fleuret Exp $" << endl; + + if(!task) { + task = load_task("dummy.task"); + task_degree = 0; + } + + cout << "Loaded task " << task->name() + << " with degree " << task_degree << "/" << task->nb_degrees() + << endl; + + if(task_degree < 0 || task_degree >= task->nb_degrees()) { + cout << "Invalid degree: " << task_degree << "." << endl; + exit(1); + } + + ////////////////////////////////////////////////////////////////////// + // Various initializations + ////////////////////////////////////////////////////////////////////// + + Universe universe(100, task->width(), task->height()); + task->init(&universe, task_degree); + Retina retina(&universe); + Manipulator manipulator(task); + manipulator.force_move(task->width()/2, task->height()/2); + retina.set_location(manipulator.hand_x(), manipulator.hand_y()); + + SimpleWindow *window_main = 0, *window_brain = 0; + + int winfd = -1; + + MapConcatener sensory_map(2); + sensory_map.add_map(&retina); + sensory_map.add_map(&manipulator); + sensory_map.init(); + + MapExpander expanded_map(1000); + expanded_map.set_input(&sensory_map); + expanded_map.init(); + + Intelligence intelligence(&expanded_map, &manipulator, nb_ticks + 1, nb_training_iterations); + intelligence.update(0, 0.0); + + if(intelligence_load_file[0]) { + cout << "Loading from " << intelligence_load_file << " ... " ; + cout.flush(); + ifstream in(intelligence_load_file); + if(in.fail()) { + cerr << "error reading " << intelligence_load_file << "." << endl; + exit(1); + } + intelligence.load(in); + cout << "done." << endl ; + } + + if(!no_window) { + window_main = new SimpleWindow("Universe (main window)", 4, 4, task->width(), task->height()); + winfd = window_main->file_descriptor(); + window_main->map(); + cout << "When the main window has the focus, press `q' to quit and click and drag to move" << endl + << "objects." << endl; + window_brain = new SimpleWindow("Universe (brain)", + 12 + task->width(), 4, + retina.width(), retina.height() + manipulator.parameter_height()); + window_brain->map(); + } else { + cout << "Started without windows." << endl; + } + + int tick = 0; + time_t last_t = 0; + scalar_t sum_reward = 0; + + ////////////////////////////////////////////////////////////////////// + // The main loop + ////////////////////////////////////////////////////////////////////// + + while(!quit && tick != nb_ticks) { + + int r; + fd_set fds; + + + if(window_main) { + struct timeval tv; + FD_ZERO (&fds); + FD_SET (winfd, &fds); + tv.tv_sec = 0; + tv.tv_usec = 5000; // 0.05s + r = select(winfd + 1, &fds, 0, 0, &tv); + } else r = 0; + + time_t t = time(0); + + if(t > last_t) { + last_t = t; + cout << tick << " " << sum_reward << " \r"; cout.flush(); + } + + if(r == 0) { // No window event, thus it's the clock tick + + int nb_it = 10; + + bool changed = got_event; + got_event = false; + + switch(action_mode) { + case IDLE: + break; + case RANDOM: + current_action = manipulator.random_action(); + break; + case INTELLIGENT: + current_action = intelligence.best_action(); + // if(drand48() < 0.5) current_action = intelligence.best_action(); + // else current_action = manipulator.random_action(); + break; + } + + manipulator.do_action(current_action); + + scalar_t dt = 1.0/scalar_t(nb_it); + for(int k = 0; k < nb_it; k++) { + manipulator.update(dt, &universe); + task->update(dt, &universe, &manipulator); + changed |= universe.update(dt); + } + + tick++; + + changed |= manipulator.hand_x() != last_hand_x || + manipulator.hand_y() != last_hand_y || + manipulator.grabbing() != last_grabbing; + + scalar_t reward = task->reward(&universe, &manipulator); + sum_reward += abs(reward); + intelligence.update(current_action, reward); + expanded_map.update_map(); + + if(changed) { + last_hand_x = manipulator.hand_x(); + last_hand_y = manipulator.hand_y(); + last_grabbing = manipulator.grabbing(); + retina.set_location(manipulator.hand_x(), manipulator.hand_y()); + + if(window_main) { + window_main->color(0.0, 0.0, 0.0); + window_main->fill(); + task->draw(window_main); + universe.draw(window_main); + manipulator.draw_on_universe(window_main); + retina.draw_on_universe(window_main); + + if(grabbed_polygon) { + int x, y, delta = 3; + x = int(grabbed_polygon->absolute_x(relative_grab_x, relative_grab_y)); + y = int(grabbed_polygon->absolute_y(relative_grab_x, relative_grab_y)); + window_main->color(0.0, 0.0, 0.0); + window_main->draw_line(x - delta, y, x + delta, y); + window_main->draw_line(x, y - delta, x, y + delta); + } + + window_main->show(); + + if(window_brain) { + retina.draw_parameters(0, 0, window_brain); + manipulator.draw_parameters(0, retina.height() + 1, window_brain); + window_brain->show(); + } + } + } + + } else if(r > 0) { // We got window events, let's process them + + got_event = true; + + if(FD_ISSET(winfd, &fds)) { + + SimpleEvent se; + + do { + se = window_main->event(); + + switch(se.type) { + + case SimpleEvent::MOUSE_CLICK_PRESS: + { + switch(se.button) { + + case 1: + if(press_shift) { + manipulator.force_move(se.x, se.y); + manipulator.do_action(Manipulator::ACTION_GRAB); + } else { + grabbed_polygon = universe.pick_polygon(se.x, se.y); + if(grabbed_polygon) { + relative_grab_x = grabbed_polygon->relative_x(se.x, se.y); + relative_grab_y = grabbed_polygon->relative_y(se.x, se.y); + } + } + break; + case 4: + { + Polygon *g = universe.pick_polygon(se.x, se.y); + if(g) g->_theta += M_PI/32; + } + break; + case 5: + { + Polygon *g = universe.pick_polygon(se.x, se.y); + if(g) g->_theta -= M_PI/32; + } + break; + } + } + break; + + case SimpleEvent::MOUSE_CLICK_RELEASE: + switch(se.button) { + case 1: + if(press_shift) manipulator.do_action(Manipulator::ACTION_RELEASE); + else grabbed_polygon = 0; + break; + default: + break; + } + + case SimpleEvent::MOUSE_MOTION: + { + if(press_shift) manipulator.force_move(se.x, se.y); + else { + if(grabbed_polygon) { + scalar_t xf, yf, force_x, force_y, f, fmax = 100; + xf = grabbed_polygon->absolute_x(relative_grab_x, relative_grab_y); + yf = grabbed_polygon->absolute_y(relative_grab_x, relative_grab_y); + force_x = se.x - xf; + force_y = se.y - yf; + f = sqrt(sq(force_x) + sq(force_y)); + if(f > fmax) { force_x = (force_x * fmax)/f; force_y = (force_y * fmax)/f; } + grabbed_polygon->apply_force(0.1, xf, yf, force_x, force_y); + } + } + break; + } + break; + + case SimpleEvent::KEY_PRESS: + { + if(strcmp(se.key, "q") == 0) quit = true; + else if(strcmp(se.key, "s") == 0) { + retina.save_as_ppm("/tmp/retina.ppm"); + cout << "Retina screen shot saved in /tmp/retina.ppm" << endl; + { + ofstream out("/tmp/universe.fig"); + universe.print_fig(out); + } + } + else if(strcmp(se.key, "Shift_L") == 0 || strcmp(se.key, "Shift_R") == 0) press_shift = true; + + else if(strcmp(se.key, "Up") == 0) manipulator.do_action(Manipulator::ACTION_MOVE_UP); + else if(strcmp(se.key, "Right") == 0) manipulator.do_action(Manipulator::ACTION_MOVE_RIGHT); + else if(strcmp(se.key, "Down") == 0) manipulator.do_action(Manipulator::ACTION_MOVE_DOWN); + else if(strcmp(se.key, "Left") == 0) manipulator.do_action(Manipulator::ACTION_MOVE_LEFT); + else if(strcmp(se.key, "g") == 0) manipulator.do_action(Manipulator::ACTION_GRAB); + else if(strcmp(se.key, "r") == 0) manipulator.do_action(Manipulator::ACTION_RELEASE); + + else if(strcmp(se.key, "space") == 0) { + switch(action_mode) { + case IDLE: + action_mode = RANDOM; + cout << "Switched to random mode" << endl; + break; + case RANDOM: + action_mode = INTELLIGENT; + cout << "Switched to intelligent mode" << endl; + break; + case INTELLIGENT: + cout << "Switched to idle mode" << endl; + action_mode = IDLE; + break; + } + } + + else cout << "Undefined key " << se.key << endl; + } + break; + + case SimpleEvent::KEY_RELEASE: + { + if(strcmp(se.key, "Shift_L") == 0 || strcmp(se.key, "Shift_R") == 0) press_shift = false; + } + break; + + default: + break; + + } + } while(se.type != SimpleEvent::NO_EVENT); + } else { + cerr << "Error on select: " << strerror(errno) << endl; + exit(1); + } + } + } + + if(proportion_for_training > 0) { + cout << "Learning ... "; cout.flush(); + intelligence.learn(proportion_for_training); + cout << "done." << endl; + } + + if(intelligence_save_file[0]) { + cout << "Saving to " << intelligence_save_file << endl; cout.flush(); + ofstream os(intelligence_save_file); + if(os.fail()) { + cerr << "error writing " << intelligence_save_file << "." << endl; + exit(1); + } + cout << "done." << endl; + intelligence.save(os); + } + + delete window_brain; + delete window_main; + +} diff --git a/manipulator.cc b/manipulator.cc new file mode 100644 index 0000000..b17da00 --- /dev/null +++ b/manipulator.cc @@ -0,0 +1,305 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +#include "manipulator.h" + +static const scalar_t force_break = 200; +static const scalar_t force_max = 50; +static const scalar_t hand_speed = 5; + +Manipulator::Manipulator(Task *task) : _touching_polygon(0), _current_polygon(0), + _grab_relative_x(0), _grab_relative_y(0), + _xmax(0), _ymax(0), + _hand_x(0), _hand_y(0), + _force_x(0), _force_y(0), + _grab(false), _release(false), + _current_action(ACTION_GRAB) { + Map::init(NB_DISCRETE_PARAMETERS + NB_CONTINUOUS_MAPS * continuous_map_size); + _xmax = task->width(); + _ymax = task->height(); + for(int m = - continuous_map_size; m <= continuous_map_size; m++) + _kernel_continuous_maps[m + continuous_map_size] = 2 * exp(-sq(m)/sq(continuous_map_size/9)) - 1; +} + +int Manipulator::nb_actions() { return NB_ACTIONS; } + +void Manipulator::do_action(int action) { + scalar_t prev_hand_x =_hand_x, prev_hand_y = _hand_y; + switch(action) { + case ACTION_IDLE: + break; + case ACTION_MOVE_RIGHT: + _hand_x += hand_speed; + if(_hand_x >= _xmax) { + _hand_x = prev_hand_x - hand_speed; + _current_action = ACTION_MOVE_LEFT; + } + break; + case ACTION_MOVE_LEFT: + _hand_x -= hand_speed; + if(_hand_x < 0) { + _hand_x = prev_hand_x + hand_speed; + _current_action = ACTION_MOVE_RIGHT; + } + break; + case ACTION_MOVE_DOWN: + _hand_y += hand_speed; + if(_hand_y >= _ymax) { + _hand_y = prev_hand_y - hand_speed; + _current_action = ACTION_MOVE_UP; + } + break; + case ACTION_MOVE_UP: + _hand_y -= hand_speed; + if(_hand_y < 0) { + _hand_y = prev_hand_y + hand_speed; + _current_action = ACTION_MOVE_DOWN; + } + break; + case ACTION_GRAB: + _grab = true; + break; + case ACTION_RELEASE: + _release = true; + break; + default: + cerr << "Unknown action number (" << action << ")" << endl; + abort(); + } +} + +void Manipulator::update_map() { + parameters[FEEL_TOUCHING_OBJECT] = (_touching_polygon ? +1 : -1); + parameters[FEEL_GRABBING_OBJECT] = (_current_polygon ? +1 : -1); + + int px = int(_hand_x * continuous_map_size / _xmax), + py = int(_hand_y * continuous_map_size / _ymax), + fx = int((continuous_map_size - 1) * (0.5 + 0.5 * _force_x / force_max)), + fy = int((continuous_map_size - 1) * (0.5 + 0.5 * _force_y / force_max)); + + ASSERT(fx >=0 && fx < continuous_map_size && fy >= 0 && fy < continuous_map_size, + "Discrete force sensations out of bounds"); + + for(int m = 0; m < continuous_map_size; m++) { + parameters[NB_DISCRETE_PARAMETERS + MAP_LOCATION_X * continuous_map_size + m] = + _kernel_continuous_maps[px - m + continuous_map_size]; + parameters[NB_DISCRETE_PARAMETERS + MAP_LOCATION_Y * continuous_map_size + m] = + _kernel_continuous_maps[py - m + continuous_map_size]; + parameters[NB_DISCRETE_PARAMETERS + MAP_RESISTANCE_X * continuous_map_size + m] = + _kernel_continuous_maps[fx - m + continuous_map_size]; + parameters[NB_DISCRETE_PARAMETERS + MAP_RESISTANCE_Y * continuous_map_size + m] = + _kernel_continuous_maps[fy - m + continuous_map_size]; + } +} + +void Manipulator::update(scalar_t dt, Universe *universe) { + _touching_polygon = universe->pick_polygon(_hand_x, _hand_y); + + if(!_current_polygon && _grab) { + _current_polygon = _touching_polygon; + if(_current_polygon) { + _grab_relative_x = _current_polygon->relative_x(_hand_x, _hand_y); + _grab_relative_y = _current_polygon->relative_y(_hand_x, _hand_y); + } + } + + if(_release) _current_polygon = 0; + + _grab = false; _release = false; + + if(_current_polygon) { + scalar_t xf = _current_polygon->absolute_x(_grab_relative_x, _grab_relative_y); + scalar_t yf = _current_polygon->absolute_y(_grab_relative_x, _grab_relative_y); + _force_x = (_hand_x - xf) * 10; + _force_y = (_hand_y - yf) * 10; + + scalar_t f = sqrt(sq(_force_x) + sq(_force_y)); + + if(f >= force_break) { + _current_polygon = false; + _force_x = 0; + _force_y = 0; + } else { + if(f >= force_max) { + _force_x *= (force_max/f); + _force_y *= (force_max/f); + } + _current_polygon->set_speed(0, 0, 0); + _current_polygon->apply_force(dt, xf, yf, _force_x, _force_y); + } + + } else { + _force_x = 0; + _force_y = 0; + } +} + +int Manipulator::random_action() { + const scalar_t proba_to_grab_when_touching = 0.1; + const scalar_t proba_to_change_motion = 0.025; + + if(_current_polygon) { + if(drand48() < 0.01) _current_action = ACTION_RELEASE; + else if(_current_action == ACTION_RELEASE || _current_action == ACTION_GRAB || + drand48() < proba_to_change_motion) { + switch(int(drand48() * 4)) { + case 0: + _current_action = ACTION_MOVE_UP; + break; + case 1: + _current_action = ACTION_MOVE_RIGHT; + break; + case 2: + _current_action = ACTION_MOVE_DOWN; + break; + case 3: + _current_action = ACTION_MOVE_LEFT; + break; + default: + abort(); + } + } + } else { + if(_touching_polygon && drand48() < proba_to_grab_when_touching) _current_action = ACTION_GRAB; + else if(_current_action == ACTION_RELEASE || _current_action == ACTION_GRAB || + drand48() < proba_to_change_motion) { + switch(int(drand48() * 4)) { + case 0: + _current_action = ACTION_MOVE_UP; + break; + case 1: + _current_action = ACTION_MOVE_RIGHT; + break; + case 2: + _current_action = ACTION_MOVE_DOWN; + break; + case 3: + _current_action = ACTION_MOVE_LEFT; + break; + default: + abort(); + } + } + } + + return _current_action; +} + +int Manipulator::parameter_width() { + return 80 + continuous_map_size * sqsize + 1; +} + +int Manipulator::parameter_height() { + return 6 * sqsize + 2; +} + +void Manipulator::draw_parameters(int x0, int y0, SimpleWindow *window) { + scalar_t s; + + int wtext = parameter_width() - continuous_map_size * sqsize - 1; + + window->color(0.8, 0.8, 0.8); + window->fill_rectangle(x0 + wtext, y0, continuous_map_size * sqsize + 1, 4 * sqsize + 1); + window->draw_text("x-position", x0 + 8, y0 + 0 * sqsize + sqsize - 4); + window->draw_text("y-position", x0 + 8, y0 + 1 * sqsize + sqsize - 4); + window->draw_text( "x-force", x0 + 8, y0 + 2 * sqsize + sqsize - 4); + window->draw_text( "y-force", x0 + 8, y0 + 3 * sqsize + sqsize - 4); + + window->fill_rectangle(x0 + wtext + 0, y0 + 4 * sqsize, sqsize + 1, sqsize + 1); + window->draw_text("grabbing", x0 + 8, y0 + 4 * sqsize + sqsize - 4); + + window->fill_rectangle(x0 + wtext + 0, y0 + 5 * sqsize, sqsize + 1, sqsize + 1); + window->draw_text("touching", x0 + 8, y0 + 5 * sqsize + sqsize - 4); + + s = parameters[FEEL_GRABBING_OBJECT]; + window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s)); + window->fill_rectangle(x0 + wtext + 1, y0 + 4 * sqsize + 1, sqsize - 1, sqsize - 1); + + s = parameters[FEEL_TOUCHING_OBJECT]; + window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s)); + window->fill_rectangle(x0 + wtext + 1, y0 + 5 * sqsize + 1, sqsize - 1, sqsize - 1); + + for(int m = 0; m < continuous_map_size; m++) { + s = parameters[NB_DISCRETE_PARAMETERS + MAP_LOCATION_X * continuous_map_size + m]; + window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s)); + window->fill_rectangle(x0 + wtext + m * sqsize + 1, y0 + 0 * sqsize + 1, + sqsize-1, sqsize-1); + + s = parameters[NB_DISCRETE_PARAMETERS + MAP_LOCATION_Y * continuous_map_size + m]; + window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s)); + window->fill_rectangle(x0 + wtext + m * sqsize + 1, y0 + 1 * sqsize + 1, + sqsize-1, sqsize-1); + + s = parameters[NB_DISCRETE_PARAMETERS + MAP_RESISTANCE_X * continuous_map_size + m]; + window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s)); + window->fill_rectangle(x0 + wtext + m * sqsize + 1, y0 + 2 * sqsize + 1, + sqsize-1, sqsize-1); + + s = parameters[NB_DISCRETE_PARAMETERS + MAP_RESISTANCE_Y * continuous_map_size + m]; + window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s)); + window->fill_rectangle(x0 + wtext + m * sqsize + 1, y0 + 3 * sqsize + 1, + sqsize-1, sqsize-1); + } +} + +void Manipulator::draw_on_universe(SimpleWindow *window) { + int xc = int(_hand_x), yc = int(_hand_y); + const int delta = 5; + + window->color(1.0, 1.0, 1.0); + + window->draw_line(xc - delta, yc, xc + delta, yc); + window->draw_line(xc, yc - delta, xc, yc + delta); + + if(_current_polygon) { + window->draw_line(xc - delta, yc - delta, xc + delta, yc - delta); + window->draw_line(xc + delta, yc - delta, xc + delta, yc + delta); + window->draw_line(xc + delta, yc + delta, xc - delta, yc + delta); + window->draw_line(xc - delta, yc + delta, xc - delta, yc - delta); + } + + if(_touching_polygon) { + window->draw_line(xc - (delta + 2), yc - (delta + 2), xc + (delta + 2), yc - (delta + 2)); + window->draw_line(xc + (delta + 2), yc - (delta + 2), xc + (delta + 2), yc + (delta + 2)); + window->draw_line(xc + (delta + 2), yc + (delta + 2), xc - (delta + 2), yc + (delta + 2)); + window->draw_line(xc - (delta + 2), yc + (delta + 2), xc - (delta + 2), yc - (delta + 2)); + } +} + +// void Manipulator::force_grab(Universe *universe, scalar_t x, scalar_t y) { +// _hand_x = x; +// _hand_y = y; +// _current_polygon = universe->pick_polygon(_hand_x, _hand_y); +// if(_current_polygon) { +// _grab_relative_x = (_hand_x - _current_polygon->_center_x) * cos(_current_polygon->_theta) +// - (_hand_y - _current_polygon->_center_y) * sin(_current_polygon->_theta); +// _grab_relative_y = + (_hand_x - _current_polygon->_center_x) * sin(_current_polygon->_theta) +// + (_hand_y - _current_polygon->_center_y) * cos(_current_polygon->_theta); +// } +// } + +void Manipulator::force_move(scalar_t x, scalar_t y) { + scalar_t prev_hand_x =_hand_x, prev_hand_y = _hand_y; + _hand_x = x; + _hand_y = y; + if(_hand_x < 0 || _hand_x >= _xmax) _hand_x = prev_hand_x; + if(_hand_y < 0 || _hand_y >= _ymax) _hand_y = prev_hand_y; +} + +void Manipulator::force_release() { + _current_polygon = 0; + _force_x = 0.0; + _force_y = 0.0; +} diff --git a/manipulator.h b/manipulator.h new file mode 100644 index 0000000..f01abd7 --- /dev/null +++ b/manipulator.h @@ -0,0 +1,66 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +#ifndef MANIPULATOR_CC +#define MANIPULATOR_CC + +#include "misc.h" +#include "map.h" +#include "universe.h" +#include "task.h" + +class Manipulator : public Map { + static const int sqsize = 16; + Polygon *_touching_polygon, *_current_polygon; + scalar_t _grab_relative_x, _grab_relative_y; + scalar_t _xmax, _ymax; + scalar_t _hand_x, _hand_y; + scalar_t _force_x, _force_y; + bool _grab, _release; + int _current_action; + enum { FEEL_TOUCHING_OBJECT, FEEL_GRABBING_OBJECT, NB_DISCRETE_PARAMETERS }; + enum { MAP_LOCATION_X, MAP_LOCATION_Y, MAP_RESISTANCE_X, MAP_RESISTANCE_Y, NB_CONTINUOUS_MAPS }; + static const int continuous_map_size = 25; + scalar_t _kernel_continuous_maps[continuous_map_size * 2 + 1]; +public: + enum { ACTION_IDLE, + ACTION_MOVE_UP, ACTION_MOVE_RIGHT, ACTION_MOVE_DOWN, ACTION_MOVE_LEFT, + ACTION_GRAB, ACTION_RELEASE, + NB_ACTIONS }; + Manipulator(Task *task); + + inline scalar_t hand_x() { return _hand_x; } + inline scalar_t hand_y() { return _hand_y; } + inline Polygon *grabbing() { return _current_polygon; } + + int nb_actions(); + void do_action(int action); + + void update_map(); + void update(scalar_t dt, Universe *universe); + + int parameter_width(); + int parameter_height(); + void draw_parameters(int x0, int y0, SimpleWindow *window); + void draw_on_universe(SimpleWindow *window); + + int random_action(); + +// void force_grab(Universe *universe, scalar_t x, scalar_t y); + void force_move(scalar_t x, scalar_t y); + void force_release(); +}; + +#endif diff --git a/map.cc b/map.cc new file mode 100644 index 0000000..64d5ced --- /dev/null +++ b/map.cc @@ -0,0 +1,126 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +// $Id: map.cc,v 1.4 2006-07-26 06:53:18 fleuret Exp $ + +#include "map.h" + +Map::Map() { parameters = 0; } + +Map::~Map() { delete[] parameters; } + +void Map::init(int np) { + nb_parameters = np; + parameters = new scalar_t[nb_parameters]; + for(int n = 0; n < nb_parameters; n++) parameters[n] = 0; +} + +MapConcatener::MapConcatener(int nb_max_maps) : _nb_max_maps(nb_max_maps), _nb_maps(0), + _maps(new Map *[_nb_max_maps]) { } + +MapConcatener::~MapConcatener() { + delete[] _maps; +} + +void MapConcatener::add_map(Map *map) { + if(_nb_maps >= _nb_max_maps) abort(); + _maps[_nb_maps++] = map; +} + +void MapConcatener::init() { + int s = 0; + for(int m = 0; m < _nb_maps; m++) s += _maps[m]->nb_parameters; + Map::init(s); +} + +void MapConcatener::update_map() { + for(int m = 0; m < _nb_maps; m++) _maps[m]->update_map(); + int k = 0; + for(int m = 0; m < _nb_maps; m++) for(int l = 0; l < _maps[m]->nb_parameters; l++) + parameters[k++] = _maps[m]->parameters[l]; +} + +////////////////////////////////////////////////////////////////////// + +MapExpander::MapExpander(int nb_units) : _nb_units(nb_units), + _unit_weights(0), + _input(0) { } + +MapExpander::~MapExpander() { + delete[] _unit_weights; + delete[] _state_switch; +} + +void MapExpander::load(istream &is) { + is.read((char *) &_nb_units, sizeof(_nb_units)); + int input_size; + is.read((char *) &input_size, sizeof(input_size)); + + if(input_size != _input->nb_parameters) { + cerr << "Loaded map expander size missmatch." << endl; + exit(1); + } + + _unit_weights = new scalar_t[_nb_units * 2 * (_input->nb_parameters + 1)]; + parameters = new scalar_t[_nb_units]; + _state_switch = new scalar_t[_nb_units]; + + is.read((char *) _unit_weights, sizeof(scalar_t) * _nb_units * 2 * (_input->nb_parameters + 1)); + + for(int u = 0; u < _nb_units; u++) { + _state_switch[u] = 0.0; + parameters[u] = 0.0; + } +} + +void MapExpander::save(ostream &os) { + os.write((char *) &_nb_units, sizeof(_nb_units)); + os.write((char *) &_input->nb_parameters, sizeof(_input->nb_parameters)); + os.write((char *) _unit_weights, sizeof(scalar_t) * _nb_units * 2 * (_input->nb_parameters + 1)); +} + +void MapExpander::set_input(Map *input) { + ASSERT(!_input, "You can not set the input of an expanding map twice."); + _input = input; +} + +void MapExpander::init() { + ASSERT(!_unit_weights, "You can not initialize a MapExpander twice."); + _unit_weights = new scalar_t[_nb_units * 2 * (_input->nb_parameters + 1)]; + _state_switch = new scalar_t[_nb_units]; + for(int k = 0; k < _nb_units * 2 * (_input->nb_parameters + 1); k++) + _unit_weights[k] = 2 * (drand48() - 0.5); + Map::init(_nb_units); + update_map(); +} + +void MapExpander::update_map() { + ASSERT(_unit_weights, "You have to call MapExpander::init() before using it."); + _input->update_map(); + scalar_t s1, s2; + int k = 0; + for(int u = 0; u < _nb_units; u++) { + s1 = _unit_weights[k++]; + for(int p = 0; p < _input->nb_parameters; p++) + s1 += _unit_weights[k++] * _input->parameters[p]; + s2 = _unit_weights[k++]; + for(int p = 0; p < _input->nb_parameters; p++) + s2 += _unit_weights[k++] * _input->parameters[p]; + parameters[u] = s1; + _state_switch[u] = s2; + } +} + +////////////////////////////////////////////////////////////////////// diff --git a/map.h b/map.h new file mode 100644 index 0000000..3202d83 --- /dev/null +++ b/map.h @@ -0,0 +1,71 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +// $Id: map.h,v 1.5 2006-07-26 06:53:19 fleuret Exp $ + +#ifndef MAP_H +#define MAP_H + +#include +#include + +using namespace std; + +#include "misc.h" + +class Map { +public: + int nb_parameters; + scalar_t *parameters; + Map(); + virtual ~Map(); + virtual void init(int np); + virtual void update_map() = 0; +}; + +// A MapConcatener groups several maps and make them appear as a +// single one + +class MapConcatener : public Map { + int _nb_max_maps, _nb_maps; + Map **_maps; +public: + MapConcatener(int nb_max_maps); + ~MapConcatener(); + void load(istream &is); + void save(ostream &os); + void add_map(Map *map); + void init(); + void update_map(); +}; + +// A MapExpander is a map whose parameters are functions of the +// parameter of an input map + +class MapExpander : public Map { + int _nb_units; + scalar_t *_unit_weights, *_state_switch; + Map *_input; +public: + MapExpander(int nb_units); + ~MapExpander(); + void load(istream &is); + void save(ostream &os); + void set_input(Map *input); + void init(); + void update_map(); +}; + +#endif diff --git a/misc.cc b/misc.cc new file mode 100644 index 0000000..fa6e7ba --- /dev/null +++ b/misc.cc @@ -0,0 +1,24 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +// $Id: misc.cc,v 1.1 2006-07-16 10:27:35 fleuret Exp $ + +#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..d5f1b24 --- /dev/null +++ b/misc.h @@ -0,0 +1,43 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +#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; + +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/move_square.cc b/move_square.cc new file mode 100644 index 0000000..1722be8 --- /dev/null +++ b/move_square.cc @@ -0,0 +1,87 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +// $Id: move_square.cc,v 1.16 2006-07-21 08:44:16 fleuret Exp $ + +#include "task.h" +#include "universe.h" +#include "manipulator.h" + +class MoveSquare : public Task { + + Polygon *_target; + + int _square_size; + const static int world_width = 500; + const static int world_height = 500; + +public: + + virtual char *name() { return "MOVE_SQUARE"; } + + virtual int nb_degrees() { return 5; } + + virtual int width() { return world_width; } + + virtual int height() { return world_height; } + + virtual void init(Universe *universe, int degree) { + if(degree == 0) _square_size = 200; + else if(degree == 1) _square_size = 150; + else if(degree == 2) _square_size = 100; + else _square_size = 50; + scalar_t x[] = { -_square_size/2, _square_size/2, _square_size/2, -_square_size/2 }; + scalar_t y[] = { -_square_size/2, -_square_size/2, _square_size/2, _square_size/2 }; + _target = new Polygon(0.5, 1.0, 1.0, 0.0, x, y, 4); + _target->set_position(_square_size/2, _square_size/2 + (world_height - _square_size) * drand48(), 0); + _target->set_speed(0, 0, 0); + universe->initialize(_target); + universe->add_polygon(_target); + if(degree == 4) { + Polygon *obstacle; + scalar_t x[] = { ( 9 * world_width)/20, (11 * world_width)/20, + (11 * world_width)/20, ( 9 * world_width)/20 }; + scalar_t y[] = { ( 3 * world_height)/20, ( 3 * world_height)/20, + (17 * world_height)/20, (17 * world_height)/20 }; + obstacle = new Polygon(1.0, 0.0, 1.0, 0.0, x, y, 4); + obstacle->set_position(world_width/2, world_height/2, 0); + obstacle->set_speed(0, 0, 0); + obstacle->_nailed = true; + universe->initialize(obstacle); + universe->add_polygon(obstacle); + } + } + + virtual void update(scalar_t dt, Universe *universe, Manipulator *manipulator) { } + + virtual scalar_t reward(Universe *universe, Manipulator *manipulator) { + if(_target->_center_x > world_width - _square_size * 0.75) { + _target->set_position(_square_size/2, _square_size/2 + (world_height - _square_size) * drand48(), 0); + _target->set_speed(0, 0, 0); + manipulator->force_release(); + return 1.0; + } else return 0.0; + } + + virtual void draw(SimpleWindow *window) { + window->color(0.75, 0.75, 0.75); + int x = int (world_width - _square_size * 0.75); + window->draw_line(x, 0, x, world_height); + } +}; + +extern "C" Task *new_task() { + return new MoveSquare(); +} diff --git a/polygon.cc b/polygon.cc new file mode 100644 index 0000000..75163cb --- /dev/null +++ b/polygon.cc @@ -0,0 +1,502 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +#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); +} + +void Polygon::print_fig(ostream &os) { + os << "2 2 0 1 0 7 50 -1 20 0.000 0 0 -1 0 0 " << _nb_vertices + 1 << endl; + os << " "; + for(int n = 0; n < _nb_vertices; n++) os << " " << int(_x[n]*10) << " " << int(_y[n]*10); + os << " " << int(_x[0]*10) << " " << int(_y[0]*10); + os << endl; +} + +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); + 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]); +} + +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..d7caa9d --- /dev/null +++ b/polygon.h @@ -0,0 +1,101 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +#ifndef POLYGON_H +#define POLYGON_H + +#include "misc.h" +#include "simple_window.h" + +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(); + + void print_fig(ostream &os); + void draw(SimpleWindow *window); + void draw_contours(SimpleWindow *window); + 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/retina.cc b/retina.cc new file mode 100644 index 0000000..45975ba --- /dev/null +++ b/retina.cc @@ -0,0 +1,319 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +#include "retina.h" + +const int ring_width[] = { 128, 64, 32, 16, 8, 4, 1, 1, 1, 1}; + +// const int ring_width[] = { 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }; + +Retina::Retina(Universe *universe) : _universe(universe), + _tag_map(new Tag *[_width * _height]), + _tag_stack(new Tag[_width * _height]), + _tag_stack_top(_tag_stack), + _red_imap(new scalar_t[(_width + 1) * (_height + 1)]), + _green_imap(new scalar_t[(_width + 1) * (_height + 1)]), + _blue_imap(new scalar_t[(_width + 1) * (_height + 1)]) { + reset(); + + int np = 0; + int w = _width; + for(int k = 0; k < int(sizeof(ring_width)/sizeof(int)); k++) { + np += 4 * (w / ring_width[k] - 1); + w -= 2 * ring_width[k]; + } + + np *= 3; + Map::init(np); +} + +Retina::~Retina() { + delete[] _tag_map; + delete[] _tag_stack; + delete[] _red_imap; + delete[] _green_imap; + delete[] _blue_imap; +} + +void Retina::reset() { + for(int k = 0; k < _width * _height; k++) _tag_map[k] = 0; + _tag_stack_top = _tag_stack; + _current_polygon = 0; +} + +void Retina::draw_polygon(Polygon *p, scalar_t delta_x, scalar_t delta_y) { + int nb = p->_nb_vertices; + + int ix[nb], iy[nb]; + + for(int n = 0; n < nb; n++) { + ix[n] = int(p->_x[n] + delta_x); + iy[n] = int(p->_y[n] + delta_y); + } + + scalar_t red = p->_red; + scalar_t green = p->_green; + scalar_t blue = p->_blue; + + int direction; + if(iy[0] > iy[nb-1]) direction = 1; + else if(iy[0] < iy[nb-1]) direction = -1; + else direction = 0; + + for(int n = 0; n < nb; n++) { + int m = (n+1)%nb; + + if(ix[n] >= 0 && ix[n] < _width && iy[n] >= 0 && iy[n] < _height && + ix[m] >= 0 && ix[m] < _width && iy[m] >= 0 && iy[m] < _height) { + + if(iy[m] > iy[n]) { // RIGHT BORDERS + + if(direction < 0) push_tag(1 + ix[n], iy[n], red, green, blue); + for(int y = iy[n] + 1; y <= iy[m]; y++) + push_tag(1 + ix[n] + ((ix[m] - ix[n]) * (y - iy[n]))/(iy[m] - iy[n]), y, + red, green, blue); + direction = 1; + + } else if(iy[m] < iy[n]) { // LEFT BORDERS + + if(direction >= 0) push_tag(ix[n], iy[n], red, green, blue); + for(int y = iy[n] - 1; y >= iy[m]; y--) + push_tag(ix[n] + ((ix[m] - ix[n]) * (y - iy[n]))/(iy[m] - iy[n]), y, + red, green, blue); + direction = -1; + + } else { + + if(direction >= 0) push_tag(ix[n]+1, iy[n], red, green, blue); + push_tag(ix[m], iy[m], red, green, blue); + direction = 0; + + } + + } else { + + if(iy[m] > iy[n]) { // RIGHT BORDERS + + if(direction < 0) protected_push_tag(1 + ix[n], iy[n], red, green, blue); + for(int y = iy[n] + 1; y <= iy[m]; y++) + protected_push_tag(1 + ix[n] + ((ix[m] - ix[n]) * (y - iy[n]))/(iy[m] - iy[n]), y, + red, green, blue); + direction = 1; + + } else if(iy[m] < iy[n]) { // LEFT BORDERS + + if(direction >= 0) protected_push_tag(ix[n], iy[n], red, green, blue); + for(int y = iy[n] - 1; y >= iy[m]; y--) + protected_push_tag(ix[n] + ((ix[m] - ix[n]) * (y - iy[n]))/(iy[m] - iy[n]), y, + red, green, blue); + direction = -1; + + } else { + + if(direction >= 0) protected_push_tag(ix[n]+1, iy[n], red, green, blue); + protected_push_tag(ix[m], iy[m], red, green, blue); + direction = 0; + + } + } + } + + _current_polygon++; +} + +void Retina::fill() { + bool in[_current_polygon]; + for(int p = 0; p < _current_polygon; p++) in[p] = false; + + scalar_t red[_current_polygon], green[_current_polygon], blue[_current_polygon]; + Tag **u = _tag_map; + + int lk = 0, k = 0; + + for(int x = 0; x < _width+1; x++) { + _red_imap[k] = 0; + _green_imap[k] = 0; + _blue_imap[k] = 0; + k++; + } + + int current_index = -1; + for(int y = 0; y < _height; y++) { + scalar_t sred = 0, sgreen = 0, sblue = 0; + _red_imap[k] = 0; + _green_imap[k] = 0; + _blue_imap[k] = 0; + k++; lk++; + + for(int x = 0; x < _width; x++) { + for(Tag *t = *u; t; t = t->next) { + if(in[t->index]) { + in[t->index] = false; + if(t->index == current_index) { + current_index--; + while(current_index >= 0 && !in[current_index]) current_index--; + } + } else { + in[t->index] = true; + red[t->index] = t->red; + green[t->index] = t->green; + blue[t->index] = t->blue; + if(t->index > current_index) current_index = t->index; + } + } + + if(current_index >= 0) { + sred += red[current_index]; + sgreen += green[current_index]; + sblue += blue[current_index]; + } + + _red_imap[k] = sred + _red_imap[lk]; + _green_imap[k] = sgreen + _green_imap[lk]; + _blue_imap[k] = sblue + _blue_imap[lk]; + + k++; lk++; + u++; + } + } +} + +void Retina::raw_rectangle(unsigned char *image, int xmin, int ymin, int w, int h, + scalar_t red, scalar_t green, scalar_t blue) { + for(int y = ymin; y < ymin + h; y++) for(int x = xmin; x < xmin + w; x++) { + image[(y * _width + x) * 3 + 0] = (unsigned char) (255 * red); + image[(y * _width + x) * 3 + 1] = (unsigned char) (255 * green); + image[(y * _width + x) * 3 + 2] = (unsigned char) (255 * blue); + } +} + +void Retina::save_as_ppm(char *filename) { + unsigned char *image = new unsigned char[_width * _height * 3]; + + + int n = 0; + int w = _width; + + for(int k = 0; k < int(sizeof(ring_width)/sizeof(int)); k++) { + int s = ring_width[k]; + int z0 = (_width - w)/2; + int z3 = z0 + w; + int z2 = z3 - s; + + for(int x = z0; x < z2; x += s) { + raw_rectangle(image, x, z0, s, s, parameters[n + 0], parameters[n + 1], parameters[n + 2]); + n += 3; + raw_rectangle(image, z2, x, s, s, parameters[n + 0], parameters[n + 1], parameters[n + 2]); + n += 3; + raw_rectangle(image, x + s, z2, s, s, parameters[n + 0], parameters[n + 1], parameters[n + 2]); + n += 3; + raw_rectangle(image, z0, x + s, s, s, parameters[n + 0], parameters[n + 1], parameters[n + 2]); + n += 3; + } + w -= 2 * s; + } + + ofstream out(filename); + out << "P6" << endl; + out << _width << " " << _height << endl; + out << 255 << endl; + out.write((char *) image, _width * _height * 3); + out.flush(); + delete[] image; +} + +void Retina::update_map() { + + reset(); + for(int p = 0; p < _universe->_nb_polygons; p++) if(_universe->_polygons[p]) + draw_polygon(_universe->_polygons[p], scalar_t(_width)/2 - _eye_x, scalar_t(_height)/2 - _eye_y); + fill(); + + int n = 0; + int w = _width; + + for(int k = 0; k < int(sizeof(ring_width)/sizeof(int)); k++) { + + int s = ring_width[k]; + int z0 = (_width - w)/2; + int z1 = z0 + s; + int z3 = z0 + w; + int z2 = z3 - s; + + for(int x = z0; x < z2; x += s) { + parameters[n++] = red_sum(x, z0, x + s + 0, z1 + 0) / scalar_t(s * s); + parameters[n++] = green_sum(x, z0, x + s + 0, z1 + 0) / scalar_t(s * s); + parameters[n++] = blue_sum(x, z0, x + s + 0, z1 + 0) / scalar_t(s * s); + + parameters[n++] = red_sum(z2, x, z3 + 0, x + s + 0) / scalar_t(s * s); + parameters[n++] = green_sum(z2, x, z3 + 0, x + s + 0) / scalar_t(s * s); + parameters[n++] = blue_sum(z2, x, z3 + 0, x + s + 0) / scalar_t(s * s); + + parameters[n++] = red_sum(x + s, z2, x + 2 * s + 0, z3 + 0) / scalar_t(s * s); + parameters[n++] = green_sum(x + s, z2, x + 2 * s + 0, z3 + 0) / scalar_t(s * s); + parameters[n++] = blue_sum(x + s, z2, x + 2 * s + 0, z3 + 0) / scalar_t(s * s); + + parameters[n++] = red_sum(z0, x + s, z1 + 0, x + 2 * s + 0) / scalar_t(s * s); + parameters[n++] = green_sum(z0, x + s, z1 + 0, x + 2 * s + 0) / scalar_t(s * s); + parameters[n++] = blue_sum(z0, x + s, z1 + 0, x + 2 * s + 0) / scalar_t(s * s); + } + w -= 2 * s; + } + +#ifdef DEBUG + if(w != 0) { + cerr << "Error in the retina ring sizes!" << endl; + abort(); + } +#endif + +} + +void Retina::draw_on_universe(SimpleWindow *window) { + window->color(1.0, 1.0, 1.0); + int xc = int(_eye_x), yc = int(_eye_y); + window->draw_line(xc - _width/2, yc - _height/2, xc + _width/2, yc - _height/2); + window->draw_line(xc + _width/2, yc - _height/2, xc + _width/2, yc + _height/2); + window->draw_line(xc + _width/2, yc + _height/2, xc - _width/2, yc + _height/2); + window->draw_line(xc - _width/2, yc + _height/2, xc - _width/2, yc - _height/2); +} + +void Retina::draw_parameters(int x0, int y0, SimpleWindow *window) { + int n = 0; + int w = _width; + + for(int k = 0; k < int(sizeof(ring_width)/sizeof(int)); k++) { + int s = ring_width[k]; + int z0 = (_width - w)/2; + int z3 = z0 + w; + int z2 = z3 - s; + + for(int x = z0; x < z2; x += s) { + window->color(parameters[n + 0], parameters[n + 1], parameters[n + 2]); + window->fill_rectangle(x0 + x, y0 + z0, s, s); + n += 3; + window->color(parameters[n + 0], parameters[n + 1], parameters[n + 2]); + window->fill_rectangle(x0 + z2, y0 + x, s, s); + n += 3; + window->color(parameters[n + 0], parameters[n + 1], parameters[n + 2]); + window->fill_rectangle(x0 + x + s, y0 + z2, s, s); + n += 3; + window->color(parameters[n + 0], parameters[n + 1], parameters[n + 2]); + window->fill_rectangle(x0 + z0, y0 + x + s, s, s); + n += 3; + } + w -= 2 * s; + } +} diff --git a/retina.h b/retina.h new file mode 100644 index 0000000..f562c49 --- /dev/null +++ b/retina.h @@ -0,0 +1,119 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +#ifndef RETINA_H +#define RETINA_H + +#include +#include + +using namespace std; + +#include "misc.h" +#include "map.h" +#include "universe.h" + +#include "simple_window.h" + +class Retina : public Map { + static const int _width = 512, _height = 512; + + class Tag { + public: + int index; + scalar_t red, green, blue; + Tag *next; + }; + + Universe *_universe; + Tag **_tag_map; + + // We have our own stack of tags to avoid numerous news and deletes + Tag *_tag_stack, *_tag_stack_top; + scalar_t *_red_imap, *_green_imap, *_blue_imap; + int _current_polygon; + scalar_t _eye_x, _eye_y; + + inline void protected_push_tag(int x, int y, scalar_t r, scalar_t g, scalar_t b) { + if(y >= 0 && y < _height) { + if(x < 0) x = 0; + else if(x >= _width) x = _width - 1; + int t = y * _width + x; + _tag_stack_top->next = _tag_map[t]; + _tag_stack_top->index = _current_polygon; + _tag_stack_top->red = r; + _tag_stack_top->green = g; + _tag_stack_top->blue = b; + _tag_map[t] = _tag_stack_top; + _tag_stack_top++; + } + } + + inline void push_tag(int x, int y, scalar_t r, scalar_t g, scalar_t b) { + int t = y * _width + x; + _tag_stack_top->next = _tag_map[t]; + _tag_stack_top->index = _current_polygon; + _tag_stack_top->red = r; + _tag_stack_top->green = g; + _tag_stack_top->blue = b; + _tag_map[t] = _tag_stack_top; + _tag_stack_top++; + } + + inline scalar_t red_sum(int xmin, int ymin, int xmax, int ymax) { + return _red_imap[xmax + (_width + 1) * ymax] + + _red_imap[xmin + (_width + 1) * ymin] + - _red_imap[xmax + (_width + 1) * ymin] + - _red_imap[xmin + (_width + 1) * ymax]; + } + + inline scalar_t green_sum(int xmin, int ymin, int xmax, int ymax) { + return _green_imap[xmax + (_width + 1) * ymax] + + _green_imap[xmin + (_width + 1) * ymin] + - _green_imap[xmax + (_width + 1) * ymin] + - _green_imap[xmin + (_width + 1) * ymax]; + } + + inline scalar_t blue_sum(int xmin, int ymin, int xmax, int ymax) { + return _blue_imap[xmax + (_width + 1) * ymax] + + _blue_imap[xmin + (_width + 1) * ymin] + - _blue_imap[xmax + (_width + 1) * ymin] + - _blue_imap[xmin + (_width + 1) * ymax]; + } + +public: + + inline int width() { return _width; } + inline int height() { return _height; } + inline void set_location(scalar_t x, scalar_t y) { _eye_x = x; _eye_y = y; } + + Retina(Universe *universe); + ~Retina(); + void reset(); + + void draw_polygon(Polygon *p, scalar_t delta_x, scalar_t delta_y); + void fill(); + + void raw_rectangle(unsigned char *image, int xmin, int ymin, int w, int h, + scalar_t red, scalar_t green, scalar_t blue); + + void update_map(); + void draw_on_universe(SimpleWindow *window); + void draw_parameters(int x0, int y0, SimpleWindow *window); + + void save_as_ppm(char *filename); +}; + +#endif diff --git a/simple_window.cc b/simple_window.cc new file mode 100644 index 0000000..449ca36 --- /dev/null +++ b/simple_window.cc @@ -0,0 +1,218 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +// $Id: simple_window.cc,v 1.2 2007-04-24 21:22:52 fleuret Exp $ + +#include "simple_window.h" + +SimpleEvent::SimpleEvent() : type(UNDEFINED) {} + +SimpleWindow::SimpleWindow(char *name, int x, int y, int w, int h) { + + _width = w; _height = h; + + _display = XOpenDisplay(0); + + if (_display) { + + Visual *v = XDefaultVisual(_display, DefaultScreen(_display)); + + _blue_mask = v->blue_mask; + _green_mask = v->green_mask; + _red_mask = v->red_mask; + + if(_blue_mask == 0 || _green_mask == 0 || _red_mask == 0) { + cerr << "Can not deal with the colors on that display.\n"; + } + + _red_shift = 1; + while(!(_red_mask & 1)) { + _red_mask = _red_mask >> 1; + _red_shift = _red_shift << 1; + } + + _green_shift = 1; + while(!(_green_mask & 1)) { + _green_mask = _green_mask >> 1; + _green_shift = _green_shift << 1; + } + + _blue_shift = 1; + while(!(_blue_mask & 1)) { + _blue_mask = _blue_mask >> 1; + _blue_shift = _blue_shift << 1; + } + + _gc = DefaultGC(_display, DefaultScreen(_display)); + + XSetWindowAttributes xswa; + + _pixmap = XCreatePixmap(_display, DefaultRootWindow(_display), + _width, _height, DisplayPlanes(_display, DefaultScreen(_display))); + + xswa.background_pixmap = _pixmap; + xswa.event_mask = ButtonPressMask | ButtonReleaseMask | ButtonMotionMask | KeyPressMask | KeyReleaseMask; + xswa.backing_store = Always; + + _window = XCreateWindow(_display, DefaultRootWindow(_display), x, y, _width, _height, + 0, 0, InputOutput, CopyFromParent, + CWBackPixmap | CWEventMask | CWBackingStore, + &xswa); + + XSizeHints size_hints; + size_hints.flags = PMinSize | PMaxSize | USPosition; + size_hints.x = x; // These two lines do not seem to be required + size_hints.y = y; // ... + size_hints.min_width = _width; + size_hints.min_height = _height; + size_hints.max_width = _width; + size_hints.max_height = _height; + XSetNormalHints(_display, _window, &size_hints); + + XStoreName(_display, _window, name); + + XSetState(_display, _gc, 0, 0, GXcopy, AllPlanes); + XFillRectangle(_display, _pixmap, _gc, 0, 0, _width, _height); + XFlush(_display); + } else abort(); +} + + +SimpleWindow::~SimpleWindow() { + XUnmapWindow(_display, _window); + XDestroyWindow(_display, _window); + XCloseDisplay(_display); +} + +int SimpleWindow::width() { + return _width; +} + +int SimpleWindow::height() { + return _height; +} + +void SimpleWindow::map() { + XMapWindow(_display, _window); + XFlush(_display); +} + +void SimpleWindow::unmap() { + XUnmapWindow(_display, _window); + XFlush(_display); +} + +void SimpleWindow::color(float red, float green, float blue) { + XSetState(_display, _gc, + ((unsigned int) ( red * _red_mask)) * _red_shift + + ((unsigned int) (green * _green_mask)) * _green_shift + + ((unsigned int) ( blue * _blue_mask)) * _blue_shift, + 0, GXcopy, AllPlanes); +} + +void SimpleWindow::draw_point(int x, int y) { + XDrawPoint(_display, _pixmap, _gc, x, y); +} + +void SimpleWindow::draw_line(int x1, int y1, int x2, int y2) { + XDrawLine(_display, _pixmap, _gc, x1, y1, x2, y2); +} + +void SimpleWindow::draw_circle(int x, int y, int r) { + XDrawArc(_display, _pixmap, _gc, x-r, y-r, 2*r, 2*r, 0, 360*64); +} + +void SimpleWindow::draw_text(char *s, int x, int y) { + XDrawString(_display, _pixmap, _gc, x, y, s, strlen(s)); +} + +void SimpleWindow::fill_rectangle(int x, int y, int w, int h) { + XFillRectangle(_display, _pixmap, _gc, x, y, w, h); +} + +void SimpleWindow::fill_polygon(int nb, int *x, int *y) { + XPoint points[nb]; + for(int n = 0; n < nb; n++) { + points[n].x = x[n]; + points[n].y = y[n]; + } + XFillPolygon(_display, _pixmap, _gc, points, nb, Nonconvex, CoordModeOrigin); +} + +void SimpleWindow::show() { + XCopyArea(_display, _pixmap, _window, _gc, 0, 0, _width, _height, 0, 0); + XFlush(_display); +} + +void SimpleWindow::fill() { + XFillRectangle(_display, _pixmap, _gc, 0, 0, _width, _height); +} + +int SimpleWindow::file_descriptor() { + return XConnectionNumber(_display); +} + +SimpleEvent SimpleWindow::event() { + SimpleEvent se; + + if(XPending(_display) > 0) { + + XEvent event; + XNextEvent(_display, &event); + + switch (event.type) { + + case ButtonPress: + se.type = SimpleEvent::MOUSE_CLICK_PRESS; + se.button = event.xbutton.button; + se.x = event.xbutton.x; + se.y = event.xbutton.y; + break; + + case ButtonRelease: + se.type = SimpleEvent::MOUSE_CLICK_RELEASE; + se.button = event.xbutton.button; + se.x = event.xbutton.x; + se.y = event.xbutton.y; + break; + + case MotionNotify: + se.type = SimpleEvent::MOUSE_MOTION; + se.button = event.xbutton.button; + se.x = event.xbutton.x; + se.y = event.xbutton.y; + break; + + case KeyPress: + se.type = SimpleEvent::KEY_PRESS; + strncpy(se.key, + XKeysymToString(XKeycodeToKeysym(_display, event.xkey.keycode, 0)), + (sizeof(se.key)/sizeof(char) - 1)); + break; + + case KeyRelease: + se.type = SimpleEvent::KEY_RELEASE; + strncpy(se.key, + XKeysymToString(XKeycodeToKeysym(_display, event.xkey.keycode, 0)), + (sizeof(se.key)/sizeof(char) - 1)); + break; + + default: + se.type = SimpleEvent::UNDEFINED; + break; + } + } else se.type = SimpleEvent::NO_EVENT; + return se; +} diff --git a/simple_window.h b/simple_window.h new file mode 100644 index 0000000..f42e931 --- /dev/null +++ b/simple_window.h @@ -0,0 +1,79 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +// $Id: simple_window.h,v 1.2 2007-04-24 21:22:52 fleuret Exp $ + +#ifndef SIMPLE_WINDOW_H +#define SIMPLE_WINDOW_H + +#include +#include + +using namespace std; + +#include +#include + +class SimpleEvent { +public: + + enum { + NO_EVENT, + UNDEFINED, + MOUSE_CLICK_PRESS, MOUSE_CLICK_RELEASE, + MOUSE_MOTION, + KEY_PRESS, KEY_RELEASE + } type; + + int x, y, button; + char key[32]; // This is the max length for the pressed key name ('a', 'Alt_L', etc.) + SimpleEvent(); +}; + +class SimpleWindow { + Display *_display; + Window _window; + Pixmap _pixmap; + GC _gc; + +protected: + int _red_mask, _green_mask, _blue_mask; + int _red_shift, _green_shift, _blue_shift; + int _width, _height; + +public: + SimpleWindow(char *name, int x, int y, int w, int h); + virtual ~SimpleWindow(); + + virtual int width(); + virtual int height(); + + virtual void map(); + virtual void unmap(); + virtual void color(float red, float green, float blue); + virtual void draw_point(int x, int y); + virtual void draw_line(int x1, int y1, int x2, int y2); + virtual void draw_circle(int x, int y, int r); + virtual void draw_text(char *s, int x, int y); + virtual void fill_rectangle(int x, int y, int w, int h); + virtual void fill_polygon(int nb, int *x, int *y); + virtual void show(); + virtual void fill(); + + virtual int file_descriptor(); + virtual SimpleEvent event(); +}; + +#endif diff --git a/task.cc b/task.cc new file mode 100644 index 0000000..71002c3 --- /dev/null +++ b/task.cc @@ -0,0 +1,48 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +// $Id: task.cc,v 1.8 2006-07-10 15:19:06 fleuret Exp $ + +#include +#include + +using namespace std; + +#include "task.h" +#include "universe.h" +#include "manipulator.h" + +Task::~Task() { } + +typedef Task *TaskConstructor(); + +Task *load_task(char *filename) { + void *handle = dlopen(filename, RTLD_NOW | RTLD_GLOBAL); + + if(handle == 0) { + cerr << "Error in dynamic loading: " << dlerror() << "." << endl; + exit(1); + } + + TaskConstructor *creator = (TaskConstructor *) dlsym(handle, "new_task"); + + if(creator == 0) { + dlclose(handle); + cerr << "Error looking for function new_task() in " << filename << "." << endl; + exit(1); + } + + return (*creator)(); +} diff --git a/task.h b/task.h new file mode 100644 index 0000000..b472f73 --- /dev/null +++ b/task.h @@ -0,0 +1,51 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +// $Id: task.h,v 1.18 2006-12-17 19:22:19 fleuret Exp $ + +#ifndef TASK_H +#define TASK_H + +#include "misc.h" +#include "simple_window.h" + +class Manipulator; +class Universe; + +class Task { +public: + virtual ~Task(); + // A canonical name for the task, upper caps, digits and "_" + virtual char *name() = 0; + // A task can be started with several degrees of difficulty + virtual int nb_degrees() = 0; + // Returns the size of the area + virtual int width() = 0; + virtual int height() = 0; + // Initializes the world (put polygons with Universe::add_polygon) + virtual void init(Universe *universe, int degree) = 0; + // Updates the world + virtual void update(scalar_t dt, Universe *universe, Manipulator *manipulator) = 0; + // Computes and returns the reward (can be positive or negative) + virtual scalar_t reward(Universe *universe, Manipulator *manipulator) = 0; + // If we need to draw something + virtual void draw(SimpleWindow *window) = 0; +}; + +// Loads a shared object file + +Task *load_task(char *filename); + +#endif diff --git a/test.sh b/test.sh new file mode 100755 index 0000000..77f554d --- /dev/null +++ b/test.sh @@ -0,0 +1,44 @@ +#!/bin/bash + +#============================================================================# +# This program is free software; you can redistribute it and/or # +# modify it under the terms of the GNU General Public License # +# version 2 as published by the Free Software Foundation. # +# # +# This program 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. # +# # +# Written and (C) by François Fleuret # +# Contact for comments & bug reports # +#============================================================================# + +make -k + +MAIN=./main +TASK=hit_shape.task +DEGREE=0 + +case $1 in + + train) + ${MAIN} --task ${TASK} ${DEGREE} \ + --no-window \ + --nb-training-iterations=1000 \ + --action-mode=random --nb-ticks=5000 \ + --proportion-for-training=1.0 --save-file=dump.mem + ;; + + test) + ${MAIN} --task ${TASK} ${DEGREE} \ + --nb-training-iterations=1000 \ + --action-mode=intelligent \ + --load-file=dump.mem + ;; + + *) + echo "$0 " + exit 1 + +esac diff --git a/universe.cc b/universe.cc new file mode 100644 index 0000000..8d8aa71 --- /dev/null +++ b/universe.cc @@ -0,0 +1,158 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +// $Id: universe.cc,v 1.88 2007-06-16 13:51:54 fleuret Exp $ + +#include "universe.h" + +Universe::Universe(int nb_max_polygons, + scalar_t xmax, scalar_t ymax) : _xmax(xmax), _ymax(ymax), + _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() { + for(int n = 0; n < _nb_polygons; n++) if(_polygons[n]) delete _polygons[n]; + delete[] _polygons; +} + +void Universe::initialize(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 << "To 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, _xmax, _ymax); + 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; +} + +void Universe::print_fig(ostream &os) { + os << "#FIG 3.2" << endl; + os << "Portrait" << endl; + os << "Center" << endl; + os << "Metric" << endl; + os << "A4 " << endl; + os << "100.00" << endl; + os << "Single" << endl; + os << "-2" << endl; + os << "1200 2" << endl; + for(int n = 0; n < _nb_polygons; n++) if(_polygons[n]) _polygons[n]->print_fig(os); +} + +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); +} + +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..3775daf --- /dev/null +++ b/universe.h @@ -0,0 +1,52 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +// $Id: universe.h,v 1.21 2006-12-17 19:22:19 fleuret Exp $ + +#ifndef UNIVERSE_H +#define UNIVERSE_H + +#include +#include + +using namespace std; + +#include "misc.h" +#include "simple_window.h" +#include "polygon.h" + +class Universe { + scalar_t _xmax, _ymax; +public: + int _nb_max_polygons, _nb_polygons; + Polygon **_polygons; + Universe(int nb_max_polygons, scalar_t xmax, scalar_t ymax); + // The destructor deletes all the added polygons + ~Universe(); + void initialize(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); + void print_fig(ostream &os); + void draw(SimpleWindow *window); +}; + +#endif diff --git a/variables.cc b/variables.cc new file mode 100644 index 0000000..5cd1994 --- /dev/null +++ b/variables.cc @@ -0,0 +1,45 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +// $Id$ + +#include "variables.h" + +Variables::Variables() { +} + +Variables::~Variables() { +} + +void Variables::add_default_int(char *name, int value) { +} + +void Variables::add_default_float(char *name, scalar_t value) { +} + +void Variables::add_default_string(char *name, char *value) { +} + +int Variables::get_int_value() { + return 0; +} + +scalar_t Variables::get_float_value() { + return 0.0; +} + +char *Variables::get_string_value() { + return ""; +} diff --git a/variables.h b/variables.h new file mode 100644 index 0000000..7c55599 --- /dev/null +++ b/variables.h @@ -0,0 +1,31 @@ + +//////////////////////////////////////////////////////////////////////////////// +// This program is free software; you can redistribute it and/or // +// modify it under the terms of the GNU General Public License // +// version 2 as published by the Free Software Foundation. // +// // +// This program 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. // +// // +// Written and (C) by François Fleuret // +// Contact for comments & bug reports // +//////////////////////////////////////////////////////////////////////////////// + +// $Id$ + +#include "misc.h" + +class Variables { + enum VariableType { INT, FLOAT, STRING }; +public: + Variables(); + virtual ~Variables(); + virtual void add_default_int(char *name, int value); + virtual void add_default_float(char *name, scalar_t value); + virtual void add_default_string(char *name, char *value); + virtual int get_int_value(); + virtual scalar_t get_float_value(); + virtual char *get_string_value(); +}; -- 2.20.1