world.cc

/**
	$Id$ 
 **/

/** @defgroup world World

    Stage simulates a 'world' composed of `models', defined in a `world
    file'.

    API: Stg::World

    <h2>Worldfile properties</h2>

    @par Summary and default values

    @verbatim

	 name                     <worldfile name>
	 interval_sim            100
	 quit_time                 0
    resolution                0.02
	 show_clock                0
	 show_clock_interval     100
    threads                   0

    @endverbatim

    @par Details

    - name <string>\n
	 An identifying name for the world, used e.g. in the title bar of
	 the GUI.

    - interval_sim <float>\n
	 The amount of simulation time run for each call of
	 World::Update(). Each model has its own configurable update
	 interval, which can be greater or less than this, but intervals
	 shorter than this are not visible in the GUI or in World update
	 callbacks. You are not likely to need to change the default of 100
	 msec: this is used internally by clients such as Player and WebSim.

    - quit_time <float>\n
	 Stop the simulation after this many simulated seconds have
	 elapsed. In libstage, World::Update() returns true. In Stage with
	 a GUI, the simulation is paused.wo In Stage without a GUI, Stage
	 quits.
 
    - resolution <float>\n
    The resolution (in meters) of the underlying bitmap model. Larger
    values speed up raytracing at the expense of fidelity in collision
    detection and sensing. The default is often a reasonable choice.

    - show_clock <int>\n
	 If non-zero, print the simulation time on stdout every
	 $show_clock_interval updates. Useful to watch the progress of
	 non-GUI simulations.

	 - show_clock_interval <int>\n
	 Sets the number of updates between printing the time on stdoutm,
	 if $show_clock is enabled. The default is once every 10 simulated
	 seconds. Smaller values slow the simulation down a little.

    - threads <int>\n 
    The number of worker threads to spawn. Some
    models can be updated in parallel (e.g. laser, ranger), and
    running 2 or more threads here may make the simulation run faster,
    depending on the number of CPU cores available and the
    worldfile. As a guideline, use one thread per core if you have
    parallel-enabled high-resolution models, e.g. a laser with
    hundreds or thousands of samples, or lots of models.
	 
    @par More examples
    The Stage source distribution contains several example world files in
    <tt>(stage src)/worlds</tt> along with the worldfile properties
    described on the manual page for each model type.
*/

#ifndef _GNU_SOURCE
#define _GNU_SOURCE
#endif

//#define DEBUG 

#include <stdlib.h>
#include <assert.h>
#include <string.h> // for strdup(3)
#include <locale.h> 
#include <limits.h>
#include <libgen.h> // for dirname(3)

#include "stage.hh"
#include "file_manager.hh"
#include "worldfile.hh"
#include "region.hh"
#include "option.hh"
using namespace Stg;

// // function objects for comparing model positions
bool World::ltx::operator()(const Model* a, const Model* b) const
{
	const meters_t ax = a->GetGlobalPose().x;
	const meters_t bx = b->GetGlobalPose().x;
	
	if( ax == bx )
		return a < b; // tie breaker
	
	return ax < bx;
}
bool World::lty::operator()(const Model* a, const Model* b) const
{
	const meters_t ay = a->GetGlobalPose().y;
	const meters_t by = b->GetGlobalPose().y;
	
	if( ay == by )
		return a < b; // tie breaker
	
	return ay < by;
}
		
// static data members
unsigned int World::next_id = 0;
bool World::quit_all = false;
std::set<World*> World::world_set;
std::string World::ctrlargs;
std::vector<std::string> World::args;

