Update of /cvsroot/playerstage/code/stage/libstage
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv487

Modified Files:
      Tag: opengl
        Makefile.manual blockgrid.cc model.cc model_laser.cc 
        model_ranger.cc stage.hh stest.cc typetable.cc world.cc 
        worldgtk.cc 
Log Message:
moved internal timing to microseconds

Index: stest.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/stest.cc,v
retrieving revision 1.1.2.11
retrieving revision 1.1.2.12
diff -C2 -d -r1.1.2.11 -r1.1.2.12
*** stest.cc    31 Oct 2007 03:55:11 -0000      1.1.2.11
--- stest.cc    1 Nov 2007 07:18:53 -0000       1.1.2.12
***************
*** 21,30 ****
  double turnrate = DTOR(60);
  
! int randint;
! int randcount = 0;
! int avoidcount = 0;
! int obs = FALSE;
  
! int POPSIZE = 50;
  
  int main( int argc, char* argv[] )
--- 21,35 ----
  double turnrate = DTOR(60);
  
! typedef struct
! {
!   StgModelLaser* laser;
!   StgModelPosition* position;
!   StgModelRanger* ranger;
!   int randcount ;
!   int avoidcount;
!   bool obs;
! } robot_t;
  
! const int POPSIZE = 10;
  
  int main( int argc, char* argv[] )
***************
*** 39,45 ****
        
    // initialize libstage
!   StgWorldGtk::Init( &argc, &argv );
  
    StgWorldGtk world;
    world.Load( argv[1] );
    
--- 44,53 ----
        
    // initialize libstage
!   //StgWorld::Init( &argc, &argv );
!   //StgWorld world;
  
+   StgWorldGtk::Init( &argc, &argv );
    StgWorldGtk world;
+ 
    world.Load( argv[1] );
    
***************
*** 62,114 ****
     //char* robotname = argv[2];
     
!    //   // generate the name of the laser attached to the robot
!    //char lasername[64];
!    //snprintf( lasername, 63, "%s.laser:0", robotname ); 
!    
!    //   // generate the name of the sonar attached to the robot
!    //   char rangername[64];
!    //   snprintf( rangername, 63, "%s.ranger:0", robotname ); 
!    
!    char namebuf[256];
!    
! 
! 
!    //StgModelPosition* position = (StgModelPosition*)world.GetModel( 
"MyWorld:0.position:0" );
!    //assert(position);
!    
!    //StgModelLaser* laser = 
(StgModelLaser*)world.GetModel("MyWorld:0.position:0.laser:0" );
!    //assert( laser );
!    
!    StgModelPosition* positions[POPSIZE];
!    StgModelLaser* lasers[POPSIZE];
! 
!    for( int i=0; i<POPSIZE; i++ )
!      {
!        sprintf( namebuf, "MyWorld:0.position:%d", i+1 );
!        positions[i] = (StgModelPosition*)world.GetModel( namebuf );
!        assert(positions[i]);
!        positions[i]->Subscribe();
         
!        sprintf( namebuf, "MyWorld:0.position:%d.laser:0", i+1 );
!        lasers[i] = (StgModelLaser*)world.GetModel( namebuf );
!        assert(lasers[i]);
!        lasers[i]->Subscribe();
!      }
!    
! //   StgModelRanger* ranger = (StgModelRanger*)world.GetModel( rangername );
! //   assert(ranger);
! 
! //   // subscribe to the laser - starts it collecting data
! 
!    //position->Subscribe();   
!    //laser->Subscribe();
! 
!    //position->Print( "Subscribed to model" );
!    //laser->Print( "Subscribed to model" );
!   //ranger->Print( "Subscribed to model" );
  
!   //printf( "Starting world clock..." ); fflush(stdout);
    // start the clock
!   //world.Start();
    //puts( "done" );
  
--- 70,100 ----
     //char* robotname = argv[2];
     
!   char namebuf[256];  
!   robot_t robots[POPSIZE];
!   
!   for( int i=0; i<POPSIZE; i++ )
!     {
!        robots[i].randcount = 0;
!        robots[i].avoidcount = 0;
!        robots[i].obs = false;
         
!        sprintf( namebuf, "MyWorld.position:%d", i );
!        robots[i].position = (StgModelPosition*)world.GetModel( namebuf );
!        assert(robots[i].position);
!        robots[i].position->Subscribe();
!        
!        sprintf( namebuf, "MyWorld.position:%d.laser:0", i );
!        robots[i].laser = (StgModelLaser*)world.GetModel( namebuf );
!        assert(robots[i].laser);
!        robots[i].laser->Subscribe();
  
!        sprintf( namebuf, "MyWorld.position:%d.ranger:0", i );
!        robots[i].ranger = (StgModelRanger*)world.GetModel( namebuf );
!        assert(robots[i].ranger);
!        robots[i].ranger->Subscribe();
!     }
!    
    // start the clock
!   world.Start();
    //puts( "done" );
  
***************
*** 120,125 ****
    //  StgModelLaser* laser = lasers[1];
  
