diff --git a/Makefile b/Makefile index 9d31b18..315c006 100644 --- a/Makefile +++ b/Makefile @@ -28,7 +28,7 @@ OBJECTS = ias_ss.o robot.o ias_robot.o gui.o ogl.o GLSLProgram.o pnglite.o phero DEPENDS = $(OBJECTS:.o=.d) CFLAGS = -ansi -pedantic -Wall -I./include CXXFLAGS = -ansi -pedantic -Wall `pkg-config --cflags playerc++` -I./include -LDLIBS = `pkg-config --libs playerc++` -lboost_system -lpthread -lz -lGLEW -lGLU -lGL -lfltk -lfltk_gl +LDLIBS = `pkg-config --libs playerc++` -lboost_system -l m -lpthread -lz -lGLEW -lGLU -lGL -lfltk -lfltk_gl all: CXXFLAGS += -O2 -D_NDEBUG all: $(TARGET) diff --git a/gui.cpp b/gui.cpp index 74f4865..6e9f435 100644 --- a/gui.cpp +++ b/gui.cpp @@ -53,7 +53,7 @@ void GlGui::draw() { ogl::reshape(w(), h()); } - phero_map->s_update(); + phero_map->s_evaporate(); ogl::display(); } diff --git a/ias_robot.cpp b/ias_robot.cpp index bc78b31..c48af02 100644 --- a/ias_robot.cpp +++ b/ias_robot.cpp @@ -24,6 +24,8 @@ *************************************************************************************/ #include +#include +#include #include #include "ias_robot.hpp" @@ -34,8 +36,15 @@ static const long HALF_SECOND_USEC = 500000; static const double MIN_DIST_M = 1.5; static const double CRIT_DIST_M = 1.0; static const float MAP_SIZE = 16.0f; +static const int PHERO_AMOUNT = 10; +static const float PHERO_RADIUS = 1.0; + +static inline float random_num() { + return (((static_cast(rand() % 256) / 256.0) - 0.5f) * 2.0f ) * PHERO_RADIUS; +} IASSS_Robot::IASSS_Robot(std::string hostname, uint32_t port, PheromoneMap * phero_map) : Robot(hostname, port) { + srand(port + time(NULL)); _phero_map = phero_map; log("Creating IAS-SS robot"); } @@ -46,7 +55,6 @@ IASSS_Robot::~IASSS_Robot() { void IASSS_Robot::run() { int rv; - float x, y; long then, now, delta, wait; struct timeval tv; double dist = std::numeric_limits::infinity(); @@ -74,12 +82,8 @@ void IASSS_Robot::run() { now = tv.tv_usec; delta = now - then; - if(_phero_map != NULL) { - x = (_p_proxy->GetXPos() + (MAP_SIZE / 2)) / MAP_SIZE; - y = (_p_proxy->GetYPos() + (MAP_SIZE / 2)) / MAP_SIZE; - _phero_map->s_draw_point(x, y); - } - + deposit_pheromone(); + // Sleep for a bit before finishing this control iteration. wait = rv == 0 ? HALF_SECOND_USEC - delta : HALF_SECOND_USEC; usleep(wait); @@ -102,3 +106,27 @@ void IASSS_Robot::avoid_wall(float front_speed, float turn_speed) { else _p_proxy->SetSpeed(front_speed, PlayerCc::dtor(turn_speed)); } + +void IASSS_Robot::deposit_pheromone() { + float x = _p_proxy->GetXPos();// + float y = _p_proxy->GetYPos();// + (MAP_SIZE / 2)) / MAP_SIZE; + float px, py; + + if(_phero_map != NULL) { + for(int i = 0; i < PHERO_AMOUNT; i++) { + px = random_num() + x; + py = random_num() + y; + if(fabs(px) < (MAP_SIZE / 2) && fabs(py) < (MAP_SIZE / 2)) { + px = (px + (MAP_SIZE / 2)) / MAP_SIZE; + py = (py + (MAP_SIZE / 2)) / MAP_SIZE; + if(!_phero_map->s_deposit_pheromone(px, py)) { + i--; + continue; + } + } else { + i--; + continue; + } + } + } +} diff --git a/ias_robot.hpp b/ias_robot.hpp index 596e28a..29fd088 100644 --- a/ias_robot.hpp +++ b/ias_robot.hpp @@ -50,6 +50,7 @@ private: PheromoneMap * _phero_map; void avoid_wall(float front_speed, float turn_speed); + void deposit_pheromone(); }; #endif diff --git a/ias_ss.cpp b/ias_ss.cpp index 7c7600c..a4d2539 100644 --- a/ias_ss.cpp +++ b/ias_ss.cpp @@ -32,8 +32,8 @@ #include "ias_robot.hpp" const char * TITLE = "Pheromone map"; -const int W = 256; -const int H = 256; +const int W = 512; +const int H = 512; const uint32_t PORT = PlayerCc::PLAYER_PORTNUM + 1; const uint32_t NUM_ROBOTS = 4; diff --git a/ogl.cpp b/ogl.cpp index af0a69f..9bd4a02 100644 --- a/ogl.cpp +++ b/ogl.cpp @@ -32,8 +32,6 @@ #include "ogl.hpp" #include "GLSLProgram.hpp" -#define BUFFER_OFFSET(i) ((char *)NULL + (i)) - namespace ogl { // Variables diff --git a/pheromone.cpp b/pheromone.cpp index 6171c57..ec653ba 100644 --- a/pheromone.cpp +++ b/pheromone.cpp @@ -4,6 +4,12 @@ #include "pheromone.hpp" +#define MAP_POS(X, Y) (data[((X) * m_height) + (Y)]) + +static const float EVAPORATION_RATE = 0.1f; +const unsigned char MAX_PHERO_INTENSITY = 250; +const unsigned char MIN_PHERO_INTENSITY = 1; + PheromoneMap::PheromoneMap(const char * file_name) { load_map(file_name); sem_init(&map_semaphore, 0, 1); @@ -12,7 +18,7 @@ PheromoneMap::PheromoneMap(const char * file_name) { } PheromoneMap::~PheromoneMap() { - free(data); + delete data; sem_destroy(&map_semaphore); if(tex_created) { @@ -27,7 +33,7 @@ void PheromoneMap::load_map(const char * file_name) { png_init(0, 0); png_open_file_read(&tex, file_name); - data = static_cast(malloc(tex.width * tex.height * tex.bpp)); + data = new unsigned char[tex.width * tex.height * tex.bpp]; png_get_data(&tex, data); std::cout << "Loaded map \"" << file_name << "\" :: " << tex.width << "x" << tex.height << "x" << (int)tex.bpp << std::endl; @@ -50,8 +56,8 @@ GLuint PheromoneMap::s_build_texture() { glActiveTexture(GL_TEXTURE0); glBindTexture(GL_TEXTURE_2D, handle); glTexImage2D(GL_TEXTURE_2D, 0, GL_LUMINANCE, m_width, m_height, 0, GL_LUMINANCE, GL_UNSIGNED_BYTE, data); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); - glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); + glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST); tex_created = true; } sem_post(&map_semaphore); @@ -59,53 +65,36 @@ GLuint PheromoneMap::s_build_texture() { return handle; } -void PheromoneMap::s_draw_point(float x, float y) { - int _x = m_width * x; - int _y = m_height - (m_height * y); +bool PheromoneMap::s_deposit_pheromone(float x, float y) { + bool valid = false; + int _x = m_width * x; + int _y = m_height - (m_height * y); sem_wait(&map_semaphore); { - data[(_y * m_height) + _x] = 249; + if(MAP_POS(_y, _x) <= MAX_PHERO_INTENSITY) { + MAP_POS(_y, _x) = MAX_PHERO_INTENSITY; + valid = true; + } } sem_post(&map_semaphore); + + return valid; } -void PheromoneMap::s_update() { +void PheromoneMap::s_evaporate() { + unsigned char p_eva; + clock_t now = clock(); - - if(static_cast(now - then) / CLOCKS_PER_SEC < 0.005) { + if(static_cast(now - then) / CLOCKS_PER_SEC < 0.05) { return; } - then = now; sem_wait(&map_semaphore); { - for(int i = 1; i < m_height - 1; i++) { - for(int j = 1; j < m_width - 1; j++) { - if(data[(i * m_height) + j] > 10 && data[(i * m_height) + j] < 250){ - data[(i * m_height) + j] -= 1; - - if(data[((i - 1) * m_height) + (j - 1)] < 250) - data[((i - 1) * m_height) + (j - 1)] = data[(i * m_height) + j]; - - if(data[((i - 1) * m_height) + j] < 250) - data[((i - 1) * m_height) + j] = data[(i * m_height) + j]; - - if(data[((i - 1) * m_height) + (j + 1)] < 250) - data[((i - 1) * m_height) + (j + 1)] = data[(i * m_height) + j]; - - if(data[(i * m_height) + (j - 1)] < 250) - data[(i * m_height) + (j - 1)] = data[(i * m_height) + j]; - - if(data[(i * m_height) + (j + 1)] < 250) - data[(i * m_height) + (j + 1)] = data[(i * m_height) + j]; - - if(data[((i + 1) * m_height) + (j - 1)] < 250) - data[((i + 1) * m_height) + (j - 1)] = data[(i * m_height) + j]; - - if(data[((i + 1) * m_height) + j] < 250) - data[((i + 1) * m_height) + j] = data[(i * m_height) + j]; - - if(data[((i + 1) * m_height) + (j + 1)] < 250) - data[((i + 1) * m_height) + (j + 1)] = data[(i * m_height) + j]; + for(unsigned i = 1; i < m_height - 1; i++) { + for(unsigned j = 1; j < m_width - 1; j++) { + if(MAP_POS(i, j) >= MIN_PHERO_INTENSITY && MAP_POS(i, j) <= MAX_PHERO_INTENSITY){ + p_eva = MAP_POS(i, j) * EVAPORATION_RATE; + MAP_POS(i, j) -= p_eva; } } } diff --git a/pheromone.hpp b/pheromone.hpp index ce6ec76..2e60d6c 100644 --- a/pheromone.hpp +++ b/pheromone.hpp @@ -5,14 +5,17 @@ #include #include +extern const unsigned char MAX_PHERO_INTENSITY; +extern const unsigned char MIN_PHERO_INTENSITY; + class PheromoneMap { public: PheromoneMap(const char * file_name); ~PheromoneMap(); GLuint s_build_texture(); - void s_draw_point(float x, float y); - void s_update(); + bool s_deposit_pheromone(float x, float y); + void s_evaporate(); private: unsigned char * data;