World::World( const std::string& name, 
							double ppm )
  : 
  // private
  destroy( false ),
  dirty( true ),
  models(),
  models_by_name(),
  models_with_fiducials(),
  models_with_fiducials_byx(),
  models_with_fiducials_byy(),
  ppm( ppm ), // raytrace resolution
  quit( false ),
  show_clock( false ),
  show_clock_interval( 100 ), // 10 simulated seconds using defaults
  sync_mutex(),
  threads_working( 0 ),
  threads_start_cond(),
  threads_done_cond(),
  total_subs( 0 ), 
  worker_threads( 0 ),

  // protected
  cb_list(NULL),
  extent(),
  graphics( false ), 
  option_table(),
  powerpack_list(),
  quit_time( 0 ),
  ray_list(),  
  sim_time( 0 ),
  superregions(),
  sr_cached(NULL),
  updates( 0 ),
  wf( NULL ),
  paused( false ),
  event_queues(1), // use 1 thread by default
	pending_update_callbacks(),
	active_energy(),
	active_velocity(),
  sim_interval( 1e5 ), // 100 msec has proved a good default
	update_cb_count(0)
{
  if( ! Stg::InitDone() )
    {
      PRINT_WARN( "Stg::Init() must be called before a World is created." );
      exit(-1);
    }
 
  pthread_mutex_init( &sync_mutex, NULL );
  pthread_cond_init( &threads_start_cond, NULL );
  pthread_cond_init( &threads_done_cond, NULL );
 
  World::world_set.insert( this );
  
  ground = new Model(this, NULL, "model");
  assert(ground);
  ground->SetToken( "_ground_model" ); // allow users to identify this unique model
	AddModelName( ground, ground->Token() ); // add this name to the world's table
  ground->ClearBlocks();
  ground->SetGuiMove(false);
}

World::~World( void )
{
  PRINT_DEBUG2( "destroying world %d %s", next_id, token.c_str() );
  if( ground ) delete ground;
  if( wf ) delete wf;
  World::world_set.erase( this );
}

SuperRegion* World::CreateSuperRegion( point_int_t origin )
{
  SuperRegion* sr = new SuperRegion( this, origin );
  superregions[origin] = sr;
  dirty = true; // force redraw
  return sr;
}

void World::DestroySuperRegion( SuperRegion* sr )
{
	superregions.erase( sr->GetOrigin() );
  delete sr;
}

bool World::UpdateAll()
{  
  bool quit = true;
  
  FOR_EACH( world_it, World::world_set )
    {
      if( (*world_it)->Update() == false )
				quit = false;
    }

  return quit;
}



void* World::update_thread_entry( std::pair<World*,int> *thread_info )
{
  World* world = thread_info->first;
  const int thread_instance = thread_info->second;
	
  //printf( "thread ID %d waiting for mutex\n", thread_instance );

  pthread_mutex_lock( &world->sync_mutex );  

  while( 1 )
    {
		//printf( "thread ID %d waiting for start\n", thread_instance );
		
      // wait until the main thread signals us
      //puts( "worker waiting for start signal" );
		
      pthread_cond_wait( &world->threads_start_cond, &world->sync_mutex );
		
		
      pthread_mutex_unlock( &world->sync_mutex );
		
      //printf( "worker %u thread awakes for task %u\n", thread_instance, task );
		
		world->ConsumeQueue( thread_instance );
		
		
		//printf( "thread %d done\n", thread_instance );
		
      // done working, so increment the counter. If this was the last
      // thread to finish working, signal the main thread, which is
      // blocked waiting for this to happen
		
      pthread_mutex_lock( &world->sync_mutex );	  
      if( --world->threads_working == 0 )
		  {
			 //puts( "last worker signalling main thread" );
			 pthread_cond_signal( &world->threads_done_cond );
		  }
      // keep lock going round the loop
    }
  
  return NULL;
}

void World::AddModel( Model*  mod )
{
  models.insert( mod );
  models_by_name[mod->token] = mod;
}

void World::AddModelName( Model* mod, const std::string& name )
{
  models_by_name[name] = mod;
}

void World::RemoveModel( Model* mod )
{
  models.erase( mod );
  models_by_name.erase( mod->token );
}

void World::LoadBlock( Worldfile* wf, int entity )
{ 
  // lookup the group in which this was defined
  Model* mod = models_by_wfentity[ wf->GetEntityParent( entity )];
  
  if( ! mod )
    PRINT_ERR( "block has no model for a parent" );
  
  mod->LoadBlock( wf, entity );
}

void World::LoadSensor( Worldfile* wf, int entity )
{ 
  // lookup the group in which this was defined
  ModelRanger* rgr = dynamic_cast<ModelRanger*>(models_by_wfentity[ wf->GetEntityParent( entity )]);
  
	//todo verify that the parent is indeed a ranger
	
  if( ! rgr )
    PRINT_ERR( "block has no ranger model for a parent" );
  
  rgr->LoadSensor( wf, entity );
}


