Permalink
Browse files

attempted bugfix in Player fiducials (untested)

  • Loading branch information...
1 parent 602b385 commit 0075eb00abbec8b15b8b11a8e44f0fb3bce4868b rtv committed Sep 29, 2009
Showing with 74 additions and 64 deletions.
  1. +22 −14 libstage/block.cc
  2. +5 −3 libstage/model.cc
  3. +2 −2 libstage/region.hh
  4. +12 −17 libstage/stage.hh
  5. +27 −25 libstageplugin/p_fiducial.cc
  6. +2 −0 todo.txt
  7. +2 −2 worlds/benchmark/hospital.world
  8. +2 −1 worlds/fasr.world
View
@@ -209,41 +209,49 @@ void Block::RemoveFromCellArray( CellPtrVec *cells )
{
Cell* cell = *it;
- // remove me from the cell
- EraseAll( this, cell->blocks );
+ // TODO - make sure this block should logically always be in the
+ // cell, then we can use this optimisation
+
+ // the vector usually has only 1 element, so this is a useful speedup
+ // std::vector<Block*>& v = cell->blocks;
+ // if( v.size() == 1 )
+ // {
+ // assert( v[0] == this );
+ // v.clear();
+ // }
+ // else
+ // remove me from the cell
+ EraseAll( this, cell->blocks );
+ // avoid the function call of EraseAll()
+ //v.erase( std::remove( v.begin(), v.end(), this ), v.end() );
+
--cell->region->count;
--cell->region->superregion->count;
}
}
-void Block::AddToCellArray( CellPtrVec *cells )
+void Block::SwitchToTestedCells()
{
- FOR_EACH( it, *cells )
+ RemoveFromCellArray( rendered_cells );
+
+ // render the block into each of the candidate cells
+ FOR_EACH( it, *candidate_cells )
{
Cell* cell = *it;
-
// record that I am rendered in this cell
rendered_cells->push_back( cell );
-
// store me in the cell
cell->blocks.push_back( this );
+ //list_entries.push_back( cell->blocks.insert( cell->blocks.begin(), this ) );
++cell->region->count;
++cell->region->superregion->count;
}
-}
-
-void Block::SwitchToTestedCells()
-{
- RemoveFromCellArray( rendered_cells );
- AddToCellArray( candidate_cells );
// switch current and candidate cell pointers
CellPtrVec *tmp = rendered_cells;
rendered_cells = candidate_cells;
candidate_cells = tmp;
- // XXXX todo
-
mapped = true;
}
View
@@ -143,6 +143,7 @@ using namespace Stg;
uint32_t Model::count = 0;
std::map<stg_id_t,Model*> Model::modelsbyid;
std::map<std::string, creator_t> Model::name_map;
+std::map<void*, std::set<Model::stg_cb_t> > Model::callbacks;
void Size::Load( Worldfile* wf, int section, const char* keyword )
{
@@ -251,7 +252,7 @@ Model::Model( World* world,
blockgroup(),
blocks_dl(0),
boundary(false),
- callbacks(),
+ // callbacks(),
color( 1,0,0 ), // red
data_fresh(false),
disabled(false),
@@ -350,9 +351,10 @@ void Model::Init()
// init is called after the model is loaded
blockgroup.CalcSize();
- UnMap(); // remove any old cruft rendered during startup
+ // remove any old cruft rendered during startup
+ UnMap();
Map();
-
+
// find the queue for update events: zero if thread safe, else we
// ask the world to assign us to a queue
event_queue_num = thread_safe ? world->GetEventQueue( this ) : 0;
View
@@ -40,8 +40,8 @@ namespace Stg
private:
Region* region;
- std::vector<Block*> blocks;
-
+ std::vector<Block*> blocks;
+
public:
Cell( Region* reg )
: region( reg ),
View
@@ -806,7 +806,6 @@ namespace Stg
/** Remove a model from the set of models with non-zero fiducials, if it exists. */
void FiducialErase( Model* mod )
{
- //EraseAll<Model*,ModelPtrVec&>( mod, models_with_fiducials );
EraseAll( mod, models_with_fiducials );
}
@@ -1113,8 +1112,7 @@ namespace Stg
std::vector<stg_point_t>& Points()
{ return pts; };
-
- inline void AddToCellArray( CellPtrVec* blocks );
+
inline void RemoveFromCellArray( CellPtrVec* blocks );
inline void GenerateCandidateCells();
@@ -1149,7 +1147,10 @@ namespace Stg
/** z extent in global coordinates */
Bounds global_z;
bool mapped;
-
+
+ /** record the list entries for the cells where this block is rendered */
+ std::vector< std::list<Block*>::iterator > list_entries;
+
/** record the cells into which this block has been rendered to
UnMapping them very quickly. */
CellPtrVec * rendered_cells;
@@ -1676,7 +1677,7 @@ namespace Stg
/** A list of callback functions can be attached to any
address. When Model::CallCallbacks( void*) is called, the
callbacks are called.*/
- std::map<void*, std::set<stg_cb_t> > callbacks;
+ static std::map<void*, std::set<stg_cb_t> > callbacks;
/** Default color of the model's blocks.*/
Color color;
@@ -1690,10 +1691,7 @@ namespace Stg
std::list<Visualizer*> cv_list;
std::list<Flag*> flag_list;
Geom geom;
- Pose global_pose;
- bool gpose_dirty; ///< set this to indicate that global pose may have changed
- /** Controls our appearance and functionality in the GUI, if used */
-
+
/** Records model state and functionality in the GUI, if used */
class GuiState
{
@@ -1709,8 +1707,6 @@ namespace Stg
bool has_default_block;
- /* hooks for attaching special callback functions (not used as
- variables - we just need unique addresses for them.) */
/* Hooks for attaching special callback functions (not used as
variables - we just need unique addresses for them.) */
class CallbackHooks
@@ -1751,11 +1747,10 @@ namespace Stg
/** list of powerpacks that this model is currently charging,
initially NULL. */
std::list<PowerPack*> pps_charging;
-
- /** GData datalist can contain arbitrary named data items. Can be used
- by derived model types to store properties, and for user code
- to associate arbitrary items with a model. */
- //GData* props;
+
+ /** Props map can contain arbitrary named data items. Can be used
+ by derived model types to store properties, and for user code
+ to associate arbitrary items with a model. */
std::map<std::string,const void*> props;
/** Visualize the most recent rasterization operation performed by this model */
@@ -1886,7 +1881,7 @@ namespace Stg
protected:
- /// Register an Option for pickup by the GUI
+ /** Register an Option for pickup by the GUI. */
void RegisterOption( Option* opt );
void AppendTouchingModels( ModelPtrSet& touchers );
@@ -61,38 +61,40 @@ void InterfaceFiducial::Publish( void )
player_fiducial_data_t pdata;
memset( &pdata, 0, sizeof(pdata) );
- unsigned int count=0;
- ModelFiducial::Fiducial* fids = ((ModelFiducial*)mod)->GetFiducials( &count );
-
- if( fids && count )
+ std::vector<ModelFiducial::Fiducial>& fids =
+ ((ModelFiducial*)mod)->GetFiducials();
+
+ pdata.fiducials_count = count;
+
+ if( pdata.fiducials_count > 0 )
{
- pdata.fiducials = new player_fiducial_item_t[count];
-
- for( unsigned int i=0; i<count; i++ )
- {
- pdata.fiducials[i].id = fids[i].id;
+ pdata.fiducials = new player_fiducial_item_t[pdata.fiducials_count];
+
+ for( unsigned int i=0; i<pdata.fiducials_count; i++ )
+ {
+ pdata.fiducials[i].id = fids[i].id;
+
+ // 2D x,y only
+ double xpos = fids[i].range * cos(fids[i].bearing);
+ double ypos = fids[i].range * sin(fids[i].bearing);
- // 2D x,y only
- double xpos = fids[i].range * cos(fids[i].bearing);
- double ypos = fids[i].range * sin(fids[i].bearing);
-
- pdata.fiducials[i].pose.px = xpos;
- pdata.fiducials[i].pose.py = ypos;
- pdata.fiducials[i].pose.pz = 0.0;
- pdata.fiducials[i].pose.proll = 0.0;
- pdata.fiducials[i].pose.ppitch = 0.0;
- pdata.fiducials[i].pose.pyaw = fids[i].geom.a;
- }
+ pdata.fiducials[i].pose.px = xpos;
+ pdata.fiducials[i].pose.py = ypos;
+ pdata.fiducials[i].pose.pz = 0.0;
+ pdata.fiducials[i].pose.proll = 0.0;
+ pdata.fiducials[i].pose.ppitch = 0.0;
+ pdata.fiducials[i].pose.pyaw = fids[i].geom.a;
+ }
}
// publish this data
this->driver->Publish( this->addr,
- PLAYER_MSGTYPE_DATA,
- PLAYER_FIDUCIAL_DATA_SCAN,
- &pdata, sizeof(pdata), NULL);
-
+ PLAYER_MSGTYPE_DATA,
+ PLAYER_FIDUCIAL_DATA_SCAN,
+ &pdata, sizeof(pdata), NULL);
+
if ( pdata.fiducials )
- delete [] pdata.fiducials;
+ delete [] pdata.fiducials;
}
int InterfaceFiducial::ProcessMessage(QueuePointer& resp_queue,
View
@@ -15,6 +15,8 @@ linux 16.36
OS X 16.20 (rev 7949)
19.94 (rev 8210 - new event queue is slower but more powerful and elegant)
+MBA 17.3
+
** 3.2.0 RELEASE *
@@ -10,12 +10,12 @@ resolution 0.02
speedup -1 # as fast as possible
-paused 1
+paused 0
# threads may help or hurt performance depending on your worldfile, machine and load
# threads 7
-# quit_time 60
+quit_time 60
# configure the GUI window
window
View
@@ -10,7 +10,8 @@ speedup 10
paused 1
# 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 3600 # 1 hour of simulated time
+quit_time 1800 # hour of simulated time
resolution 0.02

0 comments on commit 0075eb0

Please sign in to comment.