--- /dev/null
+
+#============================================================================#
+# 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 <francois.fleuret@epfl.ch> 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
--- /dev/null
+
+ - 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
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> 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;
+ }
+}
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> 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 <iostream>
+#include <cmath>
+
+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
--- /dev/null
+#include <iostream>
+#include <fstream>
+#include <cmath>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <errno.h>
+#include <string.h>
+
+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);
+ }
+}
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> 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();
+}
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> 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();
+}
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> 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;
+}
+
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> 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();
+};
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> for comments & bug reports //
+////////////////////////////////////////////////////////////////////////////////
+
+#include <iostream>
+#include <fstream>
+#include <cmath>
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <errno.h>
+#include <string.h>
+
+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=<int: number of ticks>" << endl;
+ cout << " --nb-training-iterations=<int: number of training iterations>" << endl;
+ cout << " --load-file=<filename: dump file>" << endl;
+ cout << " --save-file=<filename: dump file>" << endl;
+ cout << " --proportion-for-training=<float: proportion of samples for training>" << endl;
+ cout << " --action-mode=<idle|random|intelligent>" << endl;
+ cout << " --task <filename: task to load> <int: degree>" << 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, "<filename: task to load> <int: degree>");
+ 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;
+
+}
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> 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;
+}
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> 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
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> 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;
+ }
+}
+
+//////////////////////////////////////////////////////////////////////
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> 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 <iostream>
+#include <cmath>
+
+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
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> 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;
+}
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> for comments & bug reports //
+////////////////////////////////////////////////////////////////////////////////
+
+#ifndef MISC_H
+#define MISC_H
+
+#include <float.h>
+#include <stdlib.h>
+
+#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
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> 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();
+}
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> for comments & bug reports //
+////////////////////////////////////////////////////////////////////////////////
+
+#include <cmath>
+#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;
+}
+
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> 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
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> 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;
+ }
+}
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> for comments & bug reports //
+////////////////////////////////////////////////////////////////////////////////
+
+#ifndef RETINA_H
+#define RETINA_H
+
+#include <iostream>
+#include <fstream>
+
+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
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> 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;
+}
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> 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 <iostream>
+#include <fstream>
+
+using namespace std;
+
+#include <X11/Xlib.h>
+#include <X11/Xutil.h>
+
+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
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> for comments & bug reports //
+////////////////////////////////////////////////////////////////////////////////
+
+// $Id: task.cc,v 1.8 2006-07-10 15:19:06 fleuret Exp $
+
+#include <iostream>
+#include <dlfcn.h>
+
+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)();
+}
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> 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
--- /dev/null
+#!/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 <francois.fleuret@epfl.ch> 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 <test|train>"
+ exit 1
+
+esac
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> 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]);
+}
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> 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 <iostream>
+#include <cmath>
+
+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
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> 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 "";
+}
--- /dev/null
+
+////////////////////////////////////////////////////////////////////////////////
+// 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 <francois.fleuret@epfl.ch> 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();
+};
projects / universe.git / commitdiff