Model* World::CreateModel( Model* parent, const std::string& typestr )
{
  Model* mod = NULL; // new model to return
  
  // find the creator function pointer in the map. use the
  // vanilla model if the type is NULL.
  creator_t creator = NULL;
  
  // printf( "creating model of type %s\n", typestr );
  
  std::map< std::string, creator_t>::iterator it = 
    Model::name_map.find( typestr );
  
  if( it == Model::name_map.end() )
	 {
		puts("");
		PRINT_ERR1( "Model type %s not found in model typetable", typestr.c_str() );
	 }
  else
	 creator = it->second;

  // if we found a creator function, call it
  if( creator )
    {
      //printf( "creator fn: %p\n", creator );
      mod = (*creator)( this, parent, typestr );
    }
  else
    {
      PRINT_ERR1( "Unknown model type %s in world file.", 
						typestr.c_str() );
      exit( 1 );
    }
  
  //printf( "created model %s\n", mod->Token() );
  
  return mod;
}

void World::LoadModel( Worldfile* wf, int entity )
{ 
  const int parent_entity = wf->GetEntityParent( entity );
  
  PRINT_DEBUG2( "wf entity %d parent entity %d\n", 
					 entity, parent_entity );
  
  Model* parent = models_by_wfentity[ parent_entity ];
    
  const char *typestr = (char*)wf->GetEntityType(entity);      	  
  assert(typestr);
  
  Model* mod = CreateModel( parent, typestr );
  
  // configure the model with properties from the world file
  mod->Load(wf, entity );
 
  // record the model we created for this worldfile entry
  models_by_wfentity[entity] = mod;
}

void World::Load( const std::string& worldfile_path )
{
  // note: must call Unload() before calling Load() if a world already
  // exists TODO: unload doesn't clean up enough right now

  printf( " [Loading %s]", worldfile_path.c_str() );
  fflush(stdout);

  this->wf = new Worldfile();
  wf->Load( worldfile_path );
  PRINT_DEBUG1( "wf has %d entitys", wf->GetEntityCount() );

  // end the output line of worldfile components
  //puts("");

  const int entity = 0;
  
  this->token = 
    wf->ReadString( entity, "name", this->token );
  
  this->quit_time = 
    (usec_t)( million * wf->ReadFloat( entity, "quit_time", this->quit_time ) );
  
  this->ppm = 
    1.0 / wf->ReadFloat( entity, "resolution", 1.0 / this->ppm );
  
  this->show_clock = 
    wf->ReadInt( entity, "show_clock", this->show_clock );
  
  this->show_clock_interval = 
    wf->ReadInt( entity, "show_clock_interval", this->show_clock_interval );
  
  // read msec instead of usec: easier for user
  this->sim_interval =
    1e3 * wf->ReadFloat( entity, "interval_sim", this->sim_interval / 1e3 );
  
  this->worker_threads = wf->ReadInt( entity, "threads",  this->worker_threads );  

	pending_update_callbacks.resize( worker_threads + 1 );

  if( worker_threads > 0 )
    {
      PRINT_WARN( "\nmulti-thread support is experimental and may not work properly, if at all." );
      
      event_queues.resize( worker_threads + 1 );

		//printf( "worker threads %d\n", worker_threads );
		
      // kick off the threads
      for( unsigned int t=0; t<worker_threads; ++t )
				{
					// a little configuration for each thread can't be a local
					// stack var, since it's accssed in the threads
					std::pair<World*,int>* infop = new std::pair<World*,int>( this, t+1 );
					
					//printf( "starting thread %d with ID %d \n", (int)t, info[t].second );
					
					//normal posix pthread C function pointer
					typedef void* (*func_ptr) (void*);
					
					pthread_t pt;
					pthread_create( &pt,
													NULL,
													(func_ptr)World::update_thread_entry, 
													infop );
		  }
      
      printf( "[threads %u]", worker_threads );	
    }
  
  // Iterate through entitys and create objects of the appropriate type
  for( int entity = 1; entity < wf->GetEntityCount(); ++entity )
    {
      const char *typestr = (char*)wf->GetEntityType(entity);      	  
      
      // don't load window entries here
      if( strcmp( typestr, "window" ) == 0 )
		  {
			 /* do nothing here */
		  }
      else if( strcmp( typestr, "block" ) == 0 )
				LoadBlock( wf, entity );
      else if( strcmp( typestr, "sensor" ) == 0 )
				LoadSensor( wf, entity );
			else
				LoadModel( wf, entity );
    }
  
  // call all controller init functions
  FOR_EACH( it, models )
	 {
		// all this is a hack and shouldn't be necessary
		(*it)->blockgroup.CalcSize();
		(*it)->UnMap(updates%2);
		(*it)->Map(updates%2);
		// to here

		(*it)->InitControllers();
	 }

  putchar( '\n' );
}