!   //while( world.RealTimeUpdate() )
!   while( world.Update() )
      {
        //       nothing
--- 106,111 ----
    //  StgModelLaser* laser = lasers[1];
  
!    while( world.RealTimeUpdate() )
!      // while( world.Update() )
      {
        //       nothing
***************
*** 127,207 ****
        //if( ! world.RealTimeUpdate() )
        //break;
!      
        for( int i=0; i<POPSIZE; i++ )
        {
!         StgModelPosition* position = positions[i];
!         StgModelLaser* laser = lasers[i];
          
          // get some laser data
!         size_t laser_sample_count = laser->sample_count;      
!         stg_laser_sample_t* laserdata = laser->samples;
! 
          if( laserdata == NULL )
            continue;
        
!         // THIS IS ADAPTED FROM PLAYER'S RANDOMWALK C++ EXAMPLE
! 
!         /* See if there is an obstacle in front */
!         obs = FALSE;
!         for( unsigned int i = 0; i < laser_sample_count; i++)
!           {
!             if(laserdata[i].range < minfrontdistance)
!               obs = TRUE;
!           }
        
!         if(obs || avoidcount )
!           {
!             newspeed = 0;
          
!             /* once we start avoiding, continue avoiding for 2 seconds */
!             /* (we run at about 10Hz, so 20 loop iterations is about 2 sec) */
!             if(!avoidcount)
!               {
!                 avoidcount = 15;
!                 randcount = 0;
              
!                 // find the minimum on the left and right
              
!                 double min_left = 1e9;
!                 double min_right = 1e9;
              
!                 for( unsigned int i=0; i<laser_sample_count; i++ )
!                   {
!                     if(i>(laser_sample_count/2) && laserdata[i].range < 
min_left)
!                       min_left = laserdata[i].range;
!                     else if(i<(laser_sample_count/2) && laserdata[i].range < 
min_right)
!                       min_right = laserdata[i].range;
!                   }
! 
!                 if( min_left < min_right)
!                   newturnrate = -turnrate;
!                 else
!                   newturnrate = turnrate;
                }
!         
!             avoidcount--;
            }
-         else
-           {
-             avoidcount = 0;
-             newspeed = speed;
          
!             /* update turnrate every 3 seconds */
!             if(!randcount)
!               {
!                 /* make random int tween -20 and 20 */
!                 randint = rand() % 41 - 20;
!             
!                 newturnrate = DTOR(randint);
!                 randcount = 50;
!               }
!             randcount--;
            }
!       
!         position->Do( newspeed, 0, newturnrate );
        }
! 
!     }
!   
    //#endif  
  
--- 113,189 ----
        //if( ! world.RealTimeUpdate() )
        //break;
!       
        for( int i=0; i<POPSIZE; i++ )
        {
!         //robots[i].ranger->Print( NULL );
          
          // get some laser data
!         size_t laser_sample_count = robots[i].laser->sample_count;      
!         stg_laser_sample_t* laserdata = robots[i].laser->samples;
!       
          if( laserdata == NULL )
            continue;
        
!       // THIS IS ADAPTED FROM PLAYER'S RANDOMWALK C++ EXAMPLE
        
!       /* See if there is an obstacle in front */
!       robots[i].obs = FALSE;
!       for( unsigned int l = 0; l < laser_sample_count; l++)
!       {
!         if(laserdata[l].range < minfrontdistance)
!           robots[i].obs = TRUE;
!       }
!       
!       if( robots[i].obs || robots[i].avoidcount )
!       {
!         newspeed = 0;
          
!         /* once we start avoiding, continue avoiding for 2 seconds */
!         /* (we run at about 10Hz, so 20 loop iterations is about 2 sec) */
!         if( ! robots[i].avoidcount)
!           {
!             robots[i].avoidcount = 15;
!             robots[i].randcount = 0;
              
!             // find the minimum on the left and right
              
!             double min_left = 1e9;
!             double min_right = 1e9;
              
!             for( unsigned int i=0; i<laser_sample_count; i++ )
!               {
!                 if(i>(laser_sample_count/2) && laserdata[i].range < min_left)
!                   min_left = laserdata[i].range;
!                 else if(i<(laser_sample_count/2) && laserdata[i].range < 
min_right)
!                   min_right = laserdata[i].range;
                }
!             
!             if( min_left < min_right)
!               newturnrate = -turnrate;
!             else
!               newturnrate = turnrate;
            }
          
!         robots[i].avoidcount--;
!       }
!       else
!       {
!         robots[i].avoidcount = 0;
!         newspeed = speed;
!         
!         /* update turnrate every 3 seconds */
!         if(! robots[i].randcount)
!           {
!             /* make random int tween -20 and 20 */
!             newturnrate = DTOR(rand() % 41 - 20);
!             robots[i].randcount = 50;
            }
!         robots[i].randcount--;
        }
!       
!       robots[i].position->Do( newspeed, 0, newturnrate );
!       //robots[i].position->Do( 0, 0, 10 );
!               }
!     }  
    //#endif  
  

Index: model_ranger.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/model_ranger.cc,v
retrieving revision 1.1.2.2
retrieving revision 1.1.2.3
diff -C2 -d -r1.1.2.2 -r1.1.2.3
*** model_ranger.cc     13 Oct 2007 07:42:55 -0000      1.1.2.2
--- model_ranger.cc     1 Nov 2007 07:18:53 -0000       1.1.2.3
***************
*** 77,94 ****
  
  #include <math.h>
! #include "model.hh"
! #include "gui.h"
! #include <GL/gl.h>
  
  #define STG_RANGER_WATTS 2.0 // ranger power consumption
  
! StgModelRanger::StgModelRanger( stg_world_t* world, 
                                StgModel* parent,
                                stg_id_t id,
!                               CWorldFile* wf )
!   : StgModel( world, parent, id, wf )
  {
    PRINT_DEBUG2( "Constructing StgModelRanger %d (%s)\n", 
!               id, wf->GetEntityType( id ) );
      
    // Set up sensible defaults
--- 77,92 ----
  
  #include <math.h>
! #include "stage.hh"
  
  #define STG_RANGER_WATTS 2.0 // ranger power consumption
  
! StgModelRanger::StgModelRanger( StgWorld* world, 
                                StgModel* parent,
                                stg_id_t id,
!                               char* typestr )
!   : StgModel( world, parent, id, typestr )
  {
    PRINT_DEBUG2( "Constructing StgModelRanger %d (%s)\n", 
!               id, typestr );
      
    // Set up sensible defaults
***************
*** 154,157 ****
--- 152,157 ----
    StgModel::Load();
    
+   CWorldFile* wf = world->wf;
+ 
    if( wf->PropertyExists(id, "scount" ) )
      {
***************
*** 217,224 ****
  }
  
! int ranger_raytrace_match( StgModel* mod, StgModel* hitmod )
  {
    // Ignore myself, my children, and my ancestors.
!   return( hitmod->RangerReturn() && !mod->IsRelated( hitmod ) );
  }     
  
--- 217,225 ----
  }
  
! int ranger_raytrace_match( StgBlock* block, StgModel* finder )
  {
    // Ignore myself, my children, and my ancestors.
!   return( block->mod->RangerReturn() && !block->mod->IsRelated( finder ) );
!   //return true;
  }     
  
***************
*** 239,269 ****
      {
        // get the sensor's pose in global coords
!       stg_pose_t pz;
!       memcpy( &pz, &sensors[t].pose, sizeof(pz) ); 
!       this->LocalToGlobal( &pz );
        
!       double range = sensors[t].bounds_range.max;
        
        int r;
        for( r=0; r<sensors[t].ray_count; r++ )
        {         
!         double angle_per_ray = sensors[t].fov / sensors[t].ray_count;
!         double ray_angle = -sensors[t].fov/2.0 + angle_per_ray * r + 
angle_per_ray/2.0;
!         
!         stg_meters_t hitrange, hitx, hity;
          
!         // if the ray hits anything, 'range' will be changed
!         if( stg_first_model_on_ray( pz.x, pz.y, pz.z + 0.01, // TODO: UNHACK 
!                                     pz.a + ray_angle, 
!                                     sensors[t].bounds_range.max, 
!                                     world->matrix, 
!                                     PointToBearingRange, 
!                                     ranger_raytrace_match,
!                                     this,
!                                     &hitrange, &hitx, &hity ) )
!           {
!             if( hitrange < range )
!               range = hitrange;
!           } 
  
          
--- 240,274 ----
      {
        // get the sensor's pose in global coords
!       //stg_pose_t pz;
!       //memcpy( &pz, &sensors[t].pose, sizeof(pz) ); 
!       //this->LocalToGlobal( &pz );
        
!       double range;
        
        int r;
        for( r=0; r<sensors[t].ray_count; r++ )
        {         
!         //double angle_per_ray = sensors[t].fov / sensors[t].ray_count;
!         //double ray_angle = -sensors[t].fov/2.0 + angle_per_ray * r + 
angle_per_ray/2.0;
          
!         range = Raytrace( &sensors[t].pose,
!                           sensors[t].bounds_range.max,
!                           (stg_block_match_func_t)ranger_raytrace_match,
!                           this,
!                           NULL );                            
! 
! //      // if the ray hits anything, 'range' will be changed
! //      if( stg_first_model_on_ray( pz.x, pz.y, pz.z + 0.01, // TODO: UNHACK 
! //                                  pz.a + ray_angle, 
! //                                  sensors[t].bounds_range.max, 
! //                                  world->matrix, 
! //                                  PointToBearingRange, 
! //                                  ranger_raytrace_match,
! //                                  this,
! //                                  &hitrange, &hitx, &hity ) )
! //        {
! //          if( hitrange < range )
! //            range = hitrange;
! //        } 
  
          
***************
*** 284,288 ****
        sensors[t].range = range;
        //sensors[t].error = TODO;
! 
        
      }   
--- 289,293 ----
        sensors[t].range = range;
        //sensors[t].error = TODO;
!       
        
      }   
***************
*** 348,352 ****
  }
  
! void StgModelRanger::DListData( void )
  {
    // recreate the display list for this data
--- 353,357 ----
  }
  
! void StgModelRanger::DataVisualize( void )
  {
    // recreate the display list for this data
***************
*** 360,368 ****
  
        // now get on with rendering the ranger data 
!       glPolygonMode( GL_FRONT_AND_BACK, 
!                    world->win->show_alpha ? GL_FILL : GL_LINES );
!       push_color_rgba( 0, 1, 0, 0.2 ); // transparent pale green 
        render_rangers( sensors, sensor_count );
!       pop_color(); 
  
        // restore state 
--- 365,373 ----
  
        // now get on with rendering the ranger data 
!       glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
!                    //              world->win->show_alpha ? GL_FILL : 
GL_LINES );
!       PushColor( 0, 1, 0, 0.2 ); // transparent pale green 
        render_rangers( sensors, sensor_count );
!       PopColor(); 
  
        // restore state 

Index: model_laser.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/model_laser.cc,v
retrieving revision 1.1.2.8
retrieving revision 1.1.2.9
diff -C2 -d -r1.1.2.8 -r1.1.2.9
*** model_laser.cc      31 Oct 2007 03:55:11 -0000      1.1.2.8
--- model_laser.cc      1 Nov 2007 07:18:53 -0000       1.1.2.9
***************
*** 31,35 ****
   #define STG_DEFAULT_LASER_FOV M_PI
   #define STG_DEFAULT_LASER_SAMPLES 180
!  #define STG_DEFAULT_LASER_INTERVAL_MS 200
   #define STG_DEFAULT_LASER_RESOLUTION 1
  
--- 31,35 ----
   #define STG_DEFAULT_LASER_FOV M_PI
   #define STG_DEFAULT_LASER_SAMPLES 180
!  #define STG_DEFAULT_LASER_INTERVAL_MS 100
   #define STG_DEFAULT_LASER_RESOLUTION 1
  
***************
*** 82,86 ****
    
    // sensible laser defaults 
!   update_interval_ms = STG_DEFAULT_LASER_INTERVAL_MS; 
    laser_return = LaserVisible;
    
--- 82,86 ----
    
    // sensible laser defaults 
!   interval = 1e3 * STG_DEFAULT_LASER_INTERVAL_MS; 
    laser_return = LaserVisible;
    
***************
*** 103,110 ****
    // don't allocate sample buffer memory until Startup()
    samples = NULL;
- 
-   dl_debug_laser = glGenLists( 1 );
-   glNewList( dl_debug_laser, GL_COMPILE );
-   glEndList();
  }
  
--- 103,106 ----
***************
*** 116,119 ****
--- 112,124 ----
  }
  
+ void StgModelLaser::InitGraphics()
+ {
+   StgModel::InitGraphics();
+ 
+   dl_debug_laser = glGenLists( 1 );
+   glNewList( dl_debug_laser, GL_COMPILE );
+   glEndList();
+ }
+ 
  void StgModelLaser::SetSampleCount( unsigned int count )
  {

Index: model.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/model.cc,v
retrieving revision 1.1.2.10
retrieving revision 1.1.2.11
diff -C2 -d -r1.1.2.10 -r1.1.2.11
*** model.cc    31 Oct 2007 03:55:11 -0000      1.1.2.10
--- model.cc    1 Nov 2007 07:18:53 -0000       1.1.2.11
***************
*** 221,237 ****
    //SetVelocity( &this->velocity );
  
!   this->last_update_ms = 0;
!   this->update_interval_ms = 10;
    
    // now we can add the basic square shape
    this->AddBlockRect( -0.5,-0.5,1,1 );
! 
!   // GL
!   this->dl_body = glGenLists( 1 );
!   this->dl_data = glGenLists( 1 );
!   //this->dl_debug = glGenLists( 1 );
!   //this->dl_raytrace = glGenLists( 1 );
!   this->dl_grid = glGenLists(1);
! 
    PRINT_DEBUG2( "finished model %s (%d).", 
                this->token,
--- 221,230 ----
    //SetVelocity( &this->velocity );
  
!   this->last_update = 0;
!   this->interval = 1e4; // 10msec
    
    // now we can add the basic square shape
    this->AddBlockRect( -0.5,-0.5,1,1 );
!   
    PRINT_DEBUG2( "finished model %s (%d).", 
                this->token,
***************
*** 239,242 ****
--- 232,243 ----
  }
  
+ void StgModel::InitGraphics()
+ {
+   this->dl_body = glGenLists(1);
+   this->dl_data = glGenLists(1);
+   this->dl_grid = glGenLists(1);
+ }
+ 
+ 
  StgModel::~StgModel( void )
  {
***************
*** 583,587 ****
    //PRINT_DEBUG1( "%s.Map()", token );
  
!   if( this->debug )
      {
        double scale = 1.0 / world->ppm;
--- 584,588 ----
    //PRINT_DEBUG1( "%s.Map()", token );
  
!   if( world->graphics && this->debug )
      {
        double scale = 1.0 / world->ppm;
***************
*** 594,598 ****
      ((StgBlock*)it->data)->Map();
  
!   if( this->debug )
      glPopMatrix();
  } 
--- 595,599 ----
      ((StgBlock*)it->data)->Map();
  
!   if( world->graphics && this->debug )
      glPopMatrix();
  } 
***************
*** 685,701 ****
  }
  
- // void StgModel::UpdateTreeIfDue( void )
- // {
- //   if(  this->world->sim_time_ms  >= 
- //        (this->last_update_ms + this->update_interval_ms) )
- //     this->Update();
-   
- //   LISTMETHOD( this->children, StgModel*, UpdateTreeIfDue );
- // }
- 
  void StgModel::UpdateIfDue( void )
  {
    if(  world->sim_time  >= 
!        (last_update_ms + update_interval_ms) )
      this->Update();
  }
--- 686,693 ----
  }
  
  void StgModel::UpdateIfDue( void )
  {
    if(  world->sim_time  >= 
!        (last_update + interval) )
      this->Update();
  }
***************
*** 713,717 ****
    CallCallbacks( &update );
  
!   last_update_ms = world->sim_time;
  }
   
--- 705,709 ----
    CallCallbacks( &update );
  
!   last_update = world->sim_time;
  }
   
***************
*** 753,758 ****
  }
  
- 
- 
  void StgModel::Draw( void )
  {
--- 745,748 ----
***************
*** 1293,1297 ****
     
     // convert msec to sec
!    double interval = (double)world->interval_sim / 1e3;
    
  
--- 1283,1287 ----
     
     // convert msec to sec
!    double interval = (double)world->interval_sim / 1e6;
    
  

Index: world.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/world.cc,v
retrieving revision 1.1.2.12
retrieving revision 1.1.2.13
diff -C2 -d -r1.1.2.12 -r1.1.2.13
*** world.cc    31 Oct 2007 03:55:11 -0000      1.1.2.12
--- world.cc    1 Nov 2007 07:18:53 -0000       1.1.2.13
***************
*** 56,60 ****
  #include <locale.h> 
  
! //#define DEBUG 
  
  #include "stage.hh"
--- 56,60 ----
  #include <locale.h> 
  
! #define DEBUG 
  
  #include "stage.hh"
***************
*** 63,67 ****
  const stg_msec_t STG_DEFAULT_WORLD_INTERVAL_REAL = 100; ///< real time 
between updates
  const stg_msec_t STG_DEFAULT_WORLD_INTERVAL_SIM = 100;  ///< duration of sim 
timestep
- const stg_msec_t STG_DEFAULT_MAX_SLEEP = 20;
  
  // TODO: fix the quadtree code so we don't need a world size
--- 63,66 ----
***************
*** 136,151 ****
    assert(token);
    this->token = (char*)malloc(STG_TOKEN_MAX);
!   snprintf( this->token, STG_TOKEN_MAX, "%s:%d", token, this->id );
  
    this->quit = false;
    this->updates = 0;
    this->wf = NULL;
  
    this->models_by_id = g_hash_table_new( g_int_hash, g_int_equal );
    this->models_by_name = g_hash_table_new( g_str_hash, g_str_equal );
    this->sim_time = 0;
!   this->interval_sim = interval_sim;
!   this->interval_real = interval_real;
!   this->interval_sleep_max = STG_DEFAULT_MAX_SLEEP;
    this->real_time_start = RealTimeNow();
    this->real_time_next_update = 0;
--- 135,152 ----
    assert(token);
    this->token = (char*)malloc(STG_TOKEN_MAX);
!   snprintf( this->token, STG_TOKEN_MAX, "%s", token );//this->id );
  
    this->quit = false;
    this->updates = 0;
    this->wf = NULL;
+   this->graphics = false; // subclasses that provide GUIs should
+                         // change this
  
    this->models_by_id = g_hash_table_new( g_int_hash, g_int_equal );
    this->models_by_name = g_hash_table_new( g_str_hash, g_str_equal );
    this->sim_time = 0;
!   this->interval_sim = 1e3 * interval_sim;
!   this->interval_real = 1e3 * interval_real;
! 
    this->real_time_start = RealTimeNow();
    this->real_time_next_update = 0;
***************
*** 162,167 ****
    
    this->total_subs = 0;
!   this->paused = false; 
    this->destroy = false;   
  }
  
--- 163,172 ----
    
    this->total_subs = 0;
!   this->paused = true; 
    this->destroy = false;   
+ 
+   for( unsigned int i=0; i<INTERVAL_LOG_LEN; i++ )
+     this->interval_log[i] = this->interval_real;
+   this->real_time_now = 0;
  }
  
***************
*** 186,213 ****
  }
  
- 
  void StgWorld::ClockString( char* str, size_t maxlen )
  {
!   long unsigned int days = this->sim_time / (24*3600000);
!   long unsigned int hours = this->sim_time / 3600000;
!   long unsigned int minutes = (this->sim_time % 3600000) / 60000;
!   long unsigned int seconds = (this->sim_time % 60000) / 1000; // seconds
!   long unsigned int msec =  this->sim_time % 1000; // milliseconds
    
!   if( days > 0 )
!     snprintf( str, maxlen, "Time: %lu:%lu:%02lu:%02lu.%03lu\tsubs: %d  %s",
!             days, hours, minutes, seconds, msec,
!             this->total_subs,
!             this->paused ? "--PAUSED--" : "" );
!   else if( hours > 0 )
!     snprintf( str, maxlen, "Time: %lu:%02lu:%02lu.%03lu\tsubs: %d  %s",
              hours, minutes, seconds, msec,
!             this->total_subs,
!             this->paused ? "--PAUSED--" : "" );
    else
!     snprintf( str, maxlen, "Time: %02lu:%02lu.%03lu\tsubs: %d  %s",
              minutes, seconds, msec,
!             this->total_subs,
!             this->paused ? "--PAUSED--" : "" );
  }
  
--- 191,221 ----
  }
  
  void StgWorld::ClockString( char* str, size_t maxlen )
  {
!   uint32_t hours   =  sim_time / 3600000000; 
!   uint32_t minutes = (sim_time % 3600000000) / 60000000; 
!   uint32_t seconds = (sim_time % 60000000) / 1000000; 
!   uint32_t msec    = (sim_time % 1000000) / 1000; 
    
!   // find the average length of the last few realtime intervals;
!   stg_usec_t average_real_interval = 0;
!   for( uint32_t i=0; i<INTERVAL_LOG_LEN; i++ )
!     average_real_interval += interval_log[i];
!   average_real_interval /= INTERVAL_LOG_LEN;
!   
!   double localratio = (double)interval_sim / (double)average_real_interval;
!   
!   if( hours > 0 )
!     snprintf( str, maxlen, "Time: %uh%02um%02u.%03us\t[%.1f]\tsubs: %d  %s",
              hours, minutes, seconds, msec,
!             localratio,
!             total_subs,
!             paused ? "--PAUSED--" : "" );
    else
!     snprintf( str, maxlen, "Time: %02um%02u.%03us\t[%.1f]\tsubs: %d  %s",
              minutes, seconds, msec,
!             localratio,
!             total_subs,
!             paused ? "--PAUSED--" : "" );
  }
  
***************
*** 229,241 ****
      wf->ReadString( entity, "name", token );
    
!   this->interval_real = 
!     wf->ReadInt( entity, "interval_real", this->interval_real );
  
!   this->interval_sim = 
!     wf->ReadInt( entity, "interval_sim", this->interval_sim );
    
-   this->interval_sleep_max = 
-     wf->ReadInt( entity, "interval_sleep_max", this->interval_sleep_max );    
- 
    this->ppm = 
      1.0 / wf->ReadFloat( entity, "resolution", this->ppm ); 
--- 237,246 ----
      wf->ReadString( entity, "name", token );
    
!   this->interval_real = 1e3 *  
!     wf->ReadInt( entity, "interval_real", this->interval_real/1e3 );
  
!   this->interval_sim = 1e3 * 
!     wf->ReadInt( entity, "interval_sim", this->interval_sim/1e3 );
    
    this->ppm = 
      1.0 / wf->ReadFloat( entity, "resolution", this->ppm ); 
***************
*** 314,333 ****
  }
  
! stg_msec_t StgWorld::RealTimeNow(void)
  {
    struct timeval tv;
    gettimeofday( &tv, NULL );  // slow system call: use sparingly
!   return (stg_msec_t)( tv.tv_sec*1000 + tv.tv_usec/1000 );
  }
  
! stg_msec_t StgWorld::RealTimeSinceStart(void)
  {
!   stg_msec_t timenow = RealTimeNow();
!   stg_msec_t diff = timenow - real_time_start;
! 
!   //PRINT_DEBUG3( "timenow %lu start %lu diff %lu", timenow, real_time_start, 
diff );
  
!   return diff;
!   //return( RealTimeNow() - real_time_start );
  }
  
--- 319,338 ----
  }
  
! stg_usec_t StgWorld::RealTimeNow()
  {
    struct timeval tv;
    gettimeofday( &tv, NULL );  // slow system call: use sparingly
!   
!   return( tv.tv_sec*1000000 + tv.tv_usec );
  }
  
! stg_usec_t StgWorld::RealTimeSinceStart()
  {
!   stg_usec_t timenow = RealTimeNow();
!   
!   // subtract the start time from the current time to get the elapsed
!   // time
  
!   return timenow - real_time_start;
  }
  
***************
*** 335,365 ****
  {
    // sleep until it's time to update  
!   stg_msec_t timenow = 0;
!   for( timenow = RealTimeSinceStart();
!        timenow < real_time_next_update;
!        timenow = RealTimeSinceStart() )
      {
!       stg_msec_t sleeptime = real_time_next_update - timenow; // must be >0
!       
!       PRINT_DEBUG3( "timenow %lu nextupdate %lu sleeptime %lu",
!                   timenow, real_time_next_update, sleeptime );
!       
!       usleep( sleeptime * 1000 ); // sleep for few microseconds
      }
!   
! #ifdef DEBUG     
!   printf( "[%u %lu %lu] ufreq:%.2f\n",
!         this->id, 
!         this->sim_time,
!         this->updates,
!         //this->interval_sim,
!         //this->real_interval_measured,
!         //(double)this->interval_sim / (double)this->real_interval_measured,
!         this->updates/(timenow/1e3));
!   
!   fflush(stdout);
! #endif
!   
!   real_time_next_update = timenow + interval_real;      
  }
  
--- 340,361 ----
  {
    // sleep until it's time to update  
!   stg_usec_t timenow = RealTimeSinceStart();
!   
!   /*  printf( "\ntimesincestart %llu interval_real %llu interval_sim %llu 
real_time_next_update %llu\n",
!         timenow,
!         interval_real,
!         interval_sim,
!         real_time_next_update );
!   */
! 
!   if( timenow < real_time_next_update )
      {
!       usleep( real_time_next_update - timenow );
      }
! 
!   interval_log[updates%INTERVAL_LOG_LEN] = timenow - real_time_now;
! 
!   real_time_now = timenow;
!   real_time_next_update += interval_real;
  }
  
***************
*** 369,373 ****
      {  
        //PRINT_DEBUG( "StgWorld::Update()" );
!       
        // update any models that are due to be updated
        for( GList* it=this->update_list; it; it=it->next )
--- 365,379 ----
      {  
        //PRINT_DEBUG( "StgWorld::Update()" );
! 
!       if( interval_real > 0 || updates % 100 == 0 )
!       {
!         char str[64];
!         ClockString( str, 64 );
!         //printf( "Stage timestep: %lu simtime: %lu\n",
!         //      updates, sim_time );
!         printf( "\r%s", str );
!         fflush(stdout);
!       }
! 
        // update any models that are due to be updated
        for( GList* it=this->update_list; it; it=it->next )
***************
*** 408,411 ****
--- 414,418 ----
  }
  
+ 
  void StgWorld::AddModelName( StgModel* mod )
  {
***************
*** 413,431 ****
  }
  
- // void stg_world_print( StgWorld* world )
- // {
- //   printf( " world %d:%s (%d models)\n", 
- //      world->id, 
- //      world->token,
- //      g_hash_table_size( world->models ) );
-   
- //    g_hash_table_foreach( world->models, model_print_cb, NULL );
- // }
- 
- // void world_print_cb( gpointer key, gpointer value, gpointer user )
- // {
- //   stg_world_print( (StgWorld*)value );
- // }
- 
  StgModel* StgWorld::GetModel( const char* name )
  {
--- 420,423 ----
***************
*** 440,542 ****
  }
  
- //#define NBITS 6
- 
- // int bigtest( int x, int y, int z,
- //         void* arg1, void* arg2 )
- // {
- //   glRecti( x<<NBITS,y<<NBITS, (x+1)<<NBITS,(y+1)<<NBITS );
- //   return FALSE;
- // }
- 
- // typedef struct
- // {
- //   StgWorld* world;
- //   StgModel* finder;
- //   StgModel* hit;
- //   int32_t x, y, z; // leave point
- //   stg_block_match_func_t func;
- //   const void* arg;
- // } _raytrace_info_t;
- 
- // int raytest( int32_t x, int32_t y, int32_t z,
- //         _raytrace_info_t* rti )
- // {
- //   //glRecti( x,y, x+1,y+1 );
- 
- //    // for each block recorded at his location
- //    for( GSList* list = rti->world->bgrid->GetList( x, y );
- //         list;
- //         list = list->next )
- //      {
- //        StgBlock* block = (StgBlock*)list->data;       
- //        assert( block );
-        
- //        // if this block does not belong to the searching model and it
- //        // matches the predicate and it's in the right z range
- //        if( block && (block->mod != rti->finder) && 
- //       (*rti->func)( block, rti->arg ) )//&&
- //     //z >= block->zmin &&
- //     // z < block->zmax )    
- //     {
- //      // a hit!
- //      rti->hit = block->mod;
- //      rti->x = x;
- //      rti->y = y;
- //      rti->z = z;     
- //      return TRUE;
- //    }
- //      }
-   
- //   return FALSE;  
- // }
-  
-           
- // stg_meters_t StgWorld::Raytrace( StgModel* finder,
- //                             stg_pose_t* pose,
- //                             stg_meters_t max_range,
- //                             stg_block_match_func_t func,
- //                             const void* arg,
- //                             StgModel** hit_model )
- // {
- //   // find the global integer bitmap address of the ray  
- //   int32_t x = (int32_t)((pose->x+width/2.0)*ppm);
- //   int32_t y = (int32_t)((pose->y+height/2.0)*ppm);
-   
- //   // and the x and y offsets of the ray
- //   int32_t dx = (int32_t)(ppm*max_range * cos(pose->a));
- //   int32_t dy = (int32_t)(ppm*max_range * sin(pose->a));
-   
- //   glPushMatrix();
- //   glTranslatef( -width/2.0, -height/2.0, 0 );
- //   glScalef( 1.0/ppm, 1.0/ppm, 0 );
-          
- //   _raytrace_info_t rinfo;
- //   rinfo.world = this;
- //   rinfo.finder = finder;
- //   rinfo.func = func;
- //   rinfo.arg = arg;
- //   rinfo.hit = NULL;
-   
- //   if( stg_line_3d( x, y, 0, 
- //                       dx, dy, 0,
- //                       (stg_line3d_func_t)raytest,
- //                       &rinfo ) )
- //     {
- //       glPopMatrix();      
- 
- //       *hit_model = rinfo.hit;
- 
- //       // how far away was that strike?
- //       return hypot( (rinfo.x-x)/ppm, (rinfo.y-y)/ppm );
- //     }
- 
- 
- //   glPopMatrix();
- //   // return the range from ray start to object strike
-   
- //   // hit nothing, so return max range
- //   return max_range;
- // }
- 
  stg_meters_t StgWorld::Raytrace( StgModel* finder,
                                 stg_pose_t* pose,
--- 432,435 ----
***************
*** 559,565 ****
    int32_t dz = 0;
  
!  //  glPushMatrix();
  //   glTranslatef( -width/2.0, -height/2.0, 0 );
  //   glScalef( 1.0/ppm, 1.0/ppm, 0 );
           
    // line 3d algorithm adapted from Cohen's code from Graphics Gems IV
--- 452,459 ----
    int32_t dz = 0;
  
! //   glPushMatrix();
  //   glTranslatef( -width/2.0, -height/2.0, 0 );
  //   glScalef( 1.0/ppm, 1.0/ppm, 0 );
+ //   PushColor( 1,0,0,1 );
           
    // line 3d algorithm adapted from Cohen's code from Graphics Gems IV
***************
*** 571,599 ****
    n = ax+ay+az;
  
    while ( n-- ) 
      {          
!       for( GSList* list = bgrid->GetList( x, y );
!          list;
!          list = list->next )
        {
!         StgBlock* block = (StgBlock*)list->data;       
!         assert( block );
!         
!         // if this block does not belong to the searching model and it
!         // matches the predicate and it's in the right z range
!         if( block && (block->mod != finder) && 
!             (*func)( block, arg ) &&
!             pose->z >= block->zmin &&
!             pose->z < block->zmax )    
            {
-             // a hit!
-             //glPopMatrix();                
              
!             *hit_model = block->mod;        
!             // how far away was that strike?
!             return hypot( (x-xstart)/ppm, (y-ystart)/ppm );
            }
        }
!       
        if ( exy < 0 ) {
        if ( exz < 0 ) {
--- 465,528 ----
    n = ax+ay+az;
  
+   uint32_t bbx_last = 0;
+   uint32_t bby_last = 0;
+   uint32_t bbx = 0;
+   uint32_t bby = 0;
+   
+   const uint32_t NBITS = bgrid->numbits;
+ 
+   bool lookup_list = true;
+   
    while ( n-- ) 
      {          
!       // todo - avoid calling this multiple times for a single
!       // bigblock.
!       
!       bbx = x>>NBITS;
!       bby = y>>NBITS;
!       
!       // if we just changed grid square
!       if( ! ((bbx == bbx_last) && (bby == bby_last)) )
        {
!         // if this square has some contents, we need to look it up
!         lookup_list = !(bgrid->BigBlockOccupancy(bbx,bby) < 1 );
! 
!         bbx_last = bbx;
!         bby_last = bby;
!       }
!       
!       if( lookup_list )
!       {         
!         for( GSList* list = bgrid->GetList(x,y);
!              list;
!              list = list->next )      
            {
              
!             
!             StgBlock* block = (StgBlock*)list->data;       
!             assert( block );
!             
!             // if this block does not belong to the searching model and it
!             // matches the predicate and it's in the right z range
!             if( block && (block->mod != finder) && 
!                 (*func)( block, arg ) &&
!                 pose->z >= block->zmin &&
!                 pose->z < block->zmax )        
!               {
!                 //glPopMatrix();                    
!                 
!                 // a hit!
!                 if( hit_model )
!                   *hit_model = block->mod;          
!                 // how far away was that strike?
!                 return hypot( (x-xstart)/ppm, (y-ystart)/ppm );
!               }
            }
        }
! //        else
! //            {
! //              glRecti( x,y, x+1, y+1 );
! //            }
!         
        if ( exy < 0 ) {
        if ( exz < 0 ) {
***************
*** 619,624 ****
    
    //glPopMatrix();
-   // return the range from ray start to object strike
-   
    // hit nothing, so return max range
    return max_range;
--- 548,551 ----
***************
*** 743,747 ****
    _render_info_t rinfo;
    rinfo.grid = bgrid;
!   rinfo.block - block;
  
    // TODO - could be a little bit faster - currently considers each
--- 670,674 ----
    _render_info_t rinfo;
    rinfo.grid = bgrid;
!   rinfo.block = block;
  
    // TODO - could be a little bit faster - currently considers each

Index: typetable.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/typetable.cc,v
retrieving revision 1.1.2.3
retrieving revision 1.1.2.4
diff -C2 -d -r1.1.2.3 -r1.1.2.4
*** typetable.cc        31 Oct 2007 03:55:11 -0000      1.1.2.3
--- typetable.cc        1 Nov 2007 07:18:53 -0000       1.1.2.4
***************
*** 9,12 ****
--- 9,13 ----
    { "laser",     StgModelLaser::Create },
    { "position",  StgModelPosition::Create },
+   { "ranger",    StgModelRanger::Create },
    { NULL, NULL } // this must be the last entry
  };

Index: blockgrid.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/blockgrid.cc,v
retrieving revision 1.1.2.4
retrieving revision 1.1.2.5
diff -C2 -d -r1.1.2.4 -r1.1.2.5
*** blockgrid.cc        31 Oct 2007 03:55:11 -0000      1.1.2.4
--- blockgrid.cc        1 Nov 2007 07:18:53 -0000       1.1.2.5
***************
*** 1,12 ****
  #include "stage.hh"
  
! static const uint32_t NBITS = 6;
! static const uint32_t NSIZE = 1<<NBITS;
! static const uint32_t NSQR = NSIZE*NSIZE;
! static const uint32_t MASK = NSIZE-1;
! 
  
  StgBlockGrid::StgBlockGrid( uint32_t width, uint32_t height )
  {
    this->width = width;
    this->height = height;
--- 1,12 ----
  #include "stage.hh"
  
! const uint32_t NBITS = 5;
! const uint32_t NSIZE = 1<<NBITS;
! const uint32_t NSQR = NSIZE*NSIZE;
! const uint32_t MASK = NSIZE-1;
  
  StgBlockGrid::StgBlockGrid( uint32_t width, uint32_t height )
  {
+   this->numbits = NBITS;
    this->width = width;
    this->height = height;
***************
*** 106,114 ****
  }
  
! // uint32_t StgBlockGrid::BigBlockOccupancy( uint32_t bbx, uint32_t bby )
! // {
! //   //glRecti( bbx<<NBITS, bby<<NBITS,(bbx+1)<<NBITS,(bby+1)<<NBITS );       
! //   return map[ bbx + bby*bwidth].counter;
! // }
  
  GSList* StgBlockGrid::GetList( uint32_t x, uint32_t y )
--- 106,114 ----
  }
  
! uint32_t StgBlockGrid::BigBlockOccupancy( uint32_t bbx, uint32_t bby )
! {
!   //glRecti( bbx<<NBITS, bby<<NBITS,(bbx+1)<<NBITS,(bby+1)<<NBITS );  
!   return map[ bbx + bby*bwidth].counter;
! }
  
  GSList* StgBlockGrid::GetList( uint32_t x, uint32_t y )

Index: Makefile.manual
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/Makefile.manual,v
retrieving revision 1.1.2.5
retrieving revision 1.1.2.6
diff -C2 -d -r1.1.2.5 -r1.1.2.6
*** Makefile.manual     31 Oct 2007 03:55:11 -0000      1.1.2.5
--- Makefile.manual     1 Nov 2007 07:18:53 -0000       1.1.2.6
***************
*** 20,23 ****
--- 20,24 ----
      model_position.o \
      model_props.o \
+     model_ranger.o \
      stage.o \
      stest.o \
***************
*** 27,33 ****
      worldfile.o 
  
- #    model_ranger.o 
- #    model_position.o 
- 
  CPPFLAGS= -g -Wall -I.. -I../replace `pkg-config --cflags gtkglext-1.0`
  LIBS= `pkg-config --libs gtkglext-1.0`
--- 28,31 ----

Index: stage.hh
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/stage.hh,v
retrieving revision 1.1.2.12
retrieving revision 1.1.2.13
diff -C2 -d -r1.1.2.12 -r1.1.2.13
*** stage.hh    31 Oct 2007 03:55:11 -0000      1.1.2.12
--- stage.hh    1 Nov 2007 07:18:53 -0000       1.1.2.13
***************
*** 161,167 ****
--- 161,173 ----
  typedef double stg_radians_t;
  
+ /** time structure */
+ typedef struct timeval stg_time_t;
+ 
  /** Milliseconds: unit of (short) time */
  typedef unsigned long stg_msec_t;
  
+ /** Microseconds: unit of (very short) time */
+ typedef uint64_t stg_usec_t;
+ 
  /** Kilograms: unit of mass */
  typedef double stg_kg_t; // Kilograms (mass)
***************
*** 262,270 ****
  /** Returns the real (wall-clock) time in milliseconds since the
      epoch. */
! stg_msec_t stg_realtime( void );
  
  /** Returns the real (wall-clock) time in milliseconds since the
      simulation started. */
! stg_msec_t stg_realtime_since_start( void );
    
  
--- 268,276 ----
  /** Returns the real (wall-clock) time in milliseconds since the
      epoch. */
! //stg_msec_t stg_realtime( void );
  
  /** Returns the real (wall-clock) time in milliseconds since the
      simulation started. */
! //stg_msec_t stg_realtime_since_start( void );
    
  
***************
*** 1142,1146 ****
    GHashTable* child_types;
    char* token;
-   
    bool debug;
  
--- 1148,1151 ----
***************
*** 1177,1184 ****
    stg_bigblock_t* map;
    
!   GTrashStack* trashstack;
  
  public:
!   uint32_t width, height, bwidth, bheight;
    StgBlockGrid( uint32_t width, uint32_t height );
    ~StgBlockGrid();
--- 1182,1190 ----
    stg_bigblock_t* map;
    
!   GTrashStack* trashstack;  
!   uint32_t width, height, bwidth, bheight;
  
  public:
!   uint32_t numbits;
    StgBlockGrid( uint32_t width, uint32_t height );
    ~StgBlockGrid();
***************
*** 1195,1209 ****
    
  
  /// WORLD CLASS
  class StgWorld : public StgAncestor
  {
  protected:
!   stg_msec_t real_time_next_update;
!   stg_msec_t real_time_start;
    static bool init_done;
  
    bool quit; // quit this world ASAP
    static bool quit_all; // quit all worlds ASAP
  
  private:
    
--- 1201,1221 ----
    
  
+ 
+ 
+ const uint32_t INTERVAL_LOG_LEN = 32;
+ 
  /// WORLD CLASS
  class StgWorld : public StgAncestor
  {
  protected:
!   stg_usec_t real_time_next_update;
!   stg_usec_t real_time_start;
    static bool init_done;
  
+ 
    bool quit; // quit this world ASAP
    static bool quit_all; // quit all worlds ASAP
  
+ 
  private:
    
***************
*** 1237,1242 ****
    virtual ~StgWorld( void );
    
!   stg_msec_t RealTimeNow(void);
!   stg_msec_t RealTimeSinceStart(void);
    void PauseUntilNextUpdateTime(void);
  
--- 1249,1257 ----
    virtual ~StgWorld( void );
    
!   bool graphics;
! 
!   stg_usec_t real_time_now;
!   stg_usec_t RealTimeNow(void);
!   stg_usec_t RealTimeSinceStart(void);
    void PauseUntilNextUpdateTime(void);
  
***************
*** 1263,1268 ****
    CWorldFile* wf; ///< If set, points to the worldfile used to create this 
world
  
!   stg_msec_t sim_time; ///< the current sim time in this world in ms
!   stg_msec_t wall_last_update; ///< the real time of the last update in ms
  
    long unsigned int updates; ///< the number of simulated time steps executed 
so far
--- 1278,1283 ----
    CWorldFile* wf; ///< If set, points to the worldfile used to create this 
world
  
!   stg_usec_t sim_time; ///< the current sim time in this world in ms
!   stg_usec_t wall_last_update; ///< the real time of the last update in ms
  
    long unsigned int updates; ///< the number of simulated time steps executed 
so far
***************
*** 1270,1278 ****
    bool dirty; ///< iff true, a gui redraw would be required
  
!   stg_msec_t interval_real;   ///< real-time interval between updates - set 
this to zero for 'as fast as possible
!   stg_msec_t interval_sleep_max;
!   stg_msec_t interval_sim; ///< temporal resolution: milliseconds that elapse 
between simulated time steps 
!   //stg_msec_t real_interval_measured;
  
    int total_subs; ///< the total number of subscriptions to all models
    double ppm; ///< the resolution of the world model in pixels per meter  
--- 1285,1294 ----
    bool dirty; ///< iff true, a gui redraw would be required
  
!   stg_usec_t interval_real;   ///< real-time interval between updates - set 
this to zero for 'as fast as possible
!   stg_usec_t interval_sleep_max;
!   stg_usec_t interval_sim; ///< temporal resolution: milliseconds that elapse 
between simulated time steps 
  
+   stg_usec_t interval_log[INTERVAL_LOG_LEN];
+   
    int total_subs; ///< the total number of subscriptions to all models
    double ppm; ///< the resolution of the world model in pixels per meter  
***************
*** 1285,1290 ****
    StgModel* GetModel( const char* name );
    
!   void AddModel( StgModel* mod );
!   void RemoveModel( StgModel* mod );
    void AddModelName( StgModel* mod );  
  
--- 1301,1306 ----
    StgModel* GetModel( const char* name );
    
!   virtual void AddModel( StgModel* mod );
!   virtual void RemoveModel( StgModel* mod );
    void AddModelName( StgModel* mod );  
  
***************
*** 1375,1380 ****
    int subs;     //< the number of subscriptions to this model
    
!   stg_msec_t update_interval_ms; //< time between updates in ms
!   stg_msec_t last_update_ms; //< time of last update in ms
    
    stg_bool_t disabled; //< if non-zero, the model is disabled
--- 1391,1396 ----
    int subs;     //< the number of subscriptions to this model
    
!   stg_usec_t interval; //< time between updates in ms
!   stg_usec_t last_update; //< time of last update in ms
    
    stg_bool_t disabled; //< if non-zero, the model is disabled
***************
*** 1423,1427 ****
    virtual void DataVisualize( void );
    virtual void DrawSelected(void);
!   
    virtual void PushColor( stg_color_t col )
    { world->PushColor( col ); }
--- 1439,1445 ----
    virtual void DataVisualize( void );
    virtual void DrawSelected(void);
!   virtual void InitGraphics(void); // called by a world that supports
!                                  // graphics
! 
    virtual void PushColor( stg_color_t col )
    { world->PushColor( col ); }
***************
*** 1813,1816 ****
--- 1831,1836 ----
    GlColorStack colorstack;
    
+   virtual void AddModel( StgModel*  mod );
+ 
  private:
    GList* selected_models; ///<   a list of models that are currently selected 
by the user 
***************
*** 1954,1962 ****
  //#endif
  
! #define MILLION 1e6
! #define BILLION 1e9
  
  #ifndef M_PI
! #define M_PI 3.14159265358979323846
  #endif
  
--- 1974,1983 ----
  //#endif
  
! #define THOUSAND (1e3)
! #define MILLION (1e6)
! #define BILLION (1e9)
  
  #ifndef M_PI
! #define M_PI (3.14159265358979323846)
  #endif
  
***************
*** 2122,2125 ****
--- 2143,2147 ----
      virtual void Print( char* prefix );
      virtual void DataVisualize( void );
+     virtual void InitGraphics(void);
  
      void SetSampleCount( unsigned int count );
***************
*** 2307,2312 ****
    // constructor
    StgModelRanger( StgWorld* world,
!              StgModel* parent, 
!              stg_id_t id, CWorldFile* wf );
    
    // destructor
--- 2329,2335 ----
    // constructor
    StgModelRanger( StgWorld* world,
!                 StgModel* parent, 
!                 stg_id_t id, 
!                 char* typestr );
    
    // destructor
***************
*** 2318,2325 ****
    virtual void Load( void );
    virtual void Print( char* prefix );
!   virtual void DListData( void );
  
    size_t sensor_count;
    stg_ranger_sensor_t* sensors;
  };
  
--- 2341,2358 ----
    virtual void Load( void );
    virtual void Print( char* prefix );
!   virtual void DataVisualize( void );
  
    size_t sensor_count;
    stg_ranger_sensor_t* sensors;
+ 
+   // static wrapper for the constructor - all models must implement
+   // this method and add an entry in typetable.cc
+   static StgModel* Create( StgWorld* world,
+                          StgModel* parent, 
+                          stg_id_t id, 
+                          char* typestr )
+   { 
+     return (StgModel*)new StgModelRanger( world, parent, id, typestr ); 
+   }    
  };
  

Index: worldgtk.cc
===================================================================
RCS file: /cvsroot/playerstage/code/stage/libstage/Attic/worldgtk.cc,v
retrieving revision 1.1.2.9
retrieving revision 1.1.2.10
diff -C2 -d -r1.1.2.9 -r1.1.2.10
*** worldgtk.cc 31 Oct 2007 03:55:11 -0000      1.1.2.9
--- worldgtk.cc 1 Nov 2007 07:18:53 -0000       1.1.2.10
***************
*** 115,119 ****
  #include <gdk-pixbuf/gdk-pixdata.h>
  
! //#define DEBUG 
  
  #include "stage.hh"
--- 115,119 ----
  #include <gdk-pixbuf/gdk-pixdata.h>
  
! #define DEBUG 
  
  #include "stage.hh"
***************
*** 128,131 ****
--- 128,132 ----
  #endif
  
+ static const stg_msec_t STG_DEFAULT_REDRAW_INTERVAL = 100;
  
  // only models that have fewer rectangles than this get matrix
***************
*** 528,531 ****
--- 529,534 ----
    PRINT_DEBUG( "Constructing StgWorldGtk" );
    
+   this->graphics = true;
+ 
    // Create a top-level window
    frame = gtk_window_new(GTK_WINDOW_TOPLEVEL);
***************
*** 622,626 ****
    // neatly into the window.
    scale = width; //(1 to 1)
-   //scale = 0.2 * width; //(1 to 1)
  
    memset( &click_point, 0, sizeof(click_point));
--- 625,628 ----
***************
*** 664,668 ****
  
    this->timer_handle = 0;
!   this->redraw_interval = 100; //msec
  
    // CREATE THE MENUS
--- 666,670 ----
  
    this->timer_handle = 0;
!   this->redraw_interval = STG_DEFAULT_REDRAW_INTERVAL; //msec
  
    // CREATE THE MENUS
***************
*** 732,739 ****
    // show the window
    gtk_widget_show_all(frame);
- 
- /*   // install the timeout callback that renders the window */
- /*   win->redraw_interval = STG_DEFAULT_REDRAW_INTERVAL; */
- /*   win->timeout_id = g_timeout_add( win->redraw_interval, timeout, world ); 
*/
  }
  
--- 734,737 ----
***************
*** 1423,1426 ****
--- 1421,1437 ----
        wf->ReadInt(wf_section, "redraw_interval", redraw_interval );
        
+       const stg_msec_t lowbound = 10;
+       if( redraw_interval < lowbound )
+       {
+         PRINT_WARN2( "redraw_interval of %lu is probably too low "
+                      "to be practical. Setting to %lu msec instead.",
+                      redraw_interval, lowbound );
+ 
+         redraw_interval = lowbound;
+       }
+       
+       // there's no point redrawing faster than the world updates
+       //redraw_interval = MAX( redraw_interval, interval_real );
+ 
        if( timer_handle > 0 )
        g_source_remove( timer_handle );
***************
*** 1442,1447 ****
    PRINT_DEBUG1( "%s.Save()", token );
    
-   StgWorld::Save();
- 
    int width, height;
    gtk_window_get_size(  GTK_WINDOW(frame), &width, &height );
--- 1453,1456 ----
***************
*** 1473,1476 ****
--- 1482,1487 ----
                   gtk_toggle_action_get_active(  
GTK_TOGGLE_ACTION(tog->action) ));
      }
+ 
+   StgWorld::Save();
  }
  
***************
*** 1503,1506 ****
--- 1514,1522 ----
  }
  
+ void StgWorldGtk::AddModel( StgModel*  mod  )
+ {
+   mod->InitGraphics();
+   StgWorld::AddModel( mod );
+ }
  
  
***************
*** 1665,1670 ****
    StgWorld::Update(); // ignore return value
    
!   if( dirty )
!     Draw();
    
    while( gtk_events_pending() )
--- 1681,1686 ----
    StgWorld::Update(); // ignore return value
    
!   //if( dirty )
!   //Draw();
    
    while( gtk_events_pending() )
***************
*** 1880,1883 ****
--- 1896,1900 ----
    
    // draw the models
+   //puts( "WORLD DRAW" );
    GList* it;
    for( it=children; it; it=it->next )


-------------------------------------------------------------------------
This SF.net email is sponsored by: Splunk Inc.
Still grepping through log files to find problems?  Stop.
Now Search log events and configuration files using AJAX and a browser.
Download your FREE copy of Splunk now >> http://get.splunk.com/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit

Reply via email to