commit 11bb78c2ef8506f71cf41235bfc5f03b5443c160
Author: Hui Jun Tay <viki@c3po.(none)>
Date:   Fri Nov 1 19:44:07 2013 -0500

    BOM now works with rect. scout shape

diff --git a/scout/scoutsim/src/sim_frame.cpp b/scout/scoutsim/src/sim_frame.cpp
index f44e930..ca80e14 100644
--- a/scout/scoutsim/src/sim_frame.cpp
+++ b/scout/scoutsim/src/sim_frame.cpp
@@ -136,17 +136,17 @@ namespace scoutsim
         wireless_send = nh.subscribe("/wireless/send", 1000,
             &SimFrame::wirelessCallback, this);
 
-        //Emitter default values
-        em_aperture = PI / 3.0;
-        em_distance = 1;
-
-
         // Teleop
         teleop_type = TELEOP_OFF;
         teleop_l_speed = 0;
         teleop_r_speed = 0;
         teleop_scoutname = "scout1";
 
+        //Emitter default values
+        em_aperture = PI / 3.0;
+        em_distance = 1.0;
+
+
         teleop_pub = nh.advertise< ::messages::set_motors>("/scout1/set_motors", 1000);
 
         ROS_INFO("Starting scoutsim with node name %s",
@@ -718,19 +718,64 @@ namespace scoutsim
         M_Emitter::iterator m_it = emitters.begin();
         M_Emitter::iterator m_end = emitters.end();
 
+        //Emitter default values
+        double bom_aperture = PI / 3.0;
+        double bom_distance = 1.0;
+    
             //iterate over Emitters:
        for (; m_it != m_end; ++m_it)
         {
             geometry_msgs::Pose2D emitter_pos;
             emitter_pos = m_it->second->get_pos();
-            if (is_inrange(scout_pos, emitter_pos)) { //if scout is within emitter range
+            if (is_inrange(scout_pos, emitter_pos, em_aperture, em_distance)) { 
+            //if scout is within emitter range
             //check if emitter is within each BOM range
-                
+            double x_offset;
+            double y_offset;    
+            double offset_angle;
+            double diag_dis = pow((pow((SCOUT_W/2.0),2) + 
+                              pow((SCOUT_H/2.0),2)),(0.5));   
+                    
                 for (int i = 0; i<10; i++) {
                 geometry_msgs::Pose2D bom_pos;
-                bom_pos.x = scout_pos.x;
-                bom_pos.y = scout_pos.y;  
-                bom_pos.theta = (scout_pos.theta+(2*PI/10.0)*i);
+                
+                if (i == 2) { //middle left
+                    offset_angle = PI/2.0;
+                    bom_pos.theta = scout_pos.theta + PI/2.0;
+                }
+                else if (i == 7) { //middle right
+                    offset_angle = 3*PI/2.0;
+                    bom_pos.theta = scout_pos.theta + 3.0*PI/2.0;
+                }
+                else if (i < 2) { //top left
+                    offset_angle =  atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
+                    bom_pos.theta = scout_pos.theta + PI/2.0 * i;
+                }
+                else if (i >= 2 && i <5) { //bottom left
+                    offset_angle =  PI - atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
+                    bom_pos.theta = scout_pos.theta + PI/2.0 + PI/2.0 * (i/4);
+                }
+                else if (i >= 5 && i < 8) { //bottom right
+                    offset_angle =  PI + atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
+                    bom_pos.theta = scout_pos.theta + PI + PI/2.0 * (i/6);
+                }
+                else { //top right
+                    offset_angle =  2*PI - atan(abs((SCOUT_W/2.0)/(SCOUT_H/2.0)));
+                    bom_pos.theta = scout_pos.theta + 3*PI/2.0 + PI/2.0 * (i/9);
+                }
+                offset_angle += scout_pos.theta;
+                
+                if (i == 2 || i == 7) {
+                    x_offset = (SCOUT_W/2.0)*cos(offset_angle);
+                    y_offset = (SCOUT_W/2.0)*sin(offset_angle);
+                }
+                else {
+                    x_offset = diag_dis*cos(offset_angle);
+                    y_offset = diag_dis*sin(offset_angle);
+                }
+
+                bom_pos.x = scout_pos.x + x_offset;
+                bom_pos.y = scout_pos.y - y_offset;  
 
                 if (bom_pos.theta > 2*PI) {
                     bom_pos.theta -= 2*PI;
@@ -739,15 +784,15 @@ namespace scoutsim
                     bom_pos.theta += 2*PI;
                 }
 
-                if(is_inrange(emitter_pos, bom_pos)) {
-                        //ROS_INFO("BOM%d %f: true", i, bom_pos.theta);
-
+                if(is_inrange(emitter_pos, bom_pos, bom_aperture, bom_distance)) {
+                        
                         it->second->update_BOM(i, true);
+                        
                     }
                 else {
-                        //ROS_INFO("BOM%d %f: false", i, bom_pos.theta);
-                
+                        
                         it->second->update_BOM(i, false);
+                        
                 }
                 }
                 
@@ -783,7 +828,8 @@ namespace scoutsim
 
 
     bool SimFrame::is_inrange(geometry_msgs::Pose2D scout_pos,
-                           geometry_msgs::Pose2D emitter_pos)
+                           geometry_msgs::Pose2D emitter_pos,
+                           double aperture, double distance)
                            
     {
         //ROS_INFO("SCOUT: %f %f %f ", scout_pos.x, scout_pos.x, scout_pos.theta);
@@ -803,10 +849,10 @@ namespace scoutsim
         
         //ROS_INFO("distance: %f, angle: %f, orient: %f", se_distance, se_angle, emitter_orient);
 
-        if ((se_distance <= em_distance)) { //distance check
+        if ((se_distance <= distance)) { //distance check
 
-            double upper_limit = (emitter_orient + em_aperture/2);
-            double lower_limit = (emitter_orient - em_aperture/2);
+            double upper_limit = (emitter_orient + aperture/2);
+            double lower_limit = (emitter_orient - aperture/2);
 
             //ROS_INFO("upper: %f, lower: %f", upper_limit, lower_limit+2*PI);
 