void World::UnLoad()
{
  if( wf ) delete wf;

  FOR_EACH( it, children )
    delete (*it);
  children.clear();
 
  models_by_name.clear();
  models_by_wfentity.clear();
  
  ray_list.clear();

  // todo - clean up regions & superregions?
	
  token = "[unloaded]";
}

bool World::PastQuitTime() 
{ 
	return( (quit_time > 0) && (sim_time >= quit_time) ); 
}

std::string World::ClockString() const
{
  const uint32_t usec_per_hour   = 3600000000U;
  const uint32_t usec_per_minute = 60000000U;
  const uint32_t usec_per_second = 1000000U;
  const uint32_t usec_per_msec = 1000U;
	
	const uint32_t hours   = sim_time / usec_per_hour;
  const uint32_t minutes = (sim_time % usec_per_hour) / usec_per_minute;
  const uint32_t seconds = (sim_time % usec_per_minute) / usec_per_second;
  const uint32_t msec    = (sim_time % usec_per_second) / usec_per_msec;
	
  std::string str;  
  char buf[256];

  if( hours > 0 )
    {
      snprintf( buf, 255, "%uh", hours );
      str += buf;
    }

  snprintf( buf, 255, " %um %02us %03umsec", minutes, seconds, msec);
  str += buf;
  
  return str;
}

void World::AddUpdateCallback( world_callback_t cb, 
										 void* user )
{
  // add the callback & argument to the list
  cb_list.push_back( std::pair<world_callback_t,void*>(cb, user) );

}

int World::RemoveUpdateCallback( world_callback_t cb, 
											void* user )
{
  std::pair<world_callback_t,void*> p( cb, user );
  
  FOR_EACH( it, cb_list )
    {
      if( (*it) == p )
		  {
			 cb_list.erase( it );		
			 break;
		  }
    }
  
  // return the number of callbacks now in the list. Useful for
  // detecting when the list is empty.
  return cb_list.size();
}

void World::CallUpdateCallbacks()
{
	// call model CB_UPDATE callbacks queued up by worker threads
	size_t threads( pending_update_callbacks.size() );
	int cbcount( 0 );
	
	for( size_t t(0); t<threads; ++t )
		{
			std::queue<Model*>& q( pending_update_callbacks[t] );
			
// 			printf( "pending callbacks for thread %u: %u\n", 
// 							(unsigned int)t, 
// 							(unsigned int)q.size() );
			
			cbcount += q.size();

			while( ! q.empty() )
				{
					q.front()->CallUpdateCallbacks();
					q.pop();
				}
		}
	//	printf( "cb total %u (global %d)\n\n", (unsigned int)cbcount,update_cb_count );
	
	assert( update_cb_count >= cbcount );

	// world callbacks
  FOR_EACH( it, cb_list )
    {  
      if( ((*it).first )( this, (*it).second ) )
				it = cb_list.erase( it );
    }      
}

void World::ConsumeQueue( unsigned int queue_num )
{  
  std::priority_queue<Event>& queue = event_queues[queue_num];
  
  if( queue.empty() )
    return;
  
  //printf( "event queue len %d\n", (int)queue.size() );
  
  // update everything on the event queue that happens at this time or earlier
  do
    {
      Event ev( queue.top() );
      if( ev.time > sim_time ) break;
      queue.pop();
			
      //printf( "Q%d @ %llu next event ptr %p cb %p\n", queue_num, sim_time, ev.mod, ev.cb );
      //std::string modelType = ev.mod->GetModelType();
      //printf( "@ %llu next event <%s %llu %s>\n",  sim_time, modelType.c_str(), ev.time, ev.mod->Token() ); 
      
			ev.cb( ev.mod, ev.arg); // call the event's callback on the model			
    }
  while( !queue.empty() );
}

bool World::Update()
{
  //puts( "World::Update()" );
	
  // if we've run long enough, exit
  if( PastQuitTime() ) 
		return true;		
	
  if( show_clock && ((this->updates % show_clock_interval) == 0) )
    {
      printf( "\r[Stage: %s]", ClockString().c_str() );
      fflush( stdout );
    }
	
  sim_time += sim_interval; 
	
  
	// rebuild the sets sorted by position on x,y axis
	models_with_fiducials_byx.clear(); 
	models_with_fiducials_byy.clear(); 
	
 	FOR_EACH( it, models_with_fiducials )
		{
			models_with_fiducials_byx.insert( *it ); 
			models_with_fiducials_byy.insert( *it ); 
		}

	//printf( "x %lu y %lu\n", models_with_fiducials_byy.size(),
	//			models_with_fiducials_byx.size() );

  // handle the zeroth queue synchronously in the main thread
  ConsumeQueue( 0 );
  
  // handle all the remaining queues asynchronously in worker threads
  if( worker_threads > 0 )
	 {
		pthread_mutex_lock( &sync_mutex );
		threads_working = worker_threads; 
		// unblock the workers - they are waiting on this condition var
		//puts( "main thread signalling workers" );
		pthread_cond_broadcast( &threads_start_cond );
		pthread_mutex_unlock( &sync_mutex );		 
		
		// todo - take the 1th thread work here?
		FOR_EACH( it, active_velocity )
		  (*it)->Move();
		
		pthread_mutex_lock( &sync_mutex );
		// wait for all the last update job to complete - it will
		// signal the worker_threads_done condition var
		while( threads_working > 0  )
		  {
			 //puts( "main thread waiting for workers to finish" );
			 pthread_cond_wait( &threads_done_cond, &sync_mutex );
		  }
		pthread_mutex_unlock( &sync_mutex );		 
		//puts( "main thread awakes" );
		
		// TODO: allow threadsafe callbacks to be called in worker
		// threads		
	 }
  
  dirty = true; // need redraw 
  
  // this stuff must be done in series here
  
  // world callbacks
  CallUpdateCallbacks();
  
  FOR_EACH( it, active_energy )
	 (*it)->UpdateCharge();
  
  ++updates;  
    
  return false;
}

unsigned int World::GetEventQueue( Model* mod ) const
{
  if( worker_threads < 1 )
    return 0;
  return( (random() % worker_threads) + 1);
}

Model* World::GetModel( const std::string& name ) const
{
  PRINT_DEBUG1( "looking up model name %s in models_by_name", name.c_str() );
	
  std::map<std::string,Model*>::const_iterator it( models_by_name.find( name ) );
	
  if( it == models_by_name.end() )
    {
      PRINT_WARN1( "lookup of model name %s: no matching name", name.c_str() );
      return NULL;
    }
  else
    return it->second; // the Model*
}

void World::RecordRay( double x1, double y1, double x2, double y2 )
{  
  float* drawpts = new float[4];
  drawpts[0] = x1;
  drawpts[1] = y1;
  drawpts[2] = x2;
  drawpts[3] = y2;
  ray_list.push_back( drawpts );
}

void World::ClearRays()
{
  FOR_EACH( it, ray_list )
    {
      float* pts = *it;
      delete [] pts;
    }
  
  ray_list.clear();
}


void World::Raytrace( const Pose &gpose, // global pose
							 const meters_t range,
							 const radians_t fov,
							 const ray_test_func_t func,
							 const Model* model,			 
							 const void* arg,
							 RaytraceResult* samples, // preallocated storage for samples
							 const uint32_t sample_count, // number of samples
							 const bool ztest ) 
{
  // find the direction of the first ray
  Pose raypose = gpose;
  const double starta = fov/2.0 - raypose.a;
  
  for( uint32_t s=0; s < sample_count; ++s )
    {
      raypose.a = (s * fov / (double)(sample_count-1)) - starta;
      samples[s] = Raytrace( raypose, range, func, model, arg, ztest );
    }
}

// Stage spends 50-99% of its time in this method.
RaytraceResult World::Raytrace( const Pose &gpose, 
													const meters_t range,
													const ray_test_func_t func,
													const Model* mod,		
													const void* arg,
													const bool ztest ) 
{
  return Raytrace( Ray( mod, gpose, range, func, arg, ztest ));
}


