very much faster raytracing, with one tiny and infrequent bug that ne…

…eds hunted

CMakeLists.txt

@@ -14,9 +14,9 @@ OPTION (BUILD_PLAYER_PLUGIN "Build Player plugin" ON)
 OPTION (BUILD_LSPTEST "Build Player plugin tests" OFF)
 OPTION (CPACK_CFG "[release building] generate CPack configuration files" OFF)
 
-add_definitions(-g -Wall)
+add_definitions(-O2 -g -Wall)
 
-# todo
+# todo - this doesn't work yet. Run Stage headless with -g.
 # OPTION (BUILD_GUI "Build FLTK-based GUI. If OFF, build a gui-less Stage useful e.g. for headless compute clusters." ON ) 
 
 cmake_minimum_required( VERSION 2.4 FATAL_ERROR )

examples/ctrl/fasr.cc

@@ -329,13 +329,7 @@ class Robot
 				  }
 			 }
 
-		  assert( ! isnan(a_goal ) );
-		  assert( ! isnan(pose.a ) );
-
 		  double a_error = normalize( a_goal - pose.a );
-
-		  assert( ! isnan(a_error) );
-
 		  pos->SetTurnSpeed(  a_error );
 		}  
   }

examples/ctrl/wander.cc

@@ -126,7 +126,7 @@ int LaserUpdate( Model* mod, robot_t* robot )
 		robot->pos->SetSpeed( 0,0,0 );
 		robot->pos->SetTurnSpeed( 0 );
 	 }
-
+ 
   return 0;
 }
 

libstage/canvas.cc

@@ -19,7 +19,7 @@
 #include <sstream>
 #include <png.h>
 
-
+#include "region.hh"
 #include "file_manager.hh"
 #include "options_dlg.hh"
 
@@ -784,6 +784,98 @@ void Canvas::renderFrame()
       glPopMatrix();
     }
 
+  if( ! world->rt_cells.empty() )
+	 {
+		glPushMatrix();	  		
+		GLfloat scale = 1.0/world->Resolution();
+      glScalef( scale, scale, 1.0 ); // XX TODO - this seems slightly
+
+		world->PushColor( stg_color_pack( 0,0,1,0.5) );
+
+		glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
+
+
+		for( unsigned int i=0;
+			  i < world->rt_cells.size();
+			  i++ )
+		  {
+// 			 char str[128];
+// 			 snprintf( str, 128, "(%d,%d)", 
+// 						  world->rt_cells[i].x, 
+// 						  world->rt_cells[i].y );
+
+// 			 Gl::draw_string(  world->rt_cells[i].x+1, 
+// 									 world->rt_cells[i].y+1, 0.1, str );
+
+			 //printf( "x: %d y: %d\n", world->rt_regions[i].x, world->rt_regions[i].y );
+			 glRectf( world->rt_cells[i].x+0.3, world->rt_cells[i].y+0.3,
+						 world->rt_cells[i].x+0.7, world->rt_cells[i].y+0.7 );
+		  }
+
+#if 0
+  		world->PushColor( stg_color_pack( 0,1,0,0.2) );
+  		glBegin( GL_LINE_STRIP );
+  		for( unsigned int i=0;
+  			  i < world->rt_cells.size();
+  			  i++ )
+  		  {			 
+  			 glVertex2f( world->rt_cells[i].x+0.5, world->rt_cells[i].y+0.5 );
+  		  }
+  		glEnd();
+  		world->PopColor();
+#endif
+
+      glPopMatrix();
+		world->PopColor();
+
+		//world->rt_cells.clear();
+    }
+
+  if( ! world->rt_candidate_cells.empty() )
+	 {
+		glPushMatrix();	  		
+		GLfloat scale = 1.0/world->Resolution();
+      glScalef( scale, scale, 1.0 ); // XX TODO - this seems slightly
+
+		world->PushColor( stg_color_pack( 1,0,0,0.5) );
+
+		glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
+
+		for( unsigned int i=0;
+			  i < world->rt_candidate_cells.size();
+			  i++ )
+		  {
+// 			 char str[128];
+// 			 snprintf( str, 128, "(%d,%d)", 
+// 						  world->rt_candidate_cells[i].x, 
+// 						  world->rt_candidate_cells[i].y );
+
+// 			 Gl::draw_string(  world->rt_candidate_cells[i].x+1, 
+// 									 world->rt_candidate_cells[i].y+1, 0.1, str );
+
+			 //printf( "x: %d y: %d\n", world->rt_regions[i].x, world->rt_regions[i].y );
+			 glRectf( world->rt_candidate_cells[i].x, world->rt_candidate_cells[i].y,
+						 world->rt_candidate_cells[i].x+1, world->rt_candidate_cells[i].y+1 );
+		  }
+
+ 		world->PushColor( stg_color_pack( 0,1,0,0.2) );
+ 		glBegin( GL_LINE_STRIP );
+ 		for( unsigned int i=0;
+ 			  i < world->rt_candidate_cells.size();
+ 			  i++ )
+ 		  {			 
+ 			 glVertex2f( world->rt_candidate_cells[i].x+0.5, world->rt_candidate_cells[i].y+0.5 );
+ 		  }
+ 		glEnd();
+ 		world->PopColor();
+
+      glPopMatrix();
+		world->PopColor();
+
+		//world->rt_cells.clear();
+    }
+
+
   if( showFootprints )
 	 {
 		glDisable( GL_DEPTH_TEST );		
@@ -801,8 +893,8 @@ void Canvas::renderFrame()
 
   if( showBBoxes )
 	 DrawBoundingBoxes();
-  
-  
+
+
   //LISTMETHOD( world->puck_list, Puck*, Draw );
 
   // TODO - finish this properly

libstage/model_fiducial.cc

@@ -13,9 +13,6 @@
 
 //#define DEBUG 1
 
-#include <assert.h>
-#include <math.h>
-
 #include "stage.hh"
 #include "option.hh"
 #include "worldfile.hh"

libstage/model_getset.cc

@@ -225,9 +225,9 @@ void Model::SetPose( const Pose& newpose )
       pose = newpose;
       pose.a = normalize(pose.a);
 
-      if( isnan( pose.a ) )
-		  printf( "SetPose bad angle %s [%.2f %.2f %.2f %.2f]\n",
-					 token, pose.x, pose.y, pose.z, pose.a );
+//       if( isnan( pose.a ) )
+// 		  printf( "SetPose bad angle %s [%.2f %.2f %.2f %.2f]\n",
+// 					 token, pose.x, pose.y, pose.z, pose.a );
 
       NeedRedraw();
       MapWithChildren();

libstage/model_gripper.cc

@@ -50,7 +50,6 @@ gripper
 
 
 #include <sys/time.h>
-#include <math.h>
 #include "stage.hh"
 #include "worldfile.hh"
 using namespace Stg;

libstage/model_laser.cc

@@ -297,13 +297,14 @@ void ModelLaser::Update( void )
 {     
   assert( samples.size() == sample_count );
   assert( rays.size() == sample_count );
-
+  
   double bearing = -fov/2.0;
-  double sample_incr = fov / (double)sample_count;
-
+  // make the first and last rays exactly at the extremes of the FOV
+  double sample_incr = fov / MAX(sample_count-1,1);
+
   Pose rayorg = geom.pose;
   rayorg.z += geom.size.z/2.0;
-  rayorg.a = bearing;
+  rayorg.a = bearing;// + sample_incr/2.0;
   rayorg = LocalToGlobal(rayorg);
 
   // set up the ray origins in global coords

libstage/model_position.cc

@@ -13,10 +13,6 @@
 
 
 #include <sys/time.h>
-#include <math.h>
-#include <stdlib.h>
-
-//#define DEBUG
 
 #include "stage.hh"
 #include "worldfile.hh"
@@ -468,9 +464,9 @@ void ModelPosition::Stop()
 
 void ModelPosition::SetSpeed( double x, double y, double a ) 
 { 
-  assert( ! isnan(x) );
-  assert( ! isnan(y) );
-  assert( ! isnan(a) );
+  //assert( ! isnan(x) );
+  //assert( ! isnan(y) );
+  //assert( ! isnan(a) );
 
   control_mode = STG_POSITION_CONTROL_VELOCITY;
   goal.x = x;
@@ -481,40 +477,40 @@ void ModelPosition::SetSpeed( double x, double y, double a )
 
 void ModelPosition::SetXSpeed( double x )
 { 
-  assert( ! isnan(x) );
+  //assert( ! isnan(x) );
   control_mode = STG_POSITION_CONTROL_VELOCITY;
   goal.x = x;
 }  
 
 
 void ModelPosition::SetYSpeed( double y )
 { 
-  assert( ! isnan(y) );
+  //assert( ! isnan(y) );
   control_mode = STG_POSITION_CONTROL_VELOCITY;
   goal.y = y;
 }  
 
 void ModelPosition::SetZSpeed( double z )
 { 
-  assert( ! isnan(z) );
+  //assert( ! isnan(z) );
   control_mode = STG_POSITION_CONTROL_VELOCITY;
   goal.z = z;
 }  
 
 void ModelPosition::SetTurnSpeed( double a )
 { 
-  assert( ! isnan(a) );
+  //assert( ! isnan(a) );
   control_mode = STG_POSITION_CONTROL_VELOCITY;
   goal.a = a;
 }  
 
 
 void ModelPosition::SetSpeed( Velocity vel ) 
 { 
-  assert( ! isnan(vel.x) );
-  assert( ! isnan(vel.y) );
-  assert( ! isnan(vel.z) );
-  assert( ! isnan(vel.a) );
+  //assert( ! isnan(vel.x) );
+  //assert( ! isnan(vel.y) );
+  //assert( ! isnan(vel.z) );
+  //assert( ! isnan(vel.a) );
 
   control_mode = STG_POSITION_CONTROL_VELOCITY;
   goal.x = vel.x;
@@ -525,9 +521,9 @@ void ModelPosition::SetSpeed( Velocity vel )
 
 void ModelPosition::GoTo( double x, double y, double a ) 
 {
-  assert( ! isnan(x) );
-  assert( ! isnan(y) );
-  assert( ! isnan(a) );
+  //assert( ! isnan(x) );
+  //assert( ! isnan(y) );
+  //assert( ! isnan(a) );
 
   control_mode = STG_POSITION_CONTROL_POSITION;
   goal.x = x;

libstage/model_ranger.cc

@@ -82,8 +82,6 @@
 #include "option.hh"
 using namespace Stg;
 
-#include <math.h>
-
 static const stg_watts_t RANGER_WATTSPERSENSOR = 0.2;
 static const stg_meters_t RANGER_SIZEX = 0.4;
 static const stg_meters_t RANGER_SIZEY = 0.4;

libstage/model_wifi.cc

@@ -25,7 +25,6 @@
 
 
 #include <sys/time.h>
-#include <math.h>
 #include "gui.h"
 
 // Number pulled directly from my ass

libstage/region.cc

@@ -32,8 +32,9 @@ SuperRegion::SuperRegion( World* world, stg_point_int_t origin )
   : count(0), origin(origin), world(world)	 
 {
   //static int srcount=0;
-  //printf( "created SR number %d\n", ++srcount );
-
+  //printf( "created SR number %d\n", ++srcount ); 
+  //  printf( "superregion at %d %d\n", origin.x, origin.y );
+
   // initialize the parent pointer for all my child regions
   for( unsigned int i=0; i<SuperRegion::SIZE; i++ )
 	 regions[i].superregion = this;
@@ -62,13 +63,13 @@ void SuperRegion::Draw( bool drawall )
   for( unsigned int x=0; x<SuperRegion::WIDTH; x++ )
 	 for( unsigned int y=0; y<SuperRegion::WIDTH; y++ )
 		{
-		  Region* r = GetRegion(x,y);
+		  Region* r = GetRegionLocal(x,y);
 
 		  if( r->count )
 			 // outline regions with contents
 			 glRecti( x<<RBITS, y<<RBITS, 
 						 (x+1)<<RBITS, (y+1)<<RBITS );
-		  else if( r->cells )
+		  else// if( r->cells )
 			 {
 				double left = x << RBITS;
 				double right = (x+1) << RBITS;
@@ -122,7 +123,7 @@ void SuperRegion::Draw( bool drawall )
   for( unsigned int x=0; x<SuperRegion::WIDTH; x++ )
 	 for( unsigned int y=0; y<SuperRegion::WIDTH; y++ )
 		{
-		  Region* r = GetRegion(x,y);
+		  Region* r = GetRegionLocal( x, y);
 
 		  if( r->count < 1 )
 			 continue;