From 448ad9f128102b7cb7318a63b0c952dd7a4b9f49 Mon Sep 17 00:00:00 2001 From: rtv Date: Fri, 26 Jun 2009 01:37:35 +0000 Subject: [PATCH] working on improved multithreaded updates - currently segfauklts when thread count is > 0 --- CMakeLists.txt | 4 +- libstage/model.cc | 2 +- libstage/model_callbacks.cc | 56 ++-- libstage/model_laser.cc | 160 ++++----- libstage/model_position.cc | 2 +- libstage/stage.hh | 19 +- libstage/world.cc | 636 +++++++++++++++++++----------------- libstage/worldfile.cc | 4 +- libstage/worldgui.cc | 14 +- todo.txt | 27 +- worlds/fasr.world | 28 +- 11 files changed, 504 insertions(+), 448 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 1cf4c6629..ea1e844c2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -35,8 +35,8 @@ INCLUDE (${PROJECT_CMAKE_DIR}/internal/FindOS.cmake) IF (NOT PROJECT_OS_WIN AND NOT PROJECT_OS_SOLARIS) # Using -Wall on Windows causes MSVC to produce thousands of warnings in its # own standard headers, dramatically slowing down the build. - SET (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall") - SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") + SET (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -g -Wall") + SET (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g -Wall") ENDIF (NOT PROJECT_OS_WIN AND NOT PROJECT_OS_SOLARIS) ENABLE_TESTING() diff --git a/libstage/model.cc b/libstage/model.cc index 80eacc5e8..953f6bbfb 100644 --- a/libstage/model.cc +++ b/libstage/model.cc @@ -725,7 +725,7 @@ void Model::CallUpdateCallbacks( void ) { // if we were updated this timestep, call the callbacks if( last_update == world->sim_time ) - CallCallbacks( &hooks.update ); + CallCallbacks( &hooks.update ); } stg_meters_t Model::ModelHeight() const diff --git a/libstage/model_callbacks.cc b/libstage/model_callbacks.cc index cbc39a8ec..f533d0362 100644 --- a/libstage/model_callbacks.cc +++ b/libstage/model_callbacks.cc @@ -1,35 +1,41 @@ #include "stage.hh" using namespace Stg; -// int key_gen( Model* mod, void* address ) -// { -// return ((int*)address) - ((int*)mod); -// } void Model::AddCallback( void* address, - stg_model_callback_t cb, - void* user ) + stg_model_callback_t cb, + void* user ) { - ///int* key = (int*)g_new( int, 1 ); - //*key = key_gen( this, address ); - - GList* cb_list = (GList*)g_hash_table_lookup( callbacks, address ); - - //printf( "installing callback in model %s with key %p\n", - // token, address ); - - // add the callback & argument to the list - cb_list = g_list_prepend( cb_list, new stg_cb_t( cb, user ) ); - - // and replace the list in the hash table - g_hash_table_insert( callbacks, address, cb_list ); +// // treat update callbacks specially as they are so frequently called +// if( address == &hooks.update ) +// { +// // a dedicated vector avoids a hash table lookup +// update_callbacks.push_back( std::pair( cb, user ) ); +// } +// else + { + GList* cb_list = (GList*)g_hash_table_lookup( callbacks, address ); + + //printf( "installing callback in model %s with key %p\n", + // token, address ); + + // add the callback & argument to the list + cb_list = g_list_prepend( cb_list, new stg_cb_t( cb, user ) ); + + // and replace the list in the hash table + g_hash_table_insert( callbacks, address, cb_list ); + } } int Model::RemoveCallback( void* member, stg_model_callback_t callback ) { - //int key = key_gen( this, member ); + + // XX TODO + // if( address == &hooks.update ) + // { + // std::erase( std::remove( update_callbacks.begin(); update_callbacks.end(), GList* cb_list = (GList*)g_hash_table_lookup( callbacks, member ); @@ -72,6 +78,16 @@ void Model::CallCallbacks( void* address ) { assert( address ); + +// // avoid a hash table lookup for +// if( address == &hooks.update ) +// { +// FOR_EACH( it, update_callbacks ) +// (it->first)( this, it->second ); + +// return; +// } + //int key = key_gen( this, address ); diff --git a/libstage/model_laser.cc b/libstage/model_laser.cc index 844618a9d..716c3e1e6 100644 --- a/libstage/model_laser.cc +++ b/libstage/model_laser.cc @@ -74,9 +74,9 @@ Option ModelLaser::Vis::showBeams( "Laser beams", "show_laser_beams", "", false, ModelLaser::ModelLaser( World* world, - Model* parent ) + Model* parent ) : Model( world, parent, MODEL_TYPE_LASER ), - vis( world ), + vis( world ), sample_count( DEFAULT_SAMPLES ), samples(), range_max( DEFAULT_MAXRANGE ), @@ -85,7 +85,7 @@ ModelLaser::ModelLaser( World* world, { PRINT_DEBUG2( "Constructing ModelLaser %d (%s)\n", - id, typestr ); + id, typestr ); // Model data members interval = DEFAULT_INTERVAL_MS * (int)thousand; @@ -153,8 +153,8 @@ void ModelLaser::SetConfig( Config& cfg ) } static bool laser_raytrace_match( Model* hit, - Model* finder, - const void* dummy ) + Model* finder, + const void* dummy ) { // Ignore the model that's looking and things that are invisible to // lasers @@ -176,7 +176,7 @@ void ModelLaser::Update( void ) // make the first and last rays exactly at the extremes of the FOV double sample_incr( fov / MAX(sample_count-1,1) ); - // find the global origin of our first emmitted ray + // find the global origin of our first emmitted ray Pose rayorg( geom.pose ); rayorg.z += geom.size.z/2.0; rayorg.a = bearing; @@ -188,39 +188,39 @@ void ModelLaser::Update( void ) // trace the ray, incrementing its heading for each sample for( unsigned int t(0); tRaytrace( ray ) ); - samples[t].range = r.range; + stg_raytrace_result_t r( world->Raytrace( ray ) ); + samples[t].range = r.range; // if we hit a model and it reflects brightly, we set // reflectance high, else low if( r.mod && ( r.mod->vis.laser_return >= LaserBright ) ) - samples[t].reflectance = 1; + samples[t].reflectance = 1; else - samples[t].reflectance = 0; + samples[t].reflectance = 0; - // point the ray to the next angle - ray.origin.a += sample_incr; + // point the ray to the next angle + ray.origin.a += sample_incr; } // we may need to interpolate the samples we skipped if( resolution > 1 ) { for( unsigned int t( resolution); t= sample_count ) - break; + for( unsigned int g(1); g= sample_count ) + break; - // copy the rightmost sample data into this point - samples[t-g] = samples[t-resolution]; + // copy the rightmost sample data into this point + samples[t-g] = samples[t-resolution]; - double left( samples[t].range ); - double right( samples[t-resolution].range ); + double left( samples[t].range ); + double right( samples[t-resolution].range ); - // linear range interpolation between the left and right samples - samples[t-g].range = (left-g*(left-right)/resolution); - } - } + // linear range interpolation between the left and right samples + samples[t-g].range = (left-g*(left-right)/resolution); + } + } MapFromRoot(); @@ -251,12 +251,12 @@ void ModelLaser::Print( char* prefix ) printf( "\tRanges[ " ); for( unsigned int i=0; i(mod) ); - const std::vector& samples( laser->GetSamples() ); + const std::vector& samples( laser->GetSamples() ); - size_t sample_count( samples.size() ); + size_t sample_count( samples.size() ); glPushMatrix(); glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); @@ -299,8 +299,8 @@ void ModelLaser::Vis::Visualize( Model* mod, Camera* cam ) glTranslatef( 0,0, laser->GetGeom().size.z/2.0 ); // shoot the laser beam out at the right height // pack the laser hit points into a vertex array for fast rendering - static std::vector pts; - pts.resize( 2 * (sample_count+1) ); + static std::vector pts; + pts.resize( 2 * (sample_count+1) ); pts[0] = 0.0; pts[1] = 0.0; @@ -310,76 +310,76 @@ void ModelLaser::Vis::Visualize( Model* mod, Camera* cam ) glPointSize( 2 ); for( unsigned int s(0); sfov / (sample_count-1))) - laser->fov/2.0; - pts[2*s+2] = (float)(samples[s].range * cos(ray_angle) ); - pts[2*s+3] = (float)(samples[s].range * sin(ray_angle) ); + { + double ray_angle = (s * (laser->fov / (sample_count-1))) - laser->fov/2.0; + pts[2*s+2] = (float)(samples[s].range * cos(ray_angle) ); + pts[2*s+3] = (float)(samples[s].range * sin(ray_angle) ); - // if the sample is unusually bright, draw a little blob - if( samples[s].reflectance > 0 ) - { - glBegin( GL_POINTS ); - glVertex2f( pts[2*s+2], pts[2*s+3] ); - glEnd(); - } - } + // if the sample is unusually bright, draw a little blob + if( samples[s].reflectance > 0 ) + { + glBegin( GL_POINTS ); + glVertex2f( pts[2*s+2], pts[2*s+3] ); + glEnd(); + } + } glVertexPointer( 2, GL_FLOAT, 0, &pts[0] ); laser->PopColor(); if( showArea ) - { - // draw the filled polygon in transparent blue - laser->PushColor( 0, 0, 1, 0.1 ); - glDrawArrays( GL_POLYGON, 0, sample_count+1 ); - laser->PopColor(); - //glDepthMask( GL_TRUE ); - } + { + // draw the filled polygon in transparent blue + laser->PushColor( 0, 0, 1, 0.1 ); + glDrawArrays( GL_POLYGON, 0, sample_count+1 ); + laser->PopColor(); + //glDepthMask( GL_TRUE ); + } glDepthMask( GL_TRUE ); if( showStrikes ) - { - // draw the beam strike points - laser->PushColor( 0, 0, 1, 0.8 ); - glDrawArrays( GL_POINTS, 0, sample_count+1 ); - laser->PopColor(); - } + { + // draw the beam strike points + laser->PushColor( 0, 0, 1, 0.8 ); + glDrawArrays( GL_POINTS, 0, sample_count+1 ); + laser->PopColor(); + } if( showFov ) - { - for( unsigned int s(0); sfov / (sample_count-1))) - laser->fov/2.0); - pts[2*s+2] = (float)(laser->range_max * cos(ray_angle) ); - pts[2*s+3] = (float)(laser->range_max * sin(ray_angle) ); - } + { + for( unsigned int s(0); sfov / (sample_count-1))) - laser->fov/2.0); + pts[2*s+2] = (float)(laser->range_max * cos(ray_angle) ); + pts[2*s+3] = (float)(laser->range_max * sin(ray_angle) ); + } - glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); - laser->PushColor( 0, 0, 1, 0.5 ); - glDrawArrays( GL_POLYGON, 0, sample_count+1 ); - laser->PopColor(); - // glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); - } + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + laser->PushColor( 0, 0, 1, 0.5 ); + glDrawArrays( GL_POLYGON, 0, sample_count+1 ); + laser->PopColor(); + // glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + } if( showBeams ) - { - laser->PushColor( 0, 0, 1, 0.5 ); - glBegin( GL_LINES ); + { + laser->PushColor( 0, 0, 1, 0.5 ); + glBegin( GL_LINES ); - for( unsigned int s(0); sfov / (sample_count-1))) - laser->fov/2.0); - glVertex2f( samples[s].range * cos(ray_angle), - samples[s].range * sin(ray_angle) ); + glVertex2f( 0,0 ); + double ray_angle((s * (laser->fov / (sample_count-1))) - laser->fov/2.0); + glVertex2f( samples[s].range * cos(ray_angle), + samples[s].range * sin(ray_angle) ); } - glEnd(); - laser->PopColor(); - } + glEnd(); + laser->PopColor(); + } glPopMatrix(); } diff --git a/libstage/model_position.cc b/libstage/model_position.cc index fb9f62226..5f3e23e2f 100644 --- a/libstage/model_position.cc +++ b/libstage/model_position.cc @@ -98,7 +98,7 @@ ModelPosition::ModelPosition( World* world, id, typestr ); // assert that Update() is reentrant for this derived model - thread_safe = true; + thread_safe = false; // no power consumed until we're subscribed this->SetWatts( 0 ); diff --git a/libstage/stage.hh b/libstage/stage.hh index 5191e40e0..e959d699a 100644 --- a/libstage/stage.hh +++ b/libstage/stage.hh @@ -955,8 +955,11 @@ namespace Stg GThreadPool *threadpool; /// superregions; SuperRegion* sr_cached; ///< The last superregion looked up by this world - - std::vector reentrant_update_list; ///< It is safe to call these model's Update() in parallel - std::vector nonreentrant_update_list; ///< It is NOT safe to call these model's Update() in parallel + + std::vector nonreentrant_update_list; ///< It is NOT safe to call these model's Update() in parallel + std::vector > reentrant_update_lists; ///< It is safe to call these model's Update() in parallel long unsigned int updates; ///< the number of simulated time steps executed so far Worldfile* wf; ///< If set, points to the worldfile used to create this world @@ -1102,7 +1105,7 @@ namespace Stg void StartUpdatingModelPose( Model* mod ); void StopUpdatingModelPose( Model* mod ); - static void update_thread_entry( Model* mod, World* world ); + static gpointer update_thread_entry( std::pair* info ); public: /** returns true when time to quit, false otherwise */ @@ -1770,7 +1773,9 @@ namespace Stg /** callback functions can be attached to any field in this structure. When the internal function model_change(,) is called, the callback is triggered */ - GHashTable* callbacks; + GHashTable* callbacks; + /** Dedicated storage for update callbacks - avoids looking them up in a hash table */ + //std::vector > update_callbacks; /** Default color of the model's blocks.*/ stg_color_t color; diff --git a/libstage/world.cc b/libstage/world.cc index 9b0952243..e633b6bc7 100644 --- a/libstage/world.cc +++ b/libstage/world.cc @@ -60,8 +60,8 @@ GList* World::world_list = NULL; World::World( const char* token, - stg_msec_t interval_sim, - double ppm ) + stg_msec_t interval_sim, + double ppm ) : // private charge_list( NULL ), @@ -76,11 +76,13 @@ World::World( const char* token, show_clock( false ), show_clock_interval( 100 ), // 10 simulated seconds using defaults thread_mutex( g_mutex_new() ), - threadpool( NULL ), + //threadpool( NULL ), total_subs( 0 ), velocity_list( NULL ), worker_threads( 0 ), - worker_threads_done( g_cond_new() ), + threads_working( 0 ), + threads_start_cond( g_cond_new() ), + threads_done_cond( g_cond_new() ), models_with_fiducials(), // protected @@ -94,8 +96,8 @@ World::World( const char* token, sim_time( 0 ), superregions(), sr_cached(NULL), - reentrant_update_list(), nonreentrant_update_list(), + reentrant_update_lists(1), updates( 0 ), wf( NULL ), paused( false ) @@ -143,24 +145,49 @@ bool World::UpdateAll() for( GList* it = World::world_list; it; it=it->next ) { if( ((World*)it->data)->Update() == false ) - quit = false; + quit = false; } return quit; } -void World::update_thread_entry( Model* mod, World* world ) +gpointer World::update_thread_entry( std::pair *thread_info ) { - mod->Update(); - + World* world = thread_info->first; + int thread_instance = thread_info->second; + g_mutex_lock( world->thread_mutex ); - // world->update_jobs_pending--; - // if( world->update_jobs_pending == 0 ) + while( 1 ) + { + // wait until the main thread signals us + //puts( "worker waiting for start signal" ); + g_cond_wait( world->threads_start_cond, world->thread_mutex ); + g_mutex_unlock( world->thread_mutex ); + + //puts( "worker thread awakes" ); + + //r loop over the list of rentrant models for this thread + FOR_EACH( it, world->reentrant_update_lists[thread_instance] ) + { + Model* mod = *it; + //printf( "thread %d updating model %s (%p)\n", thread_instance, mod->Token(), mod ); + mod->UpdateIfDue(); + mod->CallUpdateCallbacks(); + } + + // 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 + g_mutex_lock( world->thread_mutex ); + if( --world->threads_working == 0 ) + { + //puts( "last worker signalling main thread" ); + g_cond_signal( world->threads_done_cond ); + } + //g_mutex_unlock( world->thread_mutex ); + } - if( g_thread_pool_unprocessed( world->threadpool ) < 1 ) - g_cond_signal( world->worker_threads_done ); - - g_mutex_unlock( world->thread_mutex ); + return NULL; } @@ -179,7 +206,7 @@ void World::LoadBlock( Worldfile* wf, int entity, GHashTable* entitytable ) { // lookup the group in which this was defined Model* mod = (Model*)g_hash_table_lookup( entitytable, - (gpointer)wf->GetEntityParent( entity ) ); + (gpointer)wf->GetEntityParent( entity ) ); if( ! mod ) PRINT_ERR( "block has no model for a parent" ); @@ -207,11 +234,11 @@ Model* World::CreateModel( Model* parent, const char* typestr ) //printf( "creating model of type %s\n", typestr ); for( int i=0; iGetEntityParent( entity ); PRINT_DEBUG2( "wf entity %d parent entity %d\n", - entity, parent_entity ); + entity, parent_entity ); Model* parent = (Model*)g_hash_table_lookup( entitytable, - (gpointer)parent_entity ); + (gpointer)parent_entity ); char *typestr = (char*)wf->GetEntityType(entity); assert(typestr); @@ -281,16 +308,13 @@ void World::Load( const char* worldfile_path ) this->token = (char*) wf->ReadString( entity, "name", token ); - this->paused = - wf->ReadInt( entity, "paused", this->paused ); - this->interval_sim = (stg_usec_t)thousand * wf->ReadInt( entity, "interval_sim", - (int)(this->interval_sim/thousand) ); + (int)(this->interval_sim/thousand) ); if( wf->PropertyExists( entity, "quit_time" ) ) { this->quit_time = (stg_usec_t) ( million * - wf->ReadFloat( entity, "quit_time", 0 ) ); + wf->ReadFloat( entity, "quit_time", 0 ) ); } if( wf->PropertyExists( entity, "resolution" ) ) @@ -298,28 +322,27 @@ void World::Load( const char* worldfile_path ) 1.0 / wf->ReadFloat( entity, "resolution", 1.0 / this->ppm ); //_stg_disable_gui = wf->ReadInt( entity, "gui_disable", _stg_disable_gui ); - - if( wf->PropertyExists( entity, "threadpool" ) ) + + if( wf->PropertyExists( entity, "threads" ) ) { - int count = wf->ReadInt( entity, "threadpool", worker_threads ); - - if( count && (count != (int)worker_threads) ) - { - worker_threads = count; - - if( threadpool == NULL ) - threadpool = g_thread_pool_new( (GFunc)update_thread_entry, - this, - worker_threads, - true, - NULL ); - else - g_thread_pool_set_max_threads( threadpool, - worker_threads, - NULL ); - - printf( "[threadpool %u]", worker_threads ); - } + this->worker_threads = wf->ReadInt( entity, "threads", this->worker_threads ); + + if( worker_threads > 0 ) + { + reentrant_update_lists.resize( worker_threads ); + + // kick off count threads. + for( unsigned int t=0; t *p = new std::pair( this, t ); + g_thread_create( (GThreadFunc)World::update_thread_entry, + p, + false, + NULL ); + } + + printf( "[threadpool %u]", worker_threads ); + } } // Iterate through entitys and create objects of the appropriate type @@ -329,15 +352,15 @@ void World::Load( const char* worldfile_path ) // don't load window entries here if( strcmp( typestr, "window" ) == 0 ) - { - /* do nothing here */ - } + { + /* do nothing here */ + } else if( strcmp( typestr, "block" ) == 0 ) - LoadBlock( wf, entity, entitytable ); - // else if( strcmp( typestr, "puck" ) == 0 ) - // LoadPuck( wf, entity, entitytable ); - else - LoadModel( wf, entity, entitytable ); + LoadBlock( wf, entity, entitytable ); + // else if( strcmp( typestr, "puck" ) == 0 ) + // LoadPuck( wf, entity, entitytable ); + else + LoadModel( wf, entity, entitytable ); } @@ -348,13 +371,13 @@ void World::Load( const char* worldfile_path ) g_hash_table_destroy( entitytable ); FOR_EACH( it, children ) - (*it)->InitRecursive(); + (*it)->InitRecursive(); stg_usec_t load_end_time = RealTimeNow(); if( debug ) printf( "[Load time %.3fsec]\n", - (load_end_time - load_start_time) / 1000000.0 ); + (load_end_time - load_start_time) / 1000000.0 ); else putchar( '\n' ); } @@ -364,12 +387,12 @@ void World::UnLoad() if( wf ) delete wf; FOR_EACH( it, children ) - delete (*it); + delete (*it); children.clear(); g_hash_table_remove_all( models_by_name ); - reentrant_update_list.clear(); + reentrant_update_lists.clear(); nonreentrant_update_list.clear(); g_list_free( ray_list ); @@ -423,10 +446,10 @@ std::string World::ClockString() char buf[256]; if( hours > 0 ) - { - snprintf( buf, 255, "%uh", hours ); - str += buf; - } + { + snprintf( buf, 255, "%uh", hours ); + str += buf; + } snprintf( buf, 255, " %um %02us %03umsec", minutes, seconds, msec); str += buf; @@ -435,7 +458,7 @@ std::string World::ClockString() } void World::AddUpdateCallback( stg_world_callback_t cb, - void* user ) + void* user ) { // add the callback & argument to the list std::pair p(cb, user); @@ -443,21 +466,21 @@ void World::AddUpdateCallback( stg_world_callback_t cb, } int World::RemoveUpdateCallback( stg_world_callback_t cb, - void* user ) + void* user ) { std::pair p( cb, user ); std::list >::iterator it; for( it = cb_list.begin(); - it != cb_list.end(); - it++ ) - { - if( (*it) == p ) - { - cb_list.erase( it ); - break; - } - } + it != cb_list.end(); + it++ ) + { + 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. @@ -469,17 +492,17 @@ void World::CallUpdateCallbacks() // for each callback in the list for( std::list >::iterator it = cb_list.begin(); - it != cb_list.end(); - it++ ) - { - //printf( "cbs %p data %p cvs->next %p\n", cbs, cbs->data, cbs->next ); + it != cb_list.end(); + it++ ) + { + //printf( "cbs %p data %p cvs->next %p\n", cbs, cbs->data, cbs->next ); - if( ((*it).first )( this, (*it).second ) ) - { - //printf( "callback returned TRUE - schedule removal from list\n" ); - it = cb_list.erase( it ); - } - } + if( ((*it).first )( this, (*it).second ) ) + { + //printf( "callback returned TRUE - schedule removal from list\n" ); + it = cb_list.erase( it ); + } + } } bool World::Update() @@ -506,50 +529,46 @@ bool World::Update() // then update all models on the update lists FOR_EACH( it, nonreentrant_update_list ) - (*it)->UpdateIfDue(); + (*it)->UpdateIfDue(); //printf( "nonre list length %d\n", g_list_length( nonreentrant_update_list ) ); //printf( "re list length %d\n", g_list_length( reentrant_update_list ) ); if( worker_threads == 0 ) // do all the work in this thread { - FOR_EACH( it, reentrant_update_list ) - (*it)->UpdateIfDue(); + FOR_EACH( it, reentrant_update_lists[0] ) + (*it)->UpdateIfDue(); } else // use worker threads { - // push the update for every model that needs it into the thread pool - FOR_EACH( it, reentrant_update_list ) - { - Model* mod = (*it); - - if( mod->UpdateDue() ) - { - //printf( "updating model %s in WORKER thread\n", mod->Token() ); - //g_mutex_lock( thread_mutex ); - //update_jobs_pending++; - //g_mutex_unlock( thread_mutex ); - g_thread_pool_push( threadpool, mod, NULL ); - } - } - + g_mutex_lock( thread_mutex ); + threads_working = worker_threads; + // unblock the workers - they are waiting on this condition var + //puts( "main thread signalling workers" ); + g_cond_broadcast( threads_start_cond ); + // wait for all the last update job to complete - it will // signal the worker_threads_done condition var - g_mutex_lock( thread_mutex ); - while( g_thread_pool_unprocessed( threadpool ) ) //update_jobs_pending ) - g_cond_wait( worker_threads_done, thread_mutex ); + while( threads_working > 0 ) + { + //puts( "main thread waiting for workers to finish" ); + g_cond_wait( threads_done_cond, thread_mutex ); + } g_mutex_unlock( thread_mutex ); - // now call all the callbacks - ignores dueness, but not a big deal - FOR_EACH( it, reentrant_update_list ) - (*it)->CallUpdateCallbacks(); + //puts( "main thread awakes" ); + + // TODO - move this back here! + // now call all the callbacks - ignores dueness, but not a big deal + //FOR_EACH( it, reentrant_update_list ) + //(*it)->CallUpdateCallbacks(); } if( show_clock && ((this->updates % show_clock_interval) == 0) ) - { - printf( "\r[Stage: %s]", ClockString().c_str() ); - fflush( stdout ); - } + { + printf( "\r[Stage: %s]", ClockString().c_str() ); + fflush( stdout ); + } CallUpdateCallbacks(); @@ -600,33 +619,33 @@ void World::ClearRays() void World::Raytrace( const Pose &gpose, // global pose - const stg_meters_t range, - const stg_radians_t fov, - const stg_ray_test_func_t func, - const Model* model, - const void* arg, - stg_raytrace_result_t* samples, // preallocated storage for samples - const uint32_t sample_count, // number of samples - const bool ztest ) + const stg_meters_t range, + const stg_radians_t fov, + const stg_ray_test_func_t func, + const Model* model, + const void* arg, + stg_raytrace_result_t* 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; double starta = fov/2.0 - raypose.a; - + for( uint32_t s=0; s < sample_count; s++ ) { - raypose.a = (s * fov / (double)sample_count) - starta; + raypose.a = (s * fov / (double)sample_count) - starta; samples[s] = Raytrace( raypose, range, func, model, arg, ztest ); } } // Stage spends 50-99% of its time in this method. stg_raytrace_result_t World::Raytrace( const Pose &gpose, - const stg_meters_t range, - const stg_ray_test_func_t func, - const Model* mod, - const void* arg, - const bool ztest ) + const stg_meters_t range, + const stg_ray_test_func_t func, + const Model* mod, + const void* arg, + const bool ztest ) { Ray r( mod, gpose, range, func, arg, ztest ); return Raytrace( r ); @@ -693,156 +712,157 @@ stg_raytrace_result_t World::Raytrace( const Ray& r ) // inline calls have a noticeable (2-3%) effect on performance while( n > 0 ) // while we are still not at the ray end { - Region* reg( GetSuperRegionCached( GETSREG(globx), GETSREG(globy) ) - ->GetRegion( GETREG(globx), GETREG(globy) )); + Region* reg( GetSuperRegionCached( GETSREG(globx), GETSREG(globy) ) + ->GetRegion( GETREG(globx), GETREG(globy) )); - if( reg->count ) // if the region contains any objects - { - // invalidate the region crossing points used to jump over - // empty regions - calculatecrossings = true; + if( reg->count ) // if the region contains any objects + { + // 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) ); + // convert from global cell to local cell coords + int32_t cx( GETCELL(globx) ); + int32_t cy( GETCELL(globy) ); - Cell* c( ®->cells[ cx + cy * REGIONWIDTH ] ); - assert(c); // should be good: we know the region contains objects + Cell* c( ®->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=0) && (cy 0 ) - { - for( std::vector::iterator it( c->blocks.begin() ); - it != c->blocks.end(); - ++it ) - { - Block* block( *it ); - - // 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; + // 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=0) && (cy 0 ) + { + for( std::vector::iterator it( c->blocks.begin() ); + it != c->blocks.end(); + ++it ) + { + 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; + // 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; + 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( stg_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 ) + 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 { - calculatecrossings = false; + 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( stg_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 - int32_t ix( globx ); - 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; + // find the coordinate in cells of the bottom left corner of + // the current region + int32_t ix( globx ); + 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 - double xdx( sx < 0 ? - regionx - globx - 1.0 : // going left - regionx + REGIONWIDTH - globx ); // going right - double xdy( xdx*tana ); + // calculate the distance to the edge of the current region + double xdx( sx < 0 ? + regionx - globx - 1.0 : // going left + regionx + REGIONWIDTH - globx ); // going right + double xdy( xdx*tana ); - double ydy( sy < 0 ? - regiony - globy - 1.0 : // going down - regiony + REGIONWIDTH - globy ); // going up - double ydx( ydy/tana ); + double ydy( sy < 0 ? + regiony - globy - 1.0 : // going down + regiony + REGIONWIDTH - globy ); // going up + double ydx( ydy/tana ); - // these stored hit points are updated as we go along - xcrossx = globx+xdx; - xcrossy = globy+xdy; + // these stored hit points are updated as we go along + xcrossx = globx+xdx; + xcrossy = globy+xdy; - ycrossx = globx+ydx; - ycrossy = globy+ydy; + 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); - } + // 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; + 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 + n -= distX; // decrement remaining manhattan distance - // calculate the next region crossing - xcrossx += xjumpx; - xcrossy += xjumpy; + // calculate the next region crossing + xcrossx += xjumpx; + xcrossy += xjumpy; - distY -= distX; - distX = xjumpdist; + distY -= distX; + distX = xjumpdist; - //rt_candidate_cells.push_back( stg_point_int_t( xcrossx, xcrossy )); - } - else // crossing a region boundary up or down - { - // move to the X crossing - globx = ycrossx; - globy = ycrossy; + //rt_candidate_cells.push_back( stg_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 + n -= distY; // decrement remaining manhattan distance - // calculate the next region crossing - ycrossx += yjumpx; - ycrossy += yjumpy; + // calculate the next region crossing + ycrossx += yjumpx; + ycrossy += yjumpy; - distX -= distY; - distY = yjumpdist; - - //rt_candidate_cells.push_back( stg_point_int_t( ycrossx, ycrossy )); - } - } - //rt_cells.push_back( stg_point_int_t( globx, globy )); - } + distX -= distY; + distY = yjumpdist; + + //rt_candidate_cells.push_back( stg_point_int_t( ycrossx, ycrossy )); + } + } + //rt_cells.push_back( stg_point_int_t( globx, globy )); + } // hit nothing sample.mod = NULL; return sample; @@ -937,14 +957,14 @@ inline SuperRegion* World::GetSuperRegion( const stg_point_int_t& sup ) Cell* World::GetCell( const stg_point_int_t& glob ) { return( ((Region*)GetSuperRegionCached( GETSREG(glob.x), GETSREG(glob.y) ) - ->GetRegion( GETREG(glob.x), GETREG(glob.y) )) - ->GetCell( GETCELL(glob.x), GETCELL(glob.y) )) ; + ->GetRegion( GETREG(glob.x), GETREG(glob.y) )) + ->GetCell( GETCELL(glob.x), GETCELL(glob.y) )) ; } void World::ForEachCellInLine( const stg_point_int_t& start, - const stg_point_int_t& end, - std::vector& cells ) + const stg_point_int_t& end, + std::vector& cells ) { // line rasterization adapted from Cohen's 3D version in // Graphics Gems II. Should be very fast. @@ -969,46 +989,46 @@ void World::ForEachCellInLine( const stg_point_int_t& start, while( n ) { - Region* reg( GetSuperRegionCached( GETSREG(globx), GETSREG(globy) ) - ->GetRegion( GETREG(globx), GETREG(globy) )); + Region* reg( GetSuperRegionCached( GETSREG(globx), GETSREG(globy) ) + ->GetRegion( GETREG(globx), GETREG(globy) )); - // add all the required cells in this region before looking up - // another region - int32_t cx( GETCELL(globx) ); - int32_t cy( GETCELL(globy) ); + // 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( (cx>=0) && (cx=0) && (cy 0 ) - { - // find the cell at this location, then add it to the vector - cells.push_back( c ); + // 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( (cx>=0) && (cx=0) && (cy 0 ) + { + // find the cell at this location, then add it to the vector + cells.push_back( c ); - // cleverly skip to the next cell (now it's safe to - // manipulate the cell pointer direcly) - if( exy < 0 ) - { - globx += sx; - exy += by; - c += sx; - cx += sx; - } - else - { - globy += sy; - exy -= bx; - c += sy * REGIONWIDTH; - cy += sy; - } - --n; - } + // cleverly skip to the next cell (now it's safe to + // manipulate the cell pointer direcly) + if( exy < 0 ) + { + globx += sx; + exy += by; + c += sx; + cx += sx; + } + else + { + globy += sy; + exy -= bx; + c += sy * REGIONWIDTH; + cy += sy; + } + --n; + } - } + } } void World::Extend( stg_point3_t pt ) @@ -1040,17 +1060,29 @@ void World:: RegisterOption( Option* opt ) void World::StartUpdatingModel( Model* mod ) { + //printf( "Adding model %s to update list ", mod->Token() ); + + int index = worker_threads > 0 ? mod->id%worker_threads : 0; + + + // if( mod->thread_safe ) + // printf( "reentrant[ %d ]\n", index ); + // else + // printf( " nonreentrant\n" ); + // choose the right update list - std::vector& vec = mod->thread_safe ? reentrant_update_list : nonreentrant_update_list; + std::vector& vec = mod->thread_safe ? reentrant_update_lists[index] : nonreentrant_update_list; // and add the model if not in the list already if( find( vec.begin(), vec.end(), mod ) == vec.end() ) - vec.push_back( mod ); + vec.push_back( mod ); } void World::StopUpdatingModel( Model* mod ) { + int index = worker_threads > 0 ? mod->id%worker_threads : 0; + // choose the right update list - std::vector& vec = mod->thread_safe ? reentrant_update_list : nonreentrant_update_list; + std::vector& vec = mod->thread_safe ? reentrant_update_lists[index] : nonreentrant_update_list; // and erase the model from it vec.erase( remove( vec.begin(), vec.end(), mod )); } @@ -1058,7 +1090,7 @@ void World::StopUpdatingModel( Model* mod ) void World::StartUpdatingModelPose( Model* mod ) { if( ! g_list_find( velocity_list, mod ) ) - velocity_list = g_list_append( velocity_list, mod ); + velocity_list = g_list_append( velocity_list, mod ); } void World::StopUpdatingModelPose( Model* mod ) diff --git a/libstage/worldfile.cc b/libstage/worldfile.cc index 227d63b76..6e7c4bf8f 100644 --- a/libstage/worldfile.cc +++ b/libstage/worldfile.cc @@ -536,7 +536,7 @@ bool Worldfile::LoadTokenInclude(FILE *file, int *line, int include) // There's no bounds-checking, but what the heck. char *tmp = strdup(this->filename); fullpath = new char[PATH_MAX]; - getcwd(fullpath, PATH_MAX); + char* dummy = getcwd(fullpath, PATH_MAX); strcat( fullpath, "/" ); strcat( fullpath, dirname(tmp)); strcat( fullpath, "/" ); @@ -1662,7 +1662,7 @@ const char *Worldfile::ReadFilename(int entity, const char *name, const char *va // There's no bounds-checking, but what the heck. char *tmp = strdup(this->filename); char* fullpath = new char[PATH_MAX]; - getcwd(fullpath, PATH_MAX); + char* dummy = getcwd(fullpath, PATH_MAX); strcat( fullpath, "/" ); strcat( fullpath, dirname(tmp)); strcat( fullpath, "/" ); diff --git a/libstage/worldgui.cc b/libstage/worldgui.cc index 777e659f1..7526f3066 100644 --- a/libstage/worldgui.cc +++ b/libstage/worldgui.cc @@ -268,6 +268,9 @@ void WorldGui::Load( const char* filename ) this->interval_real = (stg_usec_t)thousand * wf->ReadInt( world_section, "interval_real", (int)(this->interval_real/thousand) ); + // this is a world property that's only load()ed in worldgui + this->paused = + wf->ReadInt( world_section, "paused", this->paused ); // use the window section for the rest int window_section = wf->LookupEntity( "window" ); @@ -853,11 +856,12 @@ void WorldGui::UpdateOptions() std::set options; std::vector modOpts; - FOR_EACH( it, reentrant_update_list ) - { - modOpts = (*it)->getOptions(); - options.insert( modOpts.begin(), modOpts.end() ); - } + FOR_EACH( it1, reentrant_update_lists ) + FOR_EACH( it2, (*it1) ) + { + modOpts = (*it2)->getOptions(); + options.insert( modOpts.begin(), modOpts.end() ); + } FOR_EACH( it, nonreentrant_update_list ) { diff --git a/todo.txt b/todo.txt index b9c80d293..ae800b168 100644 --- a/todo.txt +++ b/todo.txt @@ -1,11 +1,18 @@ ** Optimization ** -speed records: +Timing benchmarks 3600 seconds of virtual time in real time seconds: + +simple.world +------------ +linux 8.89 +OS X 7.83 (before STL children) + +fasr.world +---------- +linux 16.36 +OS X 17.35 (before STL children) -simple.world 3600 seconds in 17.35 seconds -fasr.world 3600 seconds in 7.83 seconds (460 x real time) - ** 3.1.0 RELEASE * @@ -19,12 +26,10 @@ fasr.world 3600 seconds in 7.83 seconds (460 x real time) - docs for new models - push docs to SF -** 3.0.0 RELEASE ** - BUGS -* 3d collision detection broken - e.g. fancypioneer2dx -* [OSX only - FLTK bug?] loading window size constrains maximum window size +* [OSX only - FLTK bug?] loading window size constrains maximum window + size * must build before building source package so that make package_source picks up config.h @@ -33,12 +38,9 @@ INCOMPLETE * per-model visualization entries in view menu - DESIRABLE FEATURES * add blocks to world w/o model? -* energy model & recharging -* runtime update rate UI ** FUTURE RELEASES ** @@ -49,3 +51,6 @@ DONE * saving worldfile adds newlines after includes * alpha properly done * Fix 3D panning and scrolling +* 3d collision detection broken - e.g. fancypioneer2dx +* energy model & recharging +* runtime update rate UI diff --git a/worlds/fasr.world b/worlds/fasr.world index 72893937c..9d9eb5434 100644 --- a/worlds/fasr.world +++ b/worlds/fasr.world @@ -16,9 +16,7 @@ quit_time 3600 # 1 hour of simulated time resolution 0.02 # threads may speed things up here depending on available CPU cores & workload -# threadpool 8 -threadpool 0 - +# threads 3 # configure the GUI window window @@ -28,10 +26,6 @@ window center [ -0.067 -0.907 ] rotate [ 0 0 ] scale 40.786 - - pcam_loc [ 0 -4.000 2.000 ] - pcam_angle [ 70.000 0 ] - pcam_on 0 show_data 1 show_flags 1 @@ -156,16 +150,16 @@ define autorob pioneer2dx # ) ) -autorob( pose [6.343 6.423 0 -108.404] joules 300000 name "r0" ) -autorob( pose [7.216 6.947 0 -102.139] joules 100000 ) -autorob( pose [7.308 6.329 0 -87.236] joules 200000 ) -autorob( pose [6.750 7.165 0 -111.398] joules 400000 ) -autorob( pose [6.382 7.678 0 -93.939] joules 100000 ) -autorob( pose [6.292 7.125 0 -58.690] joules 200000 ) -autorob( pose [7.699 6.780 0 -108.018] joules 300000 ) -autorob( pose [7.545 7.515 0 -168.541] joules 400000 ) -autorob( pose [7.017 7.707 0 -179.862] joules 100000 ) -autorob( pose [6.818 6.348 0 -74.042] joules 200000 ) +autorob( pose [-1.667 -2.152 0 -90.000] joules 300000 ) +autorob( pose [1.384 7.361 0 -0.139] joules 100000 ) +autorob( pose [-5.258 -5.871 0 -120.113] joules 200000 ) +autorob( pose [6.600 5.211 0 -89.911] joules 400000 ) +autorob( pose [-6.355 1.726 0 90.000] joules 100000 ) +autorob( pose [4.452 -1.179 0 164.286] joules 200000 ) +autorob( pose [4.820 -3.446 0 24.788] joules 300000 ) +autorob( pose [-0.187 3.065 0 -153.738] joules 400000 ) +autorob( pose [4.309 3.736 0 121.988] joules 100000 ) +autorob( pose [6.261 7.461 0 0] joules 200000 ) #autorob( pose [5.060 6.868 0 -61.295] joules 300000 ) #autorob( pose [4.161 5.544 0 -147.713] joules 400000 )