RaytraceResult World::Raytrace( const Ray& r )
{
  //rt_cells.clear();
  //rt_candidate_cells.clear();
  
  // initialize the sample
  RaytraceResult sample( r.origin, r.range );
	
  // our global position in (floating point) cell coordinates
  double globx( r.origin.x * ppm );
  double globy( r.origin.y * ppm );
	
  // record our starting position
  const double startx( globx );
  const double starty( globy );
  
  // eliminate a potential divide by zero
  const double angle( r.origin.a == 0.0 ? 1e-12 : r.origin.a );
  const double cosa(cos(angle));
  const double sina(sin(angle));
  const double tana(sina/cosa); // = tan(angle)

  // the x and y components of the ray (these need to be doubles, or a
  // very weird and rare bug is produced)
  const double dx( ppm * r.range * cosa);
  const double dy( ppm * r.range * sina);
  
  // fast integer line 3d algorithm adapted from Cohen's code from
  // Graphics Gems IV  
  const int32_t sx(sgn(dx));  
  const int32_t sy(sgn(dy));  
  const int32_t ax(abs(dx)); 
  const int32_t ay(abs(dy));  
  const int32_t bx(2*ax);	
  const int32_t by(2*ay);	
  int32_t exy(ay-ax); // difference between x and y distances
  int32_t n(ax+ay); // the manhattan distance to the goal cell
    
  // the distances between region crossings in X and Y
  const double xjumpx( sx * REGIONWIDTH );
  const double xjumpy( sx * REGIONWIDTH * tana );
  const double yjumpx( sy * REGIONWIDTH / tana );
  const double yjumpy( sy * REGIONWIDTH );

  // manhattan distance between region crossings in X and Y
  const double xjumpdist( fabs(xjumpx)+fabs(xjumpy) );
  const double yjumpdist( fabs(yjumpx)+fabs(yjumpy) );

  const unsigned int layer( (updates+1) % 2 );
  
  // these are updated as we go along the ray
  double xcrossx(0), xcrossy(0);
  double ycrossx(0), ycrossy(0);
  double distX(0), distY(0);
  bool calculatecrossings( true );

  // Stage spends up to 95% of its time in this loop! It would be
  // neater with more function calls encapsulating things, but even
  // inline calls have a noticeable (2-3%) effect on performance.
  while( n > 0  ) // while we are still not at the ray end
    { 
			SuperRegion* sr( GetSuperRegion(point_int_t(GETSREG(globx),GETSREG(globy))));
			Region* reg( sr ?	sr->GetRegion(GETREG(globx),GETREG(globy)) : NULL );
			
      if( reg && reg->count ) // if the region contains any objects
				{
					//assert( reg->cells.size() );
					
					// invalidate the region crossing points used to jump over
			 // empty regions
			 calculatecrossings = true;
					
			 // convert from global cell to local cell coords
			 int32_t cx( GETCELL(globx) ); 
			 int32_t cy( GETCELL(globy) );

			 //Cell* c = reg->GetCell(cx,cy);
			 Cell* c( &reg->cells[ cx + cy * REGIONWIDTH ] );
			 assert(c); // should be good: we know the region contains objects

			 // while within the bounds of this region and while some ray remains
			 // we'll tweak the cell pointer directly to move around quickly
			 while( (cx>=0) && (cx<REGIONWIDTH) && 
					  (cy>=0) && (cy<REGIONWIDTH) && 
					  n > 0 )
				{			 
				  FOR_EACH( it, c->blocks[layer] )
					 {	      	      
						Block* block( *it );
						assert( block );

						// skip if not in the right z range
						if( r.ztest && 
							 ( r.origin.z < block->global_z.min || 
								r.origin.z > block->global_z.max ) )
						  continue; 
									
						// test the predicate we were passed
						if( (*r.func)( block->mod, (Model*)r.mod, r.arg )) 
						  {
							 // a hit!
							 sample.color = block->GetColor();
							 sample.mod = block->mod;
											
							 if( ax > ay ) // faster than the equivalent hypot() call
								sample.range = fabs((globx-startx) / cosa) / ppm;
							 else
								sample.range = fabs((globy-starty) / sina) / ppm;
											
							 return sample;
						  }				  
					 }

				  // increment our cell in the correct direction
				  if( exy < 0 ) // we're iterating along X
					 {
						globx += sx; // global coordinate
						exy += by;						
						c += sx; // move the cell left or right
						cx += sx; // cell coordinate for bounds checking
					 }
				  else  // we're iterating along Y
					 {
						globy += sy; // global coordinate
						exy -= bx;						
						c += sy * REGIONWIDTH; // move the cell up or down
						cy += sy; // cell coordinate for bounds checking
					 }			 
				  --n; // decrement the manhattan distance remaining
													 							
				  //rt_cells.push_back( point_int_t( globx, globy ));
				}					
			 //printf( "leaving populated region\n" );
		  }							 
      else // jump over the empty region
		  {		  		  		  
			 // on the first run, and when we've been iterating over
			 // cells, we need to calculate the next crossing of a region
			 // boundary along each axis
			 if( calculatecrossings )
				{
				  calculatecrossings = false;
							
				  // find the coordinate in cells of the bottom left corner of
				  // the current region
				  const int32_t ix( globx );
				  const int32_t iy( globy );				  
				  double regionx( ix/REGIONWIDTH*REGIONWIDTH );
				  double regiony( iy/REGIONWIDTH*REGIONWIDTH );
				  if( (globx < 0) && (ix % REGIONWIDTH) ) regionx -= REGIONWIDTH;
				  if( (globy < 0) && (iy % REGIONWIDTH) ) regiony -= REGIONWIDTH;
							
				  // calculate the distance to the edge of the current region
				  const double xdx( sx < 0 ? 
														regionx - globx - 1.0 : // going left
														regionx + REGIONWIDTH - globx ); // going right			 
				  const double xdy( xdx*tana );
					
				  const double ydy( sy < 0 ? 
														regiony - globy - 1.0 :  // going down
														regiony + REGIONWIDTH - globy ); // going up		 
				  const double ydx( ydy/tana );
					
				  // these stored hit points are updated as we go along
				  xcrossx = globx+xdx;
				  xcrossy = globy+xdy;
							
				  ycrossx = globx+ydx;
				  ycrossy = globy+ydy;
							
				  // find the distances to the region crossing points
				  // manhattan distance is faster than using hypot()
				  distX = fabs(xdx)+fabs(xdy);
				  distY = fabs(ydx)+fabs(ydy);		  
				}
					
			 if( distX < distY ) // crossing a region boundary left or right
				{
				  // move to the X crossing
				  globx = xcrossx; 
				  globy = xcrossy; 
							
				  n -= distX; // decrement remaining manhattan distance
							
				  // calculate the next region crossing
				  xcrossx += xjumpx; 
				  xcrossy += xjumpy;
							
				  distY -= distX;
				  distX = xjumpdist;
							
				  //rt_candidate_cells.push_back( point_int_t( xcrossx, xcrossy ));
				}			 
			 else // crossing a region boundary up or down
				{		  
				  // move to the X crossing
				  globx = ycrossx;
				  globy = ycrossy;
							
				  n -= distY; // decrement remaining manhattan distance				
							
				  // calculate the next region crossing
				  ycrossx += yjumpx;
				  ycrossy += yjumpy;
							
				  distX -= distY;
				  distY = yjumpdist;

				  //rt_candidate_cells.push_back( point_int_t( ycrossx, ycrossy ));
				}	
		  }			  	
      //rt_cells.push_back( point_int_t( globx, globy ));
    } 
  // hit nothing
  sample.mod = NULL;
  return sample;
}

