From: <rt...@us...> - 2009-05-22 02:52:29
|
Revision: 7707 http://playerstage.svn.sourceforge.net/playerstage/?rev=7707&view=rev Author: rtv Date: 2009-05-22 02:52:17 +0000 (Fri, 22 May 2009) Log Message: ----------- moved bit to STL Modified Paths: -------------- code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/region.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2009-05-21 16:46:22 UTC (rev 7706) +++ code/stage/trunk/libstage/block.cc 2009-05-22 02:52:17 UTC (rev 7707) @@ -21,8 +21,8 @@ pts( (stg_point_t*)g_memdup( pts, pt_count * sizeof(stg_point_t)) ), color( color ), inherit_color( inherit_color ), - rendered_cells( g_ptr_array_sized_new(32) ), - candidate_cells( g_ptr_array_sized_new(32) ) + rendered_cells( new std::vector<Cell*> ), + candidate_cells( new std::vector<Cell*> ) { assert( mod ); assert( pt_count > 0 ); @@ -42,8 +42,8 @@ pts(NULL), color(0), inherit_color(true), - rendered_cells( g_ptr_array_sized_new(32) ), - candidate_cells( g_ptr_array_sized_new(32) ) + rendered_cells( new std::vector<Cell*> ), + candidate_cells( new std::vector<Cell*> ) { assert(mod); assert(wf); @@ -59,8 +59,8 @@ if( pts ) delete[] pts; InvalidateModelPointCache(); - g_ptr_array_free( rendered_cells, TRUE ); - g_ptr_array_free( candidate_cells, TRUE ); + delete rendered_cells; + delete candidate_cells; } void Block::Translate( double x, double y ) @@ -143,9 +143,10 @@ GList* Block::AppendTouchingModels( GList* l ) { // for every cell we are rendered into - for( unsigned int i=0; i<rendered_cells->len; i++ ) + for( unsigned int i=0; i<rendered_cells->size(); i++ ) { - Cell* c = (Cell*)g_ptr_array_index( rendered_cells, i); + //Cell* c = (Cell*)g_ptr_array_index( rendered_cells, i); + Cell* c = (*rendered_cells)[i]; // for every block rendered into that cell for( std::list<Block*>::iterator it = c->blocks.begin(); @@ -173,10 +174,11 @@ if( mod->vis.obstacle_return ) // for every cell we may be rendered into - for( unsigned int i=0; i<candidate_cells->len; i++ ) + for( unsigned int i=0; i<candidate_cells->size(); i++ ) { - Cell* c = (Cell*)g_ptr_array_index(candidate_cells, i); - + //Cell* c = (Cell*)g_ptr_array_index(candidate_cells, i); + Cell* c = (*candidate_cells)[i]; + // for every rendered into that cell for( std::list<Block*>::iterator it = c->blocks.begin(); it != c->blocks.end(); @@ -202,26 +204,9 @@ } -void Block::RemoveFromCellArray( GPtrArray* ptrarray ) -{ - for( unsigned int i=0; i<ptrarray->len; i++ ) - ((Cell*)g_ptr_array_index(ptrarray, i))->RemoveBlock( this ); -} -void Block::AddToCellArray( GPtrArray* ptrarray ) -{ - for( unsigned int i=0; i<ptrarray->len; i++ ) - ((Cell*)g_ptr_array_index(ptrarray, i))->AddBlock( this ); -} - // used as a callback to gather an array of cells in a polygon -void AppendCellToPtrArray( Cell* c, GPtrArray* a ) -{ - g_ptr_array_add( a, c ); -} - -// used as a callback to gather an array of cells in a polygon void AddBlockToCell( Cell* c, Block* block ) { c->AddBlock( block ); @@ -241,17 +226,34 @@ { RemoveFromCellArray( rendered_cells ); - g_ptr_array_set_size( rendered_cells, 0 ); + //g_ptr_array_set_size( rendered_cells, 0 ); + rendered_cells->clear(); mapped = false; } +void Block::RemoveFromCellArray( std::vector<Cell*> * cells ) +{ + for( std::vector<Cell*>::iterator it = cells->begin(); + it != cells->end(); + ++it ) + (*it)->RemoveBlock( this); +} + +void Block::AddToCellArray( std::vector<Cell*> * cells ) +{ + for( std::vector<Cell*>::iterator it = cells->begin(); + it != cells->end(); + ++it ) + (*it)->AddBlock( this); +} + void Block::SwitchToTestedCells() { RemoveFromCellArray( rendered_cells ); AddToCellArray( candidate_cells ); // switch current and candidate cell pointers - GPtrArray* tmp = rendered_cells; + std::vector<Cell*> * tmp = rendered_cells; rendered_cells = candidate_cells; candidate_cells = tmp; @@ -291,6 +293,12 @@ } } +// callback used below +static void AppendCellToVector( Cell* c, std::vector<Cell*> * a ) +{ + a->push_back( c ); +} + void Block::GenerateCandidateCells() { stg_point_t* mpts = GetPointsInModelCoords(); @@ -300,11 +308,12 @@ for( unsigned int i=0; i<pt_count; i++ ) gpts[i] = mod->LocalToGlobal( mpts[i] ); - g_ptr_array_set_size( candidate_cells, 0 ); + //g_ptr_array_set_size( candidate_cells, 0 ); + candidate_cells->clear(); mod->world-> ForEachCellInPolygon( gpts, pt_count, - (stg_cell_callback_t)AppendCellToPtrArray, + (stg_cell_callback_t)AppendCellToVector, candidate_cells ); delete[] gpts; Modified: code/stage/trunk/libstage/region.cc =================================================================== --- code/stage/trunk/libstage/region.cc 2009-05-21 16:46:22 UTC (rev 7706) +++ code/stage/trunk/libstage/region.cc 2009-05-22 02:52:17 UTC (rev 7707) @@ -69,7 +69,7 @@ // outline regions with contents glRecti( x<<RBITS, y<<RBITS, (x+1)<<RBITS, (y+1)<<RBITS ); - else// if( r->cells ) + else if( r->cells ) { double left = x << RBITS; double right = (x+1) << RBITS; Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-05-21 16:46:22 UTC (rev 7706) +++ code/stage/trunk/libstage/stage.hh 2009-05-22 02:52:17 UTC (rev 7707) @@ -902,7 +902,6 @@ { friend class Model; // allow access to private members friend class Block; - //friend class StgTime; friend class Canvas; private: @@ -1197,7 +1196,9 @@ void SetZ( double min, double max ); void RecordRendering( Cell* cell ) - { g_ptr_array_add( rendered_cells, (gpointer)cell ); }; + { //g_ptr_array_add( rendered_cells, (gpointer)cell ); }; + rendered_cells->push_back( cell ); + } stg_point_t* Points( unsigned int *count ) { if( count ) *count = pt_count; return pts; }; @@ -1205,11 +1206,8 @@ //bool IntersectGlobalZ( stg_meters_t z ) //{ return( z >= global_zmin && z <= global_zmax ); } - void AddToCellArray( GPtrArray* ptrarray ); - void RemoveFromCellArray( GPtrArray* ptrarray ); - - //void AddToCellArray( std::vector<Cell*>& blocks ); - //void RemoveFromCellArray( std::vector<Cell*>& blocks ); + void AddToCellArray( std::vector<Cell*>* blocks ); + void RemoveFromCellArray( std::vector<Cell*>* blocks ); void GenerateCandidateCells(); @@ -1261,18 +1259,18 @@ bool mapped; - /** an array of pointers to cells into which this block has been - rendered (speeds up UnMapping) */ - GPtrArray* rendered_cells; - + /** record the cells into which this block has been rendered to + UnMapping them very quickly. */ + std::vector<Cell*> * 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).*/ - GPtrArray* candidate_cells; - + std::vector<Cell*> * candidate_cells; + /** find the position of a block's point in model coordinates (m) */ stg_point_t BlockPointToModelMeters( const stg_point_t& bpt ); Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-05-21 16:46:22 UTC (rev 7706) +++ code/stage/trunk/libstage/world.cc 2009-05-22 02:52:17 UTC (rev 7707) @@ -1006,6 +1006,9 @@ ->GetCellGlobalCreate( glob )) ; } +// TODO - each line end point is processed twice here - could save a +// teeny bit of time if we did this more cleverly +// also - replace C arrays with vectors? void World::ForEachCellInPolygon( const stg_point_t pts[], const unsigned int pt_count, stg_cell_callback_t cb, @@ -1048,6 +1051,10 @@ { // find or create the cell at this location, then call the callback // with the cell, block and user-defined argument + + // TODO - could get rid of this callback, as we only ever use + // one function here we can speed it up a bit by having the code + // inline (*cb)( GetCellCreate( x,y ), cb_arg ); // cleverly skip to the next cell This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-05-22 08:06:13
|
Revision: 7708 http://playerstage.svn.sourceforge.net/playerstage/?rev=7708&view=rev Author: rtv Date: 2009-05-22 08:06:08 +0000 (Fri, 22 May 2009) Log Message: ----------- optimizing block.cc Modified Paths: -------------- code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2009-05-22 02:52:17 UTC (rev 7707) +++ code/stage/trunk/libstage/block.cc 2009-05-22 08:06:08 UTC (rev 7708) @@ -293,30 +293,22 @@ } } -// callback used below -static void AppendCellToVector( Cell* c, std::vector<Cell*> * a ) -{ - a->push_back( c ); -} - void Block::GenerateCandidateCells() { - stg_point_t* mpts = GetPointsInModelCoords(); - - // convert the mpts in model coords into global coords - stg_point_t* gpts = new stg_point_t[pt_count]; - for( unsigned int i=0; i<pt_count; i++ ) - gpts[i] = mod->LocalToGlobal( mpts[i] ); - - //g_ptr_array_set_size( candidate_cells, 0 ); candidate_cells->clear(); - mod->world-> - ForEachCellInPolygon( gpts, pt_count, - (stg_cell_callback_t)AppendCellToVector, - candidate_cells ); - delete[] gpts; + stg_point_t* mpts = GetPointsInModelCoords(); + // convert the mpts in model coords into global pixel coords + stg_point_int_t gpts[pt_count]; + 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 ); + // set global Z Pose gpose = mod->GetGlobalPose(); gpose.z += mod->geom.pose.z; Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-05-22 02:52:17 UTC (rev 7707) +++ code/stage/trunk/libstage/model.cc 2009-05-22 08:06:08 UTC (rev 7708) @@ -117,9 +117,9 @@ // static members uint32_t Model::count = 0; -GHashTable* Model::modelsbyid = g_hash_table_new( NULL, NULL ); +//GHashTable* Model::modelsbyid = g_hash_table_new( NULL, NULL ); +std::map<stg_id_t,Model*> Model::modelsbyid; - void Size::Load( Worldfile* wf, int section, const char* keyword ) { x = wf->ReadTupleLength( section, keyword, 0, x ); @@ -237,7 +237,7 @@ world(world), world_gui( dynamic_cast<WorldGui*>( world ) ) { - assert( modelsbyid ); + //assert( modelsbyid ); assert( world ); PRINT_DEBUG3( "Constructing model world: %s parent: %s type: %d ", @@ -245,7 +245,8 @@ parent ? parent->Token() : "(null)", type ); - g_hash_table_insert( modelsbyid, (void*)id, this ); + //g_hash_table_insert( modelsbyid, (void*)id, this ); + modelsbyid[id] = this; // Adding this model to its ancestor also gives this model a // sensible default name @@ -286,7 +287,8 @@ g_datalist_clear( &props ); - g_hash_table_remove( Model::modelsbyid, (void*)id ); + //g_hash_table_remove( Model::modelsbyid, (void*)id ); + modelsbyid.erase(id); world->RemoveModel( this ); } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-05-22 02:52:17 UTC (rev 7707) +++ code/stage/trunk/libstage/stage.hh 2009-05-22 08:06:08 UTC (rev 7708) @@ -46,7 +46,6 @@ #include <vector> #include <list> #include <map> -//#include <pair> // we use GLib's data structures extensively. Perhaps we'll move to // C++ STL types to lose this dependency one day. @@ -996,22 +995,22 @@ inline Cell* GetCellNoCreate( const stg_point_int_t& glob ); inline Cell* GetCellNoCreate( const int32_t x, const int32_t y ); - inline Cell* GetCellCreate( const int32_t x, const int32_t y ); + //inline Cell* GetCellCreate( const int32_t x, const int32_t y ); + inline Cell* GetCellCreate( const stg_point_int_t& glob ); - void ForEachCellInPolygon( const stg_point_t pts[], - const unsigned int pt_count, - stg_cell_callback_t cb, - void* cb_arg ); - - void ForEachCellInLine( const stg_point_t& pt1, - const stg_point_t& pt2, - stg_cell_callback_t cb, - void* cb_arg ); - + /** add a Cell pointer to the vector for each cell on the line from + pt1 to pt2 inclusive */ + void ForEachCellInLine( const stg_point_int_t& pt1, + const stg_point_int_t& pt2, + std::vector<Cell*>& cells ); + /** convert a distance in meters to a distance in world occupancy grid pixels */ int32_t MetersToPixels( stg_meters_t x ) { return (int32_t)floor(x * ppm); }; + + stg_point_int_t MetersToPixels( const stg_point_t& pt ) + { return stg_point_int_t( MetersToPixels(pt.x), MetersToPixels(pt.y)); }; // dummy implementations to be overloaded by GUI subclasses virtual void PushColor( stg_color_t col ) { /* do nothing */ }; @@ -1748,7 +1747,8 @@ private: /** the number of models instatiated - used to assign unique IDs */ static uint32_t count; - static GHashTable* modelsbyid; + //static GHashTable* modelsbyid; + static std::map<stg_id_t,Model*> modelsbyid; std::vector<Option*> drawOptions; const std::vector<Option*>& getOptions() const { return drawOptions; } @@ -2046,7 +2046,6 @@ { world->PushColor( r,g,b,a ); } virtual void PopColor(){ world->PopColor(); } - PowerPack* FindPowerPack() const; @@ -2060,7 +2059,8 @@ /** Look up a model pointer by a unique model ID */ static Model* LookupId( uint32_t id ) - { return (Model*)g_hash_table_lookup( modelsbyid, (void*)id ); } + //{ return (Model*)g_hash_table_lookup( modelsbyid, (void*)id ); } + { return modelsbyid[id]; } /** Constructor */ Model( World* world, Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-05-22 02:52:17 UTC (rev 7707) +++ code/stage/trunk/libstage/world.cc 2009-05-22 08:06:08 UTC (rev 7708) @@ -995,41 +995,35 @@ return NULL; } -inline Cell* World::GetCellCreate( const int32_t x, const int32_t y ) -{ - stg_point_int_t glob( x, y ); +// inline Cell* World::GetCellCreate( const int32_t x, const int32_t y ) +// { +// stg_point_int_t glob( x, y ); - //printf( "GC[ %d %d ] ", glob.x, glob.y ); +// //printf( "GC[ %d %d ] ", glob.x, glob.y ); - return( GetSuperRegionCached( GETSREG(x), GETSREG(y) ) +// return( GetSuperRegionCached( GETSREG(x), GETSREG(y) ) +// ->GetRegionGlobal( glob ) +// ->GetCellGlobalCreate( glob )) ; +// } + +inline Cell* World::GetCellCreate( const stg_point_int_t& glob ) +{ + return( GetSuperRegionCached( GETSREG(glob.x), GETSREG(glob.y) ) ->GetRegionGlobal( glob ) ->GetCellGlobalCreate( glob )) ; } -// TODO - each line end point is processed twice here - could save a -// teeny bit of time if we did this more cleverly -// also - replace C arrays with vectors? -void World::ForEachCellInPolygon( const stg_point_t pts[], - const unsigned int pt_count, - stg_cell_callback_t cb, - void* cb_arg ) -{ - for( unsigned int i=0; i<pt_count; i++ ) - ForEachCellInLine( pts[i], pts[(i+1)%pt_count], cb, cb_arg ); - -} -void World::ForEachCellInLine( const stg_point_t& pt1, - const stg_point_t& pt2, - stg_cell_callback_t cb, - void* cb_arg ) +void World::ForEachCellInLine( const stg_point_int_t& start, + const stg_point_int_t& end, + std::vector<Cell*>& cells ) { - int x = MetersToPixels( pt1.x ); // global pixel coords - int y = MetersToPixels( pt1.y ); + + int dx = end.x - start.x; + int dy = end.y - start.y; + + stg_point_int_t cell = start; - int dx = MetersToPixels( pt2.x - pt1.x ); - int dy = MetersToPixels( pt2.y - pt1.y ); - // line rasterization adapted from Cohen's 3D version in // Graphics Gems II. Should be very fast. @@ -1049,23 +1043,18 @@ while( n-- ) { - // find or create the cell at this location, then call the callback - // with the cell, block and user-defined argument + // find or create the cell at this location, then add it to the vector + cells.push_back( GetCellCreate( cell ) ); - // TODO - could get rid of this callback, as we only ever use - // one function here we can speed it up a bit by having the code - // inline - (*cb)( GetCellCreate( x,y ), cb_arg ); - // cleverly skip to the next cell if( exy < 0 ) { - x += sx; + cell.x += sx; exy += by; } else { - y += sy; + cell.y += sy; exy -= bx; } } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-06-03 07:39:34
|
Revision: 7768 http://playerstage.svn.sourceforge.net/playerstage/?rev=7768&view=rev Author: rtv Date: 2009-06-03 07:38:57 +0000 (Wed, 03 Jun 2009) Log Message: ----------- tweaking ray tracing Modified Paths: -------------- code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2009-06-03 06:28:26 UTC (rev 7767) +++ code/stage/trunk/libstage/CMakeLists.txt 2009-06-03 07:38:57 UTC (rev 7768) @@ -4,6 +4,7 @@ include_directories(${PROJECT_BINARY_DIR}) set( stageSrcs + world.cc ancestor.cc block.cc blockgroup.cc @@ -37,7 +38,6 @@ typetable.cc vis_strip.cc waypoint.cc - world.cc worldfile.cc worldgui.cc canvas.cc Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2009-06-03 06:28:26 UTC (rev 7767) +++ code/stage/trunk/libstage/model_laser.cc 2009-06-03 07:38:57 UTC (rev 7768) @@ -190,7 +190,7 @@ vis( world ), sample_count( DEFAULT_SAMPLES ), samples(), - rays(), + //ray( this, ), range_max( DEFAULT_MAXRANGE ), fov( DEFAULT_FOV ), resolution( DEFAULT_RESOLUTION ) @@ -279,59 +279,43 @@ void ModelLaser::SampleConfig() { samples.resize( sample_count ); - rays.resize( sample_count ); - - for( unsigned int t=0; t<sample_count; ++t ) - { - // configure a normal ray with our laser-specific settings - rays[t].func = laser_raytrace_match; - rays[t].arg = NULL; - rays[t].ztest = true; - rays[t].range = range_max; - rays[t].mod = this; - } } void ModelLaser::Update( void ) { assert( samples.size() == sample_count ); - assert( rays.size() == sample_count ); + //assert( rays.size() == sample_count ); double bearing = -fov/2.0; // make the first and last rays exactly at the extremes of the FOV double sample_incr = fov / MAX(sample_count-1,1); + // find the global origin of our first emmitted ray Pose rayorg = geom.pose; rayorg.z += geom.size.z/2.0; rayorg.a = bearing;// + sample_incr/2.0; rayorg = LocalToGlobal(rayorg); - - // set up the ray origins in global coords + + // set up a ray to trace + Ray ray( this, rayorg, range_max, laser_raytrace_match, NULL, true ); + + // trace the ray, incrementing its heading for each sample for( unsigned int t=0; t<sample_count; t += resolution ) { - rays[t].origin = rayorg; - rayorg.a += sample_incr; - } - - // do the raytracing of all rays in one go (allows parallel implementation) - world->Raytrace( rays ); - - // now process the results and fill in our samples - for( unsigned int t=0; t<sample_count; t += resolution ) - { - stg_raytrace_result_t* r = &rays[t].result; - assert( r ); - - samples[t].range = r->range; - + stg_raytrace_result_t r = ray.Trace(); + samples[t].range = r.range; + // if we hit a model and it reflects brightly, we set // reflectance high, else low - if( r->mod && ( r->mod->vis.laser_return >= LaserBright ) ) - samples[t].reflectance = 1; + if( r.mod && ( r.mod->vis.laser_return >= LaserBright ) ) + samples[t].reflectance = 1; else - samples[t].reflectance = 0; + samples[t].reflectance = 0; + + // point the ray to the next angle + ray.origin.a += sample_incr; } - + // we may need to interpolate the samples we skipped if( resolution > 1 ) { Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-06-03 06:28:26 UTC (rev 7767) +++ code/stage/trunk/libstage/stage.hh 2009-06-03 07:38:57 UTC (rev 7768) @@ -851,13 +851,21 @@ class Ray { public: - Model* mod; + Ray( const Model* mod, const Pose& origin, const stg_meters_t range, const stg_ray_test_func_t func, const void* arg, const bool ztest ) : + mod(mod), origin(origin), range(range), func(func), arg(arg), ztest(ztest) + {} + + Ray() : mod(NULL), origin(0,0,0,0), range(0), func(NULL), arg(NULL), ztest(true) + {} + + const Model* mod; Pose origin; stg_meters_t range; stg_ray_test_func_t func; - void* arg; + const void* arg; bool ztest; - RaytraceResult result; + + RaytraceResult Trace(); }; const uint32_t INTERVAL_LOG_LEN = 32; @@ -1013,12 +1021,9 @@ SuperRegion* CreateSuperRegion( stg_point_int_t origin ); void DestroySuperRegion( SuperRegion* sr ); - - /** trace a vector of rays all in one go. */ - void Raytrace( std::vector<Ray>& rays ); - + /** trace a ray. */ - void Raytrace( Ray& ray ); + stg_raytrace_result_t Raytrace( const Ray& ray ); stg_raytrace_result_t Raytrace( const Pose& pose, const stg_meters_t range, @@ -1737,6 +1742,7 @@ friend class Region; friend class BlockGroup; friend class PowerPack; + friend class Ray; private: /** the number of models instatiated - used to assign unique IDs */ @@ -2499,8 +2505,9 @@ unsigned int sample_count; std::vector<Sample> samples; - std::vector<Ray> rays; - + //std::vector<Ray> rays; + //Ray ray; + stg_meters_t range_max; stg_radians_t fov; uint32_t resolution; Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-06-03 06:28:26 UTC (rev 7767) +++ code/stage/trunk/libstage/world.cc 2009-06-03 07:38:57 UTC (rev 7768) @@ -54,6 +54,11 @@ using namespace Stg; +RaytraceResult Ray::Trace() +{ + return mod->world->Raytrace( origin, range, func, mod, arg, ztest ); +} + // static data members unsigned int World::next_id = 0; bool World::quit_all = false; @@ -597,24 +602,6 @@ } -void World::Raytrace( std::vector<Ray>& rays ) -{ - rt_cells.clear(); - rt_candidate_cells.clear(); - - //printf( "===================\n" ); - - for( std::vector<Ray>::iterator it = rays.begin(); - it != rays.end(); - ++it ) - Raytrace( *it ); -} - -inline void World::Raytrace( Ray& r ) -{ - r.result = Raytrace( r.origin, r.range, r.func, r.mod, r.arg, r.ztest ); -} - void World::Raytrace( const Pose &gpose, // global pose const stg_meters_t range, const stg_radians_t fov, @@ -643,33 +630,39 @@ // 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 ); +} + +stg_raytrace_result_t World::Raytrace( const Ray& r ) +{ //rt_cells.clear(); //rt_candidate_cells.clear(); // initialize the sample - RaytraceResult sample( gpose, range ); + RaytraceResult sample( r.origin, r.range ); // our global position in (floating point) cell coordinates - stg_point_t glob( gpose.x * ppm, gpose.y * ppm ); + stg_point_t glob( r.origin.x * ppm, r.origin.y * ppm ); // record our starting position const stg_point_int_t start( glob.x, glob.y ); // eliminate a potential divide by zero - const double angle( gpose.a == 0.0 ? 1e-12 : gpose.a ); + const double angle( r.origin.a == 0.0 ? 1e-12 : r.origin.a ); const double cosa(cos(angle)); const double sina(sin(angle)); const double tana(sina/cosa); // = tan(angle) // and the x and y offsets of the ray - const int dx( ppm * range * cosa); - const int dy( ppm * range * sina); + const int dx( ppm * r.range * cosa); + const int dy( ppm * r.range * sina); // fast integer line 3d algorithm adapted from Cohen's code from // Graphics Gems IV @@ -719,9 +712,9 @@ // coordinates of the region inside the superregion int32_t rx = GETREG(glob.x); int32_t ry = GETREG(glob.y); - Region* r = &sr->regions[ rx + (ry*SuperRegion::WIDTH) ]; + Region* reg = &sr->regions[ rx + (ry*SuperRegion::WIDTH) ]; - if( r->count ) // if the region contains any objects + if( reg->count ) // if the region contains any objects { // invalidate the region crossing points used to jump over // empty regions @@ -731,7 +724,7 @@ int32_t cx = GETCELL(glob.x); int32_t cy = GETCELL(glob.y); - Cell* c = &r->cells[ cx + cy * Region::WIDTH ]; + Cell* c = ®->cells[ cx + cy * Region::WIDTH ]; assert(c); // should be a cell there // while within the bounds of this region and while some ray remains @@ -749,13 +742,13 @@ Block* block = *it; // skip if not in the right z range - if( ztest && - ( gpose.z < block->global_z.min || - gpose.z > block->global_z.max ) ) + 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( (*func)( block->mod, (Model*)mod, arg )) + if( (*r.func)( block->mod, (Model*)r.mod, r.arg )) { // a hit! sample.color = block->GetColor(); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-06-03 07:54:03
|
Revision: 7769 http://playerstage.svn.sourceforge.net/playerstage/?rev=7769&view=rev Author: rtv Date: 2009-06-03 07:53:46 +0000 (Wed, 03 Jun 2009) Log Message: ----------- tweaking ray tracing Modified Paths: -------------- code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2009-06-03 07:38:57 UTC (rev 7768) +++ code/stage/trunk/libstage/model_laser.cc 2009-06-03 07:53:46 UTC (rev 7769) @@ -302,7 +302,7 @@ // trace the ray, incrementing its heading for each sample for( unsigned int t=0; t<sample_count; t += resolution ) { - stg_raytrace_result_t r = ray.Trace(); + stg_raytrace_result_t r = world->Raytrace( ray ); samples[t].range = r.range; // if we hit a model and it reflects brightly, we set Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-06-03 07:38:57 UTC (rev 7768) +++ code/stage/trunk/libstage/stage.hh 2009-06-03 07:53:46 UTC (rev 7769) @@ -864,8 +864,6 @@ stg_ray_test_func_t func; const void* arg; bool ztest; - - RaytraceResult Trace(); }; const uint32_t INTERVAL_LOG_LEN = 32; Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-06-03 07:38:57 UTC (rev 7768) +++ code/stage/trunk/libstage/world.cc 2009-06-03 07:53:46 UTC (rev 7769) @@ -53,12 +53,6 @@ #include "option.hh" using namespace Stg; - -RaytraceResult Ray::Trace() -{ - return mod->world->Raytrace( origin, range, func, mod, arg, ztest ); -} - // static data members unsigned int World::next_id = 0; bool World::quit_all = false; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-06-06 07:25:26
|
Revision: 7796 http://playerstage.svn.sourceforge.net/playerstage/?rev=7796&view=rev Author: rtv Date: 2009-06-06 07:24:49 +0000 (Sat, 06 Jun 2009) Log Message: ----------- cleaning up raytrace code in a bug hunt. performance dropped a little, traded for type safety Modified Paths: -------------- code/stage/trunk/libstage/region.cc code/stage/trunk/libstage/region.hh code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/region.cc =================================================================== --- code/stage/trunk/libstage/region.cc 2009-06-06 01:27:44 UTC (rev 7795) +++ code/stage/trunk/libstage/region.cc 2009-06-06 07:24:49 UTC (rev 7796) @@ -7,18 +7,12 @@ #include "region.hh" using namespace Stg; -const uint32_t Region::WIDTH = REGIONWIDTH; -const uint32_t Region::SIZE = REGIONSIZE; -const uint32_t SuperRegion::WIDTH = SUPERREGIONWIDTH; -const uint32_t SuperRegion::SIZE = SUPERREGIONSIZE; - - Region::Region() - : cells(NULL), count(0) + : cells(), count(0) { - //for( unsigned int i=0; i<Region::SIZE; i++ ) - //cells[i].region = this; + for( int i=0; i<REGIONSIZE; i++ ) + cells[i].region = this; } Region::~Region() @@ -27,7 +21,20 @@ delete[] cells; } +void Region::DecrementOccupancy() +{ + assert( superregion ); + superregion->DecrementOccupancy(); + --count; +} +void Region::IncrementOccupancy() +{ + assert( superregion ); + superregion->IncrementOccupancy(); + ++count; +} + SuperRegion::SuperRegion( World* world, stg_point_int_t origin ) : count(0), origin(origin), world(world) { @@ -36,7 +43,7 @@ // printf( "superregion at %d %d\n", origin.x, origin.y ); // initialize the parent pointer for all my child regions - for( unsigned int i=0; i<SuperRegion::SIZE; i++ ) + for( int i=0; i<SUPERREGIONSIZE; i++ ) regions[i].superregion = this; } @@ -60,10 +67,10 @@ // outline regions glColor3f( 0,1,0 ); - for( unsigned int x=0; x<SuperRegion::WIDTH; x++ ) - for( unsigned int y=0; y<SuperRegion::WIDTH; y++ ) + for( int x=0; x<SUPERREGIONWIDTH; x++ ) + for( int y=0; y<SUPERREGIONWIDTH; y++ ) { - Region* r = GetRegionLocal(x,y); + const Region* r = GetRegion(x,y); if( r->count ) // outline regions with contents @@ -120,10 +127,10 @@ glColor3f( 1.0,0,0 ); - for( unsigned int x=0; x<SuperRegion::WIDTH; x++ ) - for( unsigned int y=0; y<SuperRegion::WIDTH; y++ ) + for( int x=0; x<SUPERREGIONWIDTH; x++ ) + for( int y=0; y<SUPERREGIONWIDTH; y++ ) { - Region* r = GetRegionLocal( x, y); + const Region* r = GetRegion( x, y); if( r->count < 1 ) continue; @@ -131,9 +138,9 @@ snprintf( buf, 15, "%lu", r->count ); Gl::draw_string( x<<RBITS, y<<RBITS, 0, buf ); - for( unsigned int p=0; p<Region::WIDTH; p++ ) - for( unsigned int q=0; q<Region::WIDTH; q++ ) - if( r->cells[p+(q*Region::WIDTH)].blocks.size() ) + for( int p=0; p<REGIONWIDTH; p++ ) + for( int q=0; q<REGIONWIDTH; q++ ) + if( r->cells[p+(q*REGIONWIDTH)].blocks.size() ) { GLfloat xx = p+(x<<RBITS); GLfloat yy = q+(y<<RBITS); @@ -145,12 +152,12 @@ } else // draw a rectangular solid { - Cell* c = &r->cells[p+(q*Region::WIDTH)]; + Cell* c = (Cell*)&r->cells[p+(q*REGIONWIDTH)]; for( std::vector<Block*>::iterator it = c->blocks.begin(); it != c->blocks.end(); ++it ) { - Block* block = *it;//(Block*)it->data; + Block* block = *it; //printf( "zb %.2f %.2f\n", ent->zbounds.min, ent->zbounds.max ); Modified: code/stage/trunk/libstage/region.hh =================================================================== --- code/stage/trunk/libstage/region.hh 2009-06-06 01:27:44 UTC (rev 7795) +++ code/stage/trunk/libstage/region.hh 2009-06-06 07:24:49 UTC (rev 7796) @@ -14,17 +14,27 @@ { // a bit of experimenting suggests that these values are fast. YMMV. -#define RBITS 4 // regions contain (2^RBITS)^2 pixels -#define SBITS 5 // superregions contain (2^SBITS)^2 regions -#define SRBITS (RBITS+SBITS) + const int32_t RBITS( 4 ); // regions contain (2^RBITS)^2 pixels + const int32_t SBITS( 5 );// superregions contain (2^SBITS)^2 regions + const int32_t SRBITS( RBITS+SBITS ); + + const int32_t REGIONWIDTH( 1<<RBITS ); + const int32_t REGIONSIZE( REGIONWIDTH*REGIONWIDTH ); -#define REGIONWIDTH (1<<RBITS) -#define REGIONSIZE REGIONWIDTH*REGIONWIDTH + const int32_t SUPERREGIONWIDTH( 1<<SBITS ); + const int32_t SUPERREGIONSIZE( SUPERREGIONWIDTH*SUPERREGIONWIDTH ); + + /** (x & CELLMASK) converts a global cell index into a local cell + index in a region */ + const int32_t CELLMASK( ~((~0x00)<< RBITS )); + /** (x & REGIONMASK)converts a global cell index into a local cell + index in a region */ + const int32_t REGIONMASK( ~((~0x00)<< SRBITS )); + + inline int32_t GETCELL( const int32_t x ) { return( x & CELLMASK); } + inline int32_t GETREG( const int32_t x ) { return( ( x & REGIONMASK ) >> RBITS); } + inline int32_t GETSREG( const int32_t x ) { return( x >> SRBITS); } -#define SUPERREGIONWIDTH (1<<SBITS) -#define SUPERREGIONSIZE SUPERREGIONWIDTH*SUPERREGIONWIDTH - - class Cell { friend class Region; @@ -44,87 +54,38 @@ { } - inline void RemoveBlock( Block* b ); - inline void AddBlock( Block* b ); - inline void AddBlockNoRecord( Block* b ); + void RemoveBlock( Block* b ); + void AddBlock( Block* b ); + void AddBlockNoRecord( Block* b ); }; - class Region { public: - Cell* cells; - SuperRegion* superregion; - - static const uint32_t WIDTH; - static const uint32_t SIZE; - - static int32_t CELL( const int32_t x ) - { - const int32_t _cell_coord_mask = ~ ( ( ~ 0x00 ) << RBITS ); - return( x & _cell_coord_mask ); - } - + Cell cells[ REGIONSIZE ]; + SuperRegion* superregion; unsigned long count; // number of blocks rendered into these cells Region(); ~Region(); - Cell* GetCellCreate( int32_t x, int32_t y ) + Cell* GetCell( int32_t x, int32_t y ) const { - if( ! cells ) - { - cells = new Cell[REGIONSIZE]; - for( unsigned int i=0; i<Region::SIZE; i++ ) - cells[i].region = this; - } - return( &cells[CELL(x) + (CELL(y)*Region::WIDTH)] ); + return( (Cell*)&cells[ x + (y*REGIONWIDTH) ] ); } - - Cell* GetCellGlobalCreate( const stg_point_int_t& c ) - { - if( ! cells ) - { - cells = new Cell[REGIONSIZE]; - for( unsigned int i=0; i<Region::SIZE; i++ ) - cells[i].region = this; - } - return( &cells[CELL(c.x) + (CELL(c.y)*Region::WIDTH)] ); - } - - Cell* GetCellGlobalNoCreate( int32_t x, int32_t y ) const - { - return( &cells[CELL(x) + (CELL(y)*Region::WIDTH)] ); - } - - Cell* GetCellLocallNoCreate( int32_t x, int32_t y ) const - { - return( &cells[x + y*Region::WIDTH] ); - } - - - Cell* GetCellGlobalNoCreate( const stg_point_int_t& c ) const - { - return( &cells[CELL(c.x) + (CELL(c.y)*Region::WIDTH)] ); - } - - - - void DecrementOccupancy(); - void IncrementOccupancy(); + + void DecrementOccupancy(); + void IncrementOccupancy(); }; - - class SuperRegion + class SuperRegion { - friend class World; - friend class Model; + friend class World; + friend class Model; private: - static const uint32_t WIDTH; - static const uint32_t SIZE; - + Region regions[SUPERREGIONSIZE]; unsigned long count; // number of blocks rendered into these regions @@ -133,31 +94,14 @@ public: - static int32_t REGION( const int32_t x ) - { - const int32_t _region_coord_mask = ~ ( ( ~ 0x00 ) << SRBITS ); - return( ( x & _region_coord_mask ) >> RBITS ); - } - - SuperRegion( World* world, stg_point_int_t origin ); ~SuperRegion(); - - Region* GetRegionGlobal( int32_t x, int32_t y ) - { - return( ®ions[ REGION(x) + (REGION(y)*SuperRegion::WIDTH) ] ); - } - - Region* GetRegionLocal( int32_t x, int32_t y ) - { - return( ®ions[ x + (y*SuperRegion::WIDTH) ] ); - } - - Region* GetRegionGlobal( const stg_point_int_t& r ) - { - return( ®ions[ REGION(r.x) + (REGION(r.y)*SuperRegion::WIDTH) ] ); - } - + + const Region* GetRegion( int32_t x, int32_t y ) const + { + return( ®ions[ x + (y*SUPERREGIONWIDTH) ] ); + } + void Draw( bool drawall ); void Floor(); @@ -165,21 +109,8 @@ void IncrementOccupancy(){ ++count; }; }; -inline void Region::DecrementOccupancy() -{ - assert( superregion ); - superregion->DecrementOccupancy(); - --count; -}; -inline void Region::IncrementOccupancy() -{ - assert( superregion ); - superregion->IncrementOccupancy(); - ++count; -} - -inline void printvec( std::vector<Block*>& vec ) +inline void printvec( std::vector<Block*>& vec ) { printf( "Vec: "); for( size_t i=0; i<vec.size(); i++ ) Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-06-06 01:27:44 UTC (rev 7795) +++ code/stage/trunk/libstage/stage.hh 2009-06-06 07:24:49 UTC (rev 7796) @@ -999,10 +999,10 @@ SuperRegion* GetSuperRegionCached( int32_t x, int32_t y ); void ExpireSuperRegion( SuperRegion* sr ); - inline Cell* GetCellNoCreate( const stg_point_int_t& glob ); - inline Cell* GetCellNoCreate( const int32_t x, const int32_t y ); + inline Cell* GetCell( const stg_point_int_t& glob ); + //inline Cell* GetCellNoCreate( const int32_t x, const int32_t y ); //inline Cell* GetCellCreate( const int32_t x, const int32_t y ); - inline Cell* GetCellCreate( const stg_point_int_t& glob ); + //inline Cell* GetCellCreate( const stg_point_int_t& glob ); /** add a Cell pointer to the vector for each cell on the line from pt1 to pt2 inclusive */ Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-06-06 01:27:44 UTC (rev 7795) +++ code/stage/trunk/libstage/world.cc 2009-06-06 07:24:49 UTC (rev 7796) @@ -60,8 +60,8 @@ World::World( const char* token, - stg_msec_t interval_sim, - double ppm ) + stg_msec_t interval_sim, + double ppm ) : // private charge_list( NULL ), @@ -145,7 +145,7 @@ for( GList* it = World::world_list; it; it=it->next ) { if( ((World*)it->data)->Update() == false ) - quit = false; + quit = false; } return quit; } @@ -156,7 +156,7 @@ 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 +181,7 @@ { // 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 +209,11 @@ //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 +224,7 @@ else { PRINT_ERR1( "Unknown model type %s in world file.", - typestr ); + typestr ); exit( 1 ); } @@ -239,10 +239,10 @@ 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 +285,11 @@ 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 +303,22 @@ 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 ); + 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 ); - } + printf( "[threadpool %u]", worker_threads ); + } } // Iterate through entitys and create objects of the appropriate type @@ -328,15 +328,15 @@ // 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 ); } @@ -352,7 +352,7 @@ 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' ); } @@ -431,10 +431,10 @@ 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 +443,7 @@ } 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 +451,21 @@ } 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 +477,17 @@ // 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() @@ -520,35 +520,35 @@ { // 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; - - 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 ); - } - } + Model* mod = (Model*)it->data; + + 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 + // now call all the callbacks - ignores dueness, but not a big deal LISTMETHOD( reentrant_update_list, Model*, 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 +599,14 @@ 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,16 +614,11 @@ 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 ); } } -// fast macros for converting from global cell coordinates to local coords -#define GETCELL(X) (((int32_t)X) & ~((~0x00)<<RBITS )) -#define GETREG(X) ((((int32_t)X) & ~((~0x00)<<SRBITS ))>>RBITS) -#define GETSREG(X) (((int32_t)X)>>SRBITS) - // Stage spends 50-99% of its time in this method. stg_raytrace_result_t World::Raytrace( const Pose &gpose, const stg_meters_t range, @@ -657,32 +652,32 @@ const double tana(sina/cosa); // = tan(angle) // and the x and y offsets of the ray - const int dx( ppm * r.range * cosa); - const int dy( ppm * r.range * sina); + const int32_t dx( ppm * r.range * cosa); + const int32_t dy( ppm * r.range * sina); // fast integer line 3d algorithm adapted from Cohen's code from // Graphics Gems IV - const int sx(sgn(dx)); // sgn() is a fast macro - const int sy(sgn(dy)); - const int ax(abs(dx)); - const int ay(abs(dy)); - const int bx(2*ax); - const int by(2*ay); - int exy(ay-ax); - int n(ax+ay); // the manhattan distance to the goal cell + const int32_t sx(sgn(dx)); // sgn() is a fast macro + 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); // the manhattan distance to the goal cell - const int rsize( Region::WIDTH ); + //const int REGIONWIDTH( REGIONWIDTH ); // fix a little issue where rays are not drawn long enough when // drawing to the right or up if( (dx > 0) || ( dy > 0 ) ) - n++; + n++; // the distances between region crossings in X and Y - const double xjumpx( sx * rsize ); - const double xjumpy( sx * rsize * tana ); - const double yjumpx( sy * rsize / tana ); - const double yjumpy( sy * rsize ); + const double xjumpx( sx * REGIONWIDTH ); + const double xjumpy( sx * REGIONWIDTH * tana ); + const double yjumpx( sy * REGIONWIDTH / tana ); + const double yjumpy( sy * REGIONWIDTH ); // manhattan distance between region crossings in X and Y const double xjumpdist( fabs(xjumpx)+fabs(xjumpy) ); const double yjumpdist( fabs(yjumpx)+fabs(yjumpy) ); @@ -693,7 +688,6 @@ double distX(0), distY(0); bool calculatecrossings( true ); - // puts( "=======================" ); // Stage spends up to 95% of its time in this loop! It would be @@ -701,189 +695,189 @@ // inline calls have a noticeable (2-3%) effect on performance while( n > 0 ) // while we are still not at the ray end { - SuperRegion* sr = - GetSuperRegionCached(GETSREG(glob.x), - GETSREG(glob.y)); - - // coordinates of the region inside the superregion - int32_t rx = GETREG(glob.x); - int32_t ry = GETREG(glob.y); - Region* reg = &sr->regions[ rx + (ry*SuperRegion::WIDTH) ]; + SuperRegion* sr = + GetSuperRegionCached(GETSREG(glob.x), GETSREG(glob.y)); - if( reg->count ) // if the region contains any objects - { - // invalidate the region crossing points used to jump over - // empty regions - calculatecrossings = true; + // coordinates of the region inside the superregion + int32_t rx( GETREG(glob.x) ); + int32_t ry( GETREG(glob.y) ); + Region* reg( &sr->regions[ rx + (ry*SUPERREGIONWIDTH) ] ); + + 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(glob.x); - int32_t cy = GETCELL(glob.y); + // convert from global cell to local cell coords + int32_t cx( GETCELL(glob.x) ); + int32_t cy( GETCELL(glob.y) ); - Cell* c = ®->cells[ cx + cy * Region::WIDTH ]; - assert(c); // should be a cell there + Cell* c( ®->cells[ cx + cy * REGIONWIDTH ] ); + assert(c); // should be a cell there - // 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<(int)Region::WIDTH) && - (cy>=0) && (cy<(int)Region::WIDTH) && - n > 0 ) - { - //printf( "cx %d cy %d\n", cx, cy ); - assert(c >= reg->cells); - - 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 ) + { + //printf( "cx %d cy %d\n", cx, cy ); + assert(c >= reg->cells); + assert(c < (reg->cells + REGIONSIZE) ); + + 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((glob.x-start.x) / cosa) / ppm; - else - sample.range = fabs((glob.y-start.y) / sina) / ppm; + if( ax > ay ) // faster than the equivalent hypot() call + sample.range = fabs((glob.x-start.x) / cosa) / ppm; + else + sample.range = fabs((glob.y-start.y) / sina) / ppm; - return sample; - } - } + return sample; + } + } - assert (sx >= -2 && sx < 2); - assert (sy >= -2 && sy < 2); - // increment our cell in the correct direction - if( exy < 0 ) // we're iterating along X - { - glob.x += 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 - { - glob.y += sy; // global coordinate - exy -= bx; - c += sy * static_cast<int> (Region::WIDTH); // move the cell up or down - cy += sy; // cell coordinate for bounds checking - } - n--; // decrement the manhattan distance remaining + assert (sx >= -2 && sx < 2); + assert (sy >= -2 && sy < 2); + // increment our cell in the correct direction + if( exy < 0 ) // we're iterating along X + { + glob.x += 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 + { + glob.y += 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( glob.x, glob.y )); - } + //rt_cells.push_back( stg_point_int_t( glob.x, glob.y )); + } - //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 region in each - // axis - if( calculatecrossings ) - { - calculatecrossings = false; + //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 region in each + // axis + if( calculatecrossings ) + { + calculatecrossings = false; - // find the coordinate in cells of the bottom left corner of - // the current region - int ix = glob.x; - int iy = glob.y; - double regionx = ix/rsize*rsize; - double regiony = iy/rsize*rsize; - if( (glob.x < 0) && (ix % rsize) ) regionx -= rsize; - if( (glob.y < 0) && (iy % rsize) ) regiony -= rsize; + // find the coordinate in cells of the bottom left corner of + // the current region + int32_t ix( glob.x ); + int32_t iy( glob.y ); + double regionx( ix/REGIONWIDTH*REGIONWIDTH ); + double regiony( iy/REGIONWIDTH*REGIONWIDTH ); + if( (glob.x < 0) && (ix % REGIONWIDTH) ) regionx -= REGIONWIDTH; + if( (glob.y < 0) && (iy % REGIONWIDTH) ) regiony -= REGIONWIDTH; - //double regionx = glob.x - fmod(glob.x,rsize); - //double regiony = glob.y - fmod(glob.y,rsize); + //double regionx = glob.x - fmod(glob.x,REGIONWIDTH); + //double regiony = glob.y - fmod(glob.y,REGIONWIDTH); - //printf( "region %.2f %.2f\n", regionx, regiony ); + //printf( "region %.2f %.2f\n", regionx, regiony ); + + // calculate the distance to the edge of the current region + double xdx( sx < 0 ? + regionx - glob.x - 1.0 : // going left + regionx + REGIONWIDTH - glob.x ); // going right + double xdy( xdx*tana ); + + double ydy( sy < 0 ? + regiony - glob.y - 1.0 : // going down + regiony + REGIONWIDTH - glob.y ); // going up + double ydx( ydy/tana ); + + // these stored hit points are updated as we go along + xcrossx = glob.x+xdx; + xcrossy = glob.y+xdy; - // calculate the distance to the edge of the current region - double xdx = sx < 0 ? - regionx - glob.x - 1.0 : // going left - regionx + rsize - glob.x; // going right - double xdy = xdx*tana; + ycrossx = glob.x+ydx; + ycrossy = glob.y+ydy; - double ydy = sy < 0 ? - regiony - glob.y - 1.0 : // going down - regiony + rsize - glob.y; // going up - double ydx = ydy/tana; - - // these stored hit points are updated as we go along - xcrossx = glob.x+xdx; - xcrossy = glob.y+xdy; - - ycrossx = glob.x+ydx; - ycrossy = glob.y+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); + } -// printf( "globx %.2f globy %.2f\n", glob.x, glob.y ); -// printf( "xcross (%.2f,%.2f) ycross(%.2f,%.2f)\n", xcrossx, xcrossy, ycrossx, ycrossy ); -// printf( "distX %.2f distY %.2f\n", distX, distY ); -// printf( "xjumpdist %.2f yjumpdist %.2f\n", xjumpdist, yjumpdist ); -// puts( "" ); + // printf( "globx %.2f globy %.2f\n", glob.x, glob.y ); + // printf( "xcross (%.2f,%.2f) ycross(%.2f,%.2f)\n", xcrossx, xcrossy, ycrossx, ycrossy ); + // printf( "distX %.2f distY %.2f\n", distX, distY ); + // printf( "xjumpdist %.2f yjumpdist %.2f\n", xjumpdist, yjumpdist ); + // puts( "" ); - if( distX < distY ) // crossing a region boundary left or right - { - //puts( "distX" ); - // move to the X crossing - glob.x = xcrossx; - glob.y = xcrossy; + if( distX < distY ) // crossing a region boundary left or right + { + //puts( "distX" ); + // move to the X crossing + glob.x = xcrossx; + glob.y = 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 - { - //puts( "distY" ); - // move to the X crossing - glob.x = ycrossx; - glob.y = ycrossy; + //rt_candidate_cells.push_back( stg_point_int_t( xcrossx, xcrossy )); + } + else // crossing a region boundary up or down + { + //puts( "distY" ); + // move to the X crossing + glob.x = ycrossx; + glob.y = 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; + distX -= distY; + distY = yjumpdist; - //rt_candidate_cells.push_back( stg_point_int_t( ycrossx, ycrossy )); - } + //rt_candidate_cells.push_back( stg_point_int_t( ycrossx, ycrossy )); + } -// if( (GETCELL(xcrossx) == GETCELL(ycrossx) ) && -// (GETCELL(xcrossy) == GETCELL(ycrossy) ) ) -// printf( "SAME %d=%d %d=%d\n", -// GETCELL(xcrossx), GETCELL(ycrossx), -// GETCELL(xcrossy), GETCELL(ycrossy) ); + // if( (GETCELL(xcrossx) == GETCELL(ycrossx) ) && + // (GETCELL(xcrossy) == GETCELL(ycrossy) ) ) + // printf( "SAME %d=%d %d=%d\n", + // GETCELL(xcrossx), GETCELL(ycrossx), + // GETCELL(xcrossy), GETCELL(ycrossy) ); - //printf( "jumped to glob (%.2f %.2f)\n", glob.x, glob.y ); - } - //rt_cells.push_back( stg_point_int_t( glob.x, glob.y )); - } + //printf( "jumped to glob (%.2f %.2f)\n", glob.x, glob.y ); + } + //rt_cells.push_back( stg_point_int_t( glob.x, glob.y )); + } // hit nothing sample.mod = NULL; return sample; @@ -936,8 +930,6 @@ return sr; } - - inline SuperRegion* World::GetSuperRegionCached( int32_t x, int32_t y ) { // around 99% of the time the SR is the same as last @@ -977,28 +969,17 @@ return sr; } -Cell* World::GetCellNoCreate( const stg_point_int_t& glob ) +Cell* World::GetCell( const stg_point_int_t& glob ) { - Region* r = GetSuperRegionCached( GETSREG(glob.x), GETSREG(glob.y) ) - ->GetRegionGlobal( glob ); - - if( r->count ) - return r->GetCellGlobalNoCreate( glob ) ; - return NULL; -} - - -Cell* World::GetCellCreate( const stg_point_int_t& glob ) -{ return( GetSuperRegionCached( GETSREG(glob.x), GETSREG(glob.y) ) - ->GetRegionGlobal( glob ) - ->GetCellGlobalCreate( glob )) ; + ->GetRegion( GETREG(glob.x), GETREG(glob.y) ) + ->GetCell( GETCELL(glob.x), GETCELL(glob.y) )) ; } void World::ForEachCellInLine( const stg_point_int_t& start, - const stg_point_int_t& end, - std::vector<Cell*>& cells ) + const stg_point_int_t& end, + std::vector<Cell*>& cells ) { int dx = end.x - start.x; @@ -1025,20 +1006,20 @@ while( n-- ) { - // find or create the cell at this location, then add it to the vector - cells.push_back( GetCellCreate( cell ) ); + // find the cell at this location, then add it to the vector + cells.push_back( GetCell( cell ) ); // cleverly skip to the next cell if( exy < 0 ) - { - cell.x += sx; - exy += by; - } + { + cell.x += sx; + exy += by; + } else - { - cell.y += sy; - exy -= bx; - } + { + cell.y += sy; + exy -= bx; + } } } @@ -1075,15 +1056,15 @@ // 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 ); - } + { + 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 ); - } + { + if( ! g_list_find( nonreentrant_update_list, mod ) ) + nonreentrant_update_list = g_list_append( nonreentrant_update_list, mod ); + } } void World::StopUpdatingModel( Model* mod ) @@ -1091,15 +1072,15 @@ // update_list = g_list_remove( update_list, mod ); if( mod->thread_safe ) - reentrant_update_list = g_list_remove( reentrant_update_list, mod ); + reentrant_update_list = g_list_remove( reentrant_update_list, mod ); else - nonreentrant_update_list = g_list_remove( nonreentrant_update_list, mod ); + nonreentrant_update_list = g_list_remove( nonreentrant_update_list, 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 ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-06-06 08:06:44
|
Revision: 7797 http://playerstage.svn.sourceforge.net/playerstage/?rev=7797&view=rev Author: rtv Date: 2009-06-06 08:06:43 +0000 (Sat, 06 Jun 2009) Log Message: ----------- cleaning up Modified Paths: -------------- code/stage/trunk/libstage/region.cc code/stage/trunk/libstage/region.hh code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/region.cc =================================================================== --- code/stage/trunk/libstage/region.cc 2009-06-06 07:24:49 UTC (rev 7796) +++ code/stage/trunk/libstage/region.cc 2009-06-06 08:06:43 UTC (rev 7797) @@ -17,8 +17,6 @@ Region::~Region() { - if(cells) - delete[] cells; } void Region::DecrementOccupancy() Modified: code/stage/trunk/libstage/region.hh =================================================================== --- code/stage/trunk/libstage/region.hh 2009-06-06 07:24:49 UTC (rev 7796) +++ code/stage/trunk/libstage/region.hh 2009-06-06 08:06:43 UTC (rev 7797) @@ -24,11 +24,7 @@ const int32_t SUPERREGIONWIDTH( 1<<SBITS ); const int32_t SUPERREGIONSIZE( SUPERREGIONWIDTH*SUPERREGIONWIDTH ); - /** (x & CELLMASK) converts a global cell index into a local cell - index in a region */ const int32_t CELLMASK( ~((~0x00)<< RBITS )); - /** (x & REGIONMASK)converts a global cell index into a local cell - index in a region */ const int32_t REGIONMASK( ~((~0x00)<< SRBITS )); inline int32_t GETCELL( const int32_t x ) { return( x & CELLMASK); } @@ -54,9 +50,9 @@ { } - void RemoveBlock( Block* b ); - void AddBlock( Block* b ); - void AddBlockNoRecord( Block* b ); + inline void RemoveBlock( Block* b ); + inline void AddBlock( Block* b ); + inline void AddBlockNoRecord( Block* b ); }; class Region @@ -72,7 +68,7 @@ Cell* GetCell( int32_t x, int32_t y ) const { - return( (Cell*)&cells[ x + (y*REGIONWIDTH) ] ); + return( (Cell*)&cells[ x + y * REGIONWIDTH ] ); } void DecrementOccupancy(); @@ -99,7 +95,7 @@ const Region* GetRegion( int32_t x, int32_t y ) const { - return( ®ions[ x + (y*SUPERREGIONWIDTH) ] ); + return( ®ions[ x + y * SUPERREGIONWIDTH ] ); } void Draw( bool drawall ); @@ -110,15 +106,15 @@ }; -inline void printvec( std::vector<Block*>& vec ) - { - printf( "Vec: "); - for( size_t i=0; i<vec.size(); i++ ) - printf( "%p ", vec[i] ); - puts( "" ); - } +// inline void printvec( std::vector<Block*>& vec ) +// { +// printf( "Vec: "); +// for( size_t i=0; i<vec.size(); i++ ) +// printf( "%p ", vec[i] ); +// puts( "" ); +// } -inline void Cell::RemoveBlock( Block* b ) +void Cell::RemoveBlock( Block* b ) { // linear time removal, but these vectors are very short, usually 1 // or 2 elements. Fast removal - our strategy is to copy the last @@ -139,7 +135,7 @@ region->DecrementOccupancy(); } -inline void Cell::AddBlock( Block* b ) +void Cell::AddBlock( Block* b ) { // constant time prepend blocks.push_back( b ); Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-06-06 07:24:49 UTC (rev 7796) +++ code/stage/trunk/libstage/world.cc 2009-06-06 08:06:43 UTC (rev 7797) @@ -651,7 +651,7 @@ const double sina(sin(angle)); const double tana(sina/cosa); // = tan(angle) - // and the x and y offsets of the ray + // the x and y components of the ray const int32_t dx( ppm * r.range * cosa); const int32_t dy( ppm * r.range * sina); @@ -666,8 +666,6 @@ int32_t exy(ay-ax); int32_t n(ax+ay); // the manhattan distance to the goal cell - //const int REGIONWIDTH( REGIONWIDTH ); - // fix a little issue where rays are not drawn long enough when // drawing to the right or up if( (dx > 0) || ( dy > 0 ) ) @@ -688,10 +686,8 @@ double distX(0), distY(0); bool calculatecrossings( true ); - // puts( "=======================" ); - // Stage spends up to 95% of its time in this loop! It would be - // neater with more function calls encapsualting things, but even + // neater with more function calls encapsulating things, but even // inline calls have a noticeable (2-3%) effect on performance while( n > 0 ) // while we are still not at the ray end { @@ -701,7 +697,7 @@ // coordinates of the region inside the superregion int32_t rx( GETREG(glob.x) ); int32_t ry( GETREG(glob.y) ); - Region* reg( &sr->regions[ rx + (ry*SUPERREGIONWIDTH) ] ); + Region* reg( &sr->regions[ rx + ry * SUPERREGIONWIDTH ] ); if( reg->count ) // if the region contains any objects { @@ -796,11 +792,6 @@ if( (glob.x < 0) && (ix % REGIONWIDTH) ) regionx -= REGIONWIDTH; if( (glob.y < 0) && (iy % REGIONWIDTH) ) regiony -= REGIONWIDTH; - //double regionx = glob.x - fmod(glob.x,REGIONWIDTH); - //double regiony = glob.y - fmod(glob.y,REGIONWIDTH); - - //printf( "region %.2f %.2f\n", regionx, regiony ); - // calculate the distance to the edge of the current region double xdx( sx < 0 ? regionx - glob.x - 1.0 : // going left @@ -867,13 +858,6 @@ //rt_candidate_cells.push_back( stg_point_int_t( ycrossx, ycrossy )); } - - // if( (GETCELL(xcrossx) == GETCELL(ycrossx) ) && - // (GETCELL(xcrossy) == GETCELL(ycrossy) ) ) - // printf( "SAME %d=%d %d=%d\n", - // GETCELL(xcrossx), GETCELL(ycrossx), - // GETCELL(xcrossy), GETCELL(ycrossy) ); - //printf( "jumped to glob (%.2f %.2f)\n", glob.x, glob.y ); } //rt_cells.push_back( stg_point_int_t( glob.x, glob.y )); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-06-10 16:56:00
|
Revision: 7834 http://playerstage.svn.sourceforge.net/playerstage/?rev=7834&view=rev Author: rtv Date: 2009-06-10 16:55:56 +0000 (Wed, 10 Jun 2009) Log Message: ----------- accepted Toby's recommended Inro patch: new models, friction, stickiness, fltk fix - good stuff. Modified Paths: -------------- code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/typetable.cc code/stage/trunk/libstage/worldgui.cc code/stage/trunk/libstageplugin/CMakeLists.txt code/stage/trunk/libstageplugin/p_driver.cc code/stage/trunk/libstageplugin/p_driver.h code/stage/trunk/libstageplugin/p_simulation.cc Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2009-06-10 15:09:50 UTC (rev 7833) +++ code/stage/trunk/libstage/CMakeLists.txt 2009-06-10 16:55:56 UTC (rev 7834) @@ -4,7 +4,6 @@ include_directories(${PROJECT_BINARY_DIR}) set( stageSrcs - world.cc ancestor.cc block.cc blockgroup.cc @@ -15,6 +14,7 @@ glcolorstack.cc logentry.cc model.cc + model_actuator.cc model_blinkenlight.cc model_blobfinder.cc model_callbacks.cc @@ -28,6 +28,8 @@ model_position.cc model_props.cc model_ranger.cc + model_loadcell.cc + model_lightindicator.cc option.cc powerpack.cc region.cc @@ -38,6 +40,7 @@ typetable.cc vis_strip.cc waypoint.cc + world.cc worldfile.cc worldgui.cc canvas.cc Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-06-10 15:09:50 UTC (rev 7833) +++ code/stage/trunk/libstage/model.cc 2009-06-10 16:55:56 UTC (rev 7834) @@ -33,6 +33,8 @@ blob_return 1 laser_return LaserVisible gripper_return 0 + gravity_return 0 + sticky_return 0 # GUI properties gui_nose 0 @@ -90,20 +92,10 @@ - gui_movemask <int>\n define how the model can be moved by the mouse in the GUI window + - friction <float>\n + Determines the proportion of velocity lost per second. For example, 0.1 would mean that the object would lose 10% of its speed due to friction per second. A value of zero (the default) means this model can not be pushed around (infinite friction). */ -/* - TODO - - - friction float [WARNING: Friction model is not yet implemented; - - details may change] if > 0 the model can be pushed around by other - - moving objects. The value determines the proportion of velocity - - lost per second. For example, 0.1 would mean that the object would - - lose 10% of its speed due to friction per second. A value of zero - - (the default) means this model can not be pushed around (infinite - - friction). -*/ - #ifndef _GNU_SOURCE #define _GNU_SOURCE #endif @@ -160,6 +152,9 @@ ranger_return( true ) { /* nothing do do */ } +//static const members +static const double DEFAULT_FRICTION = 0.0; + void Visibility::Load( Worldfile* wf, int wf_entity ) { blob_return = wf->ReadInt( wf_entity, "blob_return", blob_return); @@ -169,6 +164,8 @@ laser_return = (stg_laser_return_t)wf->ReadInt( wf_entity, "laser_return", laser_return); obstacle_return = wf->ReadInt( wf_entity, "obstacle_return", obstacle_return); ranger_return = wf->ReadInt( wf_entity, "ranger_return", ranger_return); + gravity_return = wf->ReadInt( wf_entity, "gravity_return", gravity_return); + sticky_return = wf->ReadInt( wf_entity, "sticky_return", sticky_return); } GuiState:: GuiState() : @@ -261,6 +258,7 @@ world->AddModel( this ); + this->friction = DEFAULT_FRICTION; g_datalist_init( &this->props ); // now we can add the basic square shape @@ -575,6 +573,11 @@ ((Model*)it->data)->MapWithChildren(); } +void Model::MapFromRoot() +{ + Root()->MapWithChildren(); +} + void Model::UnMapWithChildren() { UnMap(); @@ -584,6 +587,11 @@ ((Model*)it->data)->UnMapWithChildren(); } +void Model::UnMapFromRoot() +{ + Root()->UnMapWithChildren(); +} + void Model::Map() { //PRINT_DEBUG1( "%s.Map()", token ); @@ -612,7 +620,7 @@ world->total_subs--; world->dirty = true; // need redraw - printf( "unsubscribe from %s %d\n", token, subs ); + //printf( "unsubscribe from %s %d\n", token, subs ); // if this is the last sub, call shutdown if( subs < 1 ) @@ -918,7 +926,7 @@ Model* Model::GetUnsubscribedModelOfType( stg_model_type_t type ) const { if( (this->type == type) && (this->subs == 0) ) - return (Model*)this; // discard const + return const_cast<Model*> (this); // discard const // this model is no use. try children recursively for( GList* it = children; it; it=it->next ) @@ -968,7 +976,18 @@ return NULL; } +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(); + } + return sum; +} + + Model* Model::GetModel( const char* modelname ) const { // construct the full model name and look it up @@ -1035,6 +1054,12 @@ rastervis.SetData( data, width, height, cellwidth, cellheight ); } +void Model::SetFriction( double friction ) +{ + this->friction = friction; + CallCallbacks( &this->friction ); +} + //*************************************************************** // Raster data visualizer // Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2009-06-10 15:09:50 UTC (rev 7833) +++ code/stage/trunk/libstage/model_laser.cc 2009-06-10 16:55:56 UTC (rev 7834) @@ -199,6 +199,7 @@ PRINT_DEBUG2( "Constructing ModelLaser %d (%s)\n", id, typestr ); + // Model data members interval = DEFAULT_INTERVAL_MS * (int)thousand; @@ -285,6 +286,8 @@ { assert( samples.size() == sample_count ); + UnMapFromRoot(); // Don't raytrace self + double bearing = -fov/2.0; // make the first and last rays exactly at the extremes of the FOV double sample_incr = fov / MAX(sample_count-1,1); @@ -335,6 +338,8 @@ } } + MapFromRoot(); + Model::Update(); } Modified: code/stage/trunk/libstage/model_load.cc =================================================================== --- code/stage/trunk/libstage/model_load.cc 2009-06-10 15:09:50 UTC (rev 7833) +++ code/stage/trunk/libstage/model_load.cc 2009-06-10 16:55:56 UTC (rev 7834) @@ -135,6 +135,12 @@ if( wf->PropertyExists( wf_entity, "color_rgba" )) { + if (wf->GetProperty(wf_entity,"color_rgba")->value_count < 4) + { + PRINT_ERR1( "model %s color_rgba requires 4 values\n", this->token ); + } + else + { double red = wf->ReadTupleFloat( wf_entity, "color_rgba", 0, 0 ); double green = wf->ReadTupleFloat( wf_entity, "color_rgba", 1, 0 ); double blue = wf->ReadTupleFloat( wf_entity, "color_rgba", 2, 0 ); @@ -142,6 +148,7 @@ this->SetColor( stg_color_pack( red, green, blue, alpha )); } + } if( wf->PropertyExists( wf_entity, "bitmap" ) ) { @@ -193,6 +200,18 @@ if( wf->PropertyExists( wf_entity, "db_count" ) ) LoadDataBaseEntries( wf, wf_entity ); + if (vis.gravity_return) + { + Velocity vel = GetVelocity(); + this->SetVelocity( vel ); + world->StartUpdatingModel( this ); + } + + if( wf->PropertyExists( wf_entity, "friction" )) + { + this->SetFriction( wf->ReadFloat(wf_entity, "friction", this->friction )); + } + if( wf->PropertyExists( wf_entity, "ctrl" )) { char* lib = (char*)wf->ReadString(wf_entity, "ctrl", NULL ); @@ -339,10 +358,6 @@ sscanf( entry, "%[^<]<%[^>]>%[^\"]", key, type, value ); - assert( key ); - assert( type ); - assert( value ); - if( strcmp( type, "int" ) == 0 ) SetPropertyInt( strdup(key), atoi(value) ); else if( strcmp( type, "float" ) == 0 ) Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-06-10 15:09:50 UTC (rev 7833) +++ code/stage/trunk/libstage/stage.hh 2009-06-10 16:55:56 UTC (rev 7834) @@ -102,6 +102,9 @@ MODEL_TYPE_BLINKENLIGHT, MODEL_TYPE_CAMERA, MODEL_TYPE_GRIPPER, + MODEL_TYPE_ACTUATOR, + MODEL_TYPE_LOADCELL, + MODEL_TYPE_LIGHTINDICATOR, MODEL_TYPE_COUNT // must be the last entry, to count the number of // types } stg_model_type_t; @@ -981,6 +984,9 @@ /** hint that the world needs to be redrawn if a GUI is attached */ void NeedRedraw(){ dirty = true; }; + /** Special model for the floor of the world */ + Model* ground; + /** Get human readable string that describes the current simulation time. */ virtual std::string ClockString( void ); @@ -1127,6 +1133,9 @@ /// Control printing time to stdout void ShowClock( bool enable ){ show_clock = enable; }; + + /** Return the floor model */ + Model* GetGround() {return ground;}; }; class Block @@ -1681,6 +1690,8 @@ stg_laser_return_t laser_return; bool obstacle_return; bool ranger_return; + bool gravity_return; + bool sticky_return; Visibility(); void Load( Worldfile* wf, int wf_entity ); @@ -1756,6 +1767,7 @@ /** Default color of the model's blocks.*/ stg_color_t color; + double friction; /** This can be set to indicate that the model has new data that may be of interest to users. This allows polling the model @@ -1826,6 +1838,9 @@ stg_meters_t cellwidth, stg_meters_t cellheight ); + int subs; //< the number of subscriptions to this model + int used; //< the number of connections to this model + void AddPoint( stg_meters_t x, stg_meters_t y ); void ClearPts(); @@ -1924,6 +1939,10 @@ void MapWithChildren(); void UnMapWithChildren(); + // Find the root model, and map/unmap the whole tree. + void MapFromRoot(); + void UnMapFromRoot(); + int TreeToPtrArray( GPtrArray* array ) const; /** raytraces a single ray from the point and heading identified by @@ -2209,6 +2228,9 @@ /** return a model's unique process-wide identifier */ uint32_t GetId() const { return id; } + /** Get the total mass of a model and it's children recursively */ + stg_kg_t GetTotalMass(); + // stg_laser_return_t GetLaserReturn(){ return laser_return; } /** Change a model's parent - experimental*/ @@ -2230,7 +2252,9 @@ void SetColor( stg_color_t col ); void SetMass( stg_kg_t mass ); void SetStall( stg_bool_t stall ); + void SetGravityReturn( int val ); void SetGripperReturn( int val ); + void SetStickyReturn( int val ); void SetLaserReturn( stg_laser_return_t val ); void SetObstacleReturn( int val ); void SetBlobReturn( int val ); @@ -2242,6 +2266,7 @@ void SetGuiOutline( int val ); void SetWatts( stg_watts_t watts ); void SetMapResolution( stg_meters_t res ); + void SetFriction( double friction ); bool DataIsFresh() const { return this->data_fresh; } @@ -2376,6 +2401,7 @@ and has the type indicated by the string. This model is tagged as used. */ Model* GetUnusedModelOfType( const stg_model_type_t type ); + /** Returns the value of the model's stall boolean, which is true iff the model has crashed into another model */ bool Stalled() const { return this->stall; } @@ -2447,6 +2473,8 @@ }; + + // LASER MODEL -------------------------------------------------------- /// %ModelLaser class @@ -2528,7 +2556,45 @@ void SetConfig( Config& cfg ); }; - // GRIPPER MODEL -------------------------------------------------------- + +// LOAD CELL MODEL -------------------------------------------------------- + +/// %ModelLoadCell class +class ModelLoadCell : public Model +{ +private: +public: + //static const char* typestr; + // constructor + ModelLoadCell( World* world, + Model* parent ); + + // destructor + ~ModelLoadCell(); + + float GetVoltage(); +}; + + +// Light indicator model +class ModelLightIndicator : public Model +{ +public: + ModelLightIndicator(World* world, Model* parent); + ~ModelLightIndicator(); + + void SetState(bool isOn); + +protected: + virtual void DrawBlocks(); + +private: + bool m_IsOn; +}; + +// \todo GRIPPER MODEL -------------------------------------------------------- + + class ModelGripper : public Model { public: @@ -2931,6 +2997,69 @@ Pose est_origin; ///< global origin of the local coordinate system }; + +// ACTUATOR MODEL -------------------------------------------------------- + +/** Define a actuator control method */ +typedef enum +{ STG_ACTUATOR_CONTROL_VELOCITY, + STG_ACTUATOR_CONTROL_POSITION +} stg_actuator_control_mode_t; + +/** Define an actuator type */ +typedef enum +{ STG_ACTUATOR_TYPE_LINEAR, + STG_ACTUATOR_TYPE_ROTATIONAL +} stg_actuator_type_t; + + +/// %ModelActuator class +class ModelActuator : public Model +{ + private: + double goal; //< the current velocity or pose to reach, depending on the value of control_mode + double pos; + double max_speed; + double min_position; + double max_position; + stg_actuator_control_mode_t control_mode; + stg_actuator_type_t actuator_type; + stg_point3_t axis; + + Pose InitialPose; + public: + static const char* typestr; + + // constructor + ModelActuator( World* world, + Model* parent ); + // destructor + ~ModelActuator(); + + virtual void Startup(); + virtual void Shutdown(); + virtual void Update(); + virtual void Load(); + + /** Sets the control_mode to STG_ACTUATOR_CONTROL_VELOCITY and sets + the goal velocity. */ + void SetSpeed( double speed ); + + double GetSpeed() const {return goal;} + + /** Sets the control mode to STG_ACTUATOR_CONTROL_POSITION and sets + the goal pose */ + void GoTo( double pose ); + + double GetPosition() const {return pos;}; + + double GetMaxPosition() const {return max_position;}; + + double GetMinPosition() const {return min_position;}; + +}; + + }; // end namespace stg #endif Modified: code/stage/trunk/libstage/typetable.cc =================================================================== --- code/stage/trunk/libstage/typetable.cc 2009-06-10 15:09:50 UTC (rev 7833) +++ code/stage/trunk/libstage/typetable.cc 2009-06-10 16:55:56 UTC (rev 7834) @@ -19,6 +19,9 @@ static Model* CreateModelRanger( World* world, Model* parent ) { return new ModelRanger( world, parent ); } +static Model* CreateModelLoadCell( World* world, Model* parent ) +{ return new ModelLoadCell( world, parent ); } + static Model* CreateModelCamera( World* world, Model* parent ) { return new ModelCamera( world, parent ); } @@ -31,7 +34,12 @@ static Model* CreateModelGripper( World* world, Model* parent ) { return new ModelGripper( world, parent ); } +static Model* CreateModelActuator( World* world, Model* parent ) +{ return new ModelActuator( world, parent ); } +static Model* CreateModelLightIndicator( World* world, Model* parent ) +{ return new ModelLightIndicator( world, parent ); } + void Stg::RegisterModels() { RegisterModel( MODEL_TYPE_PLAIN, "model", CreateModel ); @@ -43,6 +51,9 @@ RegisterModel( MODEL_TYPE_BLOBFINDER, "blobfinder", CreateModelBlobfinder ); RegisterModel( MODEL_TYPE_BLINKENLIGHT, "blinkenlight", CreateModelBlinkenlight); RegisterModel( MODEL_TYPE_GRIPPER, "gripper", CreateModelGripper); + RegisterModel( MODEL_TYPE_ACTUATOR, "actuator", CreateModelActuator); + RegisterModel( MODEL_TYPE_LOADCELL, "loadcell", CreateModelLoadCell ); + RegisterModel( MODEL_TYPE_LIGHTINDICATOR, "lightindicator", CreateModelLightIndicator ); #if DEBUG // human-readable view of the table puts( "Stg::Typetable" ); Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2009-06-10 15:09:50 UTC (rev 7833) +++ code/stage/trunk/libstage/worldgui.cc 2009-06-10 16:55:56 UTC (rev 7834) @@ -256,6 +256,9 @@ { PRINT_DEBUG1( "%s.Load()", token ); + // needs to happen before StgWorld load, or we segfault with GL calls on some graphics cards + Fl::check(); + fileMan->newWorld( filename ); World::Load( filename ); Modified: code/stage/trunk/libstageplugin/CMakeLists.txt =================================================================== --- code/stage/trunk/libstageplugin/CMakeLists.txt 2009-06-10 15:09:50 UTC (rev 7833) +++ code/stage/trunk/libstageplugin/CMakeLists.txt 2009-06-10 16:55:56 UTC (rev 7834) @@ -5,6 +5,7 @@ set( stagepluginSrcs p_driver.h p_driver.cc + p_actarray.cc p_blobfinder.cc p_simulation.cc p_laser.cc @@ -12,6 +13,8 @@ p_position.cc p_sonar.cc p_speech.cc + p_aio.cc + p_dio.cc stg_time.cc ) Modified: code/stage/trunk/libstageplugin/p_driver.cc =================================================================== --- code/stage/trunk/libstageplugin/p_driver.cc 2009-06-10 15:09:50 UTC (rev 7833) +++ code/stage/trunk/libstageplugin/p_driver.cc 2009-06-10 16:55:56 UTC (rev 7834) @@ -306,10 +306,22 @@ switch( player_addr.interf ) { + case PLAYER_ACTARRAY_CODE: + ifsrc = new InterfaceActArray( player_addr, this, cf, section ); + break; + + case PLAYER_AIO_CODE: + ifsrc = new InterfaceAio( player_addr, this, cf, section ); + break; + case PLAYER_BLOBFINDER_CODE: ifsrc = new InterfaceBlobfinder( player_addr, this, cf, section ); break; + case PLAYER_DIO_CODE: + ifsrc = new InterfaceDio(player_addr, this, cf, section); + break; + case PLAYER_FIDUCIAL_CODE: ifsrc = new InterfaceFiducial( player_addr, this, cf, section ); break; Modified: code/stage/trunk/libstageplugin/p_driver.h =================================================================== --- code/stage/trunk/libstageplugin/p_driver.h 2009-06-10 15:09:50 UTC (rev 7833) +++ code/stage/trunk/libstageplugin/p_driver.h 2009-06-10 16:55:56 UTC (rev 7834) @@ -168,6 +168,28 @@ virtual void Publish( void ); }; +class InterfaceAio : public InterfaceModel +{ + public: + InterfaceAio( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section ); + virtual ~InterfaceAio( void ){ /* TODO: clean up*/ }; + virtual int ProcessMessage(QueuePointer & resp_queue, + player_msghdr_t* hdr, + void* data); + virtual void Publish( void ); +}; + + +class InterfaceDio : public InterfaceModel +{ +public: + InterfaceDio(player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section); + virtual ~InterfaceDio(); + virtual int ProcessMessage(QueuePointer & resp_queue, player_msghdr_t* hdr, void* data); + virtual void Publish(); +}; + + class InterfacePower : public InterfaceModel { public: @@ -194,6 +216,18 @@ }; +class InterfaceActArray : public InterfaceModel +{ + public: + InterfaceActArray( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section ); + virtual ~InterfaceActArray( void ){ /* TODO: clean up*/ }; + + virtual int ProcessMessage( QueuePointer & resp_queue, + player_msghdr * hdr, + void * data ); + virtual void Publish( void ); +}; + class InterfaceBlobfinder : public InterfaceModel { public: Modified: code/stage/trunk/libstageplugin/p_simulation.cc =================================================================== --- code/stage/trunk/libstageplugin/p_simulation.cc 2009-06-10 15:09:50 UTC (rev 7833) +++ code/stage/trunk/libstageplugin/p_simulation.cc 2009-06-10 16:55:56 UTC (rev 7834) @@ -148,6 +148,17 @@ player_msghdr_t* hdr, void* data) { + if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_CAPABILTIES_REQ, addr)) + { + PLAYER_ERROR1("%p\n", data); + player_capabilities_req_t & cap_req = * reinterpret_cast<player_capabilities_req_t *> (data); + if (cap_req.type == PLAYER_MSGTYPE_REQ && (cap_req.subtype == PLAYER_SIMULATION_REQ_SET_POSE3D || cap_req.subtype == PLAYER_SIMULATION_REQ_GET_POSE3D)) + { + this->driver->Publish(addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_CAPABILTIES_REQ); + return 0; + } + } + // Is it a request to get a model's pose in 2D? if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_SIMULATION_REQ_GET_POSE2D, This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-06-10 19:01:41
|
Revision: 7835 http://playerstage.svn.sourceforge.net/playerstage/?rev=7835&view=rev Author: rtv Date: 2009-06-10 19:01:21 +0000 (Wed, 10 Jun 2009) Log Message: ----------- added missing files from last commit Added Paths: ----------- code/stage/trunk/libstage/model_actuator.cc code/stage/trunk/libstage/model_lightindicator.cc code/stage/trunk/libstage/model_loadcell.cc code/stage/trunk/libstageplugin/p_actarray.cc code/stage/trunk/libstageplugin/p_aio.cc code/stage/trunk/libstageplugin/p_dio.cc Added: code/stage/trunk/libstage/model_actuator.cc =================================================================== --- code/stage/trunk/libstage/model_actuator.cc (rev 0) +++ code/stage/trunk/libstage/model_actuator.cc 2009-06-10 19:01:21 UTC (rev 7835) @@ -0,0 +1,298 @@ +/////////////////////////////////////////////////////////////////////////// +// +// File: model_laser.c +// Author: Richard Vaughan +// Date: 10 June 2004 +// +// CVS info: +// $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/libstage/model_position.cc,v $ +// $Author: rtv $ +// $Revision: 1.5 $ +// +/////////////////////////////////////////////////////////////////////////// + + +#include <sys/time.h> +#include <math.h> +#include <stdlib.h> + +//#define DEBUG +#include "stage.hh" +#include "worldfile.hh" + +using namespace Stg; + +/** +@ingroup model +@defgroup model_actuator Actuator model + +The actuator model simulates a simple actuator, where child objects can be displaced in a +single dimension, or rotated about the Z axis. + +API: Stg::ModelActuator + +<h2>Worldfile properties</h2> + +@par Summary and default values + +@verbatim +actuator +( + # actuator properties + type "" + axis [x y z] + + min_position + max_position + max_speed +) +@endverbatim + +@par Details + +- type "linear" or "rotational"\n + the style of actuator. Rotational actuators may only rotate about the z axis (i.e. 'yaw') +- axis + if a linear actuator the axis that the actuator will move along + */ + +const double STG_ACTUATOR_WATTS_KGMS = 5.0; // cost per kg per meter per second +const double STG_ACTUATOR_WATTS = 2.0; // base cost of position device + +const stg_actuator_control_mode_t ACTUATOR_CONTROL_DEFAULT = STG_ACTUATOR_CONTROL_VELOCITY; +const stg_actuator_type_t ACTUATOR_TYPE_DEFAULT = STG_ACTUATOR_TYPE_LINEAR; + + +ModelActuator::ModelActuator( World* world, + Model* parent ) + : Model( world, parent, MODEL_TYPE_ACTUATOR ), + goal(0), pos(0), max_speed(1), min_position(0), max_position(1), + control_mode(ACTUATOR_CONTROL_DEFAULT), + actuator_type(ACTUATOR_TYPE_DEFAULT) +{ + // no power consumed until we're subscribed + this->SetWatts( 0 ); + + // sensible position defaults + Velocity vel; + memset( &vel, 0, sizeof(vel)); + this->SetVelocity( vel ); + this->SetBlobReturn( TRUE ); + + memset(&axis,0,sizeof(axis)); + +} + + +ModelActuator::~ModelActuator( void ) +{ + // nothing to do +} + +void ModelActuator::Load( void ) +{ + Model::Load(); + InitialPose = GetPose(); + + // load steering mode + if( wf->PropertyExists( wf_entity, "type" ) ) + { + const char* type_str = + wf->ReadString( wf_entity, "type", NULL ); + + if( type_str ) + { + if( strcmp( type_str, "linear" ) == 0 ) + actuator_type = STG_ACTUATOR_TYPE_LINEAR; + else if( strcmp( type_str, "rotational" ) == 0 ) + actuator_type = STG_ACTUATOR_TYPE_ROTATIONAL; + else + { + PRINT_ERR1( "invalid actuator type specified: \"%s\" - should be one of: \"linear\" or \"rotational\". Using \"linear\" as default.", type_str ); + } + } + } + + if (actuator_type == STG_ACTUATOR_TYPE_LINEAR) + { + // if we are a linear actuator find the axis we operate in + if( wf->PropertyExists( wf_entity, "axis" ) ) + { + axis.x = wf->ReadTupleLength( wf_entity, "axis", 0, 0 ); + axis.y = wf->ReadTupleLength( wf_entity, "axis", 1, 0 ); + axis.z = wf->ReadTupleLength( wf_entity, "axis", 2, 0 ); + + // normalise the axis + double length = sqrt(axis.x*axis.x + axis.y*axis.y + axis.z*axis.z); + if (length == 0) + { + PRINT_ERR( "zero length vector specified for actuator axis, using (1,0,0) instead" ); + axis.x = 1; + } + else + { + axis.x /= length; + axis.y /= length; + axis.z /= length; + } + } + } + + if( wf->PropertyExists( wf_entity, "max_speed" ) ) + { + max_speed = wf->ReadFloat ( wf_entity, "max_speed", 1 ); + } + + if( wf->PropertyExists( wf_entity, "max_position" ) ) + { + max_position = wf->ReadFloat( wf_entity, "max_position", 1 ); + } + + if( wf->PropertyExists( wf_entity, "min_position" ) ) + { + min_position = wf->ReadFloat( wf_entity, "min_position", 0 ); + } + +} + +void ModelActuator::Update( void ) +{ + PRINT_DEBUG1( "[%lu] actuator update", 0 ); + + // stop by default + double velocity = 0; + + // update current position + Pose CurrentPose = GetPose(); + // just need to get magnitude difference; + Pose PoseDiff; + PoseDiff.x = CurrentPose.x - InitialPose.x; + PoseDiff.y = CurrentPose.y - InitialPose.y; + PoseDiff.z = CurrentPose.z - InitialPose.z; + PoseDiff.a = CurrentPose.a - InitialPose.a; + switch (actuator_type) + { + case STG_ACTUATOR_TYPE_LINEAR: + { + pos = PoseDiff.x * axis.x + PoseDiff.y * axis.y + PoseDiff.z * axis.z; // Dot product to find distance along axis + } break; + case STG_ACTUATOR_TYPE_ROTATIONAL: + { + pos = PoseDiff.a; + } break; + default: + PRINT_ERR1( "unrecognized actuator type %d", actuator_type ); + } + + + if( this->subs ) // no driving if noone is subscribed + { + switch( control_mode ) + { + case STG_ACTUATOR_CONTROL_VELOCITY : + { + PRINT_DEBUG( "actuator velocity control mode" ); + PRINT_DEBUG2( "model %s command(%.2f)", + this->token, + this->goal); + if ((pos <= min_position && goal < 0) || (pos >= max_position && goal > 0)) + velocity = 0; + else + velocity = goal; + } break; + + case STG_ACTUATOR_CONTROL_POSITION: + { + PRINT_DEBUG( "actuator position control mode" ); + + + // limit our axis + if (goal < min_position) + goal = min_position; + else if (goal > max_position) + goal = max_position; + + double error = goal - pos; + velocity = error; + + PRINT_DEBUG1( "error: %.2f\n", error); + + } + break; + + default: + PRINT_ERR1( "unrecognized actuator command mode %d", control_mode ); + } + + // simple model of power consumption + //TODO power consumption + + // speed limits for controller + if (velocity < -max_speed) + velocity = -max_speed; + else if (velocity > max_speed) + velocity = max_speed; + + Velocity outvel; + switch (actuator_type) + { + case STG_ACTUATOR_TYPE_LINEAR: + { + outvel.x = axis.x * velocity; + outvel.y = axis.y * velocity; + outvel.z = axis.z * velocity; + outvel.a = 0; + } break; + case STG_ACTUATOR_TYPE_ROTATIONAL: + { + outvel.x = outvel.y = outvel.z = 0; + outvel.a = velocity; + }break; + default: + PRINT_ERR1( "unrecognized actuator type %d", actuator_type ); + } + + this->SetVelocity( outvel ); + //this->GPoseDirtyTree(); + PRINT_DEBUG4( " Set Velocity: [ %.4f %.4f %.4f %.4f ]\n", + outvel.x, outvel.y, outvel.z, outvel.a ); + } + + Model::Update(); +} + +void ModelActuator::Startup( void ) +{ + Model::Startup(); + + PRINT_DEBUG( "position startup" ); + + this->SetWatts( STG_ACTUATOR_WATTS ); + +} + +void ModelActuator::Shutdown( void ) +{ + PRINT_DEBUG( "position shutdown" ); + + // safety features! + goal = 0; + bzero( &velocity, sizeof(velocity) ); + + this->SetWatts( 0 ); + + Model::Shutdown(); +} + +void ModelActuator::SetSpeed( double speed) +{ + control_mode = STG_ACTUATOR_CONTROL_VELOCITY; + goal = speed; +} + + +void ModelActuator::GoTo( double pos) +{ + control_mode = STG_ACTUATOR_CONTROL_POSITION; + goal = pos; +} Added: code/stage/trunk/libstage/model_lightindicator.cc =================================================================== --- code/stage/trunk/libstage/model_lightindicator.cc (rev 0) +++ code/stage/trunk/libstage/model_lightindicator.cc 2009-06-10 19:01:21 UTC (rev 7835) @@ -0,0 +1,45 @@ +#include "stage.hh" + +using namespace Stg; + +ModelLightIndicator::ModelLightIndicator(World* world, Model* parent) + : Model(world, parent, MODEL_TYPE_LIGHTINDICATOR) + , m_IsOn(false) +{ +} + + +ModelLightIndicator::~ModelLightIndicator() +{ +} + + +void ModelLightIndicator::SetState(bool isOn) +{ + m_IsOn = isOn; +} + + +void ModelLightIndicator::DrawBlocks() +{ + if(m_IsOn) + { + Model::DrawBlocks(); + } + else + { + stg_color_t currColour = this->GetColor(); + + const double scaleFactor = 0.8; + double r = 0, g = 0, b = 0, a = 0; + stg_color_unpack(currColour, &r, &g, &b, &a); + r *= scaleFactor; + g *= scaleFactor; + b *= scaleFactor; + + this->SetColor(stg_color_pack(r, g, b, a)); + Model::DrawBlocks(); + + this->SetColor(currColour); + } +} Added: code/stage/trunk/libstage/model_loadcell.cc =================================================================== --- code/stage/trunk/libstage/model_loadcell.cc (rev 0) +++ code/stage/trunk/libstage/model_loadcell.cc 2009-06-10 19:01:21 UTC (rev 7835) @@ -0,0 +1,41 @@ +/////////////////////////////////////////////////////////////////////////// +// +// File: model_loadcell.c +// Author: Richard Vaughan +// Date: 10 June 2004 +// +// CVS info: +// $Source: /home/tcollett/stagecvs/playerstage-cvs/code/stage/libstage/model_loadcell.cc,v $ +// $Author: rtv $ +// $Revision: 1.7 $ +// +/////////////////////////////////////////////////////////////////////////// + +#include <sys/time.h> +#include "stage.hh" + +using namespace Stg; + +/** +@ingroup model +@defgroup model_loadcell Load cell model +The load cell model simulates a load sensor. It provides a single voltage on an aio interface which represents the mass of it's child models combined. +*/ + +ModelLoadCell::ModelLoadCell( World* world, + Model* parent ) + : Model( world, parent, MODEL_TYPE_LOADCELL ) +{ +} + +ModelLoadCell::~ModelLoadCell() +{ +} + +float ModelLoadCell::GetVoltage() +{ + if (Parent()->Stalled()) // Assume parent is what moves + return 0.0f; // Load has hit something + else + return float(this->GetTotalMass() - this->mass); // Don't include the mass of the loadcell itself +} Added: code/stage/trunk/libstageplugin/p_actarray.cc =================================================================== --- code/stage/trunk/libstageplugin/p_actarray.cc (rev 0) +++ code/stage/trunk/libstageplugin/p_actarray.cc 2009-06-10 19:01:21 UTC (rev 7835) @@ -0,0 +1,138 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2004, 2005 Richard Vaughan + * + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +/* + * Desc: A plugin driver for Player that gives access to Stage devices. + * Author: Richard Vaughan + * Date: 10 December 2004 + * CVS: $Id: p_position.cc,v 1.2 2008-01-15 01:25:42 rtv Exp $ + */ +// DOCUMENTATION ------------------------------------------------------------ + +/** @addtogroup player +@par ActArray interface +- PLAYER_ACTARRAY_REQ_GET_GEOM +- PLAYER_ACTARRAY_CMD_POS +- PLAYER_ACTARRAY_CMD_SPEED +- PLAYER_ACTARRAY_DATA_STATE +*/ + +// CODE ---------------------------------------------------------------------- + +#include "p_driver.h" +//#include "playerclient.h" + + +InterfaceActArray::InterfaceActArray( player_devaddr_t addr, + StgDriver* driver, + ConfigFile* cf, + int section ) + + : InterfaceModel( addr, driver, cf, section, Stg::MODEL_TYPE_ACTUATOR ) +{ + //puts( "InterfacePosition constructor" ); +} + +int InterfaceActArray::ProcessMessage(QueuePointer &resp_queue, + player_msghdr_t* hdr, + void* data) +{ + Stg::ModelActuator* actmod = (Stg::ModelActuator*)this->mod; + + // Is it a new motor command? + if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_ACTARRAY_REQ_GET_GEOM, + this->addr)) + { + Stg::Geom geom = actmod->GetGeom(); + + // fill in the geometry data formatted player-like + // TODO actually get real data here + player_actarray_actuatorgeom_t actuator = {0}; + actuator.min = actmod->GetMinPosition(); + actuator.max = actmod->GetMaxPosition(); + actuator.type = PLAYER_ACTARRAY_TYPE_LINEAR; + + player_actarray_geom_t pgeom = {0}; + pgeom.base_pos.px = geom.pose.x; + pgeom.base_pos.py = geom.pose.y; + pgeom.base_pos.pz = geom.pose.z; + + pgeom.base_orientation.pyaw = geom.pose.a; + + pgeom.actuators_count = 1; + pgeom.actuators = &actuator; + + this->driver->Publish( this->addr, resp_queue, + PLAYER_MSGTYPE_RESP_ACK, + PLAYER_ACTARRAY_REQ_GET_GEOM, + (void*)&pgeom); + return 0; + } + // Is it a request to reset odometry? + else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, + PLAYER_ACTARRAY_CMD_POS, + this->addr)) + { + player_actarray_position_cmd_t &cmd = *reinterpret_cast<player_actarray_position_cmd_t*> (data); + actmod->GoTo(cmd.position); + return 0; + } + // Is it a request to reset odometry? + else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, + PLAYER_ACTARRAY_CMD_SPEED, + this->addr)) + { + player_actarray_speed_cmd_t &cmd = *reinterpret_cast<player_actarray_speed_cmd_t*> (data); + actmod->SetSpeed(cmd.speed); + return 0; + } + //else + + // Don't know how to handle this message. + PRINT_WARN2( "stg_actuator doesn't support msg with type %d subtype %d", + hdr->type, hdr->subtype); + return(-1); +} + +void InterfaceActArray::Publish( void ) +{ + Stg::ModelActuator* actmod = (Stg::ModelActuator*)this->mod; + + // TODO fill in values here + player_actarray_actuator_t act = {0}; + act.position = actmod->GetPosition(); + act.speed = actmod->GetSpeed(); + if (act.speed != 0) + act.state = PLAYER_ACTARRAY_ACTSTATE_MOVING; + else + act.state = PLAYER_ACTARRAY_ACTSTATE_IDLE; + + player_actarray_data_t actdata = {0}; + actdata.actuators_count = 1; + actdata.actuators = &act; + + + // publish this data + this->driver->Publish( this->addr, + PLAYER_MSGTYPE_DATA, PLAYER_ACTARRAY_DATA_STATE, + (void*)&actdata); +} Added: code/stage/trunk/libstageplugin/p_aio.cc =================================================================== --- code/stage/trunk/libstageplugin/p_aio.cc (rev 0) +++ code/stage/trunk/libstageplugin/p_aio.cc 2009-06-10 19:01:21 UTC (rev 7835) @@ -0,0 +1,79 @@ +/* + * Player - One Hell of a Robot Server + * Copyright (C) 2004, 2005 Richard Vaughan + * + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + * + */ + +/* + * Desc: A plugin driver for Player that gives access to Stage devices. + * Author: Richard Vaughan + * Date: 10 December 2004 + * CVS: $Id: p_laser.cc,v 1.2 2008-01-15 01:25:42 rtv Exp $ + */ + +// DOCUMENTATION ------------------------------------------------------------ + +/** @addtogroup player +@par AIO interface +- PLAYER_AIO_CMD_STATE +- PLAYER_AIO_DATA_STATE +*/ + +// CODE ---------------------------------------------------------------------- + +#include "p_driver.h" + +InterfaceAio::InterfaceAio( player_devaddr_t addr, + StgDriver* driver, + ConfigFile* cf, + int section ) + : InterfaceModel( addr, driver, cf, section, Stg::MODEL_TYPE_LOADCELL ) // TODO: AIO should not always mean a loadcell +{ +} + +void InterfaceAio::Publish( void ) +{ + Stg::ModelLoadCell* mod = (Stg::ModelLoadCell*)this->mod; + float voltage = mod->GetVoltage(); + + player_aio_data data; + data.voltages_count = 1; // Just one value + data.voltages = &voltage; // Might look bad but player does a deep copy so it should be ok. + + // Write data + this->driver->Publish(this->addr, + PLAYER_MSGTYPE_DATA, + PLAYER_AIO_DATA_STATE, + (void*)&data, sizeof(data), NULL); +} + +int InterfaceAio::ProcessMessage(QueuePointer & resp_queue, + player_msghdr_t* hdr, + void* data) +{ + Stg::ModelLoadCell* mod = (Stg::ModelLoadCell*)this->mod; + + // Is it a request to set the voltage? + if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_AIO_CMD_STATE, + this->addr)) + { + player_aio_cmd* cmd = (player_aio_cmd*)data; + PRINT_WARN2( "Got request to set AIO voltage %d to %f. Should not happen, AIO currently only works for load cells!\n", cmd->id, cmd->voltage ); + } +} Added: code/stage/trunk/libstageplugin/p_dio.cc =================================================================== --- code/stage/trunk/libstageplugin/p_dio.cc (rev 0) +++ code/stage/trunk/libstageplugin/p_dio.cc 2009-06-10 19:01:21 UTC (rev 7835) @@ -0,0 +1,37 @@ +#include "p_driver.h" + + +InterfaceDio::InterfaceDio(player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section) + : InterfaceModel( addr, driver, cf, section, Stg::MODEL_TYPE_LIGHTINDICATOR ) +{ +} + + +InterfaceDio::~InterfaceDio() +{ +} + + +int InterfaceDio::ProcessMessage(QueuePointer & resp_queue, player_msghdr_t* hdr, void* data) +{ + Stg::ModelLightIndicator* mod = (Stg::ModelLightIndicator*) this->mod; + + if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_DIO_CMD_VALUES, this->addr)) + { + player_dio_cmd* cmd = (player_dio_cmd*) data; + if(cmd->count != 1) + { + PRINT_ERR("Invalid light indicator command."); + } + + mod->SetState(cmd->digout != 0); + return 0; + } + + return -1; +} + +void InterfaceDio::Publish() +{ + +} This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-06-14 22:46:25
|
Revision: 7848 http://playerstage.svn.sourceforge.net/playerstage/?rev=7848&view=rev Author: rtv Date: 2009-06-14 22:45:25 +0000 (Sun, 14 Jun 2009) Log Message: ----------- cleaning up Block class Modified Paths: -------------- code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/region.hh code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2009-06-14 21:16:14 UTC (rev 7847) +++ code/stage/trunk/libstage/block.cc 2009-06-14 22:45:25 UTC (rev 7848) @@ -16,20 +16,23 @@ bool inherit_color ) : mod( mod ), - mpts(NULL), + mpts(), pt_count( pt_count ), - pts( (stg_point_t*)g_memdup( pts, pt_count * sizeof(stg_point_t)) ), + pts(), + local_z( zmin, zmax ), color( color ), inherit_color( inherit_color ), rendered_cells( new std::vector<Cell*> ), - candidate_cells( new std::vector<Cell*> ) + candidate_cells( new std::vector<Cell*> ), + gpts() { assert( mod ); assert( pt_count > 0 ); - assert( pts ); - local_z.min = zmin; - local_z.max = zmax; + // copy the argument point data into the member vector + this->pts.reserve( pt_count ); + for( size_t p=0; p<pt_count; p++ ) + this->pts.push_back( pts[p] ); } /** A from-file constructor */ @@ -37,13 +40,13 @@ Worldfile* wf, int entity) : mod( mod ), - mpts(NULL), + mpts(), pt_count(0), - pts(NULL), + pts(), color(0), inherit_color(true), - rendered_cells( new std::vector<Cell*> ), - candidate_cells( new std::vector<Cell*> ) + rendered_cells( new std::vector<Cell*> ), + candidate_cells( new std::vector<Cell*> ) { assert(mod); assert(wf); @@ -56,10 +59,7 @@ { if( mapped ) UnMap(); - if( pts ) delete[] pts; - InvalidateModelPointCache(); - - delete rendered_cells; + delete rendered_cells; delete candidate_cells; } @@ -256,48 +256,34 @@ (bpt.y - bgoffset.y) * (mod->geom.size.y/bgsize.y)); } -stg_point_t* Block::GetPointsInModelCoords() -{ - if( ! mpts ) - { - // no valid cache of model coord points, so generate them - mpts = new stg_point_t[pt_count]; - - for( unsigned int i=0; i<pt_count; i++ ) - mpts[i] = BlockPointToModelMeters( pts[i] ); - } - - return mpts; -} - void Block::InvalidateModelPointCache() { - // this doesn't happen often, so this simple strategy isn't too wasteful - if( mpts ) - { - delete[] mpts; - mpts = NULL; - } + // this doesn't happen often, so this simple strategy isn't too wasteful + mpts.clear(); } void Block::GenerateCandidateCells() { candidate_cells->clear(); - - stg_point_t* mpts = GetPointsInModelCoords(); - + + if( mpts.size() == 0 ) + { + // no valid cache of model coord points, so generate them + mpts.resize( pt_count ); + for( unsigned int i=0; i<pt_count; i++ ) + mpts[i] = BlockPointToModelMeters( pts[i] ); + } + // convert the mpts in model coords into global pixel coords - stg_point_int_t gpts[pt_count]; // should be plenty of room on the - // stack (crosses fingers). I don't - // want to pay for a malloc here. + gpts.resize(pt_count); + 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 ); - + mod->world->ForEachCellInLine( gpts[i], + gpts[(i+1)%pt_count], + *candidate_cells ); // set global Z Pose gpose = mod->GetGlobalPose(); gpose.z += mod->geom.pose.z; @@ -464,11 +450,8 @@ { //printf( "Block::Load entity %d\n", entity ); - if( pts ) - delete[] pts; - pt_count = wf->ReadInt( entity, "points", 0); - pts = new stg_point_t[ pt_count ]; + pts.resize( pt_count ); //printf( "reading %d points\n", // pt_count ); Modified: code/stage/trunk/libstage/region.hh =================================================================== --- code/stage/trunk/libstage/region.hh 2009-06-14 21:16:14 UTC (rev 7847) +++ code/stage/trunk/libstage/region.hh 2009-06-14 22:45:25 UTC (rev 7848) @@ -31,6 +31,10 @@ inline int32_t GETREG( const int32_t x ) { return( ( x & REGIONMASK ) >> RBITS); } inline int32_t GETSREG( const int32_t x ) { return( x >> SRBITS); } + // this is slightly faster than the inline method above, but not as safe + //#define GETREG(X) (( (static_cast<int32_t>(X)) & REGIONMASK ) >> RBITS) + + class Cell { friend class Region; Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-06-14 21:16:14 UTC (rev 7847) +++ code/stage/trunk/libstage/stage.hh 2009-06-14 22:45:25 UTC (rev 7848) @@ -370,12 +370,13 @@ class Bounds { public: - /// largest value in range, initially zero - double max; - /// smallest value in range, initially zero + /// smallest value in range, initially zero double min; + /// largest value in range, initially zero + double max; - Bounds() : max(0), min(0) { /* empty*/ } + Bounds() : min(0), max(0) { /* empty*/ } + Bounds( double min, double max ) : min(min), max(max) { /* empty*/ } }; /** Define a three-dimensional bounding box, initialized to zero */ @@ -1006,9 +1007,6 @@ void ExpireSuperRegion( SuperRegion* sr ); inline Cell* GetCell( const stg_point_int_t& glob ); - //inline Cell* GetCellNoCreate( const int32_t x, const int32_t y ); - //inline Cell* GetCellCreate( const int32_t x, const int32_t y ); - //inline Cell* GetCellCreate( const stg_point_int_t& glob ); /** add a Cell pointer to the vector for each cell on the line from pt1 to pt2 inclusive */ @@ -1164,75 +1162,58 @@ ~Block(); /** render the block into the world's raytrace data structure */ - void Map(); - + void Map(); /** remove the block from the world's raytracing data structure */ - void UnMap(); - + void UnMap(); void Draw( Model* mod ); void DrawSolid(); // draw the block in OpenGL as a solid single color - void DrawFootPrint(); // draw the projection of the block onto the z=0 plane - - /** Translate all points in the block by the indicated amounts */ - void Translate( double x, double y ); - - /** Return the center of the block on the X axis */ - double CenterX(); + void DrawFootPrint(); // draw the projection of the block onto the z=0 plane + /** Translate all points in the block by the indicated amounts */ + void Translate( double x, double y ); + /** Return the center of the block on the X axis */ + double CenterX(); /** Return the center of the block on the Y axis */ - double CenterY(); + double CenterY(); + /** Set the center of the block on the X axis */ + void SetCenterX( double y ); + /** Set the center of the block on the Y axis */ + void SetCenterY( double y ); + /** Set the center of the block */ + void SetCenter( double x, double y); + void SetZ( double min, double max ); - /** Set the center of the block on the X axis */ - void SetCenterX( double y ); - /** Set the center of the block on the Y axis */ - void SetCenterY( double y ); - /** Set the center of the block */ - void SetCenter( double x, double y); - - void SetZ( double min, double max ); - void RecordRendering( Cell* cell ) - { - rendered_cells->push_back( cell ); - } - + { rendered_cells->push_back( cell ); } + stg_point_t* Points( unsigned int *count ) - { if( count ) *count = pt_count; return pts; }; - - //bool IntersectGlobalZ( stg_meters_t z ) - //{ return( z >= global_zmin && z <= global_zmax ); } - + { if( count ) *count = pt_count; return &pts[0]; }; + + std::vector<stg_point_t>& Points() + { return pts; }; + void AddToCellArray( std::vector<Cell*>* blocks ); void RemoveFromCellArray( std::vector<Cell*>* blocks ); + void GenerateCandidateCells(); + GList* AppendTouchingModels( GList* list ); - void GenerateCandidateCells(); - - // /** Prepare to render the block in a new position in global coordinates */ - // void SetPoseTentative( const Pose pose ); - - - GList* AppendTouchingModels( GList* list ); - - /** 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; }; - - stg_color_t GetColor(); - - void Rasterize( uint8_t* data, - unsigned int width, unsigned int height, - stg_meters_t cellwidth, stg_meters_t cellheight ); - + /** 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; }; + stg_color_t GetColor(); + void Rasterize( uint8_t* data, + unsigned int width, unsigned int height, + stg_meters_t cellwidth, stg_meters_t cellheight ); + private: Model* mod; ///< model to which this block belongs - stg_point_t* mpts; ///< cache of this->pts in model coordindates + std::vector<stg_point_t> mpts; ///< cache of this->pts in model coordindates size_t pt_count; ///< the number of points - stg_point_t* pts; ///< points defining a polygon + + std::vector<stg_point_t> pts; ///< points defining a polygonx + //stg_point_t* pts; ///< points defining a polygon Size size; @@ -1261,12 +1242,14 @@ switched for next time (avoiding a memory copy).*/ std::vector<Cell*> * candidate_cells; + std::vector<stg_point_int_t> gpts; + /** find the position of a block's point in model coordinates (m) */ stg_point_t BlockPointToModelMeters( const stg_point_t& bpt ); /** Update the cache of block points converted to model coordinates */ - stg_point_t* GetPointsInModelCoords(); + //stg_point_t* GetPointsInModelCoords(); /** invalidate the cache of points in model coordinates */ void InvalidateModelPointCache(); Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-06-14 21:16:14 UTC (rev 7847) +++ code/stage/trunk/libstage/world.cc 2009-06-14 22:45:25 UTC (rev 7848) @@ -757,7 +757,7 @@ c += sy * REGIONWIDTH; // move the cell up or down cy += sy; // cell coordinate for bounds checking } - n--; // decrement the manhattan distance remaining + --n; // decrement the manhattan distance remaining //rt_cells.push_back( stg_point_int_t( globx, globy )); } @@ -945,46 +945,69 @@ const stg_point_int_t& end, std::vector<Cell*>& cells ) { + // 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); - int dx = end.x - start.x; - int dy = end.y - start.y; - - stg_point_int_t cell = start; + int32_t globx(start.x); + int32_t globy(start.y); - // line rasterization adapted from Cohen's 3D version in - // Graphics Gems II. Should be very fast. - - const int sx(sgn(dx)); - const int sy(sgn(dy)); - const int ax(abs(dx)); - const int ay(abs(dy)); - const int bx(2*ax); - const int by(2*ay); - int exy(ay-ax); - int n(ax+ay); - // fix a little issue where the edges are not drawn long enough // when drawing to the right or up - if( (dx > 0) || ( dy > 0 ) ) - n++; + // if( (dx > 0) || ( dy > 0 ) ) + // n++; - while( n-- ) + while( n ) { - // find the cell at this location, then add it to the vector - cells.push_back( GetCell( cell ) ); + 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) ); + + // 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 ); - // cleverly skip to the next cell - if( exy < 0 ) - { - cell.x += sx; - exy += by; + 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; } - else - { - cell.y += sy; - exy -= bx; - } - } + + } } void World::Extend( stg_point3_t pt ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-06-21 23:15:57
|
Revision: 7868 http://playerstage.svn.sourceforge.net/playerstage/?rev=7868&view=rev Author: rtv Date: 2009-06-21 23:15:55 +0000 (Sun, 21 Jun 2009) Log Message: ----------- more STLization. Simple.world benchmark now below below 4 seconds on my Mac. Modified Paths: -------------- code/stage/trunk/libstage/blockgroup.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/waypoint.cc code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/blockgroup.cc =================================================================== --- code/stage/trunk/libstage/blockgroup.cc 2009-06-21 13:59:20 UTC (rev 7867) +++ code/stage/trunk/libstage/blockgroup.cc 2009-06-21 23:15:55 UTC (rev 7868) @@ -7,11 +7,11 @@ #undef DEBUG using namespace Stg; +using namespace std; BlockGroup::BlockGroup() : displaylist(0), - blocks(NULL), - count(0), + blocks(), minx(0), maxx(0), miny(0), @@ -25,33 +25,46 @@ void BlockGroup::AppendBlock( Block* block ) { - blocks = g_list_append( blocks, block ); - ++count; + blocks.push_back( block ); } void BlockGroup::Clear() { - while( blocks ) - { - delete (Block*)blocks->data; - blocks = blocks->next; - } +// while( blocks ) +// { +// delete (Block*)blocks->data; +// blocks = blocks->next; +// } - g_list_free( blocks ); - blocks = NULL; + //g_list_free( blocks ); + //blocks = NULL; + + for( vector<Block*>::iterator it( blocks.begin() ); + it != blocks.end(); + ++it ) + delete *it; + + blocks.clear(); } void BlockGroup::SwitchToTestedCells() { // confirm the tentative pose for all blocks - LISTMETHOD( blocks, Block*, SwitchToTestedCells ); + //LISTMETHOD( blocks, Block*, SwitchToTestedCells ); + + for( vector<Block*>::iterator it( blocks.begin() ); + it != blocks.end(); + ++it ) + (*it)->SwitchToTestedCells(); } GList* BlockGroup::AppendTouchingModels( GList* list ) { - for( GList* it=blocks; it; it = it->next ) - list = ((Block*)it->data)->AppendTouchingModels( list ); + for( vector<Block*>::iterator it( blocks.begin() ); + it != blocks.end(); + ++it ) + list = (*it)->AppendTouchingModels( list ); return list; } @@ -60,9 +73,16 @@ { Model* hitmod = NULL; - for( GList* it=blocks; it; it = it->next ) - if( (hitmod = ((Block*)it->data)->TestCollision())) - break; // bail on the earliest collision + for( vector<Block*>::iterator it( blocks.begin() ); + it != blocks.end(); + ++it ) + 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 } @@ -78,10 +98,13 @@ size.z = 0.0; // grow to largest z we see - for( GList* it=blocks; it; it=it->next ) // examine all the blocks + for( vector<Block*>::iterator it( blocks.begin() ); + it != blocks.end(); + ++it ) + // for( GList* it=blocks; it; it=it->next ) // examine all the blocks { // examine all the points in the polygon - Block* block = (Block*)it->data; + Block* block = *it; for( unsigned int p=0; p < block->pt_count; p++ ) { @@ -109,12 +132,22 @@ void BlockGroup::Map() { - LISTMETHOD( blocks, Block*, Map ); + for( std::vector<Block*>::iterator it( blocks.begin() ); + it != blocks.end(); + ++it ) + (*it)->Map(); + + //LISTMETHOD( blocks, Block*, Map ); } void BlockGroup::UnMap() { - LISTMETHOD( blocks, Block*, UnMap ); + for( std::vector<Block*>::iterator it( blocks.begin() ); + it != blocks.end(); + ++it ) + (*it)->UnMap(); + + //LISTMETHOD( blocks, Block*, UnMap ); } void BlockGroup::DrawSolid( const Geom & geom ) @@ -128,9 +161,13 @@ geom.size.z / size.z ); glTranslatef( -offset.x, -offset.y, -offset.z ); + + for( std::vector<Block*>::iterator it( blocks.begin() ); + it != blocks.end(); + ++it ) + (*it)->DrawSolid(); + // LISTMETHOD( blocks, Block*, DrawSolid ); - LISTMETHOD( blocks, Block*, DrawSolid ); - glPopMatrix(); } @@ -144,7 +181,11 @@ glTranslatef( -offset.x, -offset.y, -offset.z ); - LISTMETHOD( blocks, Block*, DrawFootPrint); + for( std::vector<Block*>::iterator it( blocks.begin() ); + it != blocks.end(); + ++it ) + (*it)->DrawFootPrint(); +// LISTMETHOD( blocks, Block*, DrawFootPrint); glPopMatrix(); } @@ -181,9 +222,12 @@ mod->PushColor( mod->color ); - for( GList* it=blocks; it; it=it->next ) + for( vector<Block*>::iterator it( blocks.begin() ); + it != blocks.end(); + ++it ) + //for( GList* it=blocks; it; it=it->next ) { - Block* blk = (Block*)it->data; + Block* blk = (*it); if( (!blk->inherit_color) && (blk->color != mod->color) ) { @@ -206,9 +250,12 @@ 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( GList* it=blocks; it; it=it->next ) + for( vector<Block*>::iterator it( blocks.begin() ); + it != blocks.end(); + ++it ) + //for( GList* it=blocks; it; it=it->next ) { - Block* blk = (Block*)it->data; + Block* blk = *it; if( (!blk->inherit_color) && (blk->color != mod->color) ) { @@ -318,6 +365,9 @@ stg_meters_t cellwidth, stg_meters_t cellheight ) { - for( GList* it = blocks; it; it=it->next ) - ((Block*)it->data)->Rasterize( data, width, height, cellwidth, cellheight ); + for( vector<Block*>::iterator it( blocks.begin() ); + it != blocks.end(); + ++it ) + //for( GList* it = blocks; it; it=it->next ) + (*it)->Rasterize( data, width, height, cellwidth, cellheight ); } Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2009-06-21 13:59:20 UTC (rev 7867) +++ code/stage/trunk/libstage/model_position.cc 2009-06-21 23:15:55 UTC (rev 7868) @@ -654,13 +654,16 @@ glTranslatef( 0,0,0.02 ); // draw waypoints + glLineWidth( 3 ); for( unsigned int i=0; i < waypoint_count; i++ ) - waypoints[i].Draw(); - + waypoints[i].Draw(); + glLineWidth( 1 ); + // draw lines connecting the waypoints if( waypoint_count > 1 ) { - glBegin( GL_LINES ); + pos->PushColor( 1,0,0,0.3 ); + glBegin( GL_LINES ); for( unsigned int i=1; i < waypoint_count; i++ ) { @@ -669,6 +672,8 @@ } glEnd(); + + pos->PopColor(); } pos->PopColor(); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-06-21 13:59:20 UTC (rev 7867) +++ code/stage/trunk/libstage/stage.hh 2009-06-21 23:15:55 UTC (rev 7868) @@ -1265,8 +1265,8 @@ int displaylist; void BuildDisplayList( Model* mod ); - GList* blocks; - uint32_t count; + + std::vector<Block*> blocks; Size size; stg_point3_t offset; stg_meters_t minx, maxx, miny, maxy; @@ -1275,8 +1275,8 @@ BlockGroup(); ~BlockGroup(); - GList* GetBlocks(){ return blocks; }; - uint32_t GetCount(){ return count; }; + //std::vector<Block*>&GList* GetBlocks(){ return blocks; }; + uint32_t GetCount(){ return blocks.size(); }; Size GetSize(){ return size; }; stg_point3_t GetOffset(){ return offset; }; @@ -1312,7 +1312,13 @@ stg_meters_t cellwidth, stg_meters_t cellheight ); void InvalidateModelPointCache() - { LISTMETHOD( blocks, Block*, InvalidateModelPointCache ); } + { + for( std::vector<Block*>::iterator it( blocks.begin() ); + it != blocks.end(); + ++it ) + (*it)->InvalidateModelPointCache(); + } + }; Modified: code/stage/trunk/libstage/waypoint.cc =================================================================== --- code/stage/trunk/libstage/waypoint.cc 2009-06-21 13:59:20 UTC (rev 7867) +++ code/stage/trunk/libstage/waypoint.cc 2009-06-21 23:15:55 UTC (rev 7868) @@ -33,7 +33,7 @@ glVertex3f( pose.x, pose.y, pose.z ); glEnd(); - stg_meters_t quiver_length = 0.1; + stg_meters_t quiver_length = 0.15; double dx = cos(pose.a) * quiver_length; double dy = sin(pose.a) * quiver_length; Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-06-21 13:59:20 UTC (rev 7867) +++ code/stage/trunk/libstage/world.cc 2009-06-21 23:15:55 UTC (rev 7868) @@ -940,10 +940,10 @@ ->GetCell( GETCELL(glob.x), GETCELL(glob.y) )) ; } - + void World::ForEachCellInLine( const stg_point_int_t& start, - const stg_point_int_t& end, - std::vector<Cell*>& cells ) + const stg_point_int_t& end, + std::vector<Cell*>& cells ) { // line rasterization adapted from Cohen's 3D version in // Graphics Gems II. Should be very fast. This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-06-23 08:08:59
|
Revision: 7877 http://playerstage.svn.sourceforge.net/playerstage/?rev=7877&view=rev Author: rtv Date: 2009-06-23 08:08:05 +0000 (Tue, 23 Jun 2009) Log Message: ----------- more STL-ization. may reduce performance slightly in worlds with few models Modified Paths: -------------- code/stage/trunk/libstage/ancestor.cc code/stage/trunk/libstage/blockgroup.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_callbacks.cc code/stage/trunk/libstage/model_draw.cc code/stage/trunk/libstage/model_getset.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/ancestor.cc =================================================================== --- code/stage/trunk/libstage/ancestor.cc 2009-06-23 07:38:04 UTC (rev 7876) +++ code/stage/trunk/libstage/ancestor.cc 2009-06-23 08:08:05 UTC (rev 7877) @@ -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() { - 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 @@ mod->SetToken( buf ); - children = g_list_append( children, mod ); + children.push_back( mod ); child_type_counts[mod->type]++; @@ -52,7 +47,9 @@ 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 @@ 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 ); - } + } } Modified: code/stage/trunk/libstage/blockgroup.cc =================================================================== --- code/stage/trunk/libstage/blockgroup.cc 2009-06-23 07:38:04 UTC (rev 7876) +++ code/stage/trunk/libstage/blockgroup.cc 2009-06-23 08:08:05 UTC (rev 7877) @@ -30,42 +30,23 @@ 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* 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 @@ 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::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 @@ 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 @@ 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 @@ 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 @@ 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 @@ 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 ); } Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-06-23 07:38:04 UTC (rev 7876) +++ code/stage/trunk/libstage/canvas.cc 2009-06-23 08:08:05 UTC (rev 7877) @@ -204,9 +204,9 @@ // 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 @@ 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 @@ // 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 ); Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-06-23 07:38:04 UTC (rev 7876) +++ code/stage/trunk/libstage/model.cc 2009-06-23 08:08:05 UTC (rev 7877) @@ -275,17 +275,15 @@ // 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::InitRecursive() { // init children first - LISTMETHOD( children, Model*, InitRecursive ); + FOR_EACH( it, children ) + (*it)->Init(); + Init(); } @@ -531,12 +531,9 @@ 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 @@ 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 @@ 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 @@ 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 @@ 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* 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::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 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 @@ 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 @@ } // 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 @@ 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; } Modified: code/stage/trunk/libstage/model_callbacks.cc =================================================================== --- code/stage/trunk/libstage/model_callbacks.cc 2009-06-23 07:38:04 UTC (rev 7876) +++ code/stage/trunk/libstage/model_callbacks.cc 2009-06-23 08:08:05 UTC (rev 7877) @@ -15,8 +15,8 @@ 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,13 +78,13 @@ //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 @@ 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; Modified: code/stage/trunk/libstage/model_draw.cc =================================================================== --- code/stage/trunk/libstage/model_draw.cc 2009-06-23 07:38:04 UTC (rev 7876) +++ code/stage/trunk/libstage/model_draw.cc 2009-06-23 08:08:05 UTC (rev 7877) @@ -152,15 +152,19 @@ 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::DrawBoundingBoxTree() { PushLocalCoords(); - LISTMETHOD( children, Model*, DrawBoundingBoxTree ); + + FOR_EACH( it, children ) + (*it)->DrawBoundingBoxTree(); + DrawBoundingBox(); PopCoords(); } @@ -284,7 +291,8 @@ { PushLocalCoords(); DrawStatus( cam ); - LISTMETHODARG( children, Model*, DrawStatusTree, cam ); + FOR_EACH( it, children ) + (*it)->DrawStatusTree( cam ); PopCoords(); } @@ -526,7 +534,8 @@ 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 @@ } // and draw the children - LISTMETHODARG( children, Model*, DataVisualizeTree, cam ); + FOR_EACH( it, children ) + (*it)->DataVisualizeTree( cam ); PopCoords(); } Modified: code/stage/trunk/libstage/model_getset.cc =================================================================== --- code/stage/trunk/libstage/model_getset.cc 2009-06-23 07:38:04 UTC (rev 7876) +++ code/stage/trunk/libstage/model_getset.cc 2009-06-23 08:08:05 UTC (rev 7877) @@ -137,11 +137,13 @@ 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; Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-06-23 07:38:04 UTC (rev 7876) +++ code/stage/trunk/libstage/stage.hh 2009-06-23 08:08:05 UTC (rev 7877) @@ -732,7 +732,12 @@ 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 @@ 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 @@ 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 @@ 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 @@ 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 @@ 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 @@ 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 Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-06-23 07:38:04 UTC (rev 7876) +++ code/stage/trunk/libstage/world.cc 2009-06-23 08:08:05 UTC (rev 7877) @@ -60,8 +60,8 @@ 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 @@ 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 @@ 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 @@ 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 @@ 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 @@ { // 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 @@ //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 @@ else { PRINT_ERR1( "Unknown model type %s in world file.", - typestr ); + typestr ); exit( 1 ); } @@ -239,10 +237,10 @@ 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 @@ 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 @@ 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 ); + 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 ); - } + printf( "[threadpool %u]", worker_threads ); + } } // Iterate through entitys and create objects of the appropriate type @@ -328,15 +326,15 @@ // 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 @@ // 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 @@ 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 @@ } 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 @@ } 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 @@ // 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 @@ 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::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 @@ 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 @@ // 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 @@ 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 @@ // inline calls have a noticeable (2-3%) effect on performance while( n > 0 ) // while we are still not at the ray end { - Region* reg( GetSuperRegionCached( GETSREG(globx), GETSREG(globy) ) - ->GetRegion( GETREG(globx), GETREG(globy) )); + Region* reg( GetSuperRegionCached( GETSREG(globx), GETSREG(globy) ) + ->GetRegion( GETREG(globx), GETREG(globy) )); - if( reg->count ) // if the region contains any objects - { - // invalidate the region crossing points used to jump over - // empty regions - calculatecrossings = true; + if( reg->count ) // if the region contains any objects + { + // invalidate the region crossing points used to jump over + // empty regions + calculatecrossings = true; - // convert from global cell to local cell coords - int32_t cx( GETCELL(globx) ); - int32_t cy( GETCELL(globy) ); + // convert from global cell to local cell coords + int32_t cx( GETCELL(globx) ); + int32_t cy( GETCELL(globy) ); - Cell* c( ®->cells[ cx + cy * REGIONWIDTH ] ); - assert(c); // should be good: we know the region contains objects + Cell* c( ®->cells[ cx + cy * REGIONWIDTH ] ); + assert(c); // should be good: we know the region contains objects - // while within the bounds of this region and while some ray remains - // we'll tweak the cell pointer directly to move around quickly - while( (cx>=0) && (cx<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; - } - } + 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 + // 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; + 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; + //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 @@ 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 @@ // 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 @@ 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 ); + // 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 ); + 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::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 ) Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2009-06-23 07:38:04 UTC (rev 7876) +++ code/stage/trunk/libstage/worldgui.cc 2009-06-23 08:08:05 UTC (rev 7877) @@ -857,18 +857,18 @@ 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::DrawBoundingBoxTree() { - LISTMETHOD( World::children, Model*, DrawBoundingBoxTree ); + FOR_EACH( it, World::children ) + (*it)->DrawBoundingBoxTree(); } void WorldGui::PushColor( stg_color_t col ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-06-30 22:26:47
|
Revision: 7932 http://playerstage.svn.sourceforge.net/playerstage/?rev=7932&view=rev Author: rtv Date: 2009-06-30 22:26:15 +0000 (Tue, 30 Jun 2009) Log Message: ----------- tweaking velocity & charge lists Modified Paths: -------------- code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/model_load.cc =================================================================== --- code/stage/trunk/libstage/model_load.cc 2009-06-30 22:03:37 UTC (rev 7931) +++ code/stage/trunk/libstage/model_load.cc 2009-06-30 22:26:15 UTC (rev 7932) @@ -56,8 +56,9 @@ PRINT_WARN1( "Model %s: Setting \"watts_give\" has no effect unless \"joules\" is specified for this model or a parent", token ); if( watts_give ) // need to get the world to test this model for charging - if( ! g_list_find( world->charge_list, this ) ) - world->charge_list = g_list_append( world->charge_list, this ); + //if( ! g_list_find( world->charge_list, this ) ) + //world->charge_list = g_list_append( world->charge_list, this ); + world->ChargeListAdd( this ); watts_take = wf->ReadFloat( wf_entity, "take_watts", watts_take ); if( (watts_take > 0.0) & !pp ) Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-06-30 22:03:37 UTC (rev 7931) +++ code/stage/trunk/libstage/stage.hh 2009-06-30 22:26:15 UTC (rev 7932) @@ -940,7 +940,7 @@ static void UpdateCb( World* world); static unsigned int next_id; ///<initially zero, used to allocate unique sequential world ids - GList* charge_list; ///< Models which receive charge are listed here + std::set<Model*> charge_list; ///< Models which receive charge are listed here bool destroy; bool dirty; ///< iff true, a gui redraw would be required GList* event_list; //< @@ -956,7 +956,7 @@ unsigned int show_clock_interval; ///< updates between clock xoutputs GMutex* thread_mutex; ///< protect the worker thread management stuff int total_subs; ///< the total number of subscriptions to all models - std::vector<Model*> velocity_list; ///< Models with non-zero velocity and should have their poses updated + std::set<Model*> velocity_list; ///< Models with non-zero velocity and should have their poses updated unsigned int worker_threads; ///< the number of worker threads to use unsigned int threads_working; ///< the number of worker threads not yet finished @@ -980,6 +980,7 @@ std::map<stg_point_int_t,SuperRegion*> superregions; SuperRegion* sr_cached; ///< The last superregion looked up by this world + // todo - test performance of std::set std::vector<std::vector<Model*> > update_lists; long unsigned int updates; ///< the number of simulated time steps executed so far @@ -1105,6 +1106,9 @@ void VelocityListAdd( Model* mod ); void VelocityListRemove( Model* mod ); + + void ChargeListAdd( Model* mod ); + void ChargeListRemove( Model* mod ); static gpointer update_thread_entry( std::pair<World*,int>* info ); Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-06-30 22:03:37 UTC (rev 7931) +++ code/stage/trunk/libstage/world.cc 2009-06-30 22:26:15 UTC (rev 7932) @@ -78,7 +78,7 @@ double ppm ) : // private - charge_list( NULL ), + charge_list(), destroy( false ), dirty( true ), models_by_name( g_hash_table_new( g_str_hash, g_str_equal ) ), @@ -538,7 +538,8 @@ // test all models that supply charge to see if they are touching // something that takes charge - LISTMETHOD( charge_list, Model*, UpdateCharge ); + FOR_EACH( it, charge_list ) + (*it)->UpdateCharge(); // then update all models on the update lists FOR_EACH( it, update_lists[0] ) @@ -1091,14 +1092,24 @@ void World::VelocityListAdd( Model* mod ) { - velocity_list.push_back( mod ); + velocity_list.insert( mod ); } void World::VelocityListRemove( Model* mod ) { - velocity_list.erase( remove( velocity_list.begin(), velocity_list.end(), mod )); + velocity_list.erase( mod ); } +void World::ChargeListAdd( Model* mod ) +{ + charge_list.insert( mod ); +} + +void World::ChargeListRemove( Model* mod ) +{ + charge_list.erase( mod ); +} + stg_usec_t World::SimTimeNow(void) { return sim_time; } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-07-01 17:50:46
|
Revision: 7935 http://playerstage.svn.sourceforge.net/playerstage/?rev=7935&view=rev Author: rtv Date: 2009-07-01 17:50:43 +0000 (Wed, 01 Jul 2009) Log Message: ----------- cleaning up and STLization of BlockGroup Modified Paths: -------------- code/stage/trunk/libstage/ancestor.cc code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/blockgroup.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_getset.cc code/stage/trunk/libstage/stage.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/ancestor.cc =================================================================== --- code/stage/trunk/libstage/ancestor.cc 2009-07-01 15:37:14 UTC (rev 7934) +++ code/stage/trunk/libstage/ancestor.cc 2009-07-01 17:50:43 UTC (rev 7935) @@ -5,7 +5,6 @@ Ancestor::Ancestor() : children(), debug( false ), - puck_list( NULL ), token( NULL ) { for( int i=0; i<MODEL_TYPE_COUNT; i++ ) Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2009-07-01 15:37:14 UTC (rev 7934) +++ code/stage/trunk/libstage/block.cc 2009-07-01 17:50:43 UTC (rev 7935) @@ -19,47 +19,47 @@ mpts(), pt_count( pt_count ), pts(), - local_z( zmin, zmax ), + local_z( zmin, zmax ), color( color ), inherit_color( inherit_color ), - rendered_cells( new std::vector<Cell*> ), - candidate_cells( new std::vector<Cell*> ), - gpts() + rendered_cells( new CellPtrVec ), + candidate_cells( new CellPtrVec ), + gpts() { assert( mod ); assert( pt_count > 0 ); - - // copy the argument point data into the member vector - this->pts.reserve( pt_count ); - for( size_t p=0; p<pt_count; p++ ) - this->pts.push_back( pts[p] ); + + // copy the argument point data into the member vector + this->pts.reserve( pt_count ); + for( size_t p=0; p<pt_count; p++ ) + this->pts.push_back( pts[p] ); } /** A from-file constructor */ Block::Block( Model* mod, - Worldfile* wf, - int entity) + Worldfile* wf, + int entity) : mod( mod ), - mpts(), + mpts(), pt_count(0), pts(), color(0), inherit_color(true), - rendered_cells( new std::vector<Cell*> ), - candidate_cells( new std::vector<Cell*> ) + rendered_cells( new CellPtrVec ), + candidate_cells( new CellPtrVec ) { assert(mod); assert(wf); assert(entity); - + Load( wf, entity ); } Block::~Block() { if( mapped ) UnMap(); - - delete rendered_cells; + + delete rendered_cells; delete candidate_cells; } @@ -70,7 +70,7 @@ pts[p].x += x; pts[p].y += y; } - + mod->blockgroup.BuildDisplayList( mod ); } @@ -78,13 +78,13 @@ { double min = billion; double max = -billion; - + for( unsigned int p=0; p<pt_count; p++) { if( pts[p].y > max ) max = pts[p].y; if( pts[p].y < min ) min = pts[p].y; } - + // return the value half way between max and min return( min + (max - min)/2.0 ); } @@ -93,7 +93,7 @@ { double min = billion; double max = -billion; - + for( unsigned int p=0; p<pt_count; p++) { if( pts[p].x > max ) max = pts[p].x; @@ -134,34 +134,21 @@ mod->blockgroup.BuildDisplayList( mod ); } - stg_color_t Block::GetColor() { return( inherit_color ? mod->color : color ); } -GList* Block::AppendTouchingModels( GList* l ) +void Block::AppendTouchingModels( ModelPtrSet& touchers ) { // for every cell we are rendered into - for( unsigned int i=0; i<rendered_cells->size(); i++ ) - { - Cell* c = (*rendered_cells)[i]; - - // for every block rendered into that cell - for( std::vector<Block*>::iterator it = c->blocks.begin(); - it != c->blocks.end(); - ++it ) - { - //Block* testblock = *it; - Model* testmod = (*it)->mod; - - if( !mod->IsRelated( testmod )) - if( ! g_list_find( l, testmod ) ) - l = g_list_append( l, testmod ); - } - } - - return l; + 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 ); + } } Model* Block::TestCollision() @@ -170,33 +157,29 @@ // find the set of cells we would render into given the current global pose GenerateCandidateCells(); - + if( mod->vis.obstacle_return ) // for every cell we may be rendered into - for( unsigned int i=0; i<candidate_cells->size(); i++ ) + FOR_EACH( cell_it, *candidate_cells ) { - Cell* c = (*candidate_cells)[i]; - - // for every rendered into that cell - for( std::vector<Block*>::iterator it = c->blocks.begin(); - it != c->blocks.end(); - ++it ) - { - Model* testmod = (*it)->mod; - - //printf( " testing block %p of model %s\n", testblock, testmod->Token() ); - - // if the tested model is an obstacle and it's not attached to this model - if( (testmod != this->mod) && - testmod->vis.obstacle_return && - !mod->IsRelated( testmod )) - { - //puts( "HIT"); - return testmod; // bail immediately with the bad news - } - } + // for every rendered into that cell + FOR_EACH( block_it, (*cell_it)->blocks ) + { + Model* testmod = (*block_it)->mod; + + //printf( " testing block %p of model %s\n", testblock, testmod->Token() ); + + // if the tested model is an obstacle and it's not attached to this model + if( (testmod != this->mod) && + testmod->vis.obstacle_return && + !mod->IsRelated( testmod )) + { + //puts( "HIT"); + return testmod; // bail immediately with the bad news + } + } } - + //printf( "model %s block %p collision done. no hits.\n", mod->Token(), this ); return NULL; // no hit } @@ -218,20 +201,16 @@ mapped = false; } -void Block::RemoveFromCellArray( std::vector<Cell*> * cells ) +void Block::RemoveFromCellArray( CellPtrVec *cells ) { - for( std::vector<Cell*>::iterator it = cells->begin(); - it != cells->end(); - ++it ) - (*it)->RemoveBlock( this); + FOR_EACH( it, *cells ) + (*it)->RemoveBlock( this); } -void Block::AddToCellArray( std::vector<Cell*> * cells ) +void Block::AddToCellArray( CellPtrVec *cells ) { - for( std::vector<Cell*>::iterator it = cells->begin(); - it != cells->end(); - ++it ) - (*it)->AddBlock( this); + FOR_EACH( it, *cells ) + (*it)->AddBlock( this); } void Block::SwitchToTestedCells() @@ -240,7 +219,7 @@ AddToCellArray( candidate_cells ); // switch current and candidate cell pointers - std::vector<Cell*> * tmp = rendered_cells; + CellPtrVec *tmp = rendered_cells; rendered_cells = candidate_cells; candidate_cells = tmp; @@ -253,47 +232,47 @@ stg_point3_t bgoffset = mod->blockgroup.GetOffset(); return stg_point_t( (bpt.x - bgoffset.x) * (mod->geom.size.x/bgsize.x), - (bpt.y - bgoffset.y) * (mod->geom.size.y/bgsize.y)); + (bpt.y - bgoffset.y) * (mod->geom.size.y/bgsize.y)); } void Block::InvalidateModelPointCache() { - // this doesn't happen often, so this simple strategy isn't too wasteful - mpts.clear(); + // this doesn't happen often, so this simple strategy isn't too wasteful + mpts.clear(); } void Block::GenerateCandidateCells() { candidate_cells->clear(); - + if( mpts.size() == 0 ) - { - // no valid cache of model coord points, so generate them - mpts.resize( pt_count ); - for( unsigned int i=0; i<pt_count; i++ ) - mpts[i] = BlockPointToModelMeters( pts[i] ); - } - + { + // no valid cache of model coord points, so generate them + mpts.resize( pt_count ); + for( unsigned int i=0; i<pt_count; i++ ) + mpts[i] = BlockPointToModelMeters( pts[i] ); + } + // 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[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 ); + mod->world->ForEachCellInLine( gpts[i], + gpts[(i+1)%pt_count], + *candidate_cells ); // set global Z Pose gpose = mod->GetGlobalPose(); gpose.z += mod->geom.pose.z; double scalez = mod->geom.size.z / mod->blockgroup.GetSize().z; stg_meters_t z = gpose.z - mod->blockgroup.GetOffset().z; - + // store the block's absolute z bounds at this rendering global_z.min = (scalez * local_z.min) + z; global_z.max = (scalez * local_z.max) + z; - + mapped = true; } @@ -305,74 +284,74 @@ } void Block::Rasterize( uint8_t* data, - unsigned int width, - unsigned int height, - stg_meters_t cellwidth, - stg_meters_t cellheight ) + unsigned int width, + unsigned int height, + stg_meters_t cellwidth, + stg_meters_t cellheight ) { //printf( "rasterize block %p : w: %u h: %u scale %.2f %.2f offset %.2f %.2f\n", // this, width, height, scalex, scaley, offsetx, offsety ); - + for( unsigned int i=0; i<pt_count; i++ ) { - // convert points from local to model coords - stg_point_t mpt1 = BlockPointToModelMeters( pts[i] ); - stg_point_t mpt2 = BlockPointToModelMeters( pts[(i+1)%pt_count] ); - - // record for debug visualization - mod->rastervis.AddPoint( mpt1.x, mpt1.y ); - - // shift to the bottom left of the model - mpt1.x += mod->geom.size.x/2.0; - mpt1.y += mod->geom.size.y/2.0; - mpt2.x += mod->geom.size.x/2.0; - mpt2.y += mod->geom.size.y/2.0; - - // convert from meters to cells - stg_point_int_t a( floor( mpt1.x / cellwidth ), - floor( mpt1.y / cellheight )); - stg_point_int_t b( floor( mpt2.x / cellwidth ), - floor( mpt2.y / cellheight ) ); - - bool steep = abs( b.y-a.y ) > abs( b.x-a.x ); - if( steep ) - { - swap( a.x, a.y ); - swap( b.x, b.y ); - } - - if( a.x > b.x ) - { - swap( a.x, b.x ); - swap( a.y, b.y ); - } - - double dydx = (double) (b.y - a.y) / (double) (b.x - a.x); - double y = a.y; - for(int x=a.x; x<=b.x; x++) - { - if( steep ) - { - if( ! (floor(y) >= 0) ) continue; - if( ! (floor(y) < (int)width) ) continue; - if( ! (x >= 0) ) continue; - if( ! (x < (int)height) ) continue; - } - else - { - if( ! (x >= 0) ) continue; - if( ! (x < (int)width) ) continue; - if( ! (floor(y) >= 0) ) continue; - if( ! (floor(y) < (int)height) ) continue; - } - - if( steep ) - data[ (int)floor(y) + (x * width)] = 1; - else - data[ x + ((int)floor(y) * width)] = 1; - y += dydx; - } - } + // convert points from local to model coords + stg_point_t mpt1 = BlockPointToModelMeters( pts[i] ); + stg_point_t mpt2 = BlockPointToModelMeters( pts[(i+1)%pt_count] ); + + // record for debug visualization + mod->rastervis.AddPoint( mpt1.x, mpt1.y ); + + // shift to the bottom left of the model + mpt1.x += mod->geom.size.x/2.0; + mpt1.y += mod->geom.size.y/2.0; + mpt2.x += mod->geom.size.x/2.0; + mpt2.y += mod->geom.size.y/2.0; + + // convert from meters to cells + stg_point_int_t a( floor( mpt1.x / cellwidth ), + floor( mpt1.y / cellheight )); + stg_point_int_t b( floor( mpt2.x / cellwidth ), + floor( mpt2.y / cellheight ) ); + + bool steep = abs( b.y-a.y ) > abs( b.x-a.x ); + if( steep ) + { + swap( a.x, a.y ); + swap( b.x, b.y ); + } + + if( a.x > b.x ) + { + swap( a.x, b.x ); + swap( a.y, b.y ); + } + + double dydx = (double) (b.y - a.y) / (double) (b.x - a.x); + double y = a.y; + for(int x=a.x; x<=b.x; x++) + { + if( steep ) + { + if( ! (floor(y) >= 0) ) continue; + if( ! (floor(y) < (int)width) ) continue; + if( ! (x >= 0) ) continue; + if( ! (x < (int)height) ) continue; + } + else + { + if( ! (x >= 0) ) continue; + if( ! (x < (int)width) ) continue; + if( ! (floor(y) >= 0) ) continue; + if( ! (floor(y) < (int)height) ) continue; + } + + if( steep ) + data[ (int)floor(y) + (x * width)] = 1; + else + data[ x + ((int)floor(y) * width)] = 1; + y += dydx; + } + } } void Block::DrawTop() @@ -412,26 +391,26 @@ { // draw filled color polygons stg_color_t col = inherit_color ? mod->color : color; - - mod->PushColor( col ); - glEnable(GL_POLYGON_OFFSET_FILL); - glPolygonOffset(1.0, 1.0); - DrawSides(); - DrawTop(); - glDisable(GL_POLYGON_OFFSET_FILL); - + + mod->PushColor( col ); + glEnable(GL_POLYGON_OFFSET_FILL); + glPolygonOffset(1.0, 1.0); + DrawSides(); + DrawTop(); + glDisable(GL_POLYGON_OFFSET_FILL); + // // draw the block outline in a darker version of the same color double r,g,b,a; stg_color_unpack( col, &r, &g, &b, &a ); mod->PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a )); - + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); glDepthMask(GL_FALSE); DrawTop(); DrawSides(); glDepthMask(GL_TRUE); glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); - + mod->PopColor(); mod->PopColor(); } @@ -442,10 +421,6 @@ DrawTop(); } - -//#define DEBUG 1 - - void Block::Load( Worldfile* wf, int entity ) { //printf( "Block::Load entity %d\n", entity ); @@ -455,18 +430,18 @@ //printf( "reading %d points\n", // pt_count ); - + char key[128]; for( unsigned int p=0; p<pt_count; p++ ) { snprintf(key, sizeof(key), "point[%d]", p ); - + pts[p].x = wf->ReadTupleLength(entity, key, 0, 0); pts[p].y = wf->ReadTupleLength(entity, key, 1, 0); } - + local_z.min = wf->ReadTupleLength( entity, "z", 0, 0.0 ); local_z.max = wf->ReadTupleLength( entity, "z", 1, 1.0 ); - + const char* colorstr = wf->ReadString( entity, "color", NULL ); if( colorstr ) { @@ -476,6 +451,3 @@ else inherit_color = true; } - - - Modified: code/stage/trunk/libstage/blockgroup.cc =================================================================== --- code/stage/trunk/libstage/blockgroup.cc 2009-07-01 15:37:14 UTC (rev 7934) +++ code/stage/trunk/libstage/blockgroup.cc 2009-07-01 17:50:43 UTC (rev 7935) @@ -11,11 +11,11 @@ BlockGroup::BlockGroup() : displaylist(0), - blocks(), - minx(0), - maxx(0), - miny(0), - maxy(0) + blocks(), + minx(0), + maxx(0), + miny(0), + maxy(0) { /* empty */ } BlockGroup::~BlockGroup() @@ -31,7 +31,7 @@ void BlockGroup::Clear() { FOR_EACH( it, blocks ) - delete *it; + delete *it; blocks.clear(); } @@ -40,23 +40,22 @@ { // confirm the tentative pose for all blocks FOR_EACH( it, blocks ) - (*it)->SwitchToTestedCells(); + (*it)->SwitchToTestedCells(); } -GList* BlockGroup::AppendTouchingModels( GList* list ) +void BlockGroup::AppendTouchingModels( ModelPtrSet &v ) { FOR_EACH( it, blocks ) - list = (*it)->AppendTouchingModels( list ); - return list; + (*it)->AppendTouchingModels( v ); } Model* BlockGroup::TestCollision() { Model* hitmod = NULL; - + FOR_EACH( it, blocks ) - if( (hitmod = (*it)->TestCollision())) - break; // bail on the earliest collision + if( (hitmod = (*it)->TestCollision())) + break; // bail on the earliest collision return hitmod; // NULL if no collision } @@ -74,21 +73,21 @@ size.z = 0.0; // grow to largest z we see FOR_EACH( it, blocks ) - { - // examine all the points in the polygon - Block* block = *it; - - for( unsigned int p=0; p < block->pt_count; p++ ) - { - stg_point_t* pt = &block->pts[p]; - if( pt->x < minx ) minx = pt->x; - if( pt->y < miny ) miny = pt->y; - if( pt->x > maxx ) maxx = pt->x; - if( pt->y > maxy ) maxy = pt->y; - } - - size.z = MAX( block->local_z.max, size.z ); - } + { + // examine all the points in the polygon + Block* block = *it; + + for( unsigned int p=0; p < block->pt_count; p++ ) + { + stg_point_t* pt = &block->pts[p]; + if( pt->x < minx ) minx = pt->x; + if( pt->y < miny ) miny = pt->y; + if( pt->x > maxx ) maxx = pt->x; + if( pt->y > maxy ) maxy = pt->y; + } + + size.z = MAX( block->local_z.max, size.z ); + } // store these bounds for normalization purposes size.x = maxx-minx; @@ -105,13 +104,13 @@ void BlockGroup::Map() { FOR_EACH( it, blocks ) - (*it)->Map(); + (*it)->Map(); } void BlockGroup::UnMap() { FOR_EACH( it, blocks ) - (*it)->UnMap(); + (*it)->UnMap(); } void BlockGroup::DrawSolid( const Geom & geom ) @@ -121,13 +120,13 @@ 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 ); FOR_EACH( it, blocks ) - (*it)->DrawSolid(); + (*it)->DrawSolid(); glPopMatrix(); } @@ -137,13 +136,13 @@ glPushMatrix(); 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 ); FOR_EACH( it, blocks ) - (*it)->DrawFootPrint(); + (*it)->DrawFootPrint(); glPopMatrix(); } @@ -151,15 +150,15 @@ void BlockGroup::BuildDisplayList( Model* mod ) { if( ! mod->world->IsGUI() ) - return; + return; //printf( "display list for model %s\n", mod->token ); if( displaylist == 0 ) - { - displaylist = glGenLists(1); - CalcSize(); - } + { + displaylist = glGenLists(1); + CalcSize(); + } glNewList( displaylist, GL_COMPILE ); @@ -169,8 +168,8 @@ 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 ); @@ -181,18 +180,18 @@ mod->PushColor( mod->color ); 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 + if( (!blk->inherit_color) && (blk->color != mod->color) ) + { + mod->PushColor( blk->color ); blk->DrawSolid(); - } + mod->PopColor(); + } + else + blk->DrawSolid(); + } mod->PopColor(); @@ -206,19 +205,19 @@ mod->PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a )); FOR_EACH( it, blocks ) - { - Block* blk = *it; + { + Block* blk = *it; - if( (!blk->inherit_color) && (blk->color != mod->color) ) - { - stg_color_unpack( blk->color, &r, &g, &b, &a ); - mod->PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a )); - blk->DrawSolid(); - mod->PopColor(); - } - else + if( (!blk->inherit_color) && (blk->color != mod->color) ) + { + stg_color_unpack( blk->color, &r, &g, &b, &a ); + mod->PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a )); blk->DrawSolid(); - } + mod->PopColor(); + } + else + blk->DrawSolid(); + } glDepthMask(GL_TRUE); glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); @@ -231,7 +230,7 @@ void BlockGroup::CallDisplayList( Model* mod ) { if( displaylist == 0 ) - BuildDisplayList( mod ); + BuildDisplayList( mod ); glCallList( displaylist ); } @@ -248,14 +247,14 @@ char full[_POSIX_PATH_MAX]; if( bitmapfile[0] == '/' ) - strcpy( full, bitmapfile ); + strcpy( full, bitmapfile ); else - { - char *tmp = strdup(wf->filename); - snprintf( full, _POSIX_PATH_MAX, - "%s/%s", dirname(tmp), bitmapfile ); - free(tmp); - } + { + char *tmp = strdup(wf->filename); + snprintf( full, _POSIX_PATH_MAX, + "%s/%s", dirname(tmp), bitmapfile ); + free(tmp); + } PRINT_DEBUG1( "attempting to load image %s", full ); @@ -263,60 +262,60 @@ unsigned int rect_count = 0; unsigned int width, height; if( stg_rotrects_from_image_file( full, - &rects, - &rect_count, - &width, &height ) ) - { - PRINT_ERR1( "failed to load rects from image file \"%s\"", - full ); - return; - } + &rects, + &rect_count, + &width, &height ) ) + { + PRINT_ERR1( "failed to load rects from image file \"%s\"", + full ); + return; + } //printf( "found %d rects in \"%s\" at %p\n", // rect_count, full, rects ); if( rects && (rect_count > 0) ) - { - // TODO fix this - stg_color_t col = stg_color_pack( 1.0, 0,0,1.0 ); + { + // TODO fix this + stg_color_t col = stg_color_pack( 1.0, 0,0,1.0 ); - for( unsigned int r=0; r<rect_count; r++ ) - { - stg_point_t pts[4]; + for( unsigned int r=0; r<rect_count; r++ ) + { + stg_point_t pts[4]; - double x = rects[r].pose.x; - double y = rects[r].pose.y; - double w = rects[r].size.x; - double h = rects[r].size.y; + double x = rects[r].pose.x; + double y = rects[r].pose.y; + double w = rects[r].size.x; + double h = rects[r].size.y; - pts[0].x = x; - pts[0].y = y; - pts[1].x = x + w; - pts[1].y = y; - pts[2].x = x + w; - pts[2].y = y + h; - pts[3].x = x; - pts[3].y = y + h; + pts[0].x = x; + pts[0].y = y; + pts[1].x = x + w; + pts[1].y = y; + pts[2].x = x + w; + pts[2].y = y + h; + pts[3].x = x; + pts[3].y = y + h; - AppendBlock( new Block( mod, - pts,4, - 0,1, - col, - true ) ); - } - free( rects ); - } + AppendBlock( new Block( mod, + pts,4, + 0,1, + col, + true ) ); + } + free( rects ); + } CalcSize(); } void BlockGroup::Rasterize( uint8_t* data, - unsigned int width, - unsigned int height, - stg_meters_t cellwidth, - stg_meters_t cellheight ) + unsigned int width, + unsigned int height, + stg_meters_t cellwidth, + stg_meters_t cellheight ) { FOR_EACH( it, blocks ) - (*it)->Rasterize( data, width, height, cellwidth, cellheight ); + (*it)->Rasterize( data, width, height, cellwidth, cellheight ); } Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-07-01 15:37:14 UTC (rev 7934) +++ code/stage/trunk/libstage/canvas.cc 2009-07-01 17:50:43 UTC (rev 7935) @@ -901,9 +901,6 @@ if( showBBoxes ) DrawBoundingBoxes(); - - //LISTMETHOD( world->puck_list, Puck*, Draw ); - // TODO - finish this properly //LISTMETHOD( models_sorted, Model*, DrawWaypoints ); Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-07-01 15:37:14 UTC (rev 7934) +++ code/stage/trunk/libstage/model.cc 2009-07-01 17:50:43 UTC (rev 7935) @@ -278,7 +278,7 @@ // 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; + ModelPtrVec& vec = parent ? parent->children : world->children; vec.erase( std::remove( vec.begin(), vec.end(), this )); if( callbacks ) g_hash_table_destroy( callbacks ); @@ -767,13 +767,11 @@ SetPose( Pose::Random( xmin,xmax, ymin, ymax )); } - -GList* Model::AppendTouchingModels( GList* list ) +void Model::AppendTouchingModels( ModelPtrSet& touchers ) { - return blockgroup.AppendTouchingModels( list ); + blockgroup.AppendTouchingModels( touchers ); } - Model* Model::TestCollision() { //printf( "mod %s test collision...\n", token ); @@ -810,11 +808,18 @@ pps_charging = NULL; // run through and update all appropriate touchers - for( GList* touchers = AppendTouchingModels( NULL ); - touchers; - touchers = touchers->next ) + ModelPtrSet touchers; + AppendTouchingModels( touchers ); + + // for( GList* touchers = AppendTouchingModels( NULL ); + // touchers; + // touchers = touchers->next ) + FOR_EACH( it, touchers ) +// for( GList* touchers = AppendTouchingModels( NULL ); +// touchers; +// touchers = touchers->next ) { - Model* toucher = (Model*)touchers->data; + Model* toucher = (*it); //(Model*)touchers->data; PowerPack* hispp =toucher->FindPowerPack(); if( hispp && toucher->watts_take > 0.0) @@ -892,18 +897,19 @@ // convert usec to sec double interval( (double)world->interval_sim / 1e6 ); - + // find the change of pose due to our velocity vector Pose p( velocity.x * interval, - velocity.y * interval, - velocity.z * interval, - normalize( velocity.a * interval )); - + velocity.y * interval, + velocity.z * interval, + normalize( velocity.a * interval )); + // attempts to move to the new pose. If the move fails because we'd // hit another model, that model is returned. // ConditionalMove() returns a pointer to the model we hit, or // NULL. We use this as a boolean for SetStall() - SetStall( ConditionalMove( pose_sum( pose, p ) ) ); + //SetStall( ConditionalMove( pose_sum( pose, p ) ) ); + SetStall( ConditionalMove( pose + p ) ); } Modified: code/stage/trunk/libstage/model_getset.cc =================================================================== --- code/stage/trunk/libstage/model_getset.cc 2009-07-01 15:37:14 UTC (rev 7934) +++ code/stage/trunk/libstage/model_getset.cc 2009-07-01 17:50:43 UTC (rev 7935) @@ -192,7 +192,7 @@ return pose; // otherwise - Pose global_pose = pose_sum( parent->GetGlobalPose(), pose ); + Pose global_pose = parent->GetGlobalPose() + pose; // we are on top of our parent global_pose.z += parent->geom.size.z; Modified: code/stage/trunk/libstage/stage.cc =================================================================== --- code/stage/trunk/libstage/stage.cc 2009-07-01 15:37:14 UTC (rev 7934) +++ code/stage/trunk/libstage/stage.cc 2009-07-01 17:50:43 UTC (rev 7935) @@ -127,9 +127,7 @@ // map the name to the color in the table g_hash_table_insert( table, (gpointer)colorname, (gpointer)col ); - } - fclose(file); } @@ -142,19 +140,6 @@ return (stg_color_t)0; } -// returns the resultant of vector [p1] and [p2] -Pose Stg::pose_scale( const Pose& p1, const double sx, const double sy, const double sz ) -{ - Pose scaled; - scaled.x = p1.x * sx; - scaled.y = p1.y * sy; - scaled.z = p1.z * sz; - scaled.a = p1.a; - - return scaled; -} - - static guchar* pb_get_pixel( Fl_Shared_Image* img, int x, int y ) { guchar* pixels = (guchar*)(img->data()[0]); @@ -215,12 +200,10 @@ if( widthp ) *widthp = img_width; if( heightp ) *heightp = img_height; - - int y, x; - for(y = 0; y < img_height; y++) - { - for(x = 0; x < img_width; x++) - { + for(int y = 0; y < img_height; y++) + { + for(int x = 0; x < img_width; x++) + { // skip blank (white) pixels if( pb_pixel_is_set( img,x,y, threshold) ) continue; Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-07-01 15:37:14 UTC (rev 7934) +++ code/stage/trunk/libstage/stage.hh 2009-07-01 17:50:43 UTC (rev 7935) @@ -74,6 +74,7 @@ namespace Stg { // forward declare + class Block; class Canvas; class Cell; class Worldfile; @@ -84,6 +85,15 @@ class Camera; class FileManager; class Option; + + /** Set of pointers to Models. */ + typedef std::set<Model*> ModelPtrSet; + /** Vector of pointers to Models. */ + typedef std::vector<Model*> ModelPtrVec; + /** Vector of pointers to Blocks. */ + typedef std::vector<Block*> BlockPtrVec; + /** Vector of pointers to Cells.*/ + typedef std::vector<Cell*> CellPtrVec; /** Initialize the Stage library */ void Init( int* argc, char** argv[] ); @@ -106,10 +116,10 @@ MODEL_TYPE_BLOBFINDER, MODEL_TYPE_BLINKENLIGHT, MODEL_TYPE_CAMERA, - MODEL_TYPE_GRIPPER, - MODEL_TYPE_ACTUATOR, - MODEL_TYPE_LOADCELL, - MODEL_TYPE_LIGHTINDICATOR, + MODEL_TYPE_GRIPPER, + MODEL_TYPE_ACTUATOR, + MODEL_TYPE_LOADCELL, + MODEL_TYPE_LIGHTINDICATOR, MODEL_TYPE_COUNT // must be the last entry, to count the number of // types } stg_model_type_t; @@ -184,13 +194,12 @@ /** take binary sign of a, either -1, or 1 if >= 0. */ inline double sgn( double a){ return( a<0 ? -1.0 : 1.0); } - /** Describe the image format used for saving screenshots. */ typedef enum { STG_IMAGE_FORMAT_PNG, STG_IMAGE_FORMAT_JPG } stg_image_format_t; - + /** any integer value other than this is a valid fiducial ID */ enum { FiducialNone = 0 }; @@ -234,27 +243,24 @@ void stg_color_unpack( stg_color_t col, double* r, double* g, double* b, double* a ); - //typedef std::vector<Model*> ModelPtrVec; - //typedef std::vector<Model&> ModelRefVec; - /** specify a rectangular size */ class Size { public: stg_meters_t x, y, z; - + Size( stg_meters_t x, - stg_meters_t y, - stg_meters_t z ) + stg_meters_t y, + stg_meters_t z ) : x(x), y(y), z(z) {/*empty*/} - + /** default constructor uses default non-zero values */ Size() : x( 0.4 ), y( 0.4 ), z( 1.0 ) {/*empty*/} - - void Load( Worldfile* wf, int section, const char* keyword ); - void Save( Worldfile* wf, int section, const char* keyword ); + + void Load( Worldfile* wf, int section, const char* keyword ); + void Save( Worldfile* wf, int section, const char* keyword ); }; /** Specify a 3 axis position, in x, y and heading. */ @@ -265,9 +271,9 @@ stg_radians_t a;///< rotation about the z axis. Pose( stg_meters_t x, - stg_meters_t y, - stg_meters_t z, - stg_radians_t a ) + stg_meters_t y, + stg_meters_t z, + stg_radians_t a ) : x(x), y(y), z(z), a(a) { /*empty*/ } @@ -279,39 +285,50 @@ /** return a random pose within the bounding rectangle, with z=0 and angle random */ static Pose Random( stg_meters_t xmin, stg_meters_t xmax, - stg_meters_t ymin, stg_meters_t ymax ) + stg_meters_t ymin, stg_meters_t ymax ) { return Pose( xmin + drand48() * (xmax-xmin), - ymin + drand48() * (ymax-ymin), - 0, - normalize( drand48() * (2.0 * M_PI) )); + ymin + drand48() * (ymax-ymin), + 0, + normalize( drand48() * (2.0 * M_PI) )); } - /** Print pose in human-readable format on stdout - @param prefix Character string to prepend to pose output - */ + /** Print pose in human-readable format on stdout + @param prefix Character string to prepend to pose output + */ virtual void Print( const char* prefix ) { printf( "%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n", - prefix, x,y,z,a ); + prefix, x,y,z,a ); } - - std::string String() - { - char buf[256]; - snprintf( buf, 256, "[ %.3f %.3f %.3f %.3f ]", - x,y,z,a ); - return std::string(buf); - } - - /* returns true iff all components of the velocity are zero. */ - bool IsZero() const { return( !(x || y || z || a )); }; - - /** Set the pose to zero [0,0,0,0] */ - void Zero(){ x=y=z=a=0.0; } - - void Load( Worldfile* wf, int section, const char* keyword ); - void Save( Worldfile* wf, int section, const char* keyword ); + + std::string String() + { + char buf[256]; + snprintf( buf, 256, "[ %.3f %.3f %.3f %.3f ]", + x,y,z,a ); + return std::string(buf); + } + + /* returns true iff all components of the velocity are zero. */ + bool IsZero() const { return( !(x || y || z || a )); }; + + /** Set the pose to zero [0,0,0,0] */ + void Zero(){ x=y=z=a=0.0; } + + void Load( Worldfile* wf, int section, const char* keyword ); + void Save( Worldfile* wf, int section, const char* keyword ); + + inline Pose operator+( const Pose& p ) + { + const double cosa = cos(a); + const double sina = sin(a); + + return Pose( x + p.x * cosa - p.y * sina, + y + p.x * sina + p.y * cosa, + z + p.z, + normalize(a + p.a) ); + } }; @@ -424,24 +441,24 @@ { public: stg_meters_t x, y; - stg_point_t( stg_meters_t x, stg_meters_t y ) : x(x), y(y){} - stg_point_t() : x(0.0), y(0.0){} - - bool operator+=( const stg_point_t& other ) - { return ((x += other.x) && (y += other.y) ); } + stg_point_t( stg_meters_t x, stg_meters_t y ) : x(x), y(y){} + stg_point_t() : x(0.0), y(0.0){} + + bool operator+=( const stg_point_t& other ) + { return ((x += other.x) && (y += other.y) ); } }; - + /** Define a point in 3d space */ class stg_point3_t { public: stg_meters_t x,y,z; - stg_point3_t( stg_meters_t x, stg_meters_t y, stg_meters_t z ) - : x(x), y(y), z(z) {} - //stg_point3_t( int x, int y ) : x(x), y(y), z(0.0) {} - stg_point3_t() : x(0.0), y(0.0), z(0.0) {} + stg_point3_t( stg_meters_t x, stg_meters_t y, stg_meters_t z ) + : x(x), y(y), z(z) {} + + stg_point3_t() : x(0.0), y(0.0), z(0.0) {} }; - + /** Define an integer point on the 2d plane */ class stg_point_int_t { @@ -458,6 +475,7 @@ { return ((x == other.x) && (y == other.y) ); } }; + typedef std::vector<stg_point_int_t> PointIntVec; /** create an array of 4 points containing the corners of a unit square. */ @@ -588,42 +606,20 @@ /** Create a draw_t object of specified type from a vertex array */ draw_t* create( type_t type, - vertex_t* verts, - size_t vert_count ); + vertex_t* verts, + size_t vert_count ); /** Delete the draw_t object, deallocting its memory */ void destroy( draw_t* d ); } // end namespace draw - - // MACROS ------------------------------------------------------ - // Some useful macros - + /** Look up the color in the X11 database. (i.e. transform color name to color value). If the color is not found in the database, a bright red color (0xF00) will be returned instead. */ stg_color_t stg_lookup_color(const char *name); - - /** returns the sum of [p1] + [p2], in [p1]'s coordinate system */ - inline Pose pose_sum( const Pose& p1, const Pose& p2 ) - { - double cosa = cos(p1.a); - double sina = sin(p1.a); - - Pose result; - result.x = p1.x + p2.x * cosa - p2.y * sina; - result.y = p1.y + p2.x * sina + p2.y * cosa; - result.z = p1.z + p2.z; - result.a = normalize(p1.a + p2.a); - - return result; - } - - /** returns a new pose, with each axis scaled */ - inline Pose pose_scale( const Pose& p1, const double x, const double y, const double z ); - - + // PRETTY PRINTING ------------------------------------------------- /** Report an error, with a standard, friendly message header */ @@ -826,9 +822,8 @@ friend class Canvas; // allow Canvas access to our private members protected: - std::vector<Model*> children; + ModelPtrVec children; bool debug; - GList* puck_list; char* token; void Load( Worldfile* wf, int section ); @@ -837,7 +832,7 @@ public: /** get the children of the this element */ - std::vector<Model*>& GetChildren(){ return children;} + ModelPtrVec& GetChildren(){ return children;} /** recursively call func( model, arg ) for each descendant */ void ForEachDescendant( stg_model_callback_t func, void* arg ); @@ -940,7 +935,7 @@ static void UpdateCb( World* world); static unsigned int next_id; ///<initially zero, used to allocate unique sequential world ids - std::set<Model*> charge_list; ///< Models which receive charge are listed here + ModelPtrSet charge_list; ///< Models which receive charge are listed here bool destroy; bool dirty; ///< iff true, a gui redraw would be required GList* event_list; //< @@ -956,7 +951,7 @@ unsigned int show_clock_interval; ///< updates between clock xoutputs GMutex* thread_mutex; ///< protect the worker thread management stuff int total_subs; ///< the total number of subscriptions to all models - std::set<Model*> velocity_list; ///< Models with non-zero velocity and should have their poses updated + ModelPtrSet velocity_list; ///< Models with non-zero velocity and should have their poses updated unsigned int worker_threads; ///< the number of worker threads to use unsigned int threads_working; ///< the number of worker threads not yet finished @@ -965,23 +960,23 @@ /** Keep a list of all models with detectable fiducials. This avoids searching the whole world for fiducials. */ - std::set<Model*> models_with_fiducials; + ModelPtrSet models_with_fiducials; protected: - std::list<std::pair<stg_world_callback_t,void*> > cb_list; ///< List of callback functions and arguments + std::list<std::pair<stg_world_callback_t,void*> > cb_list; ///< List of callback functions and arguments stg_bounds3d_t extent; ///< Describes the 3D volume of the world bool graphics;///< true iff we have a GUI stg_usec_t interval_sim; ///< temporal resolution: microseconds that elapse between simulated time steps - GHashTable* option_table; ///< GUI options (toggles) registered by models - GList* powerpack_list; ///< List of all the powerpacks attached to models in the world + GHashTable* option_table; ///< GUI options (toggles) registered by models + GList* powerpack_list; ///< List of all the powerpacks attached to models in the world GList* ray_list;///< List of rays traced for debug visualization stg_usec_t sim_time; ///< the current sim time in this world in microseconds - std::map<stg_point_int_t,SuperRegion*> superregions; + std::map<stg_point_int_t,SuperRegion*> superregions; SuperRegion* sr_cached; ///< The last superregion looked up by this world // todo - test performance of std::set - std::vector<std::vector<Model*> > update_lists; + std::vector<ModelPtrVec > update_lists; 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 @@ -997,8 +992,8 @@ void TogglePause(){ paused = !paused; }; bool Paused(){ return( paused ); }; - std::vector<stg_point_int_t> rt_cells; - std::vector<stg_point_int_t> rt_candidate_cells; + PointIntVec rt_cells; + PointIntVec rt_candidate_cells; static const int DEFAULT_PPM = 50; // default resolution in pixels per meter static const stg_msec_t DEFAULT_INTERVAL_SIM = 100; ///< duration of sim timestep @@ -1027,7 +1022,7 @@ void LoadModel( Worldfile* wf, int entity, GHashTable* entitytable ); void LoadBlock( Worldfile* wf, int entity, GHashTable* entitytable ); void LoadBlockGroup( Worldfile* wf, int entity, GHashTable* entitytable ); - void LoadPuck( Worldfile* wf, int entity, GHashTable* entitytable ); + // void LoadPuck( Worldfile* wf, int entity, GHashTable* entitytable ); virtual Model* RecentlySelectedModel(){ return NULL; } @@ -1043,7 +1038,7 @@ pt1 to pt2 inclusive */ void ForEachCellInLine( const stg_point_int_t& pt1, const stg_point_int_t& pt2, - std::vector<Cell*>& cells ); + CellPtrVec& cells ); /** convert a distance in meters to a distance in world occupancy grid pixels */ @@ -1225,10 +1220,11 @@ std::vector<stg_point_t>& Points() { return pts; }; - void AddToCellArray( std::vector<Cell*>* blocks ); - void RemoveFromCellArray( std::vector<Cell*>* blocks ); + void AddToCellArray( CellPtrVec* blocks ); + void RemoveFromCellArray( CellPtrVec* blocks ); void GenerateCandidateCells(); - GList* AppendTouchingModels( GList* list ); + + void AppendTouchingModels( ModelPtrSet& touchers ); /** Returns the first model that shares a bitmap cell with this model */ Model* TestCollision(); @@ -1266,7 +1262,7 @@ /** record the cells into which this block has been rendered to UnMapping them very quickly. */ - std::vector<Cell*> * rendered_cells; + 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 @@ -1274,19 +1270,19 @@ 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).*/ - std::vector<Cell*> * candidate_cells; - - std::vector<stg_point_int_t> gpts; - - /** find the position of a block's point in model coordinates - (m) */ - stg_point_t BlockPointToModelMeters( const stg_point_t& bpt ); - - /** Update the cache of block points converted to model coordinates */ - //stg_point_t* GetPointsInModelCoords(); - - /** invalidate the cache of points in model coordinates */ - void InvalidateModelPointCache(); + CellPtrVec * candidate_cells; + + PointIntVec gpts; + + /** find the position of a block's point in model coordinates + (m) */ + stg_point_t BlockPointToModelMeters( const stg_point_t& bpt ); + + /** Update the cache of block points converted to model coordinates */ + //stg_point_t* GetPointsInModelCoords(); + + /** invalidate the cache of points in model coordinates */ + void InvalidateModelPointCache(); }; @@ -1300,7 +1296,7 @@ void BuildDisplayList( Model* mod ); - std::vector<Block*> blocks; + BlockPtrVec blocks; Size size; stg_point3_t offset; stg_meters_t minx, maxx, miny, maxy; @@ -1320,7 +1316,7 @@ void CallDisplayList( Model* mod ); void Clear() ; /** deletes all blocks from the group */ - GList* AppendTouchingModels( GList* list ); + void AppendTouchingModels( ModelPtrSet& touchers ); /** Returns a pointer to the first model detected to be colliding with a block in this group, or NULL, if none are detected. */ @@ -1346,7 +1342,7 @@ void InvalidateModelPointCache() { - for( std::vector<Block*>::iterator it( blocks.begin() ); + for( BlockPtrVec::iterator it( blocks.begin() ); it != blocks.end(); ++it ) (*it)->InvalidateModelPointCache(); @@ -1939,7 +1935,7 @@ /// Register an Option for pickup by the GUI void RegisterOption( Option* opt ); - GList* AppendTouchingModels( GList* list ); + void AppendTouchingModels( ModelPtrSet& touchers ); /** Check to see if the current pose will yield a collision with obstacles. Returns a pointer to the first entity we are in @@ -2393,10 +2389,10 @@ Pose GlobalToLocal( const Pose& pose ) const; /** Return the global pose (i.e. pose in world coordinates) of a - pose specified in the model's local coordinate system */ - Pose LocalToGlobal( const Pose& pose ) const - { - return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose ); + pose specified in the model's local coordinate system */ + Pose LocalToGlobal( const Pose& pose ) const + { + return( ( GetGlobalPose() + geom.pose ) + pose ); } // /** Return the 3d point in world coordinates of a 3d point Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-07-01 15:37:14 UTC (rev 7934) +++ code/stage/trunk/libstage/world.cc 2009-07-01 17:50:43 UTC (rev 7935) @@ -227,15 +227,6 @@ mod->LoadBlock( wf, entity ); } -// void World::LoadPuck( Worldfile* wf, int entity, GHashTable* entitytable ) -// { -// Puck* puck = new Puck(); -// puck->Load( wf, entity ); -// puck_list = g_list_prepend( puck_list, puck ); -// } - - - Model* World::CreateModel( Model* parent, const char* typestr ) { Model* mod = NULL; // new model to return @@ -370,8 +361,6 @@ } 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 ); } @@ -744,7 +733,7 @@ (cy>=0) && (cy<REGIONWIDTH) && n > 0 ) { - for( std::vector<Block*>::iterator it( c->blocks.begin() ); + for( BlockPtrVec::iterator it( c->blocks.begin() ); it != c->blocks.end(); ++it ) { @@ -1085,7 +1074,7 @@ void World::UpdateListRemove( Model* mod ) { // choose the right update list - std::vector<Model*>& vec = update_lists[ mod->update_list_num ]; + ModelPtrVec& vec = update_lists[ mod->update_list_num ]; // and erase the model from it vec.erase( remove( vec.begin(), vec.end(), mod )); } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-07-17 00:03:32
|
Revision: 8044 http://playerstage.svn.sourceforge.net/playerstage/?rev=8044&view=rev Author: rtv Date: 2009-07-17 00:03:28 +0000 (Fri, 17 Jul 2009) Log Message: ----------- more STLing Modified Paths: -------------- code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-07-16 21:31:57 UTC (rev 8043) +++ code/stage/trunk/libstage/canvas.cc 2009-07-17 00:03:28 UTC (rev 8044) @@ -759,7 +759,7 @@ // TODO //models_sorted = g_list_sort_with_data( models_sorted, (GCompareDataFunc)compare_distance, coords ); - // TODO: understand why this doesn't work and fix it - just cosmetic but important! + // TODO: understand why this doesn't work and fix it - cosmetic but important! //std::sort( models_sorted.begin(), models_sorted.end(), DistFuncObj(x,y) ); glEnable( GL_DEPTH_TEST ); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-07-16 21:31:57 UTC (rev 8043) +++ code/stage/trunk/libstage/stage.hh 2009-07-17 00:03:28 UTC (rev 8044) @@ -46,6 +46,7 @@ #include <vector> #include <list> #include <map> +#include <ext/hash_map> #include <set> #include <algorithm> @@ -838,33 +839,42 @@ static void UpdateCb( World* world); static unsigned int next_id; ///<initially zero, used to allocate unique sequential world ids - GMutex* access_mutex; ///< Used by Lock() and Unlock() to prevent parallel access to this model - - ModelPtrSet charge_list; ///< Models which receive charge are listed here + GMutex* access_mutex; ///< Used by Lock() and Unlock() to prevent parallel access to this model + + ModelPtrSet charge_list; ///< Models which receive charge are listed here bool destroy; bool dirty; ///< iff true, a gui redraw would be required - GHashTable* models_by_name; ///< the models that make up the world, indexed by name - /** Keep a list of all models with detectable fiducials. This - avoids searching the whole world for fiducials. */ - ModelPtrSet models_with_fiducials; + + // functor for comparing C strings in a hash_map + struct eqstr + { + bool operator()( const char* a, const char* b ) + { return( strcmp( a, b ) == 0 ); } + }; + + __gnu_cxx::hash_map<const char*, Model*, __gnu_cxx::hash<const char*>, eqstr > models_by_name; ///< the models that make up the world, indexed by name. + + /** Keep a list of all models with detectable fiducials. This + avoids searching the whole world for fiducials. */ + ModelPtrSet models_with_fiducials; double ppm; ///< the resolution of the world model in pixels per meter bool quit; ///< quit this world ASAP - + /** World::quit is set true when this simulation time is reached */ stg_usec_t quit_time; stg_usec_t real_time_now; ///< The current real time in microseconds stg_usec_t real_time_start; ///< the real time at which this world was created - bool show_clock; ///< iff true, print the sim time on stdout - unsigned int show_clock_interval; ///< updates between clock xoutputs + bool show_clock; ///< iff true, print the sim time on stdout + unsigned int show_clock_interval; ///< updates between clock xoutputs GMutex* thread_mutex; ///< protect the worker thread management stuff - unsigned int threads_working; ///< the number of worker threads not yet finished + unsigned int threads_working; ///< the number of worker threads not yet finished GCond* threads_start_cond; ///< signalled to unblock worker threads GCond* threads_done_cond; ///< signalled by last worker thread to unblock main thread int total_subs; ///< the total number of subscriptions to all models - ModelPtrSet velocity_list; ///< Models with non-zero velocity and should have their poses updated - unsigned int worker_threads; ///< the number of worker threads to use - + ModelPtrSet velocity_list; ///< Models with non-zero velocity and should have their poses updated + unsigned int worker_threads; ///< the number of worker threads to use + protected: std::list<std::pair<stg_world_callback_t,void*> > cb_list; ///< List of callback functions and arguments @@ -872,6 +882,7 @@ bool graphics;///< true iff we have a GUI stg_usec_t interval_sim; ///< temporal resolution: microseconds that elapse between simulated time steps GHashTable* option_table; ///< GUI options (toggles) registered by models + //__gnu_cxx::hash_map<Option std::list<PowerPack*> powerpack_list; ///< List of all the powerpacks attached to models in the world std::list<float*> ray_list;///< List of rays traced for debug visualization stg_usec_t sim_time; ///< the current sim time in this world in microseconds @@ -1112,34 +1123,46 @@ /** render the block into the world's raytrace data structure */ void Map(); + /** remove the block from the world's raytracing data structure */ void UnMap(); + + /** draw the block in OpenGL as a solid single color */ void DrawSolid(); void Draw( Model* mod ); - void DrawSolid(); // draw the block in OpenGL as a solid single color - void DrawFootPrint(); // draw the projection of the block onto the z=0 plane - /** Translate all points in the block by the indicated amounts */ - void Translate( double x, double y ); - /** Return the center of the block on the X axis */ - double CenterX(); + + /** draw the projection of the block onto the z=0 plane */ + void DrawFootPrint(); + + /** Translate all points in the block by the indicated amounts */ + void Translate( double x, double y ); + + /** Return the center of the block on the X axis */ + double CenterX(); + /** Return the center of the block on the Y axis */ - double CenterY(); - /** Set the center of the block on the X axis */ - void SetCenterX( double y ); - /** Set the center of the block on the Y axis */ - void SetCenterY( double y ); - /** Set the center of the block */ - void SetCenter( double x, double y); - void SetZ( double min, double max ); + double CenterY(); + /** Set the center of the block on the X axis */ + void SetCenterX( double y ); + + /** Set the center of the block on the Y axis */ + void SetCenterY( double y ); + + /** Set the center of the block */ + void SetCenter( double x, double y); + + /** Set the extent in Z of the block */ + void SetZ( double min, double max ); + void RecordRendering( Cell* cell ) { rendered_cells->push_back( cell ); } stg_point_t* Points( unsigned int *count ) { if( count ) *count = pt_count; return &pts[0]; }; - - std::vector<stg_point_t>& Points() + + std::vector<stg_point_t>& Points() { return pts; }; - + void AddToCellArray( CellPtrVec* blocks ); void RemoveFromCellArray( CellPtrVec* blocks ); void GenerateCandidateCells(); Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-07-16 21:31:57 UTC (rev 8043) +++ code/stage/trunk/libstage/world.cc 2009-07-17 00:03:28 UTC (rev 8044) @@ -81,7 +81,7 @@ charge_list(), destroy( false ), dirty( true ), - models_by_name( g_hash_table_new( g_str_hash, g_str_equal ) ), + models_by_name(),// g_hash_table_new( g_str_hash, g_str_equal ) ), models_with_fiducials(), ppm( ppm ), // raytrace resolution quit( false ), @@ -131,7 +131,7 @@ if( wf ) delete wf; - g_hash_table_destroy( models_by_name ); + //g_hash_table_destroy( models_by_name ); g_free( token ); World::world_set.erase( this ); @@ -211,7 +211,8 @@ void World::RemoveModel( Model* mod ) { - g_hash_table_remove( models_by_name, mod ); + //g_hash_table_remove( models_by_name, mod ); + models_by_name.erase( mod->token ); } // wrapper to startup all models from the hash table @@ -400,8 +401,9 @@ delete (*it); children.clear(); - g_hash_table_remove_all( models_by_name ); - + //g_hash_table_remove_all( models_by_name ); + models_by_name.clear(); + update_lists.resize(1); ray_list.clear(); @@ -596,13 +598,15 @@ void World::AddModel( Model* mod ) { - g_hash_table_insert( this->models_by_name, (gpointer)mod->Token(), mod ); + //g_hash_table_insert( this->models_by_name, (gpointer)mod->Token(), mod ); + models_by_name[mod->token] = mod; } Model* World::GetModel( const char* name ) { PRINT_DEBUG1( "looking up model name %s in models_by_name", name ); - Model* mod = (Model*)g_hash_table_lookup( this->models_by_name, name ); + //Model* mod = (Model*)g_hash_table_lookup( this->models_by_name, name ); + Model* mod = models_by_name[ name ]; if( mod == NULL ) PRINT_WARN1( "lookup of model name %s: no matching name", name ); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2009-07-17 06:47:53
|
Revision: 8048 http://playerstage.svn.sourceforge.net/playerstage/?rev=8048&view=rev Author: thjc Date: 2009-07-17 06:47:50 +0000 (Fri, 17 Jul 2009) Log Message: ----------- fixed some compiler warnings, added some extra getcwd error handling to stop some warnings Modified Paths: -------------- code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/worldfile.cc code/stage/trunk/libstageplugin/p_blobfinder.cc Modified: code/stage/trunk/libstage/model_load.cc =================================================================== --- code/stage/trunk/libstage/model_load.cc 2009-07-17 05:39:05 UTC (rev 8047) +++ code/stage/trunk/libstage/model_load.cc 2009-07-17 06:47:50 UTC (rev 8048) @@ -301,7 +301,14 @@ /* Initialise libltdl. */ int errors = lt_dlinit(); - assert(errors==0); + if (errors) + { + printf( "Libtool error: %s. Failed to init libtool. Quitting\n", + lt_dlerror() ); // report the error from libtool + puts( "libtool error #1" ); + fflush( stdout ); + exit(-1); + } lt_dlsetsearchpath( FileManager::stagePath().c_str() ); Modified: code/stage/trunk/libstage/worldfile.cc =================================================================== --- code/stage/trunk/libstage/worldfile.cc 2009-07-17 05:39:05 UTC (rev 8047) +++ code/stage/trunk/libstage/worldfile.cc 2009-07-17 06:47:50 UTC (rev 8048) @@ -537,6 +537,11 @@ char *tmp = strdup(this->filename); fullpath = new char[PATH_MAX]; char* dummy = getcwd(fullpath, PATH_MAX); + if (!dummy) + { + PRINT_ERR2("unable to get cwd %d: %s", errno, strerror(errno)); + return false; + } strcat( fullpath, "/" ); strcat( fullpath, dirname(tmp)); strcat( fullpath, "/" ); @@ -1663,6 +1668,12 @@ char *tmp = strdup(this->filename); char* fullpath = new char[PATH_MAX]; char* dummy = getcwd(fullpath, PATH_MAX); + if (!dummy) + { + PRINT_ERR2("unable to get cwd %d: %s", errno, strerror(errno)); + return value; + } + strcat( fullpath, "/" ); strcat( fullpath, dirname(tmp)); strcat( fullpath, "/" ); Modified: code/stage/trunk/libstageplugin/p_blobfinder.cc =================================================================== --- code/stage/trunk/libstageplugin/p_blobfinder.cc 2009-07-17 05:39:05 UTC (rev 8047) +++ code/stage/trunk/libstageplugin/p_blobfinder.cc 2009-07-17 06:47:50 UTC (rev 8048) @@ -96,9 +96,9 @@ bfd.blobs[b].bottom = blobs[b].bottom; bfd.blobs[b].color = - (uint8_t)(blobs[b].color.r*255.0) << 16 + - (uint8_t)(blobs[b].color.g*255.0) << 8 + - (uint8_t)(blobs[b].color.b*255.0); + ((uint8_t)(blobs[b].color.r*255.0) << 16) + + ((uint8_t)(blobs[b].color.g*255.0) << 8) + + ((uint8_t)(blobs[b].color.b*255.0)); bfd.blobs[b].area = dx * dy; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-07-21 01:49:33
|
Revision: 8061 http://playerstage.svn.sourceforge.net/playerstage/?rev=8061&view=rev Author: rtv Date: 2009-07-21 01:49:26 +0000 (Tue, 21 Jul 2009) Log Message: ----------- more STLing - option sorting broken temporarily Modified Paths: -------------- code/stage/trunk/libstage/option.hh code/stage/trunk/libstage/options_dlg.cc code/stage/trunk/libstage/options_dlg.hh code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/option.hh =================================================================== --- code/stage/trunk/libstage/option.hh 2009-07-21 01:21:39 UTC (rev 8060) +++ code/stage/trunk/libstage/option.hh 2009-07-21 01:49:26 UTC (rev 8061) @@ -39,15 +39,19 @@ inline bool val() const { return value; } inline operator bool() { return val(); } inline bool operator<( const Option& rhs ) const - { return optName<rhs.optName; } + { + puts( "comparing" ); + return optName<rhs.optName; + } void set( bool val ); void invert() { set( !value ); } - // Comparator to dereference Option pointers and compare their strings - struct optComp { - inline bool operator()( const Option* lhs, const Option* rhs ) const - { return lhs->operator<(*rhs); } - }; +// // Comparator to dereference Option pointers and compare their strings +// struct optComp { +// inline bool operator()( const Option* a, const Option* b ) const +// //{ return lhs->operator<(*rhs); } +// { return a->optName < b->optName; } +// }; void createMenuItem( Fl_Menu_Bar* menu, std::string path ); Modified: code/stage/trunk/libstage/options_dlg.cc =================================================================== --- code/stage/trunk/libstage/options_dlg.cc 2009-07-21 01:21:39 UTC (rev 8060) +++ code/stage/trunk/libstage/options_dlg.cc 2009-07-21 01:49:26 UTC (rev 8061) @@ -68,7 +68,7 @@ this->redraw(); } - void OptionsDlg::setOptions( const std::vector<Option*>& opts ) { + void OptionsDlg::setOptions( const std::set<Option*>& opts ) { options.clear(); options.assign( opts.begin(), opts.end() ); updateChecks(); Modified: code/stage/trunk/libstage/options_dlg.hh =================================================================== --- code/stage/trunk/libstage/options_dlg.hh 2009-07-21 01:21:39 UTC (rev 8060) +++ code/stage/trunk/libstage/options_dlg.hh 2009-07-21 01:49:26 UTC (rev 8061) @@ -41,7 +41,7 @@ OptionsDlg( int x, int y, int w, int h ); virtual ~OptionsDlg(); - void setOptions( const std::vector<Option*>& opts ); + void setOptions( const std::set<Option*>& opts ); void clearOptions() { options.clear(); } void showAllOpt( Option* opt ); const event_t event() const { return status; } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-07-21 01:21:39 UTC (rev 8060) +++ code/stage/trunk/libstage/stage.hh 2009-07-21 01:49:26 UTC (rev 8061) @@ -884,7 +884,8 @@ stg_bounds3d_t extent; ///< Describes the 3D volume of the world bool graphics;///< true iff we have a GUI stg_usec_t interval_sim; ///< temporal resolution: microseconds that elapse between simulated time steps - GHashTable* option_table; ///< GUI options (toggles) registered by models + //GHashTable* option_table; ///< GUI options (toggles) registered by models + std::set<Option*> option_table; ///< GUI options (toggles) registered by models std::list<PowerPack*> powerpack_list; ///< List of all the powerpacks attached to models in the world std::list<float*> ray_list;///< List of rays traced for debug visualization stg_usec_t sim_time; ///< the current sim time in this world in microseconds @@ -1444,8 +1445,6 @@ bool pause_time; stg_usec_t real_time_of_last_update; - void UpdateOptions(); - // static callback functions static void windowCb( Fl_Widget* w, void* p ); static void fileLoadCb( Fl_Widget* w, void* p ); Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-07-21 01:21:39 UTC (rev 8060) +++ code/stage/trunk/libstage/world.cc 2009-07-21 01:49:26 UTC (rev 8061) @@ -103,7 +103,8 @@ extent(), graphics( false ), interval_sim( (stg_usec_t)thousand * interval_sim ), - option_table( g_hash_table_new( g_str_hash, g_str_equal ) ), + //option_table( g_hash_table_new( g_str_hash, g_str_equal ) ), + option_table(), powerpack_list(), ray_list(), sim_time( 0 ), @@ -1072,7 +1073,8 @@ /// Register an Option for pickup by the GUI void World:: RegisterOption( Option* opt ) { - g_hash_table_insert( option_table, (void*)opt->htname, opt ); + //g_hash_table_insert( option_table, (void*)opt->htname, opt ); + option_table.insert( opt ); } int World::UpdateListAdd( Model* mod ) Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2009-07-21 01:21:39 UTC (rev 8060) +++ code/stage/trunk/libstage/worldgui.cc 2009-07-21 01:49:26 UTC (rev 8061) @@ -545,18 +545,19 @@ } } -static void append_option( char* name, Option* opt, std::vector<Option*>* optv ) -{ - //printf( "adding option %s @ %p\n", name, opt ); - optv->push_back( opt ); -} +// static void append_option( char* name, Option* opt, std::vector<Option*>* optv ) +// { +// //printf( "adding option %s @ %p\n", name, opt ); +// optv->push_back( opt ); +// } -static bool sort_option_pointer( Option* a, Option* b ) -{ - // Option class overloads operator<. Nasty nasty C++ makes code less - // readable IMHO. - return (*a) < (*b); -} +// static bool sort_option_pointer( const Option* a, +// const Option* b ) const +// { +// // Option class overloads operator<. Nasty nasty C++ makes code less +// // readable IMHO. +// return (*a) < (*b); +// } void WorldGui::resetViewCb( Fl_Widget* w, WorldGui* worldGui ) { @@ -636,14 +637,15 @@ void WorldGui::viewOptionsCb( OptionsDlg* oDlg, WorldGui* worldGui ) { // the options dialog expects a std::vector of options (annoyingly) - std::vector<Option*> optvec; + // std::vector<Option*> optvec; // adds each option to the vector - g_hash_table_foreach( worldGui->option_table, - (GHFunc)append_option, - (void*)&optvec ); + //g_hash_table_foreach( worldGui->option_table, + // (GHFunc)append_option, + // (void*)&optvec ); // sort the vector by option label alphabetically - std::sort( optvec.begin(), optvec.end(), sort_option_pointer ); + //std::sort();// worldGui->option_table.begin(), worldGui->option_table.end() );//, sort_option_pointer ); + //std::sort();// worldGui->option_table.begin(), worldGui->option_table.end() );//, sort_option_pointer ); if ( !worldGui->oDlg ) { @@ -652,7 +654,7 @@ OptionsDlg* oDlg = new OptionsDlg( x,y, 180,250 ); oDlg->callback( (Fl_Callback*)optionsDlgCb, worldGui ); - oDlg->setOptions( optvec ); + oDlg->setOptions( worldGui->option_table ); oDlg->showAllOpt( &worldGui->canvas->visualizeAll ); worldGui->oDlg = oDlg; oDlg->show(); @@ -853,25 +855,6 @@ } } -void WorldGui::UpdateOptions() -{ - std::set<Option*, Option::optComp> options; - std::vector<Option*> modOpts; - - FOR_EACH( it1, update_lists ) - FOR_EACH( it2, (*it1) ) - { - modOpts = (*it2)->getOptions(); - options.insert( modOpts.begin(), modOpts.end() ); - } - - - drawOptions.assign( options.begin(), options.end() ); - - if ( oDlg ) - oDlg->setOptions( drawOptions ); -} - void WorldGui::DrawBoundingBoxTree() { FOR_EACH( it, World::children ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2009-07-22 08:26:31
|
Revision: 8072 http://playerstage.svn.sourceforge.net/playerstage/?rev=8072&view=rev Author: thjc Date: 2009-07-22 08:26:23 +0000 (Wed, 22 Jul 2009) Log Message: ----------- fix for missing _POSIX define and inline symbol that is referenced externally Modified Paths: -------------- code/stage/trunk/libstage/blockgroup.cc code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/blockgroup.cc =================================================================== --- code/stage/trunk/libstage/blockgroup.cc 2009-07-22 07:05:57 UTC (rev 8071) +++ code/stage/trunk/libstage/blockgroup.cc 2009-07-22 08:26:23 UTC (rev 8072) @@ -3,6 +3,7 @@ #include "worldfile.hh" #include <libgen.h> // for dirname(3) +#include <limits.h> // for _POSIX_PATH_MAX #undef DEBUG Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-07-22 07:05:57 UTC (rev 8071) +++ code/stage/trunk/libstage/world.cc 2009-07-22 08:26:23 UTC (rev 8072) @@ -424,7 +424,8 @@ token = NULL; } -inline stg_usec_t World::RealTimeNow() +// cant inline a symbol that is used externally +stg_usec_t World::RealTimeNow() { struct timeval tv; gettimeofday( &tv, NULL ); // slow system call: use sparingly This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-08-19 16:26:22
|
Revision: 8210 http://playerstage.svn.sourceforge.net/playerstage/?rev=8210&view=rev Author: rtv Date: 2009-08-19 16:26:15 +0000 (Wed, 19 Aug 2009) Log Message: ----------- attempting to fix stageplugin build with removal of aio & dio Modified Paths: -------------- code/stage/trunk/libstage/model.cc code/stage/trunk/libstageplugin/p_driver.cc code/stage/trunk/libstageplugin/p_driver.h Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-08-19 02:29:19 UTC (rev 8209) +++ code/stage/trunk/libstage/model.cc 2009-08-19 16:26:15 UTC (rev 8210) @@ -171,7 +171,7 @@ GuiState:: GuiState() : grid( false ), - move( false ), + move( false ), nose( false ), outline( false ) { /* nothing to do */} @@ -1068,9 +1068,9 @@ // Gl::draw_string( x, y, 0, buf ); } - + glPolygonMode( GL_FRONT, GL_FILL ); - + mod->PopColor(); mod->PopColor(); Modified: code/stage/trunk/libstageplugin/p_driver.cc =================================================================== --- code/stage/trunk/libstageplugin/p_driver.cc 2009-08-19 02:29:19 UTC (rev 8209) +++ code/stage/trunk/libstageplugin/p_driver.cc 2009-08-19 16:26:15 UTC (rev 8210) @@ -310,17 +310,17 @@ ifsrc = new InterfaceActArray( player_addr, this, cf, section ); break; - case PLAYER_AIO_CODE: - ifsrc = new InterfaceAio( player_addr, this, cf, section ); - break; + // case PLAYER_AIO_CODE: + // ifsrc = new InterfaceAio( player_addr, this, cf, section ); + // break; case PLAYER_BLOBFINDER_CODE: ifsrc = new InterfaceBlobfinder( player_addr, this, cf, section ); break; - case PLAYER_DIO_CODE: - ifsrc = new InterfaceDio(player_addr, this, cf, section); - break; + // case PLAYER_DIO_CODE: + // ifsrc = new InterfaceDio(player_addr, this, cf, section); + // break; case PLAYER_FIDUCIAL_CODE: ifsrc = new InterfaceFiducial( player_addr, this, cf, section ); Modified: code/stage/trunk/libstageplugin/p_driver.h =================================================================== --- code/stage/trunk/libstageplugin/p_driver.h 2009-08-19 02:29:19 UTC (rev 8209) +++ code/stage/trunk/libstageplugin/p_driver.h 2009-08-19 16:26:15 UTC (rev 8210) @@ -170,26 +170,26 @@ virtual void Publish( void ); }; -class InterfaceAio : public InterfaceModel -{ - public: - InterfaceAio( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section ); - virtual ~InterfaceAio( void ){ /* TODO: clean up*/ }; - virtual int ProcessMessage(QueuePointer & resp_queue, - player_msghdr_t* hdr, - void* data); - virtual void Publish( void ); -}; +/* class InterfaceAio : public InterfaceModel */ +/* { */ +/* public: */ +/* InterfaceAio( player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section ); */ +/* virtual ~InterfaceAio( void ){ /\* TODO: clean up*\/ }; */ +/* virtual int ProcessMessage(QueuePointer & resp_queue, */ +/* player_msghdr_t* hdr, */ +/* void* data); */ +/* virtual void Publish( void ); */ +/* }; */ -class InterfaceDio : public InterfaceModel -{ -public: - InterfaceDio(player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section); - virtual ~InterfaceDio(); - virtual int ProcessMessage(QueuePointer & resp_queue, player_msghdr_t* hdr, void* data); - virtual void Publish(); -}; +/* class InterfaceDio : public InterfaceModel */ +/* { */ +/* public: */ +/* InterfaceDio(player_devaddr_t addr, StgDriver* driver, ConfigFile* cf, int section); */ +/* virtual ~InterfaceDio(); */ +/* virtual int ProcessMessage(QueuePointer & resp_queue, player_msghdr_t* hdr, void* data); */ +/* virtual void Publish(); */ +/* }; */ class InterfacePower : public InterfaceModel This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-08-22 01:38:32
|
Revision: 8216 http://playerstage.svn.sourceforge.net/playerstage/?rev=8216&view=rev Author: rtv Date: 2009-08-22 01:38:22 +0000 (Sat, 22 Aug 2009) Log Message: ----------- fixed event queuing bug when unsubscribing & subscribing Modified Paths: -------------- code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_fiducial.cc Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-08-21 22:35:30 UTC (rev 8215) +++ code/stage/trunk/libstage/model.cc 2009-08-22 01:38:22 UTC (rev 8216) @@ -790,6 +790,7 @@ } // set up the next event + if( subs > 0 ) // TODO XX ? world->Enqueue( 0, World::Event::ENERGY, interval_energy, this ); } @@ -852,7 +853,9 @@ } //if( ! velocity.IsZero() ) - world->Enqueue( 0, World::Event::POSE, interval_pose, this ); + + if( subs > 0 )// TODO XX ? + world->Enqueue( 0, World::Event::POSE, interval_pose, this ); } Model* Model::GetUnsubscribedModelOfType( const std::string& type ) const Modified: code/stage/trunk/libstage/model_fiducial.cc =================================================================== --- code/stage/trunk/libstage/model_fiducial.cc 2009-08-21 22:35:30 UTC (rev 8215) +++ code/stage/trunk/libstage/model_fiducial.cc 2009-08-22 01:38:22 UTC (rev 8216) @@ -18,12 +18,6 @@ #include "worldfile.hh" using namespace Stg; -static const stg_meters_t DEFAULT_RANGEMIN = 0.0; -static const stg_meters_t DEFAULT_RANGEMAXID = 5.0; -static const stg_meters_t DEFAULT_RANGEMAXANON = 8.0; -static const stg_radians_t DEFAULT_FOV = M_PI; -static const stg_watts_t DEFAULT_WATTS = 10.0; - //TODO make instance attempt to register an option (as customvisualizations do) Option ModelFiducial::showData( "Fiducials", "show_fiducial", "", true, NULL ); Option ModelFiducial::showFov( "Fiducial FOV", "show_fiducial_fov", "", false, NULL ); @@ -72,10 +66,10 @@ const std::string& type ) : Model( world, parent, type ), fiducials(), - max_range_anon( DEFAULT_RANGEMAXANON ), - max_range_id( DEFAULT_RANGEMAXID ), - min_range( DEFAULT_RANGEMIN ), - fov( DEFAULT_FOV ), + max_range_anon( 8.0 ), + max_range_id( 5.0 ), + min_range( 0.0 ), + fov( M_PI ), heading( 0 ), key( 0 ) { @@ -178,32 +172,36 @@ // him->Token() ); // if it was him, we can see him - if( hitmod == him ) - { - Geom hisgeom( him->GetGeom() ); - - // record where we saw him and what he looked like - Fiducial fid; - fid.mod = him; - fid.range = range; - fid.bearing = dtheta; - fid.geom.x = hisgeom.size.x; - fid.geom.y = hisgeom.size.y; - fid.geom.a = normalize( hispose.a - mypose.a); - - // store the global pose of the fiducial (mainly for the GUI) - fid.pose = hispose; + if( hitmod != him ) + return; - // if he's within ID range, get his fiducial.return value, else - // we see value 0 - fid.id = range < max_range_id ? hitmod->vis.fiducial_return : 0; - - PRINT_DEBUG2( "adding %s's value %d to my list of fiducials", - him->Token(), him->vis.fiducial_return ); - - fiducials.push_back( fid ); - } -} + assert( range >= 0 ); + + // passed all the tests! record the fiducial hit + + Geom hisgeom( him->GetGeom() ); + + // record where we saw him and what he looked like + Fiducial fid; + fid.mod = him; + fid.range = range; + fid.bearing = dtheta; + fid.geom.x = hisgeom.size.x; + fid.geom.y = hisgeom.size.y; + fid.geom.a = normalize( hispose.a - mypose.a); + + // store the global pose of the fiducial (mainly for the GUI) + fid.pose = hispose; + + // if he's within ID range, get his fiducial.return value, else + // we see value 0 + fid.id = range < max_range_id ? hitmod->vis.fiducial_return : 0; + + PRINT_DEBUG2( "adding %s's value %d to my list of fiducials", + him->Token(), him->vis.fiducial_return ); + + fiducials.push_back( fid ); +} /////////////////////////////////////////////////////////////////////////// // Update the beacon data @@ -220,10 +218,8 @@ // reset the array of detected fiducials fiducials.clear(); - for( std::set<Model*>::iterator it( world->models_with_fiducials.begin() ); - it != world->models_with_fiducials.end(); - ++it ) - AddModelIfVisible( *it ); + FOR_EACH( it, world->models_with_fiducials ) + AddModelIfVisible( *it ); } void ModelFiducial::Load( void ) @@ -272,9 +268,9 @@ glLineStipple( 1, 0x00FF ); // draw lines to the fiducials - for( unsigned int f=0; f<fiducials.size(); f++ ) + FOR_EACH( it, fiducials ) { - Fiducial& fid = fiducials[f]; + Fiducial& fid = *it; double dx = fid.range * cos( fid.bearing); double dy = fid.range * sin( fid.bearing); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-08-27 19:46:32
|
Revision: 8237 http://playerstage.svn.sourceforge.net/playerstage/?rev=8237&view=rev Author: rtv Date: 2009-08-27 19:46:25 +0000 (Thu, 27 Aug 2009) Log Message: ----------- GUI clean ups Modified Paths: -------------- code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/canvas.hh code/stage/trunk/libstage/region.cc code/stage/trunk/libstage/region.hh code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-08-26 23:43:56 UTC (rev 8236) +++ code/stage/trunk/libstage/canvas.cc 2009-08-27 19:46:25 UTC (rev 8237) @@ -52,8 +52,8 @@ } Canvas::Canvas( WorldGui* world, - int x, int y, - int width, int height) : + int x, int y, + int width, int height) : Fl_Gl_Window( x, y, width, height ), colorstack(), models_sorted(), @@ -69,7 +69,9 @@ interval( 20 ), // msec between redraws // initialize Option objects // showBlinken( "Blinkenlights", "show_blinkenlights", "", true, world ), + showBBoxes( "Debug/Bounding boxes", "show_boundingboxes", "^b", false, world ), showBlocks( "Blocks", "show_blocks", "b", true, world ), + showBlur( "Trails/Blur", "show_trailblur", "^d", false, world ), showClock( "Clock", "show_clock", "c", true, world ), showData( "Data", "show_data", "d", false, world ), showFlags( "Flags", "show_flags", "l", true, world ), @@ -82,11 +84,9 @@ showTrailArrows( "Trails/Rising Arrows", "show_trailarrows", "^a", false, world ), showTrailRise( "Trails/Rising blocks", "show_trailrise", "^r", false, world ), showTrails( "Trails/Fast", "show_trailfast", "^f", false, world ), - showTree( "Debug/Tree", "show_tree", "^t", false, world ), - showBBoxes( "Debug/Bounding boxes", "show_boundingboxes", "^b", false, world ), - showBlur( "Trails/Blur", "show_trailblur", "^d", false, world ), + showVoxels( "Debug/Voxels", "show_voxels", "^v", false, world ), pCamOn( "Perspective camera", "pcam_on", "r", false, world ), - visualizeAll( "Selected only", "vis_all", "^v", false, world ), + visualizeAll( "Selected only", "vis_all", "v", false, world ), // and the rest graphics( true ), world( world ), @@ -151,27 +151,27 @@ GLuint mains_id = TextureManager::getInstance().loadTexture( fullpath.c_str() ); TextureManager::getInstance()._mains_texture_id = mains_id; -// // generate a small glow texture -// GLubyte* pixels = new GLubyte[ 4 * 128 * 128 ]; + // // generate a small glow texture + // GLubyte* pixels = new GLubyte[ 4 * 128 * 128 ]; -// for( int x=0; x<128; x++ ) -// for( int y=0; y<128; y++ ) -// { -// GLubyte* p = &pixels[ 4 * (128*y + x)]; -// p[0] = (GLubyte)255; // red -// p[1] = (GLubyte)0; // green -// p[2] = (GLubyte)0; // blue -// p[3] = (GLubyte)128; // alpha -// } + // for( int x=0; x<128; x++ ) + // for( int y=0; y<128; y++ ) + // { + // GLubyte* p = &pixels[ 4 * (128*y + x)]; + // p[0] = (GLubyte)255; // red + // p[1] = (GLubyte)0; // green + // p[2] = (GLubyte)0; // blue + // p[3] = (GLubyte)128; // alpha + // } -// glGenTextures(1, &glowTex ); -// glBindTexture( GL_TEXTURE_2D, glowTex ); + // glGenTextures(1, &glowTex ); + // glBindTexture( GL_TEXTURE_2D, glowTex ); -// glTexImage2D( GL_TEXTURE_2D, 0, GL_RGBA, 128, 128, 0, -// GL_RGBA, GL_UNSIGNED_BYTE, pixels ); + // glTexImage2D( GL_TEXTURE_2D, 0, GL_RGBA, 128, 128, 0, + // GL_RGBA, GL_UNSIGNED_BYTE, pixels ); -// delete[] pixels; + // delete[] pixels; // draw a check into a bitmap, then load that into a texture int i, j; @@ -229,19 +229,19 @@ Model* mod = (*it); if( mod->gui.move ) - { - uint8_t rByte, gByte, bByte, aByte; - uint32_t modelId = mod->id; - rByte = modelId; - gByte = modelId >> 8; - bByte = modelId >> 16; - aByte = modelId >> 24; + { + uint8_t rByte, gByte, bByte, aByte; + uint32_t modelId = mod->id; + rByte = modelId; + gByte = modelId >> 8; + bByte = modelId >> 16; + aByte = modelId >> 24; - //printf("mod->Id(): 0x%X, rByte: 0x%X, gByte: 0x%X, bByte: 0x%X, aByte: 0x%X\n", modelId, rByte, gByte, bByte, aByte); + //printf("mod->Id(): 0x%X, rByte: 0x%X, gByte: 0x%X, bByte: 0x%X, aByte: 0x%X\n", modelId, rByte, gByte, bByte, aByte); - glColor4ub( rByte, gByte, bByte, aByte ); - mod->DrawPicker(); - } + glColor4ub( rByte, gByte, bByte, aByte ); + mod->DrawPicker(); + } } // read the color of the pixel in the back buffer under the mouse @@ -253,13 +253,13 @@ uint32_t modelId; glReadPixels( x,viewport[3]-y,1,1, - GL_RED,GL_UNSIGNED_BYTE,(void*)&rByte ); + GL_RED,GL_UNSIGNED_BYTE,(void*)&rByte ); glReadPixels( x,viewport[3]-y,1,1, - GL_GREEN,GL_UNSIGNED_BYTE,(void*)&gByte ); + GL_GREEN,GL_UNSIGNED_BYTE,(void*)&gByte ); glReadPixels( x,viewport[3]-y,1,1, - GL_BLUE,GL_UNSIGNED_BYTE,(void*)&bByte ); + GL_BLUE,GL_UNSIGNED_BYTE,(void*)&bByte ); glReadPixels( x,viewport[3]-y,1,1, - GL_ALPHA,GL_UNSIGNED_BYTE,(void*)&aByte ); + GL_ALPHA,GL_UNSIGNED_BYTE,(void*)&aByte ); modelId = rByte; modelId |= gByte << 8; @@ -316,7 +316,7 @@ // convert from 2d window pixel to 3d world coordinates void Canvas::CanvasToWorld( int px, int py, - double *wx, double *wy, double* wz ) + double *wx, double *wy, double* wz ) { if( px <= 0 ) px = 1; @@ -359,10 +359,10 @@ { case FL_MOUSEWHEEL: if( pCamOn == true ) { - perspective_camera.scroll( Fl::event_dy() / 10.0 ); + perspective_camera.scroll( Fl::event_dy() / 10.0 ); } else { - camera.scale( Fl::event_dy(), Fl::event_x(), w(), Fl::event_y(), h() ); + camera.scale( Fl::event_dy(), Fl::event_x(), w(), Fl::event_y(), h() ); } invalidate(); redraw(); @@ -370,154 +370,156 @@ case FL_MOVE: // moused moved while no button was pressed if( Fl::event_state( FL_META ) ) - { - puts( "TODO: HANDLE HISTORY" ); - //world->paused = ! world->paused; - return 1; - } + { + puts( "TODO: HANDLE HISTORY" ); + //world->paused = ! world->paused; + return 1; + } if ( startx >=0 ) - { - // mouse pointing to valid value + { + // mouse pointing to valid value - if( Fl::event_state( FL_CTRL ) ) - { - int dx = Fl::event_x() - startx; - int dy = Fl::event_y() - starty; + if( Fl::event_state( FL_CTRL ) ) + { + int dx = Fl::event_x() - startx; + int dy = Fl::event_y() - starty; - if( pCamOn == true ) { - perspective_camera.addYaw( -dx ); - perspective_camera.addPitch( -dy ); - } - else { - camera.addPitch( - 0.5 * static_cast<double>( dy ) ); - camera.addYaw( - 0.5 * static_cast<double>( dx ) ); - } - invalidate(); - redraw(); - } - else if( Fl::event_state( FL_ALT ) ) - { - int dx = Fl::event_x() - startx; - int dy = Fl::event_y() - starty; + if( pCamOn == true ) { + perspective_camera.addYaw( -dx ); + perspective_camera.addPitch( -dy ); + } + else { + camera.addPitch( - 0.5 * static_cast<double>( dy ) ); + camera.addYaw( - 0.5 * static_cast<double>( dx ) ); + } + invalidate(); + redraw(); + } + else if( Fl::event_state( FL_ALT ) ) + { + int dx = Fl::event_x() - startx; + int dy = Fl::event_y() - starty; - if( pCamOn == true ) { - perspective_camera.move( -dx, dy, 0.0 ); - } - else { - camera.move( -dx, dy ); - } - invalidate(); - } - } + if( pCamOn == true ) { + perspective_camera.move( -dx, dy, 0.0 ); + } + else { + camera.move( -dx, dy ); + } + invalidate(); + } + } startx = Fl::event_x(); starty = Fl::event_y(); return 1; case FL_PUSH: // button pressed { - //else - { - Model* mod = getModel( startx, starty ); - startx = Fl::event_x(); - starty = Fl::event_y(); - selectedModel = false; - switch( Fl::event_button() ) - { - case 1: - clicked_empty_space = ( mod == NULL ); - empty_space_startx = startx; - empty_space_starty = starty; - if( mod ) { - // clicked a model - if ( Fl::event_state( FL_SHIFT ) ) { - // holding shift, toggle selection - if ( selected( mod ) ) - unSelect( mod ); - else { - select( mod ); - selectedModel = true; // selected a model + //else + { + Model* mod = getModel( startx, starty ); + startx = Fl::event_x(); + starty = Fl::event_y(); + selectedModel = false; + switch( Fl::event_button() ) + { + case 1: + clicked_empty_space = ( mod == NULL ); + empty_space_startx = startx; + empty_space_starty = starty; + if( mod ) { + // clicked a model + if ( Fl::event_state( FL_SHIFT ) ) { + // holding shift, toggle selection + if ( selected( mod ) ) + unSelect( mod ); + else { + select( mod ); + selectedModel = true; // selected a model + } + } + else { + if ( !selected( mod ) ) { + // clicked on an unselected model while + // not holding shift, this is the new + // selection + unSelectAll(); + select( mod ); + } + selectedModel = true; // selected a model + } + } + + redraw(); // probably required + return 1; + case 3: + { + // leave selections alone + // rotating handled within FL_DRAG + return 1; + } + default: + return 0; + } } - } - else { - if ( !selected( mod ) ) { - // clicked on an unselected model while - // not holding shift, this is the new - // selection - unSelectAll(); - select( mod ); - } - selectedModel = true; // selected a model - } - } - - return 1; - case 3: - { - // leave selections alone - // rotating handled within FL_DRAG - return 1; - } - default: - return 0; - } - } } case FL_DRAG: // mouse moved while button was pressed { - int dx = Fl::event_x() - startx; - int dy = Fl::event_y() - starty; + int dx = Fl::event_x() - startx; + int dy = Fl::event_y() - starty; - if ( Fl::event_state( FL_BUTTON1 ) && Fl::event_state( FL_CTRL ) == false ) { - // Left mouse button drag - if ( selectedModel ) { - // started dragging on a selected model + if ( Fl::event_state( FL_BUTTON1 ) && Fl::event_state( FL_CTRL ) == false ) { + // Left mouse button drag + if ( selectedModel ) { + // started dragging on a selected model - double sx,sy,sz; - CanvasToWorld( startx, starty, - &sx, &sy, &sz ); - double x,y,z; - CanvasToWorld( Fl::event_x(), Fl::event_y(), - &x, &y, &z ); - // move all selected models to the mouse pointer - FOR_EACH( it, selected_models ) - { - Model* mod = *it; - mod->AddToPose( x-sx, y-sy, 0, 0 ); - } - } - else { - // started dragging on empty space or an - // unselected model, move the canvas - if( pCamOn == true ) { - perspective_camera.move( -dx, dy, 0.0 ); - } - else { - camera.move( -dx, dy ); - } - invalidate(); // so the projection gets updated - } - } - else if ( Fl::event_state( FL_BUTTON3 ) || ( Fl::event_state( FL_BUTTON1 ) && Fl::event_state( FL_CTRL ) ) ) { - // rotate all selected models - FOR_EACH( it, selected_models ) - { - Model* mod = *it; - mod->AddToPose( 0,0,0, 0.05*(dx+dy) ); - } - } + double sx,sy,sz; + CanvasToWorld( startx, starty, + &sx, &sy, &sz ); + double x,y,z; + CanvasToWorld( Fl::event_x(), Fl::event_y(), + &x, &y, &z ); + // move all selected models to the mouse pointer + FOR_EACH( it, selected_models ) + { + Model* mod = *it; + mod->AddToPose( x-sx, y-sy, 0, 0 ); + } + } + else { + // started dragging on empty space or an + // unselected model, move the canvas + if( pCamOn == true ) { + perspective_camera.move( -dx, dy, 0.0 ); + } + else { + camera.move( -dx, dy ); + } + invalidate(); // so the projection gets updated + } + } + else if ( Fl::event_state( FL_BUTTON3 ) || ( Fl::event_state( FL_BUTTON1 ) && Fl::event_state( FL_CTRL ) ) ) { + // rotate all selected models + FOR_EACH( it, selected_models ) + { + Model* mod = *it; + mod->AddToPose( 0,0,0, 0.05*(dx+dy) ); + } + } - startx = Fl::event_x(); - starty = Fl::event_y(); + startx = Fl::event_x(); + starty = Fl::event_y(); - redraw(); - return 1; + redraw(); + return 1; } // end case FL_DRAG case FL_RELEASE: // mouse button released if( empty_space_startx == Fl::event_x() && empty_space_starty == Fl::event_y() && clicked_empty_space == true ) { - // clicked on empty space, unselect all - unSelectAll(); + // clicked on empty space, unselect all + unSelectAll(); + redraw(); } return 1; @@ -528,23 +530,23 @@ case FL_KEYBOARD: switch( Fl::event_key() ) - { - case FL_Left: - if( pCamOn == false ) { camera.move( -10, 0 ); } - else { perspective_camera.strafe( -0.5 ); } break; - case FL_Right: - if( pCamOn == false ) {camera.move( 10, 0 ); } - else { perspective_camera.strafe( 0.5 ); } break; - case FL_Down: - if( pCamOn == false ) {camera.move( 0, -10 ); } - else { perspective_camera.forward( -0.5 ); } break; - case FL_Up: - if( pCamOn == false ) {camera.move( 0, 10 ); } - else { perspective_camera.forward( 0.5 ); } break; - default: - redraw(); // we probably set a display config - so need this - return 0; // keypress unhandled - } + { + case FL_Left: + if( pCamOn == false ) { camera.move( -10, 0 ); } + else { perspective_camera.strafe( -0.5 ); } break; + case FL_Right: + if( pCamOn == false ) {camera.move( 10, 0 ); } + else { perspective_camera.strafe( 0.5 ); } break; + case FL_Down: + if( pCamOn == false ) {camera.move( 0, -10 ); } + else { perspective_camera.forward( -0.5 ); } break; + case FL_Up: + if( pCamOn == false ) {camera.move( 0, 10 ); } + else { perspective_camera.forward( 0.5 ); } break; + default: + redraw(); // we probably set a display config - so need this + return 0; // keypress unhandled + } invalidate(); // update projection return 1; @@ -681,14 +683,6 @@ { FOR_EACH( it, models_sorted ) (*it)->DrawBlocksTree(); - - // some models may be carried by others - this prevents them being drawn twice - // for( GList* it = models_sorted; it; it=it->next ) - // { - // Model* mod = (Model*)it->data; - // if( mod->parent == NULL ) - // mod->DrawBlocksTree(); - // } } void Canvas::DrawBoundingBoxes() @@ -751,10 +745,10 @@ Pose b_pose = b->GetGlobalPose(); stg_meters_t a_dist = hypot( y - a_pose.y, - x - a_pose.x ); + x - a_pose.x ); stg_meters_t b_dist = hypot( y - b_pose.y, - x - b_pose.x ); + x - b_pose.x ); return ( a_dist < b_dist ); } @@ -790,23 +784,13 @@ if( ! showTrails ) glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); - if( showTree || showOccupancy ) - { - glPushMatrix(); - - GLfloat scale = 1.0/world->Resolution(); - glScalef( scale, scale, 1.0 ); // XX TODO - this seems slightly - // out for Z. look into it. - - if( showOccupancy ) - ((WorldGui*)world)->DrawTree( false ); - - if( showTree ) - ((WorldGui*)world)->DrawTree( true ); - - glPopMatrix(); - } + if( showOccupancy ) + ((WorldGui*)world)->DrawOccupancy(); + if( showVoxels ) + ((WorldGui*)world)->DrawVoxels(); + + if( ! world->rt_cells.empty() ) { glPushMatrix(); @@ -821,24 +805,24 @@ glBegin( GL_POINTS ); for( unsigned int i=0; - i < world->rt_cells.size(); - i++ ) - { - char str[128]; - snprintf( str, 128, "(%d,%d)", - world->rt_cells[i].x, - world->rt_cells[i].y ); + i < world->rt_cells.size(); + i++ ) + { + char str[128]; + snprintf( str, 128, "(%d,%d)", + world->rt_cells[i].x, + world->rt_cells[i].y ); - Gl::draw_string( world->rt_cells[i].x+1, - world->rt_cells[i].y+1, 0.1, str ); + Gl::draw_string( world->rt_cells[i].x+1, + world->rt_cells[i].y+1, 0.1, str ); - //printf( "x: %d y: %d\n", world->rt_regions[i].x, world->rt_regions[i].y ); - //glRectf( world->rt_cells[i].x+0.3, world->rt_cells[i].y+0.3, - // world->rt_cells[i].x+0.7, world->rt_cells[i].y+0.7 ); + //printf( "x: %d y: %d\n", world->rt_regions[i].x, world->rt_regions[i].y ); + //glRectf( world->rt_cells[i].x+0.3, world->rt_cells[i].y+0.3, + // world->rt_cells[i].x+0.7, world->rt_cells[i].y+0.7 ); - glVertex2f( world->rt_cells[i].x, world->rt_cells[i].y ); + glVertex2f( world->rt_cells[i].x, world->rt_cells[i].y ); - } + } glEnd(); @@ -846,11 +830,11 @@ world->PushColor( Color( 0,1,0,0.2) ); glBegin( GL_LINE_STRIP ); for( unsigned int i=0; - i < world->rt_cells.size(); - i++ ) - { - glVertex2f( world->rt_cells[i].x+0.5, world->rt_cells[i].y+0.5 ); - } + i < world->rt_cells.size(); + i++ ) + { + glVertex2f( world->rt_cells[i].x+0.5, world->rt_cells[i].y+0.5 ); + } glEnd(); world->PopColor(); #endif @@ -870,30 +854,30 @@ glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); for( unsigned int i=0; - i < world->rt_candidate_cells.size(); - i++ ) - { - // char str[128]; - // snprintf( str, 128, "(%d,%d)", - // world->rt_candidate_cells[i].x, - // world->rt_candidate_cells[i].y ); + i < world->rt_candidate_cells.size(); + i++ ) + { + // char str[128]; + // snprintf( str, 128, "(%d,%d)", + // world->rt_candidate_cells[i].x, + // world->rt_candidate_cells[i].y ); - // Gl::draw_string( world->rt_candidate_cells[i].x+1, - // world->rt_candidate_cells[i].y+1, 0.1, str ); + // Gl::draw_string( world->rt_candidate_cells[i].x+1, + // world->rt_candidate_cells[i].y+1, 0.1, str ); - //printf( "x: %d y: %d\n", world->rt_regions[i].x, world->rt_regions[i].y ); - glRectf( world->rt_candidate_cells[i].x, world->rt_candidate_cells[i].y, - world->rt_candidate_cells[i].x+1, world->rt_candidate_cells[i].y+1 ); - } + //printf( "x: %d y: %d\n", world->rt_regions[i].x, world->rt_regions[i].y ); + glRectf( world->rt_candidate_cells[i].x, world->rt_candidate_cells[i].y, + world->rt_candidate_cells[i].x+1, world->rt_candidate_cells[i].y+1 ); + } world->PushColor( Color( 0,1,0,0.2) ); glBegin( GL_LINE_STRIP ); for( unsigned int i=0; - i < world->rt_candidate_cells.size(); - i++ ) - { - glVertex2f( world->rt_candidate_cells[i].x+0.5, world->rt_candidate_cells[i].y+0.5 ); - } + i < world->rt_candidate_cells.size(); + i++ ) + { + glVertex2f( world->rt_candidate_cells[i].x+0.5, world->rt_candidate_cells[i].y+0.5 ); + } glEnd(); world->PopColor(); @@ -903,8 +887,6 @@ //world->rt_cells.clear(); } - - if( showGrid ) DrawGlobalGrid(); else @@ -915,7 +897,7 @@ glDisable( GL_DEPTH_TEST ); // using alpha blending FOR_EACH( it, models_sorted ) - (*it)->DrawTrailFootprint(); + (*it)->DrawTrailFootprint(); glEnable( GL_DEPTH_TEST ); } @@ -952,11 +934,11 @@ if( showData ) { if ( ! visualizeAll ) { FOR_EACH( it, world->World::children ) - (*it)->DataVisualizeTree( current_camera ); + (*it)->DataVisualizeTree( current_camera ); } else if ( selected_models.size() > 0 ) { FOR_EACH( it, world->World::children ) - (*it)->DataVisualizeTree( current_camera ); + (*it)->DataVisualizeTree( current_camera ); } else if ( last_selection ) { last_selection->DataVisualizeTree( current_camera ); @@ -972,10 +954,10 @@ glPushMatrix(); //ensure two icons can't be in the exact same plane if( camera.pitch() == 0 && !pCamOn ) - glTranslatef( 0, 0, 0.1 ); + glTranslatef( 0, 0, 0.1 ); FOR_EACH( it, models_sorted ) - (*it)->DrawStatusTree( &camera ); + (*it)->DrawStatusTree( &camera ); glPopMatrix(); } @@ -985,13 +967,13 @@ glDisable( GL_DEPTH_TEST ); PushColor( 0,0,0,0.5 ); FOR_EACH( it, world->ray_list ) - { - float* pts = *it; - glBegin( GL_LINES ); - glVertex2f( pts[0], pts[1] ); - glVertex2f( pts[2], pts[3] ); - glEnd(); - } + { + float* pts = *it; + glBegin( GL_LINES ); + glVertex2f( pts[0], pts[1] ); + glVertex2f( pts[2], pts[3] ); + glEnd(); + } PopColor(); glEnable( GL_DEPTH_TEST ); @@ -1015,7 +997,7 @@ std::string clockstr = world->ClockString(); if( showFollow == true && last_selection ) - clockstr.append( " [FOLLOW MODE]" ); + clockstr.append( " [FOLLOW MODE]" ); float txtWidth = gl_width( clockstr.c_str()); if( txtWidth < 200 ) txtWidth = 200; @@ -1036,16 +1018,16 @@ // ENERGY BOX if( PowerPack::global_capacity > 0 ) - { - colorstack.Push( 0.8,1.0,0.8,0.85 ); // pale green - glRectf( 0, height, width, 90 ); - colorstack.Push( 0,0,0 ); // black - Gl::draw_string_multiline( margin, height + margin, width, 50, - world->EnergyString().c_str(), - (Fl_Align)( FL_ALIGN_LEFT | FL_ALIGN_BOTTOM) ); - colorstack.Pop(); - colorstack.Pop(); - } + { + colorstack.Push( 0.8,1.0,0.8,0.85 ); // pale green + glRectf( 0, height, width, 90 ); + colorstack.Push( 0,0,0 ); // black + Gl::draw_string_multiline( margin, height + margin, width, 50, + world->EnergyString().c_str(), + (Fl_Align)( FL_ALIGN_LEFT | FL_ALIGN_BOTTOM) ); + colorstack.Pop(); + colorstack.Pop(); + } glEnable( GL_DEPTH_TEST ); glPopMatrix(); @@ -1133,11 +1115,11 @@ png_set_rows( pp, info, rowpointers ); png_set_IHDR( pp, info, - width, height, 8, - PNG_COLOR_TYPE_RGBA, - PNG_INTERLACE_NONE, - PNG_COMPRESSION_TYPE_DEFAULT, - PNG_FILTER_TYPE_DEFAULT); + width, height, 8, + PNG_COLOR_TYPE_RGBA, + PNG_INTERLACE_NONE, + PNG_COMPRESSION_TYPE_DEFAULT, + PNG_FILTER_TYPE_DEFAULT); png_write_png( pp, info, PNG_TRANSFORM_IDENTITY, NULL ); @@ -1183,7 +1165,7 @@ showTrails.createMenuItem( menu, path ); showTrailRise.createMenuItem( menu, path ); // broken showBBoxes.createMenuItem( menu, path ); - showTree.createMenuItem( menu, path ); + showVoxels.createMenuItem( menu, path ); showScreenshots.createMenuItem( menu, path ); } @@ -1213,15 +1195,15 @@ showTrailArrows.Load( wf, sec ); showTrailRise.Load( wf, sec ); showTrails.Load( wf, sec ); - showTree.Load( wf, sec ); + showVoxels.Load( wf, sec ); showScreenshots.Load( wf, sec ); pCamOn.Load( wf, sec ); if( ! world->paused ) // // start the timer that causes regular redraws Fl::add_timeout( ((double)interval/1000), - (Fl_Timeout_Handler)Canvas::TimerCallback, - this); + (Fl_Timeout_Handler)Canvas::TimerCallback, + this); invalidate(); // we probably changed something } @@ -1246,7 +1228,7 @@ showTrailArrows.Save( wf, sec ); showTrailRise.Save( wf, sec ); showTrails.Save( wf, sec ); - showTree.Save( wf, sec ); + showVoxels.Save( wf, sec ); showScreenshots.Save( wf, sec ); pCamOn.Save( wf, sec ); } @@ -1254,31 +1236,27 @@ void Canvas::draw() { - // static unsigned long calls=0; - // printf( "Draw calls %lu\n", ++calls ); - - //Enable the following to debug camera model // if( loaded_texture == true && pCamOn == true ) // return; - + if (!valid() ) { if( ! init_done ) - InitGl(); + InitGl(); if( pCamOn == true ) - { - perspective_camera.setAspect( static_cast< float >( w() ) / static_cast< float >( h() ) ); - perspective_camera.SetProjection(); - current_camera = &perspective_camera; - } + { + perspective_camera.setAspect( static_cast< float >( w() ) / static_cast< float >( h() ) ); + perspective_camera.SetProjection(); + current_camera = &perspective_camera; + } else - { - stg_bounds3d_t extent = world->GetExtent(); - camera.SetProjection( w(), h(), extent.y.min, extent.y.max ); - current_camera = &camera; - } + { + stg_bounds3d_t extent = world->GetExtent(); + camera.SetProjection( w(), h(), extent.y.min, extent.y.max ); + current_camera = &camera; + } glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); } @@ -1288,14 +1266,14 @@ { Pose gpose = last_selection->GetGlobalPose(); if( pCamOn == true ) - { - perspective_camera.setPose( gpose.x, gpose.y, 0.2 ); - perspective_camera.setYaw( rtod( gpose.a ) - 90.0 ); - } + { + perspective_camera.setPose( gpose.x, gpose.y, 0.2 ); + perspective_camera.setYaw( rtod( gpose.a ) - 90.0 ); + } else - { - camera.setPose( gpose.x, gpose.y ); - } + { + camera.setPose( gpose.x, gpose.y ); + } } current_camera->Draw(); Modified: code/stage/trunk/libstage/canvas.hh =================================================================== --- code/stage/trunk/libstage/canvas.hh 2009-08-26 23:43:56 UTC (rev 8236) +++ code/stage/trunk/libstage/canvas.hh 2009-08-27 19:46:25 UTC (rev 8237) @@ -78,7 +78,9 @@ void RemoveModel( Model* mod ); Option //showBlinken, + showBBoxes, showBlocks, + showBlur, showClock, showData, showFlags, @@ -91,9 +93,7 @@ showTrailArrows, showTrailRise, showTrails, - showTree, - showBBoxes, - showBlur, + showVoxels, pCamOn, visualizeAll; Modified: code/stage/trunk/libstage/region.cc =================================================================== --- code/stage/trunk/libstage/region.cc 2009-08-26 23:43:56 UTC (rev 8236) +++ code/stage/trunk/libstage/region.cc 2009-08-27 19:46:25 UTC (rev 8237) @@ -7,10 +7,6 @@ #include "region.hh" using namespace Stg; -// static member for accumulating empty regions for occasional garbage -// collection -// std::set<Region*> Region::empty_regions; - Region::Region( SuperRegion* sr) : cells(), superregion(sr), @@ -29,25 +25,21 @@ count(0) { // populate the regions - regions.insert( regions.begin(), SUPERREGIONSIZE, Region( this ) ); - - //static int srcount=0; - //printf( "created SR number %d\n", ++srcount ); - // printf( "superregion at %d %d\n", origin.x, origin.y ); + regions.insert( regions.begin(), SUPERREGIONSIZE, Region( this ) ); } SuperRegion::~SuperRegion() { - //printf( "deleting SR %p at [%d,%d]\n", this, origin.x, origin.y ); } -void SuperRegion::Draw( bool drawall ) +void SuperRegion::DrawOccupancy() { - glEnable( GL_DEPTH_TEST ); - - glPushMatrix(); + glPushMatrix(); + GLfloat scale = 1.0/world->Resolution(); + glScalef( scale, scale, 1.0 ); // XX TODO - this seems slightly glTranslatef( origin.x<<SRBITS, origin.y<<SRBITS,0); - + + glEnable( GL_DEPTH_TEST ); glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); // outline superregion @@ -62,27 +54,27 @@ for( int y=0; y<SUPERREGIONWIDTH; y++ ) for( int x=0; x<SUPERREGIONWIDTH; x++ ) { - if( r->count ) + if( r->count ) // region contains some occupied cells { - // outline regions with contents + // outline the region glRecti( x<<RBITS, y<<RBITS, (x+1)<<RBITS, (y+1)<<RBITS ); - + + // show how many cells are occupied snprintf( buf, 15, "%lu", r->count ); Gl::draw_string( x<<RBITS, y<<RBITS, 0, buf ); + // draw a rectangle around each occupied cell for( int p=0; p<REGIONWIDTH; p++ ) for( int q=0; q<REGIONWIDTH; q++ ) if( r->cells[p+(q*REGIONWIDTH)].blocks.size() ) { GLfloat xx = p+(x<<RBITS); - GLfloat yy = q+(y<<RBITS); - - glRecti( xx, yy, - xx+1, yy+1); + GLfloat yy = q+(y<<RBITS); + glRecti( xx, yy, xx+1, yy+1); } } - else if( ! r->cells.empty() ) + else if( ! r->cells.empty() ) // empty but used previously { double left = x << RBITS; double right = (x+1) << RBITS; @@ -90,7 +82,7 @@ double top = (y+1) << RBITS; double d = 3.0; - + // draw little corner markers for regions with memory // allocated but no contents glBegin( GL_LINES ); @@ -122,144 +114,92 @@ glPopMatrix(); } -// TODO -#if 0 -void SuperRegion::Draw( bool drawall ) + +static void DrawBlock( GLfloat x, GLfloat y, GLfloat zmin, GLfloat zmax ) { - glEnable( GL_DEPTH_TEST ); + glBegin( GL_QUADS ); + + // TOP + glVertex3f( x, y, zmax ); + glVertex3f( 1+x, y, zmax ); + glVertex3f( 1+x, 1+y, zmax ); + glVertex3f( x, 1+y, zmax ); + + // sides + glVertex3f( x, y, zmax ); + glVertex3f( x, 1+y, zmax ); + glVertex3f( x, 1+y, zmin ); + glVertex3f( x, y, zmin ); + + glVertex3f( 1+x, y, zmax ); + glVertex3f( x, y, zmax ); + glVertex3f( x, y, zmin ); + glVertex3f( 1+x, y, zmin ); + + glVertex3f( 1+x, 1+y, zmax ); + glVertex3f( 1+x, y, zmax ); + glVertex3f( 1+x, y, zmin ); + glVertex3f( 1+x, 1+y, zmin ); + + glVertex3f( x, 1+y, zmax ); + glVertex3f( 1+x, 1+y, zmax ); + glVertex3f( 1+x, 1+y, zmin ); + glVertex3f( x, 1+y, zmin ); - glPushMatrix(); + glEnd(); +} + +void SuperRegion::DrawVoxels() +{ + glPushMatrix(); + GLfloat scale = 1.0/world->Resolution(); + glScalef( scale, scale, 1.0 ); // XX TODO - this seems slightly glTranslatef( origin.x<<SRBITS, origin.y<<SRBITS,0); + + glEnable( GL_DEPTH_TEST ); glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); - r = GetRegion( 0, 0); - + Region* r = GetRegion( 0, 0); + for( int y=0; y<SUPERREGIONWIDTH; y++ ) for( int x=0; x<SUPERREGIONWIDTH; x++ ) { - if( r->count < 1 ) - { - r++; - continue; - } - - snprintf( buf, 15, "%lu", r->count ); - Gl::draw_string( x<<RBITS, y<<RBITS, 0, buf ); - - for( int p=0; p<REGIONWIDTH; p++ ) - for( int q=0; q<REGIONWIDTH; q++ ) - if( r->cells[p+(q*REGIONWIDTH)].blocks.size() ) - { - GLfloat xx = p+(x<<RBITS); - GLfloat yy = q+(y<<RBITS); + if( r->count ) + for( int p=0; p<REGIONWIDTH; p++ ) + for( int q=0; q<REGIONWIDTH; q++ ) + { + Cell* c = (Cell*)&r->cells[p+(q*REGIONWIDTH)]; - if( ! drawall ) // draw a rectangle on the floor - { - glRecti( xx, yy, - xx+1, yy+1); - } - else // draw a rectangular solid - { - Cell* c = (Cell*)&r->cells[p+(q*REGIONWIDTH)]; - + if( c->blocks.size() ) + { + GLfloat xx = p+(x<<RBITS); + GLfloat yy = q+(y<<RBITS); + FOR_EACH( it, c->blocks ) { Block* block = *it; - - //printf( "zb %.2f %.2f\n", ent->zbounds.min, ent->zbounds.max ); - + // first draw filled polygons Color c = block->GetColor(); glColor4f( c.r, c.g, c.b, 1.0 ); - + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); glEnable(GL_POLYGON_OFFSET_FILL); // TODO - these numbers need tweaking for // better-looking rendering - glPolygonOffset(0.01, 0.1); - - // // TOP - glBegin( GL_POLYGON ); - glVertex3f( xx, yy, block->global_z.max ); - glVertex3f( 1+xx, yy, block->global_z.max ); - glVertex3f( 1+xx, 1+yy, block->global_z.max ); - glVertex3f( xx, 1+yy, block->global_z.max ); - glEnd(); - - // sides - glBegin( GL_QUADS ); - glVertex3f( xx, yy, block->global_z.max ); - glVertex3f( xx, 1+yy, block->global_z.max ); - glVertex3f( xx, 1+yy, block->global_z.min ); - glVertex3f( xx, yy, block->global_z.min ); - - glVertex3f( 1+xx, yy, block->global_z.max ); - glVertex3f( xx, yy, block->global_z.max ); - glVertex3f( xx, yy, block->global_z.min ); - glVertex3f( 1+xx, yy, block->global_z.min ); - - glVertex3f( 1+xx, 1+yy, block->global_z.max ); - glVertex3f( 1+xx, yy, block->global_z.max ); - glVertex3f( 1+xx, yy, block->global_z.min ); - glVertex3f( 1+xx, 1+yy, block->global_z.min ); - - glVertex3f( xx, 1+yy, block->global_z.max ); - glVertex3f( 1+xx, 1+yy, block->global_z.max ); - glVertex3f( 1+xx, 1+yy, block->global_z.min ); - glVertex3f( xx, 1+yy, block->global_z.min ); - glEnd(); - - glDisable(GL_POLYGON_OFFSET_FILL); - + glPolygonOffset(0.01, 0.1); + DrawBlock( xx, yy, block->global_z.min, block->global_z.max ); + + // draw again in outline + glDisable(GL_POLYGON_OFFSET_FILL); glColor4f( c.r/2.0, c.g/2.0, c.b/2.0, c.a ); - glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); - - // TOP - glBegin( GL_POLYGON ); - glVertex3f( xx, yy, block->global_z.max ); - glVertex3f( 1+xx, yy, block->global_z.max ); - glVertex3f( 1+xx, 1+yy, block->global_z.max ); - glVertex3f( xx, 1+yy, block->global_z.max ); - glEnd(); - - // sides - glBegin( GL_QUADS ); - glVertex3f( xx, yy, block->global_z.max ); - glVertex3f( xx, 1+yy, block->global_z.max ); - glVertex3f( xx, 1+yy, block->global_z.min ); - glVertex3f( xx, yy, block->global_z.min ); - - glVertex3f( 1+xx, yy, block->global_z.max ); - glVertex3f( xx, yy, block->global_z.max ); - glVertex3f( xx, yy, block->global_z.min ); - glVertex3f( 1+xx, yy, block->global_z.min ); - - glVertex3f( 1+xx, 1+yy, block->global_z.max ); - glVertex3f( 1+xx, yy, block->global_z.max ); - glVertex3f( 1+xx, yy, block->global_z.min ); - glVertex3f( 1+xx, 1+yy, block->global_z.min ); - - glVertex3f( xx, 1+yy, block->global_z.max ); - glVertex3f( 1+xx, 1+yy, block->global_z.max ); - glVertex3f( 1+xx, 1+yy, block->global_z.min ); - glVertex3f( xx, 1+yy, block->global_z.min ); - glEnd(); - } - } - } - + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + DrawBlock( xx, yy, block->global_z.min, block->global_z.max ); + } + } + } + ++r; } - glPopMatrix(); -} -#endif - -//inline -void SuperRegion::Floor() -{ - glPushMatrix(); - glTranslatef( origin.x<<SRBITS, origin.y<<SRBITS, 0 ); - glRecti( 0,0, 1<<SRBITS, 1<<SRBITS ); glPopMatrix(); } - Modified: code/stage/trunk/libstage/region.hh =================================================================== --- code/stage/trunk/libstage/region.hh 2009-08-26 23:43:56 UTC (rev 8236) +++ code/stage/trunk/libstage/region.hh 2009-08-27 19:46:25 UTC (rev 8237) @@ -94,8 +94,8 @@ Region* GetRegion( int32_t x, int32_t y ) { return( ®ions[ x + y * SUPERREGIONWIDTH ] ); } - void Draw( bool drawall ); - void Floor(); + void DrawOccupancy(); + void DrawVoxels(); unsigned long count; // number of blocks rendered into this superregion }; // class SuperRegion; Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-08-26 23:43:56 UTC (rev 8236) +++ code/stage/trunk/libstage/stage.hh 2009-08-27 19:46:25 UTC (rev 8237) @@ -1462,9 +1462,9 @@ virtual void PushColor( double r, double g, double b, double a ); virtual void PopColor(); - void DrawTree( bool leaves ); - void DrawFloor(); - + void DrawOccupancy(); + void DrawVoxels(); + public: WorldGui(int W,int H,const char*L=0); Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2009-08-26 23:43:56 UTC (rev 8236) +++ code/stage/trunk/libstage/worldgui.cc 2009-08-27 19:46:25 UTC (rev 8237) @@ -417,20 +417,16 @@ return std::string( str ); } -void WorldGui::DrawTree( bool drawall ) +void WorldGui::DrawOccupancy() { FOR_EACH( it, superregions ) - (*it).second->Draw( drawall ); + (*it).second->DrawOccupancy(); } -void WorldGui::DrawFloor() -{ - PushColor( 1,1,1,1 ); - +void WorldGui::DrawVoxels() +{ FOR_EACH( it, superregions ) - (*it).second->Floor(); - - PopColor(); + (*it).second->DrawVoxels(); } void WorldGui::windowCb( Fl_Widget* w, WorldGui* wg ) @@ -523,7 +519,13 @@ void WorldGui::slowerCb( Fl_Widget* w, WorldGui* wg ) { - wg->speedup *= 0.8; + if( wg->speedup <= 0 ) + { + wg->speedup = 100.0; + wg->SetTimeouts(); + } + else + wg->speedup *= 0.8; } void WorldGui::fasterCb( Fl_Widget* w, WorldGui* wg ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |