Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with
or
.
Download ZIP
Browse files

more STL-ization. may reduce performance slightly in worlds with few …

…models
  • Loading branch information...
commit 5dee8f1251bb973d08b6a1998e0c7c6aecbe6068 1 parent af3b692
rtv authored
View
24 libstage/ancestor.cc
@@ -2,7 +2,7 @@
using namespace Stg;
Ancestor::Ancestor() :
- children( NULL ),
+ children(),
debug( false ),
puck_list( NULL ),
token( NULL )
@@ -13,13 +13,8 @@ Ancestor::Ancestor() :
Ancestor::~Ancestor()
{
- if( children )
- {
- for( GList* it = children; it; it=it->next )
- delete (Model*)it->data;
-
- g_list_free( children );
- }
+ FOR_EACH( it, children )
+ delete (*it);
}
void Ancestor::AddChild( Model* mod )
@@ -42,7 +37,7 @@ void Ancestor::AddChild( Model* mod )
mod->SetToken( buf );
- children = g_list_append( children, mod );
+ children.push_back( mod );
child_type_counts[mod->type]++;
@@ -52,7 +47,9 @@ void Ancestor::AddChild( Model* mod )
void Ancestor::RemoveChild( Model* mod )
{
child_type_counts[mod->type]--;
- children = g_list_remove( children, mod );
+
+ //children = g_list_remove( children, mod );
+ children.erase( remove( children.begin(), children.end(), mod ) );
}
Pose Ancestor::GetGlobalPose()
@@ -64,11 +61,12 @@ Pose Ancestor::GetGlobalPose()
void Ancestor::ForEachDescendant( stg_model_callback_t func, void* arg )
{
- for( GList* it=children; it; it=it->next ) {
- Model* mod = (Model*)it->data;
+ FOR_EACH( it, children )
+ {
+ Model* mod = (*it);
func( mod, arg );
mod->ForEachDescendant( func, arg );
- }
+ }
}
View
83 libstage/blockgroup.cc
@@ -30,42 +30,23 @@ void BlockGroup::AppendBlock( Block* block )
void BlockGroup::Clear()
{
-// while( blocks )
-// {
-// delete (Block*)blocks->data;
-// blocks = blocks->next;
-// }
-
- //g_list_free( blocks );
- //blocks = NULL;
-
- for( vector<Block*>::iterator it( blocks.begin() );
- it != blocks.end();
- ++it )
+ FOR_EACH( it, blocks )
delete *it;
-
+
blocks.clear();
}
-
void BlockGroup::SwitchToTestedCells()
{
// confirm the tentative pose for all blocks
- //LISTMETHOD( blocks, Block*, SwitchToTestedCells );
-
- for( vector<Block*>::iterator it( blocks.begin() );
- it != blocks.end();
- ++it )
- (*it)->SwitchToTestedCells();
+ FOR_EACH( it, blocks )
+ (*it)->SwitchToTestedCells();
}
GList* BlockGroup::AppendTouchingModels( GList* list )
{
- for( vector<Block*>::iterator it( blocks.begin() );
- it != blocks.end();
- ++it )
- list = (*it)->AppendTouchingModels( list );
-
+ FOR_EACH( it, blocks )
+ list = (*it)->AppendTouchingModels( list );
return list;
}
@@ -73,16 +54,10 @@ Model* BlockGroup::TestCollision()
{
Model* hitmod = NULL;
- for( vector<Block*>::iterator it( blocks.begin() );
- it != blocks.end();
- ++it )
+ FOR_EACH( it, blocks )
if( (hitmod = (*it)->TestCollision()))
break; // bail on the earliest collision
-// for( GList* it=blocks; it; it = it->next )
-// if( (hitmod = ((Block*)it->data)->TestCollision()))
-// break; // bail on the earliest collision
-
return hitmod; // NULL if no collision
}
@@ -98,10 +73,7 @@ void BlockGroup::CalcSize()
size.z = 0.0; // grow to largest z we see
- for( vector<Block*>::iterator it( blocks.begin() );
- it != blocks.end();
- ++it )
- // for( GList* it=blocks; it; it=it->next ) // examine all the blocks
+ FOR_EACH( it, blocks )
{
// examine all the points in the polygon
Block* block = *it;
@@ -132,22 +104,14 @@ void BlockGroup::CalcSize()
void BlockGroup::Map()
{
- for( std::vector<Block*>::iterator it( blocks.begin() );
- it != blocks.end();
- ++it )
+ FOR_EACH( it, blocks )
(*it)->Map();
-
- //LISTMETHOD( blocks, Block*, Map );
}
void BlockGroup::UnMap()
{
- for( std::vector<Block*>::iterator it( blocks.begin() );
- it != blocks.end();
- ++it )
+ FOR_EACH( it, blocks )
(*it)->UnMap();
-
- //LISTMETHOD( blocks, Block*, UnMap );
}
void BlockGroup::DrawSolid( const Geom & geom )
@@ -162,11 +126,8 @@ void BlockGroup::DrawSolid( const Geom & geom )
glTranslatef( -offset.x, -offset.y, -offset.z );
- for( std::vector<Block*>::iterator it( blocks.begin() );
- it != blocks.end();
- ++it )
+ FOR_EACH( it, blocks )
(*it)->DrawSolid();
- // LISTMETHOD( blocks, Block*, DrawSolid );
glPopMatrix();
}
@@ -180,12 +141,9 @@ void BlockGroup::DrawFootPrint( const Geom & geom )
geom.size.z / size.z );
glTranslatef( -offset.x, -offset.y, -offset.z );
-
- for( std::vector<Block*>::iterator it( blocks.begin() );
- it != blocks.end();
- ++it )
+
+ FOR_EACH( it, blocks )
(*it)->DrawFootPrint();
-// LISTMETHOD( blocks, Block*, DrawFootPrint);
glPopMatrix();
}
@@ -222,10 +180,7 @@ void BlockGroup::BuildDisplayList( Model* mod )
mod->PushColor( mod->color );
- for( vector<Block*>::iterator it( blocks.begin() );
- it != blocks.end();
- ++it )
- //for( GList* it=blocks; it; it=it->next )
+ FOR_EACH( it, blocks )
{
Block* blk = (*it);
@@ -250,10 +205,7 @@ void BlockGroup::BuildDisplayList( Model* mod )
stg_color_unpack( mod->color, &r, &g, &b, &a );
mod->PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a ));
- for( vector<Block*>::iterator it( blocks.begin() );
- it != blocks.end();
- ++it )
- //for( GList* it=blocks; it; it=it->next )
+ FOR_EACH( it, blocks )
{
Block* blk = *it;
@@ -365,9 +317,6 @@ void BlockGroup::Rasterize( uint8_t* data,
stg_meters_t cellwidth,
stg_meters_t cellheight )
{
- for( vector<Block*>::iterator it( blocks.begin() );
- it != blocks.end();
- ++it )
- //for( GList* it = blocks; it; it=it->next )
+ FOR_EACH( it, blocks )
(*it)->Rasterize( data, width, height, cellwidth, cellheight );
}
View
17 libstage/canvas.cc
@@ -204,9 +204,9 @@ Model* Canvas::getModel( int x, int y )
// render all top-level, draggable models in a color that is their
// id
- for( GList* it= world->World::children; it; it=it->next )
+ FOR_EACH( it, world->World::children )
{
- Model* mod = (Model*)it->data;
+ Model* mod = (*it);
if( mod->gui.mask & (STG_MOVE_TRANS | STG_MOVE_ROT ))
{
@@ -694,8 +694,9 @@ void Canvas::resetCamera()
float max_x = 0, max_y = 0, min_x = 0, min_y = 0;
//TODO take orrientation ( `a' ) and geom.pose offset into consideration
- for( GList* it=world->World::children; it; it=it->next ) {
- Model* ptr = (Model*) it->data;
+ FOR_EACH( it, world->World::children )
+ {
+ Model* ptr = (*it);
Pose pose = ptr->GetPose();
Geom geom = ptr->GetGeom();
@@ -1012,12 +1013,12 @@ void Canvas::renderFrame()
// draw the model-specific visualizations
if( showData ) {
if ( ! visualizeAll ) {
- for( GList* it = world->World::children; it; it=it->next )
- ((Model*)it->data)->DataVisualizeTree( current_camera );
+ FOR_EACH( it, world->World::children )
+ (*it)->DataVisualizeTree( current_camera );
}
else if ( selected_models ) {
- for( GList* it = selected_models; it; it=it->next )
- ((Model*)it->data)->DataVisualizeTree( current_camera );
+ FOR_EACH( it, world->World::children )
+ (*it)->DataVisualizeTree( current_camera );
}
else if ( last_selection ) {
last_selection->DataVisualizeTree( current_camera );
View
88 libstage/model.cc
@@ -275,17 +275,15 @@ Model::~Model( void )
// children are removed in ancestor class
- // remove from parent, if there is one
- if( parent )
- parent->children = g_list_remove( parent->children, this );
- else
- world->children = g_list_remove( world->children, this );
-
+ // remove myself from my parent's child list, or the world's child
+ // list if I have no parent
+ std::vector<Model*>& vec = parent ? parent->children : world->children;
+ vec.erase( remove( vec.begin(), vec.end(), this ));
+
if( callbacks ) g_hash_table_destroy( callbacks );
g_datalist_clear( &props );
- //g_hash_table_remove( Model::modelsbyid, (void*)id );
modelsbyid.erase(id);
world->RemoveModel( this );
@@ -324,7 +322,9 @@ void Model::Init()
void Model::InitRecursive()
{
// init children first
- LISTMETHOD( children, Model*, InitRecursive );
+ FOR_EACH( it, children )
+ (*it)->Init();
+
Init();
}
@@ -531,12 +531,9 @@ bool Model::IsDescendent( const Model* testmod ) const
if( this == testmod )
return true;
- for( GList* it=this->children; it; it=it->next )
- {
- Model* child = (Model*)it->data;
- if( child->IsDescendent( testmod ) )
- return true;
- }
+ FOR_EACH( it, children )
+ if( (*it)->IsDescendent( testmod ) )
+ return true;
// neither mod nor a child of this matches testmod
return false;
@@ -569,8 +566,8 @@ void Model::MapWithChildren()
Map();
// recursive call for all the model's children
- for( GList* it=children; it; it=it->next )
- ((Model*)it->data)->MapWithChildren();
+ FOR_EACH( it, children )
+ (*it)->MapWithChildren();
}
void Model::MapFromRoot()
@@ -583,8 +580,8 @@ void Model::UnMapWithChildren()
UnMap();
// recursive call for all the model's children
- for( GList* it=children; it; it=it->next )
- ((Model*)it->data)->UnMapWithChildren();
+ FOR_EACH( it, children )
+ (*it)->UnMapWithChildren();
}
void Model::UnMapFromRoot()
@@ -647,8 +644,8 @@ void Model::Print( char* prefix ) const
world->Token(),
token );
- for( GList* it=children; it; it=it->next )
- ((Model*)it->data)->Print( prefix );
+ FOR_EACH( it, children )
+ (*it)->Print( prefix );
}
const char* Model::PrintWithPose() const
@@ -734,12 +731,11 @@ void Model::CallUpdateCallbacks( void )
stg_meters_t Model::ModelHeight() const
{
stg_meters_t m_child = 0; //max size of any child
- for( GList* it=this->children; it; it=it->next )
+ FOR_EACH( it, children )
{
- Model* child = (Model*)it->data;
- stg_meters_t tmp_h = child->ModelHeight();
+ stg_meters_t tmp_h = (*it)->ModelHeight();
if( tmp_h > m_child )
- m_child = tmp_h;
+ m_child = tmp_h;
}
//height of model + max( child height )
@@ -786,10 +782,10 @@ Model* Model::TestCollisionTree()
{
Model* hitmod = TestCollision();
- if( hitmod == NULL )
- for( GList* it = children; it; it=it->next )
+ if( hitmod == NULL )
+ FOR_EACH( it, children )
{
- hitmod = ((Model*)it->data)->TestCollisionTree();
+ hitmod = (*it)->TestCollisionTree();
if( hitmod )
break;
}
@@ -844,8 +840,8 @@ void Model::UpdateCharge()
void Model::CommitTestedPose()
{
- for( GList* it = children; it; it=it->next )
- ((Model*)it->data)->CommitTestedPose();
+ FOR_EACH( it, children )
+ (*it)->CommitTestedPose();
blockgroup.SwitchToTestedCells();
}
@@ -917,8 +913,8 @@ int Model::TreeToPtrArray( GPtrArray* array ) const
int added = 1;
- for(GList* it=children; it; it=it->next )
- added += ((Model*)it->data)->TreeToPtrArray( array );
+ FOR_EACH( it, children )
+ added += (*it)->TreeToPtrArray( array );
return added;
}
@@ -929,15 +925,13 @@ Model* Model::GetUnsubscribedModelOfType( stg_model_type_t type ) const
return const_cast<Model*> (this); // discard const
// this model is no use. try children recursively
- for( GList* it = children; it; it=it->next )
- {
- Model* child = (Model*)it->data;
-
- Model* found = child->GetUnsubscribedModelOfType( type );
- if( found )
- return found;
+ FOR_EACH( it, children )
+ {
+ Model* found = (*it)->GetUnsubscribedModelOfType( type );
+ if( found )
+ return found;
}
-
+
// nothing matching below this model
return NULL;
}
@@ -963,13 +957,11 @@ Model* Model::GetUnusedModelOfType( stg_model_type_t type )
}
// this model is no use. try children recursively
- for( GList* it = children; it; it=it->next )
+ FOR_EACH( it, children )
{
- Model* child = (Model*)it->data;
-
- Model* found = child->GetUnusedModelOfType( type );
+ Model* found = (*it)->GetUnusedModelOfType( type );
if( found )
- return found;
+ return found;
}
// nothing matching below this model
@@ -979,10 +971,10 @@ Model* Model::GetUnusedModelOfType( stg_model_type_t type )
stg_kg_t Model::GetTotalMass()
{
stg_kg_t sum = mass;
- for (GList* child = this->children; child; child = child->next) {
- Model* childModel = (Model*) child->data;
- sum += childModel->GetTotalMass();
- }
+
+ FOR_EACH( it, children )
+ sum += (*it)->GetTotalMass();
+
return sum;
}
View
18 libstage/model_callbacks.cc
@@ -15,8 +15,8 @@ void Model::AddCallback( void* address,
GList* cb_list = (GList*)g_hash_table_lookup( callbacks, address );
- //printf( "installing callback in model %s with key %d\n",
- // mod->token, *key );
+ //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 ) );
@@ -78,12 +78,12 @@ void Model::CallCallbacks( void* address )
//printf( "CallCallbacks for model %s %p key %p\n", this->Token(), this, address );
//printf( "Model %s has %d callbacks. Checking key %d\n",
- // this->token, g_hash_table_size( this->callbacks ), key );
-
+ // this->token, g_hash_table_size( this->callbacks ), key );
+
GList* cbs = (GList*)g_hash_table_lookup( callbacks, address );
-
- //printf( "key %d has %d callbacks registered\n",
- // key, g_list_length( cbs ) );
+
+ //printf( "key %p has %d callbacks registered\n",
+ // address, g_list_length( cbs ) );
// maintain a list of callbacks that should be cancelled
GList* doomed = NULL;
@@ -98,12 +98,12 @@ void Model::CallCallbacks( void* address )
if( (cba->callback)( this, cba->arg ) )
{
- //printf( "callback returned TRUE - schedule removal from list\n" );
+ //printf( "callback returned TRUE - schedule removal from list\n" );
doomed = g_list_prepend( doomed, (void*)cba->callback );
}
else
{
- //printf( "callback returned FALSE - keep in list\n" );
+ //printf( "callback returned FALSE - keep in list\n" );
}
cbs = cbs->next;
View
24 libstage/model_draw.cc
@@ -152,15 +152,19 @@ void Model::DrawTrailArrows()
void Model::DrawOriginTree()
{
DrawPose( GetGlobalPose() );
- for( GList* it=children; it; it=it->next )
- ((Model*)it->data)->DrawOriginTree();
+
+ FOR_EACH( it, children )
+ (*it)->DrawOriginTree();
}
void Model::DrawBlocksTree( )
{
PushLocalCoords();
- LISTMETHOD( children, Model*, DrawBlocksTree );
+
+ FOR_EACH( it, children )
+ (*it)->DrawBlocksTree();
+
DrawBlocks();
PopCoords();
}
@@ -185,7 +189,10 @@ void Model::DrawBlocks( )
void Model::DrawBoundingBoxTree()
{
PushLocalCoords();
- LISTMETHOD( children, Model*, DrawBoundingBoxTree );
+
+ FOR_EACH( it, children )
+ (*it)->DrawBoundingBoxTree();
+
DrawBoundingBox();
PopCoords();
}
@@ -284,7 +291,8 @@ void Model::DrawStatusTree( Camera* cam )
{
PushLocalCoords();
DrawStatus( cam );
- LISTMETHODARG( children, Model*, DrawStatusTree, cam );
+ FOR_EACH( it, children )
+ (*it)->DrawStatusTree( cam );
PopCoords();
}
@@ -526,7 +534,8 @@ void Model::DrawPicker( void )
blockgroup.DrawSolid( geom );
// recursively draw the tree below this model
- LISTMETHOD( this->children, Model*, DrawPicker );
+ FOR_EACH( it, children )
+ (*it)->DrawPicker();
PopCoords();
}
@@ -548,7 +557,8 @@ void Model::DataVisualizeTree( Camera* cam )
}
// and draw the children
- LISTMETHODARG( children, Model*, DataVisualizeTree, cam );
+ FOR_EACH( it, children )
+ (*it)->DataVisualizeTree( cam );
PopCoords();
}
View
10 libstage/model_getset.cc
@@ -137,11 +137,13 @@ void Model::SetGlobalPose( const Pose& gpose )
int Model::SetParent( Model* newparent)
{
// remove the model from its old parent (if it has one)
- if( this->parent )
- this->parent->children = g_list_remove( this->parent->children, this );
-
+ if( parent )
+ //this->parent->children = g_list_remove( this->parent->children, this );
+ parent->children.erase( remove( parent->children.begin(), parent->children.end(), this ) );
+
if( newparent )
- newparent->children = g_list_append( newparent->children, this );
+ //newparent->children = g_list_append( newparent->children, this );
+ newparent->children.push_back( this );
// link from the model to its new parent
this->parent = newparent;
View
16 libstage/stage.hh
@@ -732,7 +732,12 @@ namespace Stg
Model* finder,
const void* arg );
+ // STL container iterator macros
+#define VAR(V,init) __typeof(init) V=(init)
+#define FOR_EACH(I,C) for(VAR(I,(C).begin());I!=(C).end();I++)
+
// list iterator macros
+ // todo: retire these along with glib
#define LISTFUNCTION( LIST, TYPE, FUNC ) for( GList* it=LIST; it; it=it->next ) FUNC((TYPE)it->data);
#define LISTMETHOD( LIST, TYPE, METHOD ) for( GList* it=LIST; it; it=it->next ) ((TYPE)it->data)->METHOD();
#define LISTFUNCTIONARG( LIST, TYPE, FUNC, ARG ) for( GList* it=LIST; it; it=it->next ) FUNC((TYPE)it->data, ARG);
@@ -802,7 +807,7 @@ namespace Stg
friend class Canvas; // allow Canvas access to our private members
protected:
- GList* children;
+ std::vector<Model*> children;
bool debug;
GList* puck_list;
char* token;
@@ -813,7 +818,7 @@ namespace Stg
public:
/** get the children of the this element */
- GList* GetChildren(){ return children;}
+ std::vector<Model*>& GetChildren(){ return children;}
/** recursively call func( model, arg ) for each descendant */
void ForEachDescendant( stg_model_callback_t func, void* arg );
@@ -933,7 +938,6 @@ namespace Stg
GMutex* thread_mutex; ///< protect the worker thread management stuff
GThreadPool *threadpool; ///<worker threads for updating some sensor models in parallel
int total_subs; ///< the total number of subscriptions to all models
- //unsigned int update_jobs_pending;
GList* velocity_list; ///< Models with non-zero velocity and should have their poses updated
unsigned int worker_threads; ///< the number of worker threads to use
GCond* worker_threads_done; ///< signalled when there are no more updates for the worker threads to do
@@ -955,8 +959,8 @@ namespace Stg
std::map<stg_point_int_t,SuperRegion*> superregions;
SuperRegion* sr_cached; ///< The last superregion looked up by this world
- GList* reentrant_update_list; ///< It is safe to call these model's Update() in parallel
- GList* nonreentrant_update_list; ///< It is NOT safe to call these model's Update() in parallel
+ std::vector<Model*> reentrant_update_list; ///< It is safe to call these model's Update() in parallel
+ std::vector<Model*> nonreentrant_update_list; ///< It is NOT 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
@@ -965,7 +969,6 @@ namespace Stg
public:
- // std::vector<stg_point_int_t> rt_regions;
std::vector<stg_point_int_t> rt_cells;
std::vector<stg_point_int_t> rt_candidate_cells;
@@ -1908,7 +1911,6 @@ namespace Stg
void RegisterOption( Option* opt );
GList* AppendTouchingModels( GList* list );
- //void AddTouchingModelsToList( GList* list );
/** Check to see if the current pose will yield a collision with
obstacles. Returns a pointer to the first entity we are in
View
617 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 ),
@@ -82,7 +82,7 @@ World::World( const char* token,
velocity_list( NULL ),
worker_threads( 0 ),
worker_threads_done( g_cond_new() ),
- models_with_fiducials(),
+ models_with_fiducials(),
// protected
cb_list(NULL),
@@ -94,11 +94,9 @@ World::World( const char* token,
ray_list( NULL ),
sim_time( 0 ),
superregions(),
- // g_hash_table_new( (GHashFunc)PointIntHash,
- // (GEqualFunc)PointIntEqual ) ),
sr_cached(NULL),
- reentrant_update_list( NULL ),
- nonreentrant_update_list( NULL ),
+ reentrant_update_list(),
+ nonreentrant_update_list(),
updates( 0 ),
wf( NULL )
{
@@ -145,7 +143,7 @@ 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;
}
@@ -156,7 +154,7 @@ void World::update_thread_entry( Model* mod, World* world )
g_mutex_lock( world->thread_mutex );
- // world->update_jobs_pending--;
+ // world->update_jobs_pending--;
// if( world->update_jobs_pending == 0 )
if( g_thread_pool_unprocessed( world->threadpool ) < 1 )
@@ -181,7 +179,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" );
@@ -209,11 +207,11 @@ Model* World::CreateModel( Model* parent, const char* typestr )
//printf( "creating model of type %s\n", typestr );
for( int i=0; i<MODEL_TYPE_COUNT; i++ )
- if( strcmp( typestr, typetable[i].token ) == 0 )
- {
- creator = typetable[i].creator;
- break;
- }
+ if( strcmp( typestr, typetable[i].token ) == 0 )
+ {
+ creator = typetable[i].creator;
+ break;
+ }
// if we found a creator function, call it
if( creator )
@@ -224,7 +222,7 @@ Model* World::CreateModel( Model* parent, const char* typestr )
else
{
PRINT_ERR1( "Unknown model type %s in world file.",
- typestr );
+ typestr );
exit( 1 );
}
@@ -239,10 +237,10 @@ void World::LoadModel( Worldfile* wf, int entity, GHashTable* entitytable )
int parent_entity = wf->GetEntityParent( 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);
@@ -285,11 +283,11 @@ void World::Load( const char* worldfile_path )
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" ) )
@@ -303,22 +301,22 @@ void World::Load( const char* worldfile_path )
int count = wf->ReadInt( entity, "threadpool", worker_threads );
if( count && (count != (int)worker_threads) )
- {
- worker_threads = count;
+ {
+ 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 );
- }
+ 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 );
+ }
}
// Iterate through entitys and create objects of the appropriate type
@@ -328,15 +326,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 );
}
@@ -346,40 +344,31 @@ void World::Load( const char* worldfile_path )
// now we're done with the worldfile entry lookup
g_hash_table_destroy( entitytable );
- LISTMETHOD( children, Model*, InitRecursive );
+ FOR_EACH( it, children )
+ (*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' );
}
-// delete a model from the hash table
-static void destroy_model( gpointer dummy1, Model* mod, gpointer dummy2 )
-{
- free(mod);
-}
-
-
void World::UnLoad()
{
if( wf ) delete wf;
- g_list_foreach( children, (GFunc)destroy_model, NULL );
- g_list_free( children );
- children = NULL;
-
+ FOR_EACH( it, children )
+ delete (*it);
+ children.clear();
+
g_hash_table_remove_all( models_by_name );
- g_list_free( reentrant_update_list );
- reentrant_update_list = NULL;
+ reentrant_update_list.clear();
+ nonreentrant_update_list.clear();
- g_list_free( nonreentrant_update_list );
- nonreentrant_update_list = NULL;
-
g_list_free( ray_list );
ray_list = NULL;
@@ -431,10 +420,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;
@@ -443,7 +432,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<stg_world_callback_t,void*> p(cb, user);
@@ -451,21 +440,21 @@ void World::AddUpdateCallback( stg_world_callback_t cb,
}
int World::RemoveUpdateCallback( stg_world_callback_t cb,
- void* user )
+ void* user )
{
std::pair<stg_world_callback_t,void*> p( cb, user );
std::list<std::pair<stg_world_callback_t,void*> >::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.
@@ -477,17 +466,17 @@ void World::CallUpdateCallbacks()
// for each callback in the list
for( std::list<std::pair<stg_world_callback_t,void*> >::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()
@@ -510,45 +499,51 @@ bool World::Update()
LISTMETHOD( charge_list, Model*, UpdateCharge );
// then update all models on the update lists
- LISTMETHOD( nonreentrant_update_list, Model*, UpdateIfDue );
+ FOR_EACH( it, nonreentrant_update_list )
+ (*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
{
- LISTMETHOD( reentrant_update_list, Model*, UpdateIfDue );
+ FOR_EACH( it, reentrant_update_list )
+ (*it)->UpdateIfDue();
}
else // use worker threads
{
// push the update for every model that needs it into the thread pool
- for( GList* it = reentrant_update_list; it; it=it->next )
- {
- Model* mod = (Model*)it->data;
+ 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 );
- }
- }
+ 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 );
+ }
+ }
// 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 );
+ g_cond_wait( worker_threads_done, thread_mutex );
g_mutex_unlock( thread_mutex );
- // now call all the callbacks - ignores dueness, but not a big deal
- LISTMETHOD( reentrant_update_list, Model*, CallUpdateCallbacks );
+ // 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();
@@ -599,14 +594,14 @@ 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;
@@ -614,21 +609,21 @@ void World::Raytrace( const Pose &gpose, // global pose
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 );
+ Ray r( mod, gpose, range, func, arg, ztest );
+ return Raytrace( r );
}
stg_raytrace_result_t World::Raytrace( const Ray& r )
@@ -642,12 +637,12 @@ stg_raytrace_result_t World::Raytrace( const Ray& r )
// our global position in (floating point) cell coordinates
//stg_point_t glob( r.origin.x * ppm, r.origin.y * ppm );
double globx( r.origin.x * ppm );
- double globy( r.origin.y * ppm );
+ double globy( r.origin.y * ppm );
// record our starting position
//const stg_point_t start( glob.x, glob.y );
const double startx( globx );
- const double starty( globy );
+ const double starty( globy );
// eliminate a potential divide by zero
const double angle( r.origin.a == 0.0 ? 1e-12 : r.origin.a );
@@ -656,7 +651,7 @@ stg_raytrace_result_t World::Raytrace( const Ray& r )
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)
+ // very weird and rare bug is produced)
const double dx( ppm * r.range * cosa);
const double dy( ppm * r.range * sina);
@@ -692,159 +687,159 @@ 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( &reg->cells[ cx + cy * REGIONWIDTH ] );
- assert(c); // should be good: we know the region contains objects
+ 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( std::vector<Block*>::iterator it( c->blocks.begin() );
- it != c->blocks.end();
- ++it )
- {
- Block* block( *it );
+ // 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( std::vector<Block*>::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;
+ // 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
+ 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 )
- {
- calculatecrossings = false;
+ //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 ));
- }
- // hit nothing
- sample.mod = NULL;
- return sample;
+ 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;
}
static int _save_cb( Model* mod, void* dummy )
@@ -936,8 +931,8 @@ 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) )) ;
}
@@ -948,7 +943,7 @@ void World::ForEachCellInLine( const stg_point_int_t& start,
// 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 dy( end.y - start.y );
const int32_t sx(sgn(dx));
const int32_t sy(sgn(dy));
const int32_t ax(abs(dx));
@@ -959,55 +954,55 @@ void World::ForEachCellInLine( const stg_point_int_t& start,
int32_t n(ax+ay);
int32_t globx(start.x);
- int32_t globy(start.y);
+ int32_t globy(start.y);
// fix a little issue where the edges are not drawn long enough
// when drawing to the right or up
- // if( (dx > 0) || ( dy > 0 ) )
+ // if( (dx > 0) || ( dy > 0 ) )
// n++;
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<REGIONWIDTH) &&
- (cy>=0) && (cy<REGIONWIDTH) &&
- n > 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<REGIONWIDTH) &&
+ (cy>=0) && (cy<REGIONWIDTH) &&
+ n > 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 )
@@ -1039,35 +1034,25 @@ void World:: RegisterOption( Option* opt )
void World::StartUpdatingModel( Model* mod )
{
- //if( ! g_list_find( update_list, mod ) )
- // update_list = g_list_append( update_list, mod );
-
- if( mod->thread_safe )
- {
- if( ! g_list_find( reentrant_update_list, mod ) )
- reentrant_update_list = g_list_append( reentrant_update_list, mod );
- }
- else
- {
- if( ! g_list_find( nonreentrant_update_list, mod ) )
- nonreentrant_update_list = g_list_append( nonreentrant_update_list, mod );
- }
+ // choose the right update list
+ std::vector<Model*>& vec = mod->thread_safe ? reentrant_update_list : 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 );
}
void World::StopUpdatingModel( Model* mod )
{
- // update_list = g_list_remove( update_list, mod );
-
- if( mod->thread_safe )
- reentrant_update_list = g_list_remove( reentrant_update_list, mod );
- else
- nonreentrant_update_list = g_list_remove( nonreentrant_update_list, mod );
+ // choose the right update list
+ std::vector<Model*>& vec = mod->thread_safe ? reentrant_update_list : nonreentrant_update_list;
+ // and erase the model from it
+ vec.erase( remove( vec.begin(), vec.end(), 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 )
View
15 libstage/worldgui.cc
@@ -857,18 +857,18 @@ void WorldGui::UpdateOptions()
std::set<Option*, Option::optComp> options;
std::vector<Option*> modOpts;
- for( GList* it=reentrant_update_list; it; it=it->next )
+ FOR_EACH( it, reentrant_update_list )
{
- modOpts = ((Model*)it->data)->getOptions();
+ modOpts = (*it)->getOptions();
options.insert( modOpts.begin(), modOpts.end() );
}
-
- for( GList* it=nonreentrant_update_list; it; it=it->next )
+
+ FOR_EACH( it, nonreentrant_update_list )
{
- modOpts = ((Model*)it->data)->getOptions();
+ modOpts = (*it)->getOptions();
options.insert( modOpts.begin(), modOpts.end() );
}
-
+
drawOptions.assign( options.begin(), options.end() );
if ( oDlg )
@@ -877,7 +877,8 @@ void WorldGui::UpdateOptions()
void WorldGui::DrawBoundingBoxTree()
{
- LISTMETHOD( World::children, Model*, DrawBoundingBoxTree );
+ FOR_EACH( it, World::children )
+ (*it)->DrawBoundingBoxTree();
}
void WorldGui::PushColor( stg_color_t col )
Please sign in to comment.
Something went wrong with that request. Please try again.