From cecd37420d7022552f29f9d287f72b3f183a1de0 Mon Sep 17 00:00:00 2001 From: Miguel Angel Astor Romero Date: Mon, 27 Jun 2016 16:15:10 -0400 Subject: [PATCH] Almost there... --- ant.world | 8 +++--- ias_robot.cpp | 16 ++++++------ maps/cave_mask.png | Bin 911 -> 953 bytes maps/cave_mask_copy.png | Bin 0 -> 911 bytes pheromone.cpp | 56 +++++++++++++++++++++++++++++----------- pheromone.hpp | 27 ++++++++++++++++--- shaders/basic.frag | 3 ++- 7 files changed, 79 insertions(+), 31 deletions(-) create mode 100644 maps/cave_mask_copy.png diff --git a/ant.world b/ant.world index 9fa7966..12e00db 100644 --- a/ant.world +++ b/ant.world @@ -42,27 +42,27 @@ floorplan ant_bot ( name "ant_robot_1" - pose [ -4.044 7.232 0.000 5.000 ] + pose [ -6.432 -5.895 0 45.000 ] color "green" ) ant_bot ( name "ant_robot_2" - pose [ -5.443 -5.199 0.000 111.000 ] + pose [ 6.432 -3.895 0 135.000 ] color "green" ) ant_bot ( name "ant_robot_3" - pose [ -3.915 3.631 0.000 151.000 ] + pose [ -6.432 5.895 0 -45.000 ] color "green" ) ant_bot ( name "ant_robot_4" - pose [ -0.310 5.411 0.000 77.000 ] + pose [ 6.432 5.895 0 -135.000 ] color "green" ) diff --git a/ias_robot.cpp b/ias_robot.cpp index d9d81bd..aa398ec 100644 --- a/ias_robot.cpp +++ b/ias_robot.cpp @@ -36,7 +36,7 @@ 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 int PHERO_AMOUNT = 25; static const float PHERO_RADIUS = 1.0; static const float SENSOR_RADIUS = 2.0; @@ -59,13 +59,17 @@ void IASSS_Robot::run() { int rv; long then, now, delta, wait; struct timeval tv; - double dist = std::numeric_limits::infinity(); + double dist = std::numeric_limits::infinity(); _p_client->Read(); rv = gettimeofday(&tv, NULL); then = tv.tv_usec; + x = (_p_proxy->GetXPos() + (MAP_SIZE / 2)) / MAP_SIZE; + y = (_p_proxy->GetYPos() + (MAP_SIZE / 2)) / MAP_SIZE; + _phero_map->s_sample(&_phero_sensor, x, y, _p_proxy->GetYaw(), SENSOR_RADIUS / MAP_SIZE); + /****************************************************************************** * WALL AVOIDANCE START * ******************************************************************************/ @@ -75,17 +79,13 @@ void IASSS_Robot::run() { if(dist < MIN_DIST_M && dist > CRIT_DIST_M) { avoid_wall(METERS_PER_SEC, TURN_DEG_PER_SEC); - } else if(dist < CRIT_DIST_M) { + } else if(dist <= CRIT_DIST_M) { avoid_wall(0.0f, TURN_DEG_PER_SEC); } else _p_proxy->SetSpeed(METERS_PER_SEC, 0.0f); /****************************************************************************** * WALL AVOIDANCE END * ******************************************************************************/ - - x = (_p_proxy->GetXPos() + (MAP_SIZE / 2)) / MAP_SIZE; - y = (_p_proxy->GetYPos() + (MAP_SIZE / 2)) / MAP_SIZE; - _phero_map->s_sample(&_phero_sensor, x, y, _p_proxy->GetYaw(), SENSOR_RADIUS / MAP_SIZE); deposit_pheromone(); @@ -109,7 +109,7 @@ void IASSS_Robot::avoid_wall(float front_speed, float turn_speed) { for(unsigned int i = 126; i < _r_proxy->GetRangeCount(); i++) dist_l += _r_proxy->GetRange(i); dist_l /= (_r_proxy->GetRangeCount() - 126); - + if(dist_r >= dist_l) _p_proxy->SetSpeed(front_speed, PlayerCc::dtor(-turn_speed)); else diff --git a/maps/cave_mask.png b/maps/cave_mask.png index 7e06d6b86e950cc882e430170421a856b9b0f7e7..d3738657a4fa17e927352e8e44c54304c64b739a 100644 GIT binary patch delta 845 zcmeBY-^o6~zg}8|S+Z?m%4P-zW-m_{$B>F!Z|~kNYE|HAO%(t7H{NpZ>yll&x3ep? zJbAv!cj=VxH6AFiFi}=4<=HXBCskx_|4qw`}|O z@4aw_@uN@ zbqcHQ%bAOLOipALuJ1QjW(0cAA(dfb!z%`@`3(2u1-u*@-}6RYW64ob2w)TW^KC~c zdxf`x`rIE3|EFuN-;mKNswBSOFXKGJoV$X~99JqC?lNrHxmfD;-4&%wH~tjPt<3qf zw69gn`~RMH-`e^o{AvHg)7#4yKig~DfBv1`-YXM01z}MzIg^>Yz9CEZ!IBrJzsz36 zaO3uTZQ+K$yQ}%{3H+GTaQyP)PdCoayWMez^VWm=-E0EyjeUEVp_&tJIrOkFY_L0U ztNnQ0J}>W^wr{R~-;%oO|GAcq7cfN(4HJEjGKSbN^7Glb+nc2qdd+A4a6ny1jJZPJ z;JH%vxyg&&%r-s}V1OG7Ha(&2_T;Ode`fvun!VGj*f8ZNKcn_N_B-*-XCfIIM0{0% ziY$01fs`)LXqedWh(U=F7@T603z*q~m__2i^KZOvr4?c_hYyv5RC&7kxvX; zF)U)*G?!1^UQ#ZGaS{6y=73U$X^w%6Tc;nKH#NYM@f=sZMmFmlkqgtrA53sqS$#k_ zG0#njf9HCa3CsV4^LU*5G~Z6GpCQ=&X|a07RYo7CKcBYhdI9 zjU7rX2iDv;9=1k{>Ce3N%Y+!>qe3hi&CLE+?3c`J{;a=&OXat!;%|_K1x5T{q8LBd zZwvnY=gb+;SJ@1A<{DgNNtU(B^Iq^|y>V&E6ve03lA4jf?s{qhrDk4Z{4t?Cv?2W8RmL6mFs~zMuD0FFPX{vs z1sq}b1%$e+%UDS>}zqZM8jHL#ofObmLJc5tv<2$hX*&$2dP%Egk6<^D|-L!5nO+l z_iu&aI!>m^jm*OJA>8ffMfcS_G_z@_{ZpRGv|)Su_XnJf=Q(}$YvuI!U9K12a`OLq z>4yE+O*ZmFUE7cf_kZrrpx!f|!y82B zGhKi0_^g^StnpECgC+xmZA0n=AjM#EaFy)@t^~;?VAsn_ICTDC^g7L#5>RT|kN{$=lt9;eUJonf*W>XMsm# zF#`j)5C}6~x?A@LD9B#o>Fdh=fK7@^!F0)$X?hF{%nF_^jv*Dd-rl`iwAz5jEzpkt z{r^+1VwYW3V{G2|b#rNDnqRVR%1r^LTYZPOU0$dgm67sGkoDG$KaX9t=lw5aWtd=N zQpUETid7;QXaNv3>|$8Nv}rD%y1k@a4C5m9C(Hq*4AUF~8MjV9IB#lzC*wITjcnFA zA{VBKKbYXKvig8V4%y-8)ctw;RfhOYYlxQ>$vVKtl;!rgvebYIOwGnQC;y+9ZrFd_WFtRRS3|0_p%z2K4~C7)kJs6Gd*77%_D?x?=BNEdjxJx|CMTq= zuxRKCZ*cnea(m*vW6wWZI(Y@g>}5J-wwq7zQ(T4$N0R~w9jZ;SR{UAF?eG8Gok6{4 zK8H7m&S$#*-tk#AV_4&(;s#9y2HS?z2|$X$v+q8 SEy2ENAaPGuKbLh*2~7YdFIY(c literal 0 HcmV?d00001 diff --git a/pheromone.cpp b/pheromone.cpp index 917384c..b18972e 100644 --- a/pheromone.cpp +++ b/pheromone.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include @@ -33,10 +34,18 @@ #define MAP_POS(X, Y) (data[((X) * m_height) + (Y)]) -static const float EVAPORATION_RATE = 0.1f; -const unsigned char MAX_PHERO_INTENSITY = 250; +static const float EVAPORATION_RATE = 0.05f; +const unsigned char MAX_PHERO_INTENSITY = 255; const unsigned char MIN_PHERO_INTENSITY = 1; +static inline int sign(float f) { + return (f < 0.0f) ? -1 : ((f > 0.0f) ? 1 : 0); +} + +static inline int side(glm::vec3 line, glm::vec3 v) { + return sign(glm::cross(line, v).z); +} + PheromoneMap::PheromoneMap(const char * file_name) { load_map(file_name); sem_init(&map_semaphore, 0, 1); @@ -99,12 +108,11 @@ bool PheromoneMap::s_deposit_pheromone(float x, float y) { if((y > 1.0 || y < 0.0) || (x > 1.0 || x < 0.0)) return false; - + if((_y >= m_height || _y < 0) || (_x >= m_width || _x < 0)) + return false; + sem_wait(&map_semaphore); { - if((_y >= m_height || _y < 0) || (_x >= m_width || _x < 0)) - return false; - - if(MAP_POS(_y, _x) <= MAX_PHERO_INTENSITY) { + if(MAP_POS(_y, _x) >= MIN_PHERO_INTENSITY) { MAP_POS(_y, _x) = MAX_PHERO_INTENSITY; valid = true; } @@ -125,7 +133,7 @@ void PheromoneMap::s_evaporate() { sem_wait(&map_semaphore); { 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) { + if(MAP_POS(i, j) >= MIN_PHERO_INTENSITY) { p_eva = MAP_POS(i, j) * EVAPORATION_RATE; MAP_POS(i, j) -= p_eva; } @@ -135,18 +143,22 @@ void PheromoneMap::s_evaporate() { } void PheromoneMap::s_sample(phero_sensor_t * sensor, float x, float y, float yaw, float radius) { - float _x = x; - float _y = y; - float _r = radius; + float _x = x; + float _y = y; float dist; float cos_theta; + float ang; glm::vec2 v; glm::vec2 vp; + glm::vec3 line; + glm::vec3 v3d; if(sensor == NULL) return; else { - v = glm::vec2(_r * cos(yaw), _r * sin(yaw)) - glm::vec2(0.0, 0.0); + sensor->reset(); + + v = glm::vec2(radius * cos(yaw), radius * sin(yaw)) - glm::vec2(0.0, 0.0); v = glm::normalize(v); sem_wait(&map_semaphore); { @@ -157,14 +169,28 @@ void PheromoneMap::s_sample(phero_sensor_t * sensor, float x, float y, float yaw vp = glm::normalize(vp); cos_theta = glm::dot(vp, v); - if(cos_theta > 0.0f && dist <= _r) { - if(MAP_POS(i, j) >= MIN_PHERO_INTENSITY && MAP_POS(i, j) <= MAX_PHERO_INTENSITY) { - MAP_POS(i, j) = MAX_PHERO_INTENSITY; + if(cos_theta > 0.0f && dist <= radius) { + ang = acos(cos_theta); + line = glm::vec3(v.x, v.y, 0.0f); + v3d = glm::vec3(vp.x, vp.y, 0.0f); + if(side(line, v3d) > 0) { + assert(static_cast(90.0f - ang) <= 90); + sensor->samples[static_cast(90.0f - ang)] += MAP_POS(i, j); + sensor->sample_amnt[static_cast(90.0f - ang)] += 1; + //MAP_POS(i, j) = MAX_PHERO_INTENSITY; + } else { + assert(static_cast(90.0f + ang) < 180); + sensor->samples[static_cast(90.0f + ang)] += MAP_POS(i, j); + sensor->sample_amnt[static_cast(90.0f + ang)] += 1; + //MAP_POS(i, j) = MAX_PHERO_INTENSITY; } } else continue; } } } sem_post(&map_semaphore); + + for(unsigned int i = 0; i < NUM_PHERO_SAMPLES; i++) + sensor->samples[i] = (sensor->sample_amnt[i] > 0) ? (sensor->samples[i] / sensor->sample_amnt[i]) : 0.0f; } } diff --git a/pheromone.hpp b/pheromone.hpp index 4a70b4f..3c148c8 100644 --- a/pheromone.hpp +++ b/pheromone.hpp @@ -37,17 +37,38 @@ extern const unsigned char MIN_PHERO_INTENSITY; const unsigned int NUM_PHERO_SAMPLES = 180; typedef struct PHERO_SENSOR { - float samples[NUM_PHERO_SAMPLES]; + float samples[NUM_PHERO_SAMPLES]; + unsigned int sample_amnt[NUM_PHERO_SAMPLES]; + float probs[NUM_PHERO_SAMPLES]; PHERO_SENSOR() { - memset(this->samples, 0.0f, NUM_PHERO_SAMPLES); + reset(); + } + + void reset() { + memset(sample_amnt, 0, sizeof(unsigned int) * NUM_PHERO_SAMPLES); + for(unsigned int i = 0; i < NUM_PHERO_SAMPLES; i++) { + samples[i] = 0.0f; + probs[i] = 0.0f; + } + } + + void set_probabilities() { + float phero_sum = 0.0f; + + for(unsigned int i = 0; i < NUM_PHERO_SAMPLES; i++) + phero_sum += samples[i]; + + for(unsigned int i = 0; i < NUM_PHERO_SAMPLES; i++) { + probs[i] = 1.0f / (samples[i] / phero_sum); + } } float operator[](unsigned int index) { if(index >= NUM_PHERO_SAMPLES) return -1.0f; else - return this->samples[index]; + return samples[index]; } } phero_sensor_t; diff --git a/shaders/basic.frag b/shaders/basic.frag index 8d85ca2..29599cb 100644 --- a/shaders/basic.frag +++ b/shaders/basic.frag @@ -4,5 +4,6 @@ uniform sampler2D sTexture; void main() { vec4 tex = texture2D(sTexture, gl_TexCoord[0].st); - gl_FragColor = clamp(tex, 0.0, 1.0); + vec4 color = mix(vec4(0.0, 0.0, 0.3, 1.0), vec4(1.0, 1.0, 0.0, 1.0), tex.r); + gl_FragColor = clamp(color, 0.0, 1.0); }