diff --git a/docsrc/Markdown.pl b/docsrc/Markdown.pl old mode 100644 new mode 100755 diff --git a/docsrc/sourcedocs.sh b/docsrc/sourcedocs.sh old mode 100644 new mode 100755 diff --git a/examples/ctrl/CMakeLists.txt b/examples/ctrl/CMakeLists.txt index 3fb74ed96..d52c9b878 100644 --- a/examples/ctrl/CMakeLists.txt +++ b/examples/ctrl/CMakeLists.txt @@ -24,10 +24,11 @@ foreach( PLUGIN ${PLUGINS} ) endforeach( PLUGIN ) ADD_LIBRARY( fasr2 MODULE fasr2.cc astar/findpath.cpp ) +ADD_LIBRARY( fasr3 MODULE fasr3.cc astar/findpath.cpp ) #ADD_LIBRARY( energy MODULE energy.cc astar/findpath.cpp ) # add extras to the list of plugins -SET( PLUGINS ${PLUGINS} fasr2 ) +SET( PLUGINS ${PLUGINS} fasr2 fasr3 ) #SET( PLUGINS ${PLUGINS} energy) set_source_files_properties( ${PLUGINS} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" ) diff --git a/libstage/block.cc b/libstage/block.cc index 1de77cd0f..99a83fc91 100644 --- a/libstage/block.cc +++ b/libstage/block.cc @@ -11,19 +11,19 @@ static void canonicalize_winding(vector& pts); blocks. The point data is copied, so pts can safely be freed after calling this.*/ Block::Block( Model* mod, - const std::vector& pts, - meters_t zmin, - meters_t zmax, - Color color, - bool inherit_color, - bool wheel ) : + const std::vector& pts, + meters_t zmin, + meters_t zmax, + Color color, + bool inherit_color, + bool wheel ) : mod( mod ), mpts(), pts(pts), local_z( zmin, zmax ), color( color ), inherit_color( inherit_color ), - wheel(wheel), + wheel(wheel), rendered_cells(), gpts() { @@ -38,12 +38,12 @@ Block::Block( Model* mod, : mod( mod ), mpts(), pts(), - local_z(), + local_z(), color(), inherit_color(true), - wheel(), + wheel(), rendered_cells(), - gpts() + gpts() { assert(mod); assert(wf); @@ -55,7 +55,11 @@ Block::Block( Model* mod, Block::~Block() { - if( mapped ) UnMap(); + if( mapped ) + { + UnMap(0); + UnMap(1); + } } void Block::Translate( double x, double y ) @@ -136,10 +140,12 @@ const Color& Block::GetColor() void Block::AppendTouchingModels( ModelPtrSet& touchers ) { + unsigned int layer = mod->world->updates % 2; + // for every cell we are rendered into - FOR_EACH( cell_it, rendered_cells ) - // for every block rendered into that cell - FOR_EACH( block_it, (*cell_it)->GetBlocks() ) + FOR_EACH( cell_it, rendered_cells[layer] ) + // for every block rendered into that cell + FOR_EACH( block_it, (*cell_it)->GetBlocks(layer) ) { if( !mod->IsRelated( (*block_it)->mod )) touchers.insert( (*block_it)->mod ); @@ -156,13 +162,15 @@ Model* Block::TestCollision() if( mod->vis.obstacle_return ) { if ( global_z.min < 0 ) - return this->mod->world->GetGround(); + return mod->world->GetGround(); + unsigned int layer = mod->world->updates % 2; + // for every cell we may be rendered into - FOR_EACH( cell_it, rendered_cells ) + FOR_EACH( cell_it, rendered_cells[layer] ) { // for every block rendered into that cell - FOR_EACH( block_it, (*cell_it)->GetBlocks() ) + FOR_EACH( block_it, (*cell_it)->GetBlocks(layer) ) { Block* testblock = *block_it; Model* testmod = testblock->mod; @@ -188,12 +196,9 @@ Model* Block::TestCollision() return NULL; // no hit } -void Block::Map() +void Block::Map( unsigned int layer ) { - // clear out of the old cells - RemoveFromCellArray( rendered_cells ); - - // now calculate the local coords of the block vertices + // calculate the local coords of the block vertices const size_t pt_count(pts.size()); if( mpts.size() == 0 ) @@ -210,7 +215,7 @@ void Block::Map() mod->LocalToPixels( mpts, gpts ); // and render this block's polygon into the world - mod->GetWorld()->MapPoly( gpts, this ); + mod->world->MapPoly( gpts, this, layer ); // update the block's absolute z bounds at this rendering Pose gpose( mod->GetGlobalPose() ); @@ -223,30 +228,20 @@ void Block::Map() mapped = true; } -void Block::UnMap() -{ - RemoveFromCellArray( rendered_cells ); - rendered_cells.clear(); - mapped = false; -} - #include #include -inline void Block::RemoveFromCellArray( CellPtrVec& cells ) -{ - // FOR_EACH( it, *cells ) - // (*it)->RemoveBlock(this); - - // this is equivalent to the above commented code - experimenting - // with optimizations - std::for_each( cells.begin(), - cells.end(), - std::bind2nd( std::mem_fun(&Cell::RemoveBlock), this)); -} - -void Block::SwitchToTestedCells() +void Block::UnMap( unsigned int layer ) { + // std::for_each( rendered_cells.begin(), + // rendered_cells.end(), + // std::bind2nd( std::mem_fun(&Cell::RemoveBlock), this)); + + FOR_EACH( it, rendered_cells[layer] ) + (*it)->RemoveBlock(this, layer ); + + rendered_cells[layer].clear(); + mapped = false; } inline point_t Block::BlockPointToModelMeters( const point_t& bpt ) @@ -377,10 +372,10 @@ void Block::DrawFootPrint() glEnd(); } -void Block::DrawSolid() +void Block::DrawSolid( bool topview ) { -// if( wheel ) -// { +// if( wheel )x + // { // glPushMatrix(); // glRotatef( 90,0,1,0 ); @@ -397,10 +392,12 @@ void Block::DrawSolid() // glPopMatrix(); // } // else - { - DrawSides(); - DrawTop(); - } + { + if( ! topview ) + DrawSides(); + + DrawTop(); + } } void Block::Load( Worldfile* wf, int entity ) @@ -410,7 +407,7 @@ void Block::Load( Worldfile* wf, int entity ) char key[128]; for( size_t p=0; pReadTupleLength(entity, key, 0, 0), wf->ReadTupleLength(entity, key, 1, 0) )); @@ -432,74 +429,6 @@ void Block::Load( Worldfile* wf, int entity ) wheel = wf->ReadInt( entity, "wheel", wheel ); } - -// void Block::MapLine( const point_int_t& start, -// const point_int_t& end ) -// { -// // 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( mod->GetWorld() -// ->GetSuperRegionCreate( point_int_t(GETSREG(globx), -// GETSREG(globy))) -// ->GetRegion( GETREG(globx), -// GETREG(globy))); - -// //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=0) && (cy 0 ) -// { -// c->AddBlock(this); - -// // 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; -// } -// } -// } - ///////////////////////////////////////////////////////////////////////////////////////// // utility functions to ensure block winding is consistent and matches OpenGL's default @@ -518,11 +447,9 @@ void pi_ize(radians_t& angle) while (M_PI < angle) angle -= 2 * M_PI; } -typedef point_t V2; - static /// util; How much was v1 rotated to get to v2? -radians_t angle_change(V2 v1, V2 v2) +radians_t angle_change(point_t v1, point_t v2) { radians_t a1 = atan2(v1.y, v1.x); positivize(a1); @@ -545,7 +472,7 @@ vector find_vectors(vector const& pts) for (unsigned i = 0, n = pts.size(); i < n; ++i) { unsigned j = (i + 1) % n; - vs.push_back(V2(pts[j].x - pts[i].x, pts[j].y - pts[i].y)); + vs.push_back(point_t(pts[j].x - pts[i].x, pts[j].y - pts[i].y)); } assert(vs.size() == pts.size()); return vs; diff --git a/libstage/blockgroup.cc b/libstage/blockgroup.cc index ca0ec02d3..94f74c90a 100644 --- a/libstage/blockgroup.cc +++ b/libstage/blockgroup.cc @@ -37,13 +37,6 @@ void BlockGroup::Clear() blocks.clear(); } -void BlockGroup::SwitchToTestedCells() -{ - // confirm the tentative pose for all blocks - FOR_EACH( it, blocks ) - (*it)->SwitchToTestedCells(); -} - void BlockGroup::AppendTouchingModels( ModelPtrSet &v ) { FOR_EACH( it, blocks ) @@ -101,16 +94,19 @@ void BlockGroup::CalcSize() } -void BlockGroup::Map() +void BlockGroup::Map( unsigned int layer ) { FOR_EACH( it, blocks ) - (*it)->Map(); + (*it)->Map(layer); } -void BlockGroup::UnMap() +// defined in world.cc +//void SwitchSuperRegionLock( SuperRegion* current, SuperRegion* next ); + +void BlockGroup::UnMap( unsigned int layer ) { - FOR_EACH( it, blocks ) - (*it)->UnMap(); + FOR_EACH( it, blocks ) + (*it)->UnMap(layer); } void BlockGroup::DrawSolid( const Geom & geom ) @@ -126,7 +122,7 @@ void BlockGroup::DrawSolid( const Geom & geom ) glTranslatef( -offset.x, -offset.y, -offset.z ); FOR_EACH( it, blocks ) - (*it)->DrawSolid(); + (*it)->DrawSolid(false); glPopMatrix(); } @@ -160,16 +156,20 @@ void BlockGroup::BuildDisplayList( Model* mod ) displaylist = glGenLists(1); CalcSize(); } - + + glNewList( displaylist, GL_COMPILE ); - Geom geom = mod->GetGeom(); + // render each block as a polygon extruded into Z + + Geom geom = mod->GetGeom(); + Gl::pose_shift( geom.pose ); - + glScalef( geom.size.x / size.x, - geom.size.y / size.y, - geom.size.z / size.z ); + geom.size.y / size.y, + geom.size.z / size.z ); glTranslatef( -offset.x, -offset.y, -offset.z ); @@ -179,22 +179,26 @@ void BlockGroup::BuildDisplayList( Model* mod ) mod->PushColor( mod->color ); + //const bool topview = dynamic_cast(mod->world)->IsTopView(); + FOR_EACH( it, blocks ) - { - Block* blk = (*it); + { + Block* blk = (*it); - if( (!blk->inherit_color) && (blk->color != mod->color) ) - { - mod->PushColor( blk->color ); - blk->DrawSolid(); - mod->PopColor(); - } - else - blk->DrawSolid(); - } + if( (!blk->inherit_color) && (blk->color != mod->color) ) + { + mod->PushColor( blk->color ); + blk->DrawSolid(false); + mod->PopColor(); + } + else + blk->DrawSolid(false); + } mod->PopColor(); + // outline each poly in a darker version of the same color + glDisable(GL_POLYGON_OFFSET_FILL); glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); @@ -205,29 +209,25 @@ void BlockGroup::BuildDisplayList( Model* mod ) c.g /= 2.0; c.b /= 2.0; mod->PushColor( c ); - - //c.Print( "color" ); - - + FOR_EACH( it, blocks ) - { - Block* blk = *it; + { + Block* blk = *it; - if( (!blk->inherit_color) && (blk->color != mod->color) ) - { - Color c = blk->color; - c.r /= 2.0; - c.g /= 2.0; - c.b /= 2.0; - mod->PushColor( c ); - //c.Print( "bar" ); - blk->DrawSolid(); - mod->PopColor(); - } - else - blk->DrawSolid(); - } - + if( (!blk->inherit_color) && (blk->color != mod->color) ) + { + Color c = blk->color; + c.r /= 2.0; + c.g /= 2.0; + c.b /= 2.0; + mod->PushColor( c ); + blk->DrawSolid(false); + mod->PopColor(); + } + else + blk->DrawSolid(false); + } + glDepthMask(GL_TRUE); glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); diff --git a/libstage/canvas.cc b/libstage/canvas.cc index 4f4a2d7e3..6f39a7147 100644 --- a/libstage/canvas.cc +++ b/libstage/canvas.cc @@ -357,12 +357,13 @@ void Canvas::CanvasToWorld( int px, int py, GLfloat pz; glReadPixels( px, h()-py, 1, 1, GL_DEPTH_COMPONENT, GL_FLOAT, &pz ); - gluUnProject( px, w()-py, pz, modelview, projection, viewport, wx,wy,wz ); - + gluUnProject( px, w()-py, pz, modelview, projection, viewport, wx,wy,wz ); } int Canvas::handle(int event) { + //printf( "cam %.2f %.2f\n", camera.yaw(), camera.pitch() ); + switch(event) { case FL_MOUSEWHEEL: @@ -938,20 +939,23 @@ void Canvas::renderFrame() // ((Model*)it->data)->DrawOriginTree(); // draw the model-specific visualizations - if( showData ) { - if ( ! visualizeAll ) { - FOR_EACH( it, world->World::children ) - (*it)->DataVisualizeTree( current_camera ); - } - else if ( selected_models.size() > 0 ) { - FOR_EACH( it, world->World::children ) - (*it)->DataVisualizeTree( current_camera ); - } - else if ( last_selection ) { - last_selection->DataVisualizeTree( current_camera ); - } - } - + if( world->sim_time > 0 ) + { + if( showData ) { + if ( ! visualizeAll ) { + FOR_EACH( it, world->World::children ) + (*it)->DataVisualizeTree( current_camera ); + } + else if ( selected_models.size() > 0 ) { + FOR_EACH( it, world->World::children ) + (*it)->DataVisualizeTree( current_camera ); + } + else if ( last_selection ) { + last_selection->DataVisualizeTree( current_camera ); + } + } + } + if( showGrid ) FOR_EACH( it, models_sorted ) (*it)->DrawGrid(); diff --git a/libstage/canvas.hh b/libstage/canvas.hh index 461d7ffdb..ce6536b3d 100644 --- a/libstage/canvas.hh +++ b/libstage/canvas.hh @@ -155,6 +155,9 @@ namespace Stg void Load( Worldfile* wf, int section ); void Save( Worldfile* wf, int section ); + + bool IsTopView(){ return( (fabs( camera.yaw() ) < 0.1) && + (fabs( camera.pitch() ) < 0.1) ); } }; } // namespace Stg diff --git a/libstage/model.cc b/libstage/model.cc index 102f608fb..62a4df65c 100644 --- a/libstage/model.cc +++ b/libstage/model.cc @@ -342,9 +342,8 @@ Model::Model( World* world, Model* parent, const std::string& type ) : Ancestor(), - mapped(false), - drawOptions(), - //hitmod(NULL), + mapped(false), + drawOptions(), alwayson(false), blockgroup(), blocks_dl(0), @@ -426,17 +425,20 @@ Model::~Model( void ) { // children are removed in ancestor class - // remove myself from my parent's child list, or the world's child - // list if I have no parent - - if( world ) // if I'm not a worldless dummy model - { - UnMap(); // remove from the raytrace bitmap - - EraseAll( this, parent ? parent->children : world->children ); - modelsbyid.erase(id); - world->RemoveModel( this ); - } + if( world ) // if I'm not a worldless dummy model + { + UnMap(0); // remove from the movable model array + UnMap(1); // remove from the moveable model array + + // remove myself from my parent's child list, or the world's child + // list if I have no parent + EraseAll( this, parent ? parent->children : world->children ); + + // erase from the static map of all models + modelsbyid.erase(id); + + world->RemoveModel( this ); + } } @@ -486,9 +488,10 @@ Model::Flag* Model::PopFlag() return flag; } -void Model::ClearBlocks( void ) +void Model::ClearBlocks() { - UnMap(); + blockgroup.UnMap(0); + blockgroup.UnMap(1); blockgroup.Clear(); //no need to Map() - we have no blocks NeedRedraw(); @@ -512,9 +515,10 @@ Block* Model::AddBlockRect( meters_t x, meters_t dy, meters_t dz ) { - UnMap(); - - std::vector pts(4); + UnMap(0); + UnMap(1); + + std::vector pts(4); pts[0].x = x; pts[0].y = y; pts[1].x = x + dx; @@ -525,16 +529,17 @@ Block* Model::AddBlockRect( meters_t x, pts[3].y = y + dy; Block* newblock = new Block( this, - pts, - 0, dz, - color, - true, - false ); - + pts, + 0, dz, + color, + true, + false ); + blockgroup.AppendBlock( newblock ); - - Map(); - + + Map(0); + Map(1); + return newblock; } @@ -669,46 +674,35 @@ void Model::LocalToPixels( const std::vector& local, } } -void Model::MapWithChildren() +void Model::MapWithChildren( unsigned int layer ) { - UnMap(); - Map(); + UnMap(layer); + Map(layer); // recursive call for all the model's children FOR_EACH( it, children ) - (*it)->MapWithChildren(); + (*it)->MapWithChildren(layer); } -void Model::MapFromRoot() +void Model::MapFromRoot( unsigned int layer ) { - Root()->MapWithChildren(); + Root()->MapWithChildren(layer); } -void Model::UnMapWithChildren() +void Model::UnMapWithChildren(unsigned int layer) { - UnMap(); + UnMap(layer); // recursive call for all the model's children FOR_EACH( it, children ) - (*it)->UnMapWithChildren(); + (*it)->UnMapWithChildren(layer); } -void Model::UnMapFromRoot() +void Model::UnMapFromRoot(unsigned int layer) { - Root()->UnMapWithChildren(); + Root()->UnMapWithChildren(layer); } -void Model::Map() -{ - if( ! mapped ) - { - // render all blocks in the group at my global pose and size - blockgroup.Map(); - mapped = true; - } -} - - void Model::Subscribe( void ) { subs++; @@ -775,19 +769,21 @@ void Model::Startup( void ) { //printf( "Startup model %s\n", this->token ); //printf( "model %s using queue %d\n", token, event_queue_num ); - + // if we're thread safe, we can use an event queue >0 - if( thread_safe ) - event_queue_num = world->GetEventQueue( this ); - + event_queue_num = world->GetEventQueue( this ); + // put my first update request in the world's queue - world->Enqueue( event_queue_num, interval, this ); + if( thread_safe ) + world->Enqueue( event_queue_num, interval, this, UpdateWrapper, NULL ); + else + world->Enqueue( 0, interval, this, UpdateWrapper, NULL ); if( velocity_enable ) - world->active_velocity.insert( this ); + world->active_velocity.insert(this); if( FindPowerPack() ) - world->active_energy.insert( this ); + world->active_energy.insert( this ); CallCallbacks( CB_STARTUP ); } @@ -795,8 +791,8 @@ void Model::Startup( void ) void Model::Shutdown( void ) { //printf( "Shutdown model %s\n", this->token ); - CallCallbacks( CB_SHUTDOWN ); - + CallCallbacks( CB_SHUTDOWN ); + world->active_energy.erase( this ); world->active_velocity.erase( this ); @@ -807,12 +803,29 @@ void Model::Shutdown( void ) void Model::Update( void ) { - CallCallbacks( CB_UPDATE ); + //printf( "Q%d model %p %s update\n", event_queue_num, this, Token() ); + // CallCallbacks( CB_UPDATE ); + last_update = world->sim_time; - world->Enqueue( event_queue_num, interval, this ); + + if( subs > 0 ) // no subscriptions means we don't need to be updated + world->Enqueue( event_queue_num, interval, this, UpdateWrapper, NULL ); + + // if we updated the model then it needs to have its update + // callback called in series back in the main thread. It's + // not safe to run user callbacks in a worker thread, as + // they may make OpenGL calls or unsafe Stage API calls, + // etc. We queue up the callback into a queue specific to + + if( ! callbacks[Model::CB_UPDATE].empty() ) + world->pending_update_callbacks[event_queue_num].push(this); } +void Model::CallUpdateCallbacks( void ) +{ + CallCallbacks( CB_UPDATE ); +} meters_t Model::ModelHeight() const { @@ -921,50 +934,52 @@ void Model::UpdateCharge() } -void Model::UpdatePose( void ) -{ +void Model::Move( void ) +{ if( velocity.IsZero() ) return; if( disabled ) - return; - + return; + // convert usec to sec - double interval( (double)world->sim_interval / 1e6 ); + const double interval( (double)world->sim_interval / 1e6 ); // find the change of pose due to our velocity vector const Pose p( velocity.x * interval, - velocity.y * interval, - velocity.z * interval, - normalize( velocity.a * interval )); - - // the pose we're trying to achieve (unless something stops us) - const Pose newpose( pose + p ); - - // stash the original pose so we can put things back if we hit + velocity.y * interval, + velocity.z * interval, + normalize( velocity.a * interval )); + + // the pose we're trying to achieve (unless something stops us) + const Pose newpose( pose + p ); + + // stash the original pose so we can put things back if we hit const Pose startpose( pose ); - + pose = newpose; // do the move provisionally - we might undo it below - UnMapWithChildren(); // remove from all blocks - MapWithChildren(); // render into new blocks - - Model* hitmod = TestCollision(); - - if( hitmod ) // crunch! - { - // put things back the way they were - // this is expensive, but it happens very rarely for most people - pose = startpose; - UnMapWithChildren(); - MapWithChildren(); - SetStall(true); - } - else - { - world->dirty = true; // need redraw - SetStall(false); - } + //const unsigned int layer( world->updates%2 ); + + const unsigned int layer( world->updates%2 ); + + UnMapWithChildren( layer ); // remove from all blocks + MapWithChildren( layer ); // render into new blocks + + if( TestCollision() ) // crunch! + { + // put things back the way they were + // this is expensive, but it happens _very_ rarely for most people + pose = startpose; + UnMapWithChildren( layer ); + MapWithChildren( layer ); + SetStall(true); + } + else + { + world->dirty = true; // need redraw + SetStall(false); + } } @@ -1052,13 +1067,23 @@ kg_t Model::GetMassOfChildren() const return( GetTotalMass() - mass); } -void Model::UnMap() +void Model::Map( unsigned int layer ) { - if( mapped ) - { - blockgroup.UnMap(); - mapped = false; - } + if( ! mapped ) + { + // render all blocks in the group at my global pose and size + blockgroup.Map( layer ); + mapped = true; + } +} + +void Model::UnMap( unsigned int layer ) +{ + if( mapped ) + { + blockgroup.UnMap(layer); + mapped = false; + } } void Model::BecomeParentOf( Model* child ) diff --git a/libstage/model_callbacks.cc b/libstage/model_callbacks.cc index 38ad192aa..c2031d9e7 100644 --- a/libstage/model_callbacks.cc +++ b/libstage/model_callbacks.cc @@ -2,12 +2,20 @@ using namespace Stg; using namespace std; + void Model::AddCallback( callback_type_t type, model_callback_t cb, void* user ) { //callbacks[address].insert( cb_t( cb, user )); callbacks[type].insert( cb_t( cb, user )); + + // debug info - record the global number of registered callbacks + if( type == CB_UPDATE ) + { + assert( world->update_cb_count >= 0 ); + world->update_cb_count++; + } } @@ -16,6 +24,13 @@ int Model::RemoveCallback( callback_type_t type, { set& callset = callbacks[type]; callset.erase( cb_t( callback, NULL) ); + + + if( type == CB_UPDATE ) + { + world->update_cb_count--; + assert( world->update_cb_count >= 0 ); + } // return the number of callbacks remaining for this address. Useful // for detecting when there are none. @@ -33,6 +48,7 @@ int Model::CallCallbacks( callback_type_t type ) FOR_EACH( it, callset ) { const cb_t& cba = *it; + // callbacks return true if they should be cancelled if( (cba.callback)( this, cba.arg ) ) doomed.push_back( cba ); } diff --git a/libstage/model_getset.cc b/libstage/model_getset.cc index e790371ff..b626fa9a9 100644 --- a/libstage/model_getset.cc +++ b/libstage/model_getset.cc @@ -3,8 +3,9 @@ using namespace Stg; void Model::SetGeom( const Geom& val ) -{ - UnMapWithChildren(); +{ + UnMapWithChildren(0); + UnMapWithChildren(1); geom = val; @@ -12,8 +13,9 @@ void Model::SetGeom( const Geom& val ) NeedRedraw(); - MapWithChildren(); - + MapWithChildren(0); + MapWithChildren(1); + CallCallbacks( CB_GEOM ); } @@ -216,7 +218,13 @@ void Model::SetPose( const Pose& newpose ) // token, pose.x, pose.y, pose.z, pose.a ); NeedRedraw(); - MapWithChildren(); + + UnMapWithChildren(0); + UnMapWithChildren(1); + + MapWithChildren(0); + MapWithChildren(1); + world->dirty = true; } diff --git a/libstage/model_gripper.cc b/libstage/model_gripper.cc index e3e0f612e..570a6009a 100644 --- a/libstage/model_gripper.cc +++ b/libstage/model_gripper.cc @@ -184,7 +184,9 @@ void ModelGripper::FixBlocks() // Update the blocks that are the gripper's body void ModelGripper::PositionPaddles() { - UnMap(); + unsigned int layer = world->GetUpdateCount()%2; + UnMap(layer); + double paddle_center_pos = cfg.paddle_position * (0.5 - cfg.paddle_size.y ); paddle_left->SetCenterY( paddle_center_pos + cfg.paddle_size.y/2.0 ); paddle_right->SetCenterY( 1.0 - paddle_center_pos - cfg.paddle_size.y/2.0); @@ -195,7 +197,7 @@ void ModelGripper::PositionPaddles() paddle_left->SetZ( paddle_bottom, paddle_top ); paddle_right->SetZ( paddle_bottom, paddle_top ); - Map(); + Map(layer); } diff --git a/libstage/model_load.cc b/libstage/model_load.cc index c5a836b1e..98028e2b6 100644 --- a/libstage/model_load.cc +++ b/libstage/model_load.cc @@ -245,12 +245,15 @@ void Model::Load() // call any type-specific load callbacks this->CallCallbacks( CB_LOAD ); - + // we may well have changed blocks or geometry blockgroup.CalcSize(); - UnMapWithChildren(); - MapWithChildren(); - + + UnMapWithChildren(0); + UnMapWithChildren(1); + MapWithChildren(0); + MapWithChildren(1); + if( this->debug ) printf( "Model \"%s\" is in debug mode\n", token.c_str() ); @@ -350,7 +353,6 @@ void Model::LoadControllerModule( const char* lib ) fflush( stdout ); exit(-1); } - //else AddCallback( CB_INIT, initfunc, new CtrlArgs(lib,World::ctrlargs) ); // pass complete string into initfunc } diff --git a/libstage/region.cc b/libstage/region.cc index 4801e04fd..89ddabb3a 100644 --- a/libstage/region.cc +++ b/libstage/region.cc @@ -4,6 +4,7 @@ Copyright Richard Vaughan 2008 */ +#include #include "region.hh" using namespace Stg; @@ -38,26 +39,30 @@ void Region::RemoveBlock() // if there's nothing in this region, we can garbage collect the // cells to keep memory usage under control - if( count == 0 ) + /* if( count == 0 ) { if( cells ) - { - // stash this on the pool for reuse - dead_pool.push_back(cells); - //printf( "retiring cells @ %p (pool %u)\n", cells, dead_pool.size() ); - cells = NULL; - } + { + // stash this on the pool for reuse + dead_pool.push_back(cells); + //printf( "retiring cells @ %p (pool %u)\n", cells, dead_pool.size() ); + cells = NULL; + } else - PRINT_ERR( "region.count == 0 but cells == NULL" ); + PRINT_ERR( "region.count == 0 but cells == NULL" ); } + */ } SuperRegion::SuperRegion( World* world, point_int_t origin ) - : regions(), - origin(origin), - world(world), - count(0) + : count(0), + rwlock(), + origin(origin), + regions(), + world(world) { + pthread_rwlock_init(&rwlock,NULL); + for( int32_t c=0; ccount ) // region contains some occupied cells - { - // outline the region - glRecti( x<count ); - Gl::draw_string( x<cells[p+(q*REGIONWIDTH)].blocks.size() ) - { - GLfloat xx = p+(x<cells ) // empty but used previously - { - double left = x << RBITS; - double right = (x+1) << RBITS; - double bottom = y << RBITS; - double top = (y+1) << RBITS; - - double d = 3.0; - - // draw little corner markers for regions with memory - // allocated but no contents - glBegin( GL_LINES ); - glVertex2f( left, bottom ); - glVertex2f( left+d, bottom ); - glVertex2f( left, bottom ); - glVertex2f( left, bottom+d ); - glVertex2f( left, top ); - glVertex2f( left+d, top ); - glVertex2f( left, top ); - glVertex2f( left, top-d ); - glVertex2f( right, top ); - glVertex2f( right-d, top ); - glVertex2f( right, top ); - glVertex2f( right, top-d ); - glVertex2f( right, bottom ); - glVertex2f( right-d, bottom ); - glVertex2f( right, bottom ); - glVertex2f( right, bottom+d ); - glEnd(); - } - - ++r; // next region quickly - } - } - else - { // outline region-collected superregion - glColor3f( 1,1,0 ); - glRecti( 0,0, (1<count ) // region contains some occupied cells + { + // outline the region + glRecti( x<count ); + Gl::draw_string( x<cells[p+(q*REGIONWIDTH)].blocks[layer].size() ) + { + GLfloat xx = p+(x<cells ) // empty but used previously + { + double left = x << RBITS; + double right = (x+1) << RBITS; + double bottom = y << RBITS; + double top = (y+1) << RBITS; + + double d = 3.0; + + // draw little corner markers for regions with memory + // allocated but no contents + glBegin( GL_LINES ); + glVertex2f( left, bottom ); + glVertex2f( left+d, bottom ); + glVertex2f( left, bottom ); + glVertex2f( left, bottom+d ); + glVertex2f( left, top ); + glVertex2f( left+d, top ); + glVertex2f( left, top ); + glVertex2f( left, top-d ); + glVertex2f( right, top ); + glVertex2f( right-d, top ); + glVertex2f( right, top ); + glVertex2f( right, top-d ); + glVertex2f( right, bottom ); + glVertex2f( right-d, bottom ); + glVertex2f( right, bottom ); + glVertex2f( right, bottom+d ); + glEnd(); + } + */ + ++r; // next region quickly + } + } + else + { // outline region-collected superregion + glColor3f( 1,1,0 ); + glRecti( 0,0, (1<Resolution(); @@ -220,7 +241,7 @@ void SuperRegion::DrawVoxels() const glEnable( GL_DEPTH_TEST ); glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); - const Region* r = ®ions[0];//GetRegion( 0, 0); + const Region* r = ®ions[0]; for( int y=0; ycells[p+(q*REGIONWIDTH)]; + const std::vector& blocks = + r->cells[p+(q*REGIONWIDTH)].blocks[layer]; - if( c->blocks.size() ) + if( blocks.size() ) { GLfloat xx = p+(x<blocks ) + FOR_EACH( it, blocks ) { Block* block = *it; // first draw filled polygons @@ -267,56 +289,56 @@ void SuperRegion::DrawVoxels() const //std::set mapped_blocks; -void Cell::AddBlock( Block* b ) +void Cell::AddBlock( Block* b, unsigned int layer ) { - blocks.push_back( b ); - b->rendered_cells.push_back(this); - region->AddBlock(); - - //mapped_blocks.insert(b); + assert( layer < 2 ); + blocks[layer].push_back( b ); + b->rendered_cells[layer].push_back(this); + region->AddBlock(); } - -void Cell::RemoveBlock( Block* b ) +void Cell::RemoveBlock( Block* b, unsigned int layer ) { - //if( mapped_blocks.find(b) == mapped_blocks.end() ) - //printf( "REMOVE BLOCK %p that was never mapped\n", b ); + assert( layer<2 ); + + std::vector& blks = blocks[layer]; - size_t len = blocks.size(); - if( len ) - { -#if 0 // Use conventional STL style - - // this special-case test is faster for worlds with simple models, - // which are the ones we want to be really fast. It's a small - // extra cost for worlds with several models in each cell. It - // gives a 5% overall speed increase in fasr.world. - - if( (blocks.size() == 1) && - (blocks[0] == b) ) // special but common case - { - blocks.clear(); // cheap - } - else // the general but relatively expensive case - { - EraseAll( b, blocks ); - } + size_t len = blks.size(); + if( len ) + { +#if 1 + // Use conventional STL style + + // this special-case test is faster for worlds with simple models, + // which are the ones we want to be really fast. It's a small + // extra cost for worlds with several models in each cell. It + // gives a 5% overall speed increase in fasr.world. + + if( (blks.size() == 1) && + (blks[0] == b) ) // special but common case + { + blks.clear(); // cheap + } + else // the general but relatively expensive case + { + EraseAll( b, blks ); + } #else // attempt faster removal loop - // O(n) * low constant array element removal - // this C-style pointer work looks to be very slightly faster than the STL way - Block **start = &blocks[0]; // start of array - Block **r = &blocks[0]; // read from here - Block **w = &blocks[0]; // write to here - - while( r < start + len ) // scan down array, skipping 'this' - { - if( *r != b ) - *w++ = *r; - ++r; - } - blocks.resize( w-start ); + // O(n) * low constant array element removal + // this C-style pointer work looks to be very slightly faster than the STL way + Block **start = &blks[0]; // start of array + Block **r = &blks[0]; // read from here + Block **w = &blks[0]; // write to here + + while( r < start + len ) // scan down array, skipping 'this' + { + if( *r != b ) + *w++ = *r; + ++r; + } + blks.resize( w-start ); #endif - } - - region->RemoveBlock(); + } + + region->RemoveBlock(); } diff --git a/libstage/region.hh b/libstage/region.hh index 357aecaa5..cfc33525b 100644 --- a/libstage/region.hh +++ b/libstage/region.hh @@ -35,97 +35,98 @@ namespace Stg { friend class SuperRegion; friend class World; - + private: - std::vector blocks; - + std::vector blocks[2]; + public: - Cell() - : blocks(), - region(NULL) - { /* nothing to do */ } - - void RemoveBlock( Block* b ); - void AddBlock( Block* b ); - - const std::vector& GetBlocks(){ return blocks; } - - Region* region; + Cell() + : blocks(), + region(NULL) + { /* nothing to do */ } + + void RemoveBlock( Block* b, unsigned int index ); + void AddBlock( Block* b, unsigned int index ); + + const std::vector& GetBlocks( unsigned int index ){ return blocks[index]; } + + Region* region; }; // class Cell - + class Region { - friend class SuperRegion; - friend class World; // for raytracing - + friend class SuperRegion; + friend class World; // for raytracing + private: - Cell* cells; - unsigned long count; // number of blocks rendered into this region - - // vector of garbage collected cell arrays to reallocate before - // using new in GetCell() - static std::vector dead_pool; - - public: - Region(); - ~Region(); - - inline Cell* GetCell( int32_t x, int32_t y ) - { - if( cells == NULL ) + Cell* cells; + unsigned long count; // number of blocks rendered into this region + + // vector of garbage collected cell arrays to reallocate before + // using new in GetCell() + static std::vector dead_pool; + + public: + Region(); + ~Region(); + + inline Cell* GetCell( int32_t x, int32_t y ) + { + if( cells == NULL ) + { + assert(count == 0 ); + + if( dead_pool.size() ) { - assert(count == 0 ); - - if( dead_pool.size() ) - { - cells = dead_pool.back(); - dead_pool.pop_back(); - - //printf( "reusing cells @ %p (pool %u)\n", cells, dead_pool.size() ); - } - else - cells = new Cell[REGIONSIZE]; - - for( int32_t c=0; c jit_render; - + /** Pointers to all the models in this world. */ std::set models; @@ -854,7 +863,12 @@ namespace Stg bool operator()(const Model* a, const Model* b) const; }; + /** maintain a set of models with fiducials sorted by pose.x, for + quickly finding nearby fidcucials */ std::set models_with_fiducials_byx; + + /** maintain a set of models with fiducials sorted by pose.y, for + quickly finding nearby fidcucials */ std::set models_with_fiducials_byy; /** Add a model to the set of models with non-zero fiducials, if not already there. */ @@ -875,9 +889,9 @@ namespace Stg bool show_clock; ///< iff true, print the sim time on stdout unsigned int show_clock_interval; ///< updates between clock outputs - - pthread_mutex_t thread_mutex; ///< protect the worker thread management stuff - unsigned int threads_working; ///< the number of worker threads not yet finished + + pthread_mutex_t sync_mutex; ///< protect the worker thread management stuff + unsigned int threads_working; ///< the number of worker threads not yet finished pthread_cond_t threads_start_cond; ///< signalled to unblock worker threads pthread_cond_t threads_done_cond; ///< signalled by last worker thread to unblock main thread int total_subs; ///< the total number of subscriptions to all models @@ -957,7 +971,8 @@ namespace Stg /** call Cell::AddBlock(block) for each cell on the polygon */ void MapPoly( const PointIntVec& poly, - Block* block ); + Block* block, + unsigned int layer ); SuperRegion* AddSuperRegion( const point_int_t& coord ); SuperRegion* GetSuperRegion( const point_int_t& org ); @@ -1005,7 +1020,7 @@ namespace Stg /** Enlarge the bounding volume to include this point */ - void Extend( point3_t pt ); + inline void Extend( point3_t pt ); virtual void AddModel( Model* mod ); virtual void RemoveModel( Model* mod ); @@ -1030,13 +1045,15 @@ namespace Stg { public: - Event( usec_t time, Model* mod ) - : time(time), mod(mod) {} - + Event( usec_t time, Model* mod, model_callback_t cb, void* arg ) + : time(time), mod(mod), cb(cb), arg(arg) {} + usec_t time; ///< time that event occurs - Model* mod; ///< model to update - - /** order by time. Break ties by value of Model*. + Model* mod; ///< model to pass into callback + model_callback_t cb; + void* arg; + + /** order by time. Break ties by value of Model*, then cb*. @param event to compare with this one. */ bool operator<( const Event& other ) const; }; @@ -1044,6 +1061,9 @@ namespace Stg /** Queue of pending simulation events for the main thread to handle. */ std::vector > event_queues; + /** Queue of pending simulation events for the main thread to handle. */ + std::vector > pending_update_callbacks; + /** Create a new simulation event to be handled in the future. @param queue_num Specify which queue the event should be on. The main @@ -1055,17 +1075,23 @@ namespace Stg @param mod The model that should have its Update() method called at the specified time. */ - void Enqueue( unsigned int queue_num, usec_t delay, Model* mod ); - + void Enqueue( unsigned int queue_num, usec_t delay, Model* mod, model_callback_t cb, void* arg ) + { event_queues[queue_num].push( Event( sim_time + delay, mod, cb, arg ) ); } + /** Set of models that require energy calculations at each World::Update(). */ std::set active_energy; /** Set of models that require their positions to be recalculated at each World::Update(). */ std::set active_velocity; - + /** The amount of simulated time to run for each call to Update() */ usec_t sim_interval; - + + // debug instrumentation - making sure the number of update callbacks + // in each thread is consistent with the number that have been + // registered globally + int update_cb_count; + /** consume events from the queue up to and including the current sim_time */ void ConsumeQueue( unsigned int queue_num ); @@ -1160,7 +1186,7 @@ namespace Stg Model* GetGround() {return ground;}; }; - + class Block { friend class BlockGroup; @@ -1170,10 +1196,10 @@ namespace Stg friend class Canvas; friend class Cell; public: - + /** Block Constructor. A model's body is a list of these - blocks. The point data is copied, so pts can safely be freed - after constructing the block.*/ + blocks. The point data is copied, so pts can safely be freed + after constructing the block.*/ Block( Model* mod, const std::vector& pts, meters_t zmin, @@ -1181,20 +1207,20 @@ namespace Stg Color color, bool inherit_color, bool wheel ); - + /** A from-file constructor */ Block( Model* mod, Worldfile* wf, int entity); - + ~Block(); /** render the block into the world's raytrace data structure */ - void Map(); + void Map( unsigned int layer ); /** remove the block from the world's raytracing data structure */ - void UnMap(); - + void UnMap( unsigned int layer ); + /** draw the block in OpenGL as a solid single color */ - void DrawSolid(); + void DrawSolid(bool topview); /** draw the projection of the block onto the z=0 plane */ void DrawFootPrint(); @@ -1220,15 +1246,11 @@ namespace Stg /** Set the extent in Z of the block */ void SetZ( double min, double max ); - inline void RemoveFromCellArray( CellPtrVec& blocks ); - //inline void GenerateCandidateCells(); - void AppendTouchingModels( ModelPtrSet& touchers ); /** Returns the first model that shares a bitmap cell with this model */ Model* TestCollision(); - void SwitchToTestedCells(); void Load( Worldfile* wf, int entity ); Model* GetModel(){ return mod; }; const Color& GetColor(); @@ -1259,16 +1281,8 @@ namespace Stg /** record the cells into which this block has been rendered to UnMapping them very quickly. */ - CellPtrVec rendered_cells; - - /** When moving a model, we test for collisions by generating, for - each block, a list of the cells in which it would be rendered if the - move were to be successful. If no collision occurs, the move is - allowed - the rendered cells are cleared, the potential cells are - written, and the pointers to the rendered and potential cells are - switched for next time (avoiding a memory copy).*/ - //CellPtrVec * candidate_cells; - + CellPtrVec rendered_cells[2]; + PointIntVec gpts; /** find the position of a block's point in model coordinates @@ -1318,11 +1332,9 @@ namespace Stg with a block in this group, or NULL, if none are detected. */ Model* TestCollision(); - void SwitchToTestedCells(); - - void Map(); - void UnMap(); - + void Map( unsigned int layer ); + void UnMap( unsigned int layer ); + /** Draw the block in OpenGL as a solid single color. */ void DrawSolid( const Geom &geom); @@ -1564,6 +1576,8 @@ namespace Stg /** Get human readable string that describes the current global energy state. */ std::string EnergyString( void ) const; virtual void RemoveChild( Model* mod ); + + bool IsTopView(); }; @@ -1716,7 +1730,7 @@ namespace Stg friend class BlockGroup; friend class PowerPack; friend class Ray; - friend class ModelFiducial; + friend class ModelFiducial; private: /** the number of models instatiated - used to assign unique IDs */ @@ -1726,21 +1740,15 @@ namespace Stg /** records if this model has been mapped into the world bitmap*/ bool mapped; - std::vector drawOptions; - const std::vector& getOptions() const { return drawOptions; } + std::vector drawOptions; + const std::vector& getOptions() const { return drawOptions; } - /** EXP */ - // Model* hitmod; // thing we hit in a (parallel) collision test - protected: /** If true, the model always has at least one subscription, so always runs. Defaults to false. */ bool alwayson; - /** If true, the model is rendered lazily into the regions, to reduce memory use. */ - // TODO bool background; - BlockGroup blockgroup; /** OpenGL display list identifier for the blockgroup */ int blocks_dl; @@ -2058,15 +2066,15 @@ namespace Stg void CommitTestedPose(); - void Map(); - void UnMap(); + void Map( unsigned int layer ); + void UnMap( unsigned int layer ); - void MapWithChildren(); - void UnMapWithChildren(); + void MapWithChildren( unsigned int layer ); + void UnMapWithChildren( unsigned int layer ); // Find the root model, and map/unmap the whole tree. - void MapFromRoot(); - void UnMapFromRoot(); + void MapFromRoot( unsigned int layer ); + void UnMapFromRoot( unsigned int layer ); /** raytraces a single ray from the point and heading identified by pose, in local coords */ @@ -2102,23 +2110,17 @@ namespace Stg const uint32_t sample_count, const bool ztest = true ); + virtual void Startup(); + virtual void Shutdown(); + virtual void Update(); + virtual void Move(); + virtual void UpdateCharge(); + + static int UpdateWrapper( Model* mod, void* arg ){ mod->Update(); return 0; } + static int MoveWrapper( Model* mod, void* arg ){ mod->Move(); return 0; } - /** Causes this model and its children to recompute their global - position instead of using a cached pose in - Model::GetGlobalPose()..*/ - //void GPoseDirtyTree(); - - virtual void Startup(); - virtual void Shutdown(); - virtual void Update(); - virtual void UpdatePose(); - virtual void UpdateCharge(); - - //Model* Move( const Pose& newpose ); - - // EXP - //void ConditionalMove_calc( const Pose& newpose ); - //void ConditionalMove_commit( void ); + /** Calls CallCallback( CB_UPDATE ) */ + void CallUpdateCallbacks( void ); meters_t ModelHeight() const; diff --git a/libstage/world.cc b/libstage/world.cc index a189ef386..192d0dbae 100644 --- a/libstage/world.cc +++ b/libstage/world.cc @@ -118,7 +118,6 @@ bool World::lty::operator()(const Model* a, const Model* b) const return ay < by; } - // static data members unsigned int World::next_id = 0; bool World::quit_all = false; @@ -141,7 +140,7 @@ World::World( const std::string& name, quit( false ), show_clock( false ), show_clock_interval( 100 ), // 10 simulated seconds using defaults - thread_mutex(), + sync_mutex(), threads_working( 0 ), threads_start_cond(), threads_done_cond(), @@ -163,7 +162,11 @@ World::World( const std::string& name, wf( NULL ), paused( false ), event_queues(1), // use 1 thread by default - sim_interval( 1e5 ) // 100 msec has proved a good 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() ) { @@ -171,7 +174,7 @@ World::World( const std::string& name, exit(-1); } - pthread_mutex_init( &thread_mutex, NULL ); + pthread_mutex_init( &sync_mutex, NULL ); pthread_cond_init( &threads_start_cond, NULL ); pthread_cond_init( &threads_done_cond, NULL ); @@ -220,6 +223,8 @@ bool World::UpdateAll() return quit; } + + void* World::update_thread_entry( std::pair *thread_info ) { World* world = thread_info->first; @@ -227,30 +232,32 @@ void* World::update_thread_entry( std::pair *thread_info ) //printf( "thread ID %d waiting for mutex\n", thread_instance ); - pthread_mutex_lock( &world->thread_mutex ); - + 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->thread_mutex ); - pthread_mutex_unlock( &world->thread_mutex ); - - //puts( "worker thread awakes" ); - - // consume events on the queue up to the current sim time - world->ConsumeQueue( thread_instance ); - + + 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->thread_mutex ); + + pthread_mutex_lock( &world->sync_mutex ); if( --world->threads_working == 0 ) { //puts( "last worker signalling main thread" ); @@ -401,6 +408,9 @@ void World::Load( const std::string& worldfile_path ) 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." ); @@ -454,8 +464,8 @@ void World::Load( const std::string& worldfile_path ) { // all this is a hack and shouldn't be necessary (*it)->blockgroup.CalcSize(); - (*it)->UnMap(); - (*it)->Map(); + (*it)->UnMap(updates%2); + (*it)->Map(updates%2); // to here (*it)->InitControllers(); @@ -519,6 +529,7 @@ void World::AddUpdateCallback( world_callback_t cb, { // add the callback & argument to the list cb_list.push_back( std::pair(cb, user) ); + } int World::RemoveUpdateCallback( world_callback_t cb, @@ -542,6 +553,31 @@ int World::RemoveUpdateCallback( world_callback_t cb, 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& 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 ) ) @@ -564,40 +600,34 @@ void World::ConsumeQueue( unsigned int queue_num ) Event ev( queue.top() ); if( ev.time > sim_time ) break; queue.pop(); - - //printf( "@ %llu next event ptr %p\n", sim_time, ev.mod ); + + //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() ); - if( ev.mod->subs > 0 ) // no subscriptions means the event is discarded - ev.mod->Update(); // update the model + 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 ); } - - - //puts( "World::Update()" ); - - // if we've run long enough, exit - if( PastQuitTime() ) - return true; - - sim_time += sim_interval; - - FOR_EACH( it, active_velocity ) - (*it)->UpdatePose(); + sim_time += sim_interval; + // rebuild the sets sorted by position on x,y axis -#if( 1 ) models_with_fiducials_byx.clear(); models_with_fiducials_byy.clear(); @@ -606,41 +636,46 @@ bool World::Update() models_with_fiducials_byx.insert( *it ); models_with_fiducials_byy.insert( *it ); } -#endif //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( &thread_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 ); - - // todo - take the 1th thread work here? - - // wait for all the last update job to complete - it will - // signal the worker_threads_done condition var - while( threads_working > 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, &thread_mutex ); + pthread_cond_wait( &threads_done_cond, &sync_mutex ); } - pthread_mutex_unlock( &thread_mutex ); - //puts( "main thread awakes" ); + pthread_mutex_unlock( &sync_mutex ); + //puts( "main thread awakes" ); - // TODO: allow threadsafe callbacks to be called in worker - // threads - } - + // 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(); @@ -662,9 +697,8 @@ unsigned int World::GetEventQueue( Model* mod ) const Model* World::GetModel( const std::string& name ) const { PRINT_DEBUG1( "looking up model name %s in models_by_name", name.c_str() ); - - std::map::const_iterator it = - models_by_name.find( name ); + + std::map::const_iterator it( models_by_name.find( name ) ); if( it == models_by_name.end() ) { @@ -728,7 +762,8 @@ RaytraceResult World::Raytrace( const Pose &gpose, { return Raytrace( Ray( mod, gpose, range, func, arg, ztest )); } - + + RaytraceResult World::Raytrace( const Ray& r ) { //rt_cells.clear(); @@ -777,12 +812,14 @@ RaytraceResult World::Raytrace( const Ray& r ) 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. @@ -813,7 +850,7 @@ RaytraceResult World::Raytrace( const Ray& r ) (cy>=0) && (cy 0 ) { - FOR_EACH( it, c->blocks ) + FOR_EACH( it, c->blocks[layer] ) { Block* block( *it ); assert( block ); @@ -970,19 +1007,18 @@ void World::Reload( void ) ForEachDescendant( _reload_cb, NULL ); } - -void World::MapPoly( const PointIntVec& pts, Block* block ) +void World::MapPoly( const PointIntVec& pts, Block* block, unsigned int layer ) { - const size_t pt_count = pts.size(); - - for( size_t i(0); iGetRegion( GETREG(globx), - GETREG(globy))); + GETREG(globy))); + assert(reg); //printf( "REGION %p\n", reg ); @@ -1020,7 +1057,7 @@ void World::MapPoly( const PointIntVec& pts, Block* block ) (cy>=0) && (cy 0 ) { - c->AddBlock(block); + c->AddBlock(block, layer ); // cleverly skip to the next cell (now it's safe to // manipulate the cell pointer) @@ -1040,7 +1077,7 @@ void World::MapPoly( const PointIntVec& pts, Block* block ) } --n; } - } + } } } @@ -1056,19 +1093,15 @@ SuperRegion* World::AddSuperRegion( const point_int_t& sup ) // set the lower left corner of the new superregion Extend( point3_t( (sup.x << SRBITS) / ppm, - (sup.y << SRBITS) / ppm, - 0 )); + (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 )); + ((sup.y+1) << SRBITS) / ppm, + 0 )); //printf( "top right (%.2f,%.2f,%.2f)\n", pt.x, pt.y, pt.z ); -// // map all jit models -// FOR_EACH( it, jit_render ) -// (*it)->Map(); - return sr; } @@ -1140,18 +1173,10 @@ void World:: RegisterOption( Option* opt ) void World::Log( Model* mod ) { LogEntry( sim_time, mod); - printf( "log entry count %u\n", (unsigned int)LogEntry::Count() ); //LogEntry::Print(); } -void World::Enqueue( unsigned int queue_num, usec_t delay, Model* mod ) -{ - //printf( "enqueue at %llu %p %s\n", sim_time + delay, mod, mod->Token() ); - - event_queues[queue_num].push( Event( sim_time + delay, mod ) ); -} - bool World::Event::operator<( const Event& other ) const { return( time > other.time ); diff --git a/libstage/worldgui.cc b/libstage/worldgui.cc index c5b3d907b..96074be64 100644 --- a/libstage/worldgui.cc +++ b/libstage/worldgui.cc @@ -429,14 +429,21 @@ void WorldGui::DrawOccupancy() const // printf( "sr %d [%d,%d] %p\n", count++, it->first.x, it->first.y, it->second ); // printf( "done\n" ); +// unsigned int layer( updates % 2 ); + FOR_EACH( it, superregions ) - it->second->DrawOccupancy(); + { + it->second->DrawOccupancy(0); + it->second->DrawOccupancy(1); + } } void WorldGui::DrawVoxels() const { + unsigned int layer( updates % 2 ); + FOR_EACH( it, superregions ) - it->second->DrawVoxels(); + it->second->DrawVoxels( layer ); } void WorldGui::windowCb( Fl_Widget* w, WorldGui* wg ) @@ -565,7 +572,7 @@ void WorldGui::fasttimeCb( Fl_Widget* w, WorldGui* wg ) void WorldGui::Redraw() { - puts( "redrawing\n" ); + //puts( "redrawing\n" ); canvas->redraw(); } @@ -852,3 +859,6 @@ usec_t WorldGui::RealTimeNow() const gettimeofday( &tv, NULL ); // slow system call: use sparingly return( tv.tv_sec*1000000 + tv.tv_usec ); } + +bool WorldGui::IsTopView() +{ return canvas->IsTopView(); } diff --git a/worlds/benchmark/cave.world b/worlds/benchmark/cave.world index 957a8fa9e..e68340663 100644 --- a/worlds/benchmark/cave.world +++ b/worlds/benchmark/cave.world @@ -12,6 +12,8 @@ speedup -1 # as fast as possible paused 1 +threads 7 + # configure the GUI window window ( diff --git a/worlds/benchmark/expand_pioneer.cc b/worlds/benchmark/expand_pioneer.cc index 8ef1f02ec..c19bdb261 100644 --- a/worlds/benchmark/expand_pioneer.cc +++ b/worlds/benchmark/expand_pioneer.cc @@ -46,8 +46,8 @@ extern "C" int Init( Model* mod ) robot->ranger->AddCallback( Model::CB_UPDATE, (model_callback_t)RangerUpdate, robot ); // subscribe to the laser, though we don't use it for navigating - //robot->laser = (ModelLaser*)mod->GetUnusedModelOfType( "ranger ); - //assert( robot->laser ); + robot->laser = (ModelRanger*)mod->GetUnusedModelOfType( "ranger" ); + assert( robot->laser ); // start the models updating robot->ranger->Subscribe(); diff --git a/worlds/benchmark/hospital.world b/worlds/benchmark/hospital.world index 9f8f5dea9..f4e0658f3 100644 --- a/worlds/benchmark/hospital.world +++ b/worlds/benchmark/hospital.world @@ -13,7 +13,7 @@ speedup -1 # as fast as possible paused 0 # threads may help or hurt performance depending on your worldfile, machine and load - threads 2 + threads 7 quit_time 60 @@ -26,7 +26,7 @@ window scale 12.611 show_data 1 - interval 100 + interval 200 ) # load an environment bitmap diff --git a/worlds/fasr.world b/worlds/fasr.world index b7583ea8b..427d10a92 100644 --- a/worlds/fasr.world +++ b/worlds/fasr.world @@ -7,16 +7,16 @@ include "pioneer.inc" include "map.inc" include "sick.inc" -speedup 0 # 20.000 +# speedup 0 # 20.000 paused 0 # time at which to pause (in GUI mode) or quit (in headless mode) the simulation #quit_time 3600 # 1 hour of simulated time -quit_time 1800 # 30 mins of simulated time +#quit_time 1800 # 30 mins of simulated time resolution 0.02 -threads 0 +threads 4 # configure the GUI window window diff --git a/worlds/fasr2.world b/worlds/fasr2.world index 7559285bb..a33bc8921 100644 --- a/worlds/fasr2.world +++ b/worlds/fasr2.world @@ -12,11 +12,11 @@ paused 0 # time at which to pause (in GUI mode) or quit (in headless mode) the simulation # quit_time 3600 # 1 hour of simulated time -quit_time 200 +# quit_time 2000 resolution 0.02 -threads 8 +threads 4 # configure the GUI window window diff --git a/worlds/fasr2_open.world b/worlds/fasr2_open.world index 36b9e3a0c..b39e3c600 100644 --- a/worlds/fasr2_open.world +++ b/worlds/fasr2_open.world @@ -16,7 +16,7 @@ paused 1 resolution 0.02 -threads 8 +threads 4 # configure the GUI window window @@ -99,7 +99,7 @@ define charge_station model size [ 0.100 0.100 0.300 ] color "purple" - laser_return 0 + ranger_return 0 obstacle_return 0 fiducial_return 2 # look for this in the fiducial sensor @@ -175,7 +175,12 @@ define blinker model define autorob pioneer2dx ( - sicklaser( samples 32 range_max 2 laser_return 2 watts 30 ) + # sicklaser( samples 32 range_max 2 ranger_return 2 watts 30 ) + + sickbase( sensor( samples 32 range [0 2] fov 180 color_rgba [ 0 0 1 0.15 ] ) + ranger_return 2.000 watts 30 ) + + # ctrl "fasr2" kjoules 400 kjoules_capacity 400 diff --git a/worlds/pioneer_flocking.world b/worlds/pioneer_flocking.world index 8896ecb86..54e248b1b 100644 --- a/worlds/pioneer_flocking.world +++ b/worlds/pioneer_flocking.world @@ -13,7 +13,7 @@ paused 1 resolution 0.1 # this is very helpful if you have multiple CPUs - a good value is $(number of CPU cores) - -threads 4 +threads 6 # configure the GUI window window diff --git a/worlds/simple.world b/worlds/simple.world index 732aff566..37172dab2 100644 --- a/worlds/simple.world +++ b/worlds/simple.world @@ -9,10 +9,12 @@ include "sick.inc" # time to pause (in GUI mode) or quit (in headless mode (-g)) the simulation quit_time 3600 # 1 hour of simulated time -paused 0 +paused 1 resolution 0.02 +threads 1 + # configure the GUI window window (