Revision: 8286
http://playerstage.svn.sourceforge.net/playerstage/?rev=8286&view=rev
Author: rtv
Date: 2009-10-13 03:20:17 +0000 (Tue, 13 Oct 2009)
Log Message:
-----------
performance tweaks
Modified Paths:
--------------
code/stage/trunk/libstage/block.cc
code/stage/trunk/libstage/model.cc
code/stage/trunk/libstage/model_callbacks.cc
code/stage/trunk/libstage/model_getset.cc
code/stage/trunk/libstage/model_load.cc
code/stage/trunk/libstage/model_position.cc
code/stage/trunk/libstage/region.hh
code/stage/trunk/libstage/stage.hh
code/stage/trunk/libstage/world.cc
code/stage/trunk/worlds/fasr.world
code/stage/trunk/worlds/pioneer_flocking.world
Removed Paths:
-------------
code/stage/trunk/libstage/blockgrid.cc
Modified: code/stage/trunk/libstage/block.cc
===================================================================
--- code/stage/trunk/libstage/block.cc 2009-10-11 01:57:26 UTC (rev 8285)
+++ code/stage/trunk/libstage/block.cc 2009-10-13 03:20:17 UTC (rev 8286)
@@ -143,12 +143,12 @@
{
// 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)->blocks )
- {
- if( !mod->IsRelated( (*block_it)->mod ))
- touchers.insert( (*block_it)->mod );
- }
+ // for every block rendered into that cell
+ FOR_EACH( block_it, (*cell_it)->blocks )
+ {
+ if( !mod->IsRelated( (*block_it)->mod ))
+ touchers.insert( (*block_it)->mod );
+ }
}
Model* Block::TestCollision()
@@ -208,30 +208,40 @@
FOR_EACH( it, *cells )
{
Cell* cell = *it;
+
+ // 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%
+ // speed increase in fasr.world.
+ if( (cell->blocks.size() == 1) &&
+ (cell->blocks[0] == this) ) // special, common
case
+ {
+ cell->blocks.clear(); // cheap
+ }
+ else // the general but relatively expensive case
+ {
+ 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;
}
+
+ //printf( "%d %d %.2f\n", count1, countmore,
((float)count1)/float(countmore));
}
void Block::SwitchToTestedCells()
{
+ // todo:
+
+ // 1. find the set of cells in rendered but not candidate and remove
+ // them
+
+ // 2. find the set of cells in candidate but not rendered and insert
+ // them
+
+ // .. and see if that is faster
+
RemoveFromCellArray( rendered_cells );
// render the block into each of the candidate cells
@@ -242,7 +252,7 @@
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;
}
@@ -283,12 +293,14 @@
}
// convert the mpts in model coords into global pixel coords
- gpts.resize(pt_count);
-
+ //gpts.resize(pt_count);
+ //for( unsigned int i=0; i<pt_count; i++ )
+ //gpts[i] = mod->world->MetersToPixels( mod->LocalToGlobal( mpts[i] ));
+
+ gpts.clear();
+ mod->LocalToPixels( mpts, gpts );
+
for( unsigned int i=0; i<pt_count; i++ )
- gpts[i] = mod->world->MetersToPixels( mod->LocalToGlobal( mpts[i] ));
-
- for( unsigned int i=0; i<pt_count; i++ )
mod->world->ForEachCellInLine( gpts[i],
gpts[(i+1)%pt_count],
*candidate_cells );
Deleted: code/stage/trunk/libstage/blockgrid.cc
===================================================================
--- code/stage/trunk/libstage/blockgrid.cc 2009-10-11 01:57:26 UTC (rev
8285)
+++ code/stage/trunk/libstage/blockgrid.cc 2009-10-13 03:20:17 UTC (rev
8286)
@@ -1,96 +0,0 @@
-#include "stage_internal.hh"
-
-// todo - make these configurable
-
-static const uint32_t NBITS = 5;
-static const uint32_t NSIZE = 1<<NBITS;
-static const uint32_t NSQR = NSIZE*NSIZE;
-static const uint32_t MASK = NSIZE-1;
-
-BlockGrid::BlockGrid( uint32_t width, uint32_t height )
-{
- this->width = width;
- this->height = height;
- this->cells = (GSList**)calloc( sizeof(GList*), width * height );
-
- assert(this->cells);
-}
-
-BlockGrid::~BlockGrid()
-{
- for( uint32_t x=0; x<width; x++ )
- for( uint32_t y=0; y<height; y++ )
- {
- if( cells[x + y*width] )
- g_slist_free( cells[x+y*width] );
- }
-
- delete[] cells;
- //delete[] map;
-}
-
-void BlockGrid::AddBlock( uint32_t x, uint32_t y, Block* block )
-{
- //printf( "add block %u %u\n", x, y );
-
- if( x < width && y < height )
- {
- cells[ x+y*width ] = g_slist_prepend( cells[ x+y*width ], block
);
- block->RecordRenderPoint( x, y );
- }
-}
-
-void BlockGrid::RemoveBlock( uint32_t x, uint32_t y, Block* block )
-{
- //printf( "remove block %u %u\n", x, y );
-
- if( x < width && y < height )
- {
- cells[ x+y*width ] = g_slist_remove( cells[ x+y*width ], block
);
- }
-}
-
-GSList* BlockGrid::GetList( uint32_t x, uint32_t y )
-{
- if( x < width && y < height )
- return cells[ x+y*width ];
- else
- return NULL;
-}
-
-void BlockGrid::GlobalRemoveBlock( Block* block )
-{
- for( uint32_t x=0; x<width; x++ )
- for( uint32_t y=0; y<height; y++ )
- RemoveBlock(x,y,block );
-}
-
-void BlockGrid::Draw( bool drawall )
-{
- for( uint32_t x=0; x<width; x++ )
- for( uint32_t y=0; y<height; y++ )
- {
- //stg_bigblock_t* bb = &map[ x + y*bwidth ];
-
- //if( drawall || bb->lists )
- // {
- // glRecti(
x<<numbits,y<<numbits,(x+1)<<numbits,(y+1)<<numbits );
-
- // if( bb->lists )
- // for( uint32_t a=0; a<NSQR; a++ )
- // {
- // if( drawall || bb->lists[ a ] )
- // glRecti( (x<<numbits)+a%NSIZE,
- // (y<<numbits)+a/NSIZE,
- // (x<<numbits)+a%NSIZE+1,
- // (y<<numbits)+a/NSIZE+1 );
-
- // }
- // }
-
- if( cells[x+y*width] )
- glRecti( x,y, x+1, y+ 1 );
-
- }
-}
-
Modified: code/stage/trunk/libstage/model.cc
===================================================================
--- code/stage/trunk/libstage/model.cc 2009-10-11 01:57:26 UTC (rev 8285)
+++ code/stage/trunk/libstage/model.cc 2009-10-13 03:20:17 UTC (rev 8286)
@@ -66,6 +66,14 @@
that if the model hits an obstacle, its velocity will be set to
zero.
+ - velocity_enable int (defaults to 0)\n Most models ignore their
+ velocity state. This saves on processing time since most models
+ never have their velocity set to anything but zero. Some
+ subclasses (e.g. ModelPosition) change this default as they are
+ expecting to move. Users can specify a non-zero value here to
+ enable velocity control of this model. This achieves the same as
+ calling Model::VelocityEnable()
+
- color <string>\n specify the color of the object using a color
name from the X11 database (rgb.txt)
@@ -285,8 +293,9 @@
type(type),
event_queue_num( 0 ),
used(false),
- velocity(),
- watts(0.0),
+ velocity(),
+ velocity_enable( false ),
+ watts(0.0),
watts_give(0.0),
watts_take(0.0),
wf(NULL),
@@ -517,7 +526,6 @@
return org;
}
-
void Model::Say( const std::string& str )
{
say_string = str;
@@ -570,6 +578,16 @@
return stg_point_t( gpose.x, gpose.y );
}
+
+void Model::LocalToPixels( const std::vector<stg_point_t>& local,
+
std::vector<stg_point_int_t>& global) const
+{
+ FOR_EACH( it, local )
+ global.push_back( world->MetersToPixels( LocalToGlobal( *it )));
+}
+
+
+
void Model::MapWithChildren()
{
UnMap();
@@ -681,9 +699,10 @@
// put my first update request in the world's queue
world->Enqueue( event_queue_num, interval, this );
-
- world->active_velocity.insert( this );
-
+
+ if( velocity_enable )
+ world->active_velocity.insert( this );
+
if( FindPowerPack() )
world->active_energy.insert( this );
@@ -705,7 +724,9 @@
void Model::Update( void )
{
- CallCallbacks( &hooks.update );
+ if( hooks.attached_update )
+ CallCallbacks( &hooks.update );
+
last_update = world->sim_time;
world->Enqueue( event_queue_num, interval, this );
}
@@ -715,11 +736,7 @@
{
stg_meters_t m_child = 0; //max size of any child
FOR_EACH( it, children )
- {
- stg_meters_t tmp_h = (*it)->ModelHeight();
- if( tmp_h > m_child )
- m_child = tmp_h;
- }
+ m_child = std::max( m_child, (*it)->ModelHeight() );
//height of model + max( child height )
return geom.size.z + m_child;
@@ -825,9 +842,6 @@
}
}
}
-
- // set up the next event
- //world->Enqueue( 0, World::Event::ENERGY, interval_energy, this );
}
void Model::CommitTestedPose()
Modified: code/stage/trunk/libstage/model_callbacks.cc
===================================================================
--- code/stage/trunk/libstage/model_callbacks.cc 2009-10-11 01:57:26 UTC
(rev 8285)
+++ code/stage/trunk/libstage/model_callbacks.cc 2009-10-13 03:20:17 UTC
(rev 8286)
@@ -7,6 +7,16 @@
void* user )
{
callbacks[address].insert( stg_cb_t( cb, user ));
+
+ // count the number of callbacks attached to pose and
+ // velocity. These are changed very often, so we avoid looking up
+ // the callbacks if these values are zero. */
+ if( address == &this->pose )
+ ++hooks.attached_pose;
+ else if( address == &this->velocity )
+ ++hooks.attached_velocity;
+ else if( address == &this->hooks.update )
+ ++hooks.attached_update;
}
@@ -17,6 +27,15 @@
callset.erase( stg_cb_t( callback, NULL) );
+ // count the number of callbacks attached to pose and velocity. Can
+ // not go below zero.
+ if( address == &pose )
+ hooks.attached_pose = max( --hooks.attached_pose, 0 );
+ else if( address == &velocity )
+ hooks.attached_velocity = max( --hooks.attached_velocity, 0 );
+ else if( address == &hooks.update )
+ hooks.attached_update = max( --hooks.attached_update, 0 );
+
// return the number of callbacks remaining for this address. Useful
// for detecting when there are none.
return callset.size();
Modified: code/stage/trunk/libstage/model_getset.cc
===================================================================
--- code/stage/trunk/libstage/model_getset.cc 2009-10-11 01:57:26 UTC (rev
8285)
+++ code/stage/trunk/libstage/model_getset.cc 2009-10-13 03:20:17 UTC (rev
8286)
@@ -198,29 +198,24 @@
return global_pose;
}
+void Model::VelocityEnable()
+{
+ velocity_enable = true;
+ world->active_velocity.insert( this );
+}
+void Model::VelocityDisable()
+{
+ velocity_enable = false;
+ world->active_velocity.erase( this );
+}
+
void Model::SetVelocity( const Velocity& val )
{
-// assert( ! isnan(vel.x) );
-// assert( ! isnan(vel.y) );
-// assert( ! isnan(vel.z) );
-// assert( ! isnan(vel.a) );
+ velocity = val;
- velocity = val;
-
-// if( on_velocity_list && vel.IsZero() )
-// {
-// world->VelocityListRemove( this );
-// on_velocity_list = false;
-// }
-
-// if( (!on_velocity_list) && (!vel.IsZero()) )
-// {
-// world->VelocityListAdd( this );
-// on_velocity_list = true;
-// }
-
- CallCallbacks( &velocity );
+ if( hooks.attached_velocity )
+ CallCallbacks( &velocity );
}
@@ -242,5 +237,6 @@
world->dirty = true;
}
- CallCallbacks( &this->pose );
+ if( hooks.attached_pose )
+ CallCallbacks( &this->pose );
}
Modified: code/stage/trunk/libstage/model_load.cc
===================================================================
--- code/stage/trunk/libstage/model_load.cc 2009-10-11 01:57:26 UTC (rev
8285)
+++ code/stage/trunk/libstage/model_load.cc 2009-10-13 03:20:17 UTC (rev
8286)
@@ -198,7 +198,9 @@
this->SetVelocity( vel );
//StartUpdating();
}
-
+
+ velocity_enable = wf->ReadInt( wf_entity, "enable_velocity",
velocity_enable );
+
if( wf->PropertyExists( wf_entity, "friction" ))
{
this->SetFriction( wf->ReadFloat(wf_entity, "friction", this->friction ));
Modified: code/stage/trunk/libstage/model_position.cc
===================================================================
--- code/stage/trunk/libstage/model_position.cc 2009-10-11 01:57:26 UTC (rev
8285)
+++ code/stage/trunk/libstage/model_position.cc 2009-10-13 03:20:17 UTC (rev
8286)
@@ -48,6 +48,9 @@
odom_error [0.03 0.03 0.00 0.05]
# model properties
+
+ # update position according to the current velocity state
+ velocity_enable 1
)
@endverbatim
@@ -99,7 +102,10 @@
// assert that Update() is reentrant for this derived model
thread_safe = false;
-
+
+ // position devices respond to velocity settings by default
+ velocity_enable = true;
+
this->SetBlobReturn( true );
AddVisualizer( &wpvis, true );
Modified: code/stage/trunk/libstage/region.hh
===================================================================
--- code/stage/trunk/libstage/region.hh 2009-10-11 01:57:26 UTC (rev 8285)
+++ code/stage/trunk/libstage/region.hh 2009-10-13 03:20:17 UTC (rev 8286)
@@ -48,10 +48,14 @@
blocks()
{
}
-
- inline void RemoveBlock( Block* b );
- inline void AddBlock( Block* b );
- inline void AddBlockNoRecord( Block* b );
+
+ // virtual void RemoveBlock( Block* b )
+ // {
+ // EraseAll( b, blocks );
+ // }
+
+ //inline void AddBlock( Block* b );
+ //inline void AddBlockNoRecord( Block* b );
};
class Region
Modified: code/stage/trunk/libstage/stage.hh
===================================================================
--- code/stage/trunk/libstage/stage.hh 2009-10-11 01:57:26 UTC (rev 8285)
+++ code/stage/trunk/libstage/stage.hh 2009-10-13 03:20:17 UTC (rev 8286)
@@ -980,7 +980,7 @@
//void Execute();
//static const char* TypeStr( type_t type );
bool operator<( const Event& other ) const;
- bool operator==( const Event& other ) const;
+ // bool operator==( const Event& other ) const;
};
std::vector<std::priority_queue<Event> > event_queues;
@@ -1724,6 +1724,21 @@
int startup;
int update;
int update_done;
+
+ /* optimization: record the number of attached callbacks for
pose
+ and velocity, so we can cheaply determine
whether we need to
+ call a callback for SetPose() and
SetVelocity(), which happen
+ very frequently. */
+ int attached_velocity;
+ int attached_pose;
+ int attached_update;
+
+ CallbackHooks() :
+ attached_velocity(0),
+ attached_pose(0),
+ attached_update(0)
+ {}
+
} hooks;
/** unique process-wide identifier for this model */
@@ -1818,11 +1833,17 @@
//stg_model_type_t type;
const std::string type;
/** The index into the world's vector of event queues. Initially
- -1, to indicate that it is not on a list yet. */
- unsigned int event_queue_num;
- bool used; ///< TRUE iff this model has been returned by
GetUnusedModelOfType()
- Velocity velocity;
- stg_watts_t watts;///< power consumed by this model
+ -1, to indicate that it is not on a list yet. */
+ unsigned int event_queue_num;
+ bool used; ///< TRUE iff this model has been returned by
GetUnusedModelOfType()
+ Velocity velocity;
+
+ /** respond to velocity state by changing position. Initially
+ false, set to true by subclass, worldfile, or
explcicit call
+ to Model::VelocityEnable(). */
+ bool velocity_enable;
+
+ stg_watts_t watts;///< power consumed by this model
/** If >0, this model can transfer energy to models that have
watts_take >0 */
@@ -2141,6 +2162,12 @@
/** set a model's velocity in its parent's coordinate system */
void SetVelocity( const Velocity& vel );
+ /** Enable update of model pose according to velocity state */
+ void VelocityEnable();
+
+ /** Disable update of model pose according to velocity state */
+ void VelocityDisable();
+
/** set a model's pose in its parent's coordinate system */
void SetPose( const Pose& pose );
@@ -2325,14 +2352,15 @@
Pose LocalToGlobal( const Pose& pose ) const
{
return( ( GetGlobalPose() + geom.pose ) + pose );
- }
+ }
- // /** Return the 3d point in world coordinates of a 3d point
- // specified in the model's local coordinate system */
- // stg_point3_t LocalToGlobal( const stg_point3_t local ) const;
+ /** Fill an array of global pixels from an array of local
points. */
+ void LocalToPixels( const std::vector<stg_point_t>& local,
+
std::vector<stg_point_int_t>& pixels) const;
+
/** Return the 2d point in world coordinates of a 2d point
specified in the model's local coordinate system */
- stg_point_t LocalToGlobal( const stg_point_t& pt) const;
+ stg_point_t LocalToGlobal( const stg_point_t& pt) const;
/** returns the first descendent of this model that is unsubscribed
and has the type indicated by the string */
@@ -2919,7 +2947,7 @@
/** Set the current pose estimate.*/
void SetOdom( Pose odom );
-
+
/** Sets the control_mode to CONTROL_VELOCITY and sets
the goal velocity. */
void SetSpeed( double x, double y, double a );
Modified: code/stage/trunk/libstage/world.cc
===================================================================
--- code/stage/trunk/libstage/world.cc 2009-10-11 01:57:26 UTC (rev 8285)
+++ code/stage/trunk/libstage/world.cc 2009-10-13 03:20:17 UTC (rev 8286)
@@ -1064,26 +1064,8 @@
event_queues[queue_num].push( Event( sim_time + delay, mod ) );
}
-
bool World::Event::operator<( const Event& other ) const
{
- // sort by time, type, then model, in that order.
-
- if( time > other.time )
- return true;
-
-// if( time == other.time )
-// {
-// if( type > other.type )
-// return true;
-
-// if( type == other.type )
-// {
-// if( mod < other.mod ) // tends to do children first
-// return true;
-// }
-// }
-
- return false;
+ return( time > other.time );
}
Modified: code/stage/trunk/worlds/fasr.world
===================================================================
--- code/stage/trunk/worlds/fasr.world 2009-10-11 01:57:26 UTC (rev 8285)
+++ code/stage/trunk/worlds/fasr.world 2009-10-13 03:20:17 UTC (rev 8286)
@@ -90,7 +90,6 @@
)
-
charge_station( pose [ 7.940 -2.000 0 0 ] )
charge_station( pose [ 7.940 -3.000 0 0 ] )
charge_station( pose [ 7.940 -4.000 0 0 ] )
@@ -102,7 +101,7 @@
joules 100000
joules_capacity 400000
fiducial_return 0
- charging_bump( fiducial( range_max 3 pose [ 0 0 -0.100 0 ] ) )
+ charging_bump( fiducial( range_max 5 pose [ 0 0 -0.100 0 ] ) )
)
autorob( pose [7.062 -1.563 0 152.684] joules 300000 name "r0" )
Modified: code/stage/trunk/worlds/pioneer_flocking.world
===================================================================
--- code/stage/trunk/worlds/pioneer_flocking.world 2009-10-11 01:57:26 UTC
(rev 8285)
+++ code/stage/trunk/worlds/pioneer_flocking.world 2009-10-13 03:20:17 UTC
(rev 8286)
@@ -13,7 +13,7 @@
resolution 0.1
# this is very helpful if you have multiple CPUs - a good value is $(number of
CPU cores) -
-threads 8
+threads 2
# configure the GUI window
window
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Come build with us! The BlackBerry(R) Developer Conference in SF, CA
is the only developer event you need to attend this year. Jumpstart your
developing skills, take BlackBerry mobile applications to market and stay
ahead of the curve. Join us from November 9 - 12, 2009. Register now!
http://p.sf.net/sfu/devconference
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