static int _save_cb( Model* mod, void* dummy )
{
  mod->Save();
  return 0;
}

bool World::Save( const char *filename )
{
  ForEachDescendant( _save_cb, NULL );

  return this->wf->Save( filename );
}

static int _reload_cb(  Model* mod, void* dummy )
{
  mod->Load();
  return 0;
}

// reload the current worldfile
void World::Reload( void )
{
  ForEachDescendant( _reload_cb, NULL );
}

void World::MapPoly( const PointIntVec& pts, Block* block, unsigned int layer )
{
  const size_t pt_count = pts.size();
  
  for( size_t i(0); i<pt_count; ++i )
		{
			const point_int_t& start(pts[i] );
			const point_int_t& end(pts[(i+1)%pt_count]);
			
			// line rasterization adapted from Cohen's 3D version in
			// Graphics Gems II. Should be very fast.  
			const int32_t dx( end.x - start.x );
			const int32_t dy( end.y - start.y );
			const int32_t sx(sgn(dx));  
			const int32_t sy(sgn(dy));  
			const int32_t ax(abs(dx));  
			const int32_t ay(abs(dy));  
			const int32_t bx(2*ax);	
			const int32_t by(2*ay);	 
			int32_t exy(ay-ax); 
			int32_t n(ax+ay);
			
			int32_t globx(start.x);
			int32_t globy(start.y);
			
			while( n ) 
				{				
					Region* reg( GetSuperRegionCreate( point_int_t(GETSREG(globx), 
																												 GETSREG(globy)))
											 ->GetRegion( GETREG(globx), 
																		GETREG(globy)));										
					assert(reg);
					
					//printf( "REGION %p\n", reg );
					
					// add all the required cells in this region before looking up
					// another region			
					int32_t cx( GETCELL(globx) ); 
					int32_t cy( GETCELL(globy) );
					
					// need to call Region::GetCell() before using a Cell pointer
					// directly, because the region allocates cells lazily, waiting
					// for a call of this method
					Cell* c( reg->GetCell( cx, cy ) );
					
					// while inside the region, manipulate the Cell pointer directly
					while( (cx>=0) && (cx<REGIONWIDTH) && 
								 (cy>=0) && (cy<REGIONWIDTH) && 
								 n > 0 )
						{					
							c->AddBlock(block, layer ); 
							
							// cleverly skip to the next cell (now it's safe to
							// manipulate the cell pointer)
							if( exy < 0 ) 
								{
									globx += sx;
									exy += by;
									c += sx;
									cx += sx;
								}
							else 
								{
									globy += sy;
									exy -= bx; 
									c += sy * REGIONWIDTH;
									cy += sy;
								}
							--n;
						}
				}			
		}
}


