From fdfe9557877bd25db1aef25795970ff3cc740469 Mon Sep 17 00:00:00 2001
From: Miguel Angel Astor Romero <sonofgrendel@gmail.com>
Date: Mon, 27 Jun 2016 10:46:11 -0400
Subject: [PATCH] Pheromone sensing complete.

---
 ant.world     | 16 ++++++++--------
 ias_robot.cpp |  4 ++--
 pheromone.cpp | 22 ++++++++++++++--------
 3 files changed, 24 insertions(+), 18 deletions(-)

diff --git a/ant.world b/ant.world
index 9a7b7e4..9fa7966 100644
--- a/ant.world
+++ b/ant.world
@@ -26,43 +26,43 @@ include "ant_bot.inc"
 
 window
 (
-	size [ 635.000 666.000 ]
+	size [ 606 768 ]
 	scale 36.995
-	center [ -0.040  -0.274 ]
-	rotate [ 0  0 ]
+	center [ -0.094  -0.274 ]
+	rotate [ 0.000  0.000 ]
 	show_data 1
 )
 
 floorplan
 (
 	bitmap "maps/cave.png"
-	size [ 16 16 1.500 ]
+	size [ 16.000 16.000 1.500 ]
 )
 
 ant_bot
 (
 	name "ant_robot_1"
-	pose [ -6.432 -5.895 0 45.000 ]
+	pose [ -4.044 7.232 0.000 5.000 ]
 	color "green"
 )
 
 ant_bot
 (
 	name "ant_robot_2"
-	pose [ 6.432 -3.895 0 135.000 ]
+	pose [ -5.443 -5.199 0.000 111.000 ]
 	color "green"
 )
 
 ant_bot
 (
 	name "ant_robot_3"
-	pose [ -6.432 5.895 0 -45.000 ]
+	pose [ -3.915 3.631 0.000 151.000 ]
 	color "green"
 )
 
 ant_bot
 (
 	name "ant_robot_4"
-	pose [ 6.432 5.895 0 -135.000 ]
+	pose [ -0.310 5.411 0.000 77.000 ]
 	color "green"
 )
diff --git a/ias_robot.cpp b/ias_robot.cpp
index a8030b0..d9d81bd 100644
--- a/ias_robot.cpp
+++ b/ias_robot.cpp
@@ -130,11 +130,11 @@ void IASSS_Robot::deposit_pheromone() {
 	py = (py + (MAP_SIZE / 2)) / MAP_SIZE;
 	if(!_phero_map->s_deposit_pheromone(px, py)) {
 	  i--;
-	  continue;
+	  break;
 	}
       } else {
 	i--;
-	continue;
+	break;
       }
     }
   }
diff --git a/pheromone.cpp b/pheromone.cpp
index 7cd109c..917384c 100644
--- a/pheromone.cpp
+++ b/pheromone.cpp
@@ -94,10 +94,16 @@ GLuint PheromoneMap::s_build_texture() {
 
 bool PheromoneMap::s_deposit_pheromone(float x, float y) {
   bool valid = false;
-  int  _x    = m_width * x;
-  int  _y    = m_height - (m_height * y);
+  unsigned int  _x    = m_width * x;
+  unsigned int  _y    = m_height - (m_height * y);
 
+  if((y > 1.0 || y < 0.0) || (x > 1.0 || x < 0.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) {
       MAP_POS(_y, _x) = MAX_PHERO_INTENSITY;
       valid = true;
@@ -129,9 +135,9 @@ void PheromoneMap::s_evaporate() {
 }
 
 void PheromoneMap::s_sample(phero_sensor_t * sensor, float x, float y, float yaw, float radius) {
-  int       _x = m_width * x;
-  int       _y = m_height - (m_height * y);
-  float     _r = m_width * radius;
+  float       _x = x;
+  float       _y = y;
+  float     _r = radius;
   float     dist;
   float     cos_theta;
   glm::vec2 v;
@@ -140,13 +146,13 @@ void PheromoneMap::s_sample(phero_sensor_t * sensor, float x, float y, float yaw
   if(sensor == NULL)
     return;
   else {
-    v  = glm::vec2(_r * cos(yaw), - _r * sin(yaw)) - glm::vec2(0.0, 0.0);
+    v  = glm::vec2(_r * cos(yaw), _r * sin(yaw)) - glm::vec2(0.0, 0.0);
     v  = glm::normalize(v);
-
+    
     sem_wait(&map_semaphore); {
       for(unsigned i = 1; i < m_height - 1; i++) {
 	for(unsigned j = 1; j < m_width - 1; j++) {
-	  vp        = glm::vec2(i, j) - glm::vec2(_y, _x);
+	  vp        = glm::vec2(j/float(m_width), 1.0f - (i/float(m_height))) - glm::vec2(_x, _y);
 	  dist      = glm::length(vp);
 	  vp        = glm::normalize(vp);
 	  cos_theta = glm::dot(vp, v);