SuperRegion* World::AddSuperRegion( const point_int_t& sup )
{
  SuperRegion* sr = CreateSuperRegion( sup );

  //printf( "Created super region [%d, %d] %p\n", sup.x, sup.y, sr );

  // the bounds of the world have changed
  //printf( "lower left (%.2f,%.2f,%.2f)\n", pt.x, pt.y, pt.z );
	
	// set the lower left corner of the new superregion
  Extend( point3_t( (sup.x << SRBITS) / ppm,
										(sup.y << SRBITS) / ppm,
										0 ));
	
	// top right corner of the new superregion
  Extend( point3_t( ((sup.x+1) << SRBITS) / ppm,
										((sup.y+1) << SRBITS) / ppm,
										0 ));
  //printf( "top right (%.2f,%.2f,%.2f)\n", pt.x, pt.y, pt.z );
  
  return sr;
}


inline SuperRegion* World::GetSuperRegion( const point_int_t& org )
{
  // around 99% of the time the SR is the same as last
  // lookup - cache  gives a 4% overall speed up :)
	
  if( sr_cached && sr_cached->GetOrigin() == org )
		return sr_cached;
	
  SuperRegion* sr = NULL;
	
	// I despise some STL syntax sometimes...
	std::map<point_int_t,SuperRegion*>::iterator it = 
		superregions.find(org);
	
	if( it != superregions.end() )
		sr = it->second;
  
  if( sr ) sr_cached = sr;
  
  return sr;
}

inline SuperRegion* World::GetSuperRegionCreate( const point_int_t& org )
{
  SuperRegion* sr = GetSuperRegion(org);
	
  if( sr == NULL ) // no superregion exists! make a new one
		{
			sr = AddSuperRegion( org );  
			assert( sr ); 
			sr_cached = sr;
		}
	
  return sr;
}


void World::Extend( point3_t pt )
{
  extent.x.min = std::min( extent.x.min, pt.x );
  extent.x.max = std::max( extent.x.max, pt.x );
  extent.y.min = std::min( extent.y.min, pt.y );
  extent.y.max = std::max( extent.y.max, pt.y );
  extent.z.min = std::min( extent.z.min, pt.z );
  extent.z.max = std::max( extent.z.max, pt.z );
}


void World::AddPowerPack( PowerPack* pp )
{
  powerpack_list.push_back( pp ); 
}

void World::RemovePowerPack( PowerPack* pp )
{
  EraseAll( pp, powerpack_list );
}

/// Register an Option for pickup by the GUI
void World:: RegisterOption( Option* opt )
{
  option_table.insert( opt );
}

void World::Log( Model* mod )
{
  LogEntry( sim_time, mod);
  printf( "log entry count %u\n", (unsigned int)LogEntry::Count() );
  //LogEntry::Print();
}

bool World::Event::operator<( const Event& other ) const 
{
  return( time > other.time );
}