From: <al...@us...> - 2008-07-21 16:21:33
|
Revision: 6898 http://playerstage.svn.sourceforge.net/playerstage/?rev=6898&view=rev Author: alexcb Date: 2008-07-21 23:21:42 +0000 (Mon, 21 Jul 2008) Log Message: ----------- optimized raytracing code Modified Paths: -------------- code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/region.hh code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2008-07-21 18:27:33 UTC (rev 6897) +++ code/stage/trunk/libstage/block.cc 2008-07-21 23:21:42 UTC (rev 6898) @@ -67,6 +67,7 @@ glPopMatrix(); } +//TODO FIXME - this is really SLOW void StgBlock::DrawSides() { // construct a strip that wraps around the polygon Modified: code/stage/trunk/libstage/region.hh =================================================================== --- code/stage/trunk/libstage/region.hh 2008-07-21 18:27:33 UTC (rev 6897) +++ code/stage/trunk/libstage/region.hh 2008-07-21 23:21:42 UTC (rev 6898) @@ -35,8 +35,10 @@ stg_cell_t* GetCell( int32_t x, int32_t y ) { uint32_t index = x + (y*REGIONWIDTH); +#ifdef DEBUG assert( index >=0 ); - assert( index < REGIONSIZE ); + assert( index < REGIONSIZE ); +#endif return &cells[index]; } Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2008-07-21 18:27:33 UTC (rev 6897) +++ code/stage/trunk/libstage/world.cc 2008-07-21 23:21:42 UTC (rev 6898) @@ -448,152 +448,172 @@ stg_raytrace_sample_t* sample, bool ztest ) { - // printf( "raytracing at [ %.2f %.2f %.2f %.2f ] for %.2f \n", -// pose.x, -// pose.y, -// pose.z, -// pose.a, -// range ); + // printf( "raytracing at [ %.2f %.2f %.2f %.2f ] for %.2f \n", + // pose.x, + // pose.y, + // pose.z, + // pose.a, + // range ); + + // initialize the sample + sample->pose = pose; + sample->range = range; // we might change this below + sample->block = NULL; // we might change this below + + // find the global integer bitmap address of the ray + int32_t x = (int32_t)(pose.x*ppm); + int32_t y = (int32_t)(pose.y*ppm); + int32_t z = 0; + + int32_t xstart = x; + int32_t ystart = y; + + // and the x and y offsets of the ray + int32_t dx = (int32_t)(ppm*range * cos(pose.a)); + int32_t dy = (int32_t)(ppm*range * sin(pose.a)); + int32_t dz = 0; + + // if( finder->debug ) + // RecordRay( pose.x, + // pose.y, + // pose.x + range.max * cos(pose.a), + // pose.y + range.max * sin(pose.a) ); + + // fast integer line 3d algorithm adapted from Cohen's code from + // Graphics Gems IV + int n, sx, sy, sz, exy, exz, ezy, ax, ay, az, bx, by, bz; + sx = sgn(dx); sy = sgn(dy); sz = sgn(dz); + ax = abs(dx); ay = abs(dy); az = abs(dz); + bx = 2*ax; by = 2*ay; bz = 2*az; + exy = ay-ax; exz = az-ax; ezy = ay-az; + n = ax+ay+az; + + // printf( "Raytracing from (%d,%d,%d) steps (%d,%d,%d) %d\n", + // x,y,z, dx,dy,dz, n ); + + // superregion coords + stg_point_int_t lastsup; + lastsup.x = INT_MAX; // an unlikely first raytrace + lastsup.y = INT_MAX; + + stg_point_int_t lastreg = {0,0}; + lastsup.x = INT_MAX; // an unlikely first raytrace + lastsup.y = INT_MAX; + + SuperRegion* sr = NULL; + Region* r = NULL; + bool nonempty_region = false; + + //puts( "RAYTRACE" ); + + //bitmasks to calculate region, and cell values + static int32_t reg_coord_mask = ~ ( ( ~ 0x00 ) << SRBITS ); //this then needs to be shifted >> + static int32_t cell_coord_mask = ~ ( ( ~ 0x00 ) << RBITS ); - // initialize the sample - sample->pose = pose; - sample->range = range; // we might change this below - sample->block = NULL; // we might change this below - - // find the global integer bitmap address of the ray - int32_t x = (int32_t)(pose.x*ppm); - int32_t y = (int32_t)(pose.y*ppm); - int32_t z = 0; - - int32_t xstart = x; - int32_t ystart = y; - - // and the x and y offsets of the ray - int32_t dx = (int32_t)(ppm*range * cos(pose.a)); - int32_t dy = (int32_t)(ppm*range * sin(pose.a)); - int32_t dz = 0; - - // if( finder->debug ) - // RecordRay( pose.x, - // pose.y, - // pose.x + range.max * cos(pose.a), - // pose.y + range.max * sin(pose.a) ); - - // fast integer line 3d algorithm adapted from Cohen's code from - // Graphics Gems IV - int n, sx, sy, sz, exy, exz, ezy, ax, ay, az, bx, by, bz; - sx = sgn(dx); sy = sgn(dy); sz = sgn(dz); - ax = abs(dx); ay = abs(dy); az = abs(dz); - bx = 2*ax; by = 2*ay; bz = 2*az; - exy = ay-ax; exz = az-ax; ezy = ay-az; - n = ax+ay+az; - - // printf( "Raytracing from (%d,%d,%d) steps (%d,%d,%d) %d\n", - // x,y,z, dx,dy,dz, n ); - - // superregion coords - stg_point_int_t lastsup; - lastsup.x = INT_MAX; // an unlikely first raytrace - lastsup.y = INT_MAX; - - stg_point_int_t lastreg = {0,0}; - lastsup.x = INT_MAX; // an unlikely first raytrace - lastsup.y = INT_MAX; - - SuperRegion* sr = NULL; - Region* r = NULL; - - //puts( "RAYTRACE" ); - - while ( n-- ) + // superregion coords (must be updated every time x or y changes + // but only one variable changes at a time in the loop, so its + // more efficient to compute at the end of the loop, when we know what's changed) + stg_point_int_t sup; + sup.x = x >> SRBITS; + sup.y = y >> SRBITS; + + // find the region coords inside this superregion (again: only updated when x or y changes) + stg_point_int_t reg; + reg.x = ( x & reg_coord_mask ) >> RBITS; + reg.y = ( y & reg_coord_mask ) >> RBITS; + + // compute the pixel offset inside this region (again: only updated when x or y changes) + stg_point_int_t cell; + cell.x = x & cell_coord_mask; + cell.y = y & cell_coord_mask; + + + while ( n-- ) { - // superregion coords - stg_point_int_t sup; - sup.x = x >> SRBITS; - sup.y = y >> SRBITS; - - // printf( "pixel [%d %d]\tS[ %d %d ]\t", - // x, y, sup.x, sup.y ); + + // printf( "pixel [%d %d]\tS[ %d %d ]\t", + // x, y, sup.x, sup.y ); - if( ! (sup.x == lastsup.x && sup.y == lastsup.y )) - { - sr = (SuperRegion*)g_hash_table_lookup( superregions, (void*)&sup ); - lastsup = sup; // remember these coords - } + if( sr ) + { + // printf( "R[ %d %d ]\t", reg.x, reg.y ); + + if( reg.x != lastreg.x || reg.y != lastreg.y ) + { + r = sr->GetRegion( reg.x, reg.y ); + lastreg = reg; + nonempty_region = ( r && r->count ); + } + + if( nonempty_region ) + { + // printf( "C[ %d %d ]\t", cell.x, cell.y ); + + for( GSList* list = r->GetCell( cell.x, cell.y )->list; + list; + list = list->next ) + { + StgBlock* block = (StgBlock*)list->data; + //assert( block ); + + // if this block does not belong to the searching model and it + // matches the predicate and it's in the right z range + if( //block && (block->Model() != finder) && + (ztest ? block->IntersectGlobalZ( pose.z ) : true) && + (*func)( block, mod, arg ) ) + { + // a hit! + sample->block = block; + sample->range = hypot( (x-xstart)/ppm, (y-ystart)/ppm ); + return; + } + } + } + } + + // printf( "\t step %d n %d pixel [ %d, %d ] block [ %d %d ] index [ %d %d ] \n", + // //coarse [ %d %d ]\n", + // count++, n, x, y, blockx, blocky, b_dx, b_dy ); + + // increment our pixel in the correct direction + if ( exy < 0 ) { + x += sx; exy += by; //exz += bz; + + //update super region + sup.x = x >> SRBITS; + if( sup.x != lastsup.x ) { + sr = (SuperRegion*)g_hash_table_lookup( superregions, (void*)&sup ); + lastsup = sup; // remember these coords + } - if( sr ) - { - // find the region coords inside this superregion - stg_point_int_t reg; - reg.x = (x - ( sup.x << SRBITS)) >> RBITS; - reg.y = (y - ( sup.y << SRBITS)) >> RBITS; - - // printf( "R[ %d %d ]\t", reg.x, reg.y ); - - if( ! (reg.x == lastreg.x && reg.y == lastreg.y )) - { - r = sr->GetRegion( reg.x, reg.y ); - lastreg = reg; - } - - if( r && r->count ) - { - // compute the pixel offset inside this region - stg_point_int_t cell; - cell.x = x - ((sup.x << SRBITS) + (reg.x << RBITS)); - cell.y = y - ((sup.y << SRBITS) + (reg.y << RBITS)); - - // printf( "C[ %d %d ]\t", cell.x, cell.y ); - - for( GSList* list = r->GetCell( cell.x, cell.y )->list; - list; - list = list->next ) - { - StgBlock* block = (StgBlock*)list->data; - assert( block ); - - // if this block does not belong to the searching model and it - // matches the predicate and it's in the right z range - if( //block && (block->Model() != finder) && - (ztest ? block->IntersectGlobalZ( pose.z ) : true) && - (*func)( block, mod, arg ) ) - { - // a hit! - sample->block = block; - sample->range = hypot( (x-xstart)/ppm, (y-ystart)/ppm ); - return; - } + //the next two computations are only needed when sr is valid; + //however, profiling shows it is faster without the if-branch + //recompute region x value - this can be skipped if sr == NULL, but might be cheaper without the if() cond + reg.x = ( x & reg_coord_mask ) >> RBITS; + + //recompute cell x value - can be skipped if sr == NULL + cell.x = x & cell_coord_mask; } - } - } - - // printf( "\t step %d n %d pixel [ %d, %d ] block [ %d %d ] index [ %d %d ] \n", - // //coarse [ %d %d ]\n", - // count++, n, x, y, blockx, blocky, b_dx, b_dy ); - - // increment our pixel in the correct direction - if ( exy < 0 ) { - if ( exz < 0 ) { - x += sx; exy += by; exz += bz; - } - else { - z += sz; exz -= bx; ezy += by; - } - } - else { - if ( ezy < 0 ) { - z += sz; - exz -= bx; ezy += by; - } - else { - y += sy; exy -= bx; ezy -= bz; - } - } - // puts(""); + else { + y += sy; exy -= bx; //ezy -= bz; + //update super region + sup.y = y >> SRBITS; + if( sup.y != lastsup.y ) { + sr = (SuperRegion*)g_hash_table_lookup( superregions, (void*)&sup ); + lastsup = sup; // remember these coords + } + + //same deal with profiling - it is faster without the if() cond + //recompute region y value - this can be skipped if sr == NULL + reg.y = ( y & reg_coord_mask ) >> RBITS; + + //recompute cell y value - can be skipped if sr == NULL + cell.y = y & cell_coord_mask; + } } - - // hit nothing - return; + + // hit nothing + return; } static void _save_cb( gpointer key, gpointer data, gpointer user ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <al...@us...> - 2008-07-22 13:23:12
|
Revision: 6903 http://playerstage.svn.sourceforge.net/playerstage/?rev=6903&view=rev Author: alexcb Date: 2008-07-22 20:23:21 +0000 (Tue, 22 Jul 2008) Log Message: ----------- fixed non-gui mode, and quit_time Modified Paths: -------------- code/stage/trunk/libstage/main.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/main.cc =================================================================== --- code/stage/trunk/libstage/main.cc 2008-07-22 17:58:47 UTC (rev 6902) +++ code/stage/trunk/libstage/main.cc 2008-07-22 20:23:21 UTC (rev 6903) @@ -51,12 +51,13 @@ bool loaded_world_file = false; + optindex = optind; //points to first non-option while( optindex < argc ) { if( optindex > 0 ) { const char* worldfilename = argv[optindex]; - StgWorldGui* world = new StgWorldGui( 400, 300, worldfilename ); + StgWorld* world = ( usegui ? new StgWorldGui( 400, 300, worldfilename ) : new StgWorld( worldfilename ) ); world->Load( worldfilename ); loaded_world_file = true; } @@ -68,8 +69,15 @@ new StgWorldGui( 400, 300 ); } - - while(true) - StgWorld::UpdateAll(); + if( usegui == true ) { + //don't close the window once time has finished + while( true ) + StgWorld::UpdateAll(); + } else { + //close program once time has completed + bool quit = false; + while( quit == false ) + quit = StgWorld::UpdateAll(); + } } Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-07-22 17:58:47 UTC (rev 6902) +++ code/stage/trunk/libstage/model.cc 2008-07-22 20:23:21 UTC (rev 6903) @@ -199,7 +199,8 @@ this->subs = 0; this->stall = false; - this->blocks_dl = glGenLists( 1 ); + if( world->IsGUI() ) + this->blocks_dl = glGenLists( 1 ); this->geom.size.x = DEFAULT_GEOM_SIZEX; this->geom.size.y = DEFAULT_GEOM_SIZEY; Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2008-07-22 17:58:47 UTC (rev 6902) +++ code/stage/trunk/libstage/model_laser.cc 2008-07-22 20:23:21 UTC (rev 6903) @@ -101,7 +101,8 @@ // don't allocate sample buffer memory until Update() is called samples = NULL; - data_dl = glGenLists(1); + if( world->IsGUI() ) + data_dl = glGenLists(1); registerOption( &showLaserData ); registerOption( &showLaserStrikes ); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-07-22 17:58:47 UTC (rev 6902) +++ code/stage/trunk/libstage/stage.hh 2008-07-22 20:23:21 UTC (rev 6903) @@ -945,7 +945,7 @@ class Region; class SuperRegion; - + /// %StgWorld class class StgWorld : public StgAncestor { @@ -959,7 +959,7 @@ /** Coordinate system stack - experimental*/ GQueue* csstack; - + void PushPose(); void PopPose(); stg_pose_t* PeekPose(); @@ -988,6 +988,7 @@ virtual void PushColor( stg_color_t col ) { /* do nothing */ }; virtual void PushColor( double r, double g, double b, double a ) { /* do nothing */ }; virtual void PopColor(){ /* do nothing */ }; + /** The current real time in microseconds */ stg_usec_t real_time_now; @@ -1039,6 +1040,7 @@ static void UpdateCb( StgWorld* world); stg_usec_t sim_time; ///< the current sim time in this world in ms + inline bool PastQuitTime() const { return (quit_time > 0) && (sim_time >= quit_time); } GList* ray_list; // store rays traced for debugging purposes @@ -1066,7 +1068,7 @@ public: 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 - static void UpdateAll(); + static bool UpdateAll(); //returns true when time to quit, false otherwise StgWorld( const char* token = "MyWorld", stg_msec_t interval_sim = DEFAULT_INTERVAL_SIM, @@ -1087,6 +1089,8 @@ Worldfile* GetWorldFile(){ return wf; }; + inline virtual bool IsGUI() { return false; } + virtual void Load( const char* worldfile_path ); virtual void UnLoad(); virtual void Reload(); @@ -2000,6 +2004,9 @@ bool saveAsDialog(); bool closeWindowQuery(); + // Quit time pause + bool pause_time; + protected: virtual void PushColor( stg_color_t col ) { canvas->PushColor( col ); } @@ -2027,6 +2034,7 @@ virtual void UnLoad(); virtual bool Save( const char* filename ); + inline virtual bool IsGUI() { return true; } void Start(){ paused = false; }; void Stop(){ paused = true; }; Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2008-07-22 17:58:47 UTC (rev 6902) +++ code/stage/trunk/libstage/world.cc 2008-07-22 20:23:21 UTC (rev 6903) @@ -49,7 +49,6 @@ #include "region.hh" #include "file_manager.hh" - // static data members unsigned int StgWorld::next_id = 0; bool StgWorld::quit_all = false; @@ -81,15 +80,19 @@ delete sr; } -void StgWorld::UpdateAll() +bool StgWorld::UpdateAll() { - for( GList* it = StgWorld::world_list; it; it=it->next ) - ((StgWorld*)it->data)->Update(); + bool quit = true; + for( GList* it = StgWorld::world_list; it; it=it->next ) { + if( ((StgWorld*)it->data)->Update() == false ) + quit = false; + } + return quit; } StgWorld::StgWorld( const char* token, stg_msec_t interval_sim, - double ppm ) + double ppm ) { Initialize( token, interval_sim, ppm ); } @@ -223,9 +226,10 @@ wf->ReadInt( entity, "interval_sim", (int)(this->interval_sim/thousand) ); - if( wf->PropertyExists( entity, "quit_time" ) ) + if( wf->PropertyExists( entity, "quit_time" ) ) { this->quit_time = (stg_usec_t)million * - wf->ReadInt( entity, "quit_time", 0 ); + wf->ReadFloat( entity, "quit_time", 0 ); + } if( wf->PropertyExists( entity, "resolution" ) ) this->ppm = @@ -361,6 +365,12 @@ { PRINT_DEBUG( "StgWorld::Update()" ); + // if we've run long enough, exit + if( PastQuitTime() ) { + if( IsGUI() == false ) + return true; + } + // update any models that are due to be updated for( GList* it=this->update_list; it; it=it->next ) ((StgModel*)it->data)->UpdateIfDue(); @@ -371,12 +381,8 @@ this->sim_time += this->interval_sim; this->updates++; - - // if we've run long enough, set the quit flag - if( (quit_time > 0) && (sim_time >= quit_time) ) - quit = true; - - return true; + + return false; } void StgWorld::AddModel( StgModel* mod ) Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-07-22 17:58:47 UTC (rev 6902) +++ code/stage/trunk/libstage/worldgui.cc 2008-07-22 20:23:21 UTC (rev 6903) @@ -165,6 +165,8 @@ label( PROJECT ); + pause_time = false; + interval_real = (stg_usec_t)thousand * DEFAULT_INTERVAL_REAL; for( unsigned int i=0; i<INTERVAL_LOG_LEN; i++ ) @@ -288,6 +290,12 @@ if( real_time_of_last_update == 0 ) real_time_of_last_update = RealTimeNow(); + //pause the simulation if quit time is set + if( PastQuitTime() && pause_time == false ) { + TogglePause(); + pause_time = true; + } + bool val = paused ? true : StgWorld::Update(); stg_usec_t interval; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-07-23 17:00:23
|
Revision: 6918 http://playerstage.svn.sourceforge.net/playerstage/?rev=6918&view=rev Author: jeremy_asher Date: 2008-07-24 00:00:30 +0000 (Thu, 24 Jul 2008) Log Message: ----------- stage: fixed ForEachModel being called multiple times for some models by replacing with new ForEachDescendant method in StgAncestor Modified Paths: -------------- code/stage/trunk/libstage/ancestor.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_fiducial.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 2008-07-23 21:50:03 UTC (rev 6917) +++ code/stage/trunk/libstage/ancestor.cc 2008-07-24 00:00:30 UTC (rev 6918) @@ -63,3 +63,12 @@ return pose; } +void StgAncestor::ForEachDescendant( stg_model_callback_t func, void* arg ) +{ + for( GList* it=children; it; it=it->next ) { + StgModel* mod = (StgModel*)it->data; + func( mod, arg ); + mod->ForEachDescendant( func, arg ); + } +} + Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-07-23 21:50:03 UTC (rev 6917) +++ code/stage/trunk/libstage/model.cc 2008-07-24 00:00:30 UTC (rev 6918) @@ -705,6 +705,7 @@ { //printf( "Startup model %s\n", this->token ); + // TODO: this could be a callback if( initfunc ) initfunc( this ); Modified: code/stage/trunk/libstage/model_fiducial.cc =================================================================== --- code/stage/trunk/libstage/model_fiducial.cc 2008-07-23 21:50:03 UTC (rev 6917) +++ code/stage/trunk/libstage/model_fiducial.cc 2008-07-24 00:00:30 UTC (rev 6918) @@ -228,7 +228,7 @@ // TODO - add a fiducial-only hash table to the world to speed this // up a lot for large populations - world->ForEachModel( (GHFunc)(StgModelFiducial::AddModelIfVisibleStatic), + world->ForEachDescendant( (stg_model_callback_t)(StgModelFiducial::AddModelIfVisibleStatic), this ); PRINT_DEBUG2( "model %s saw %d fiducials", token, data->len ); @@ -310,3 +310,15 @@ PopColor(); } } + +void StgModelFiducial::Shutdown( void ) +{ + PRINT_DEBUG( "fiducial shutdown" ); + + // clear the data + data = g_array_set_size( data, 0 ); + fiducials = NULL; + fiducial_count = 0; + + StgModel::Shutdown(); +} Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-07-23 21:50:03 UTC (rev 6917) +++ code/stage/trunk/libstage/stage.hh 2008-07-24 00:00:30 UTC (rev 6918) @@ -848,13 +848,14 @@ protected: GList* children; - //GHashTable* child_types; - char* token; bool debug; public: + + /** recursively call func( model, arg ) for each descendant */ + void ForEachDescendant( stg_model_callback_t func, void* arg ); /** array contains the number of each type of child model */ unsigned int child_type_counts[MODEL_TYPE_COUNT]; @@ -1118,10 +1119,6 @@ /** Return the 3D bounding box of the world, in meters */ stg_bounds3d_t GetExtent(){ return extent; }; - /** call func( model, arg ) for each model in the world */ - void ForEachModel( GHFunc func, void* arg ) - { g_hash_table_foreach( models_by_name, func, arg ); }; - /** Return the number of times the world has been updated. */ long unsigned int GetUpdateCount() { return updates; } }; @@ -2354,9 +2351,7 @@ void AddModelIfVisible( StgModel* him ); // static wrapper function can be used as a function pointer - static void AddModelIfVisibleStatic( gpointer key, - StgModel* him, - StgModelFiducial* me ) + static int AddModelIfVisibleStatic( StgModel* him, StgModelFiducial* me ) { if( him != me ) me->AddModelIfVisible( him ); }; virtual void Update(); @@ -2375,6 +2370,7 @@ virtual ~StgModelFiducial(); virtual void Load(); + void Shutdown( void ); stg_meters_t max_range_anon; //< maximum detection range stg_meters_t max_range_id; ///< maximum range at which the ID can be read Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2008-07-23 21:50:03 UTC (rev 6917) +++ code/stage/trunk/libstage/world.cc 2008-07-24 00:00:30 UTC (rev 6918) @@ -622,9 +622,9 @@ return; } -static void _save_cb( gpointer key, gpointer data, gpointer user ) +static int _save_cb( StgModel* mod, void* dummy ) { - ((StgModel*)data)->Save(); + mod->Save(); } bool StgWorld::Save( const char *filename ) @@ -632,20 +632,20 @@ // ask every model to save itself //g_hash_table_foreach( this->models_by_id, stg_model_save_cb, NULL ); - ForEachModel( _save_cb, NULL ); + ForEachDescendant( _save_cb, NULL ); return this->wf->Save( filename ); } -static void _reload_cb( gpointer key, gpointer data, gpointer user ) +static int _reload_cb( StgModel* mod, void* dummy ) { - ((StgModel*)data)->Load(); + mod->Load(); } // reload the current worldfile void StgWorld::Reload( void ) { - ForEachModel( _reload_cb, NULL ); + ForEachDescendant( _reload_cb, NULL ); } void StgWorld::StartUpdatingModel( StgModel* mod ) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-07-24 12:22:04
|
Revision: 6923 http://playerstage.svn.sourceforge.net/playerstage/?rev=6923&view=rev Author: jeremy_asher Date: 2008-07-24 19:22:12 +0000 (Thu, 24 Jul 2008) Log Message: ----------- stage: fixed polygon fill mode inconsistencies by setting Front&Back/Fill once and reverting to filled mode after changing to line mode every time Modified Paths: -------------- code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_blobfinder.cc code/stage/trunk/libstage/model_camera.cc code/stage/trunk/libstage/model_fiducial.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/model_ranger.cc Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2008-07-24 18:45:15 UTC (rev 6922) +++ code/stage/trunk/libstage/block.cc 2008-07-24 19:22:12 UTC (rev 6923) @@ -99,7 +99,6 @@ // draw filled color polygons stg_color_t color = Color(); - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL ); PushColor( color ); glEnable(GL_POLYGON_OFFSET_FILL); glPolygonOffset(1.0, 1.0); @@ -112,19 +111,19 @@ stg_color_unpack( color, &r, &g, &b, &a ); PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a )); - glPolygonMode(GL_FRONT_AND_BACK, GL_LINE ); + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); glDepthMask(GL_FALSE); DrawTop(); DrawSides(); glDepthMask(GL_TRUE); - + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + PopColor(); PopColor(); } void StgBlock::DrawSolid( void ) { - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL ); DrawSides(); DrawTop(); } Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-07-24 18:45:15 UTC (rev 6922) +++ code/stage/trunk/libstage/canvas.cc 2008-07-24 19:22:12 UTC (rev 6923) @@ -91,10 +91,9 @@ make_current(); // make sure the GL context is current glClearColor ( 1,1,1,1 ); // white glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); - glLoadIdentity(); - current_camera->SetProjection(); - current_camera->Draw(); + glLoadIdentity(); + current_camera->SetProjection(); + current_camera->Draw(); glDisable(GL_DITHER); glDisable(GL_BLEND); // turns off alpha blending, so we read back @@ -466,8 +465,6 @@ void StgCanvas::DrawGlobalGrid() { - - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); stg_bounds3d_t bounds = world->GetExtent(); char str[16]; @@ -519,7 +516,6 @@ stg_bounds3d_t bounds = world->GetExtent(); float z = 0; - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL ); glEnable(GL_POLYGON_OFFSET_FILL); glPolygonOffset(2.0, 2.0); @@ -623,6 +619,7 @@ ((StgWorldGui*)world)->DrawTree( true ); colorstack.Pop(); + glPolygonMode( GL_FRONT, GL_FILL ); glPopMatrix(); } @@ -632,7 +629,6 @@ if( showFootprints ) { glDisable( GL_DEPTH_TEST ); - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL ); for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { i->second->DrawTrailFootprint(); @@ -642,12 +638,9 @@ if( showTrailRise ) { - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL ); - for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { i->second->DrawTrailBlocks(); } - } if( showTrailArrows ) @@ -743,8 +736,6 @@ // if trails are on, we need to clear the clock background - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); - std::string clockstr = world->ClockString(); if( showFollow == true && last_selection ) clockstr.append( " [ FOLLOW MODE ]" ); @@ -954,7 +945,8 @@ glHint( GL_LINE_SMOOTH_HINT, GL_FASTEST ); glDepthMask( GL_TRUE ); glEnable( GL_TEXTURE_2D ); - glEnableClientState( GL_VERTEX_ARRAY ); + glEnableClientState( GL_VERTEX_ARRAY ); + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); //TODO find a better home for loading textures if( loaded_texture == false ) { Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-07-24 18:45:15 UTC (rev 6922) +++ code/stage/trunk/libstage/model.cc 2008-07-24 19:22:12 UTC (rev 6923) @@ -785,17 +785,15 @@ stg_color_unpack( checkpoint->color, &r, &g, &b, &a ); PushColor( r, g, b, 0.1 ); - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL ); LISTMETHOD( this->blocks, StgBlock*, DrawFootPrint ); - glPolygonMode(GL_FRONT_AND_BACK, GL_LINE ); + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); PushColor( r/2, g/2, b/2, 0.1 ); LISTMETHOD( this->blocks, StgBlock*, DrawFootPrint ); - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL ); - PopColor(); PopColor(); + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); glPopMatrix(); } } @@ -840,7 +838,6 @@ PushColor( checkpoint->color ); - glPolygonMode(GL_FRONT_AND_BACK, GL_FILL ); glEnable(GL_POLYGON_OFFSET_FILL); glPolygonOffset(1.0, 1.0); @@ -851,7 +848,7 @@ glEnd(); glDisable(GL_POLYGON_OFFSET_FILL); - glPolygonMode(GL_FRONT_AND_BACK, GL_LINE ); + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); stg_color_unpack( checkpoint->color, &r, &g, &b, &a ); PushColor( r/2, g/2, b/2, 1 ); // darker color @@ -864,6 +861,7 @@ glEnd(); glDepthMask(GL_TRUE); + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); PopColor(); PopColor(); PopCoords(); @@ -1091,7 +1089,6 @@ glEnable(GL_TEXTURE_2D); glBindTexture( GL_TEXTURE_2D, texture_id ); - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); glColor4f( 1.0, 1.0, 1.0, alpha ); glPushMatrix(); @@ -1144,7 +1141,6 @@ PushColor( flag->color ); - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); gluQuadricDrawStyle( quadric, GLU_FILL ); gluSphere( quadric, flag->size/2.0, 4,2 ); @@ -1180,8 +1176,6 @@ //stg_pose_t gpose = GetGlobalPose(); //glRotatef( 180 + rtod(-gpose.a),0,0,1 ); - //glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); - for( unsigned int i=0; i<blinkenlights->len; i++ ) { stg_blinkenlight_t* b = Modified: code/stage/trunk/libstage/model_blobfinder.cc =================================================================== --- code/stage/trunk/libstage/model_blobfinder.cc 2008-07-24 18:45:15 UTC (rev 6922) +++ code/stage/trunk/libstage/model_blobfinder.cc 2008-07-24 19:22:12 UTC (rev 6923) @@ -308,7 +308,6 @@ PushColor( 0,0,0,0.2 ); gluQuadricDrawStyle( quadric, GLU_SILHOUETTE ); - //glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); gluPartialDisk( quadric, 0, range, @@ -337,7 +336,6 @@ glScalef( 0.025, 0.025, 1 ); // draw a white screen with a black border - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); PushColor( 0xFFFFFFFF ); glRectf( 0,0, scan_width, scan_height ); PopColor(); @@ -350,7 +348,6 @@ PopColor(); // draw the blobs on the screen - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); for( unsigned int s=0; s<blobs->len; s++ ) { stg_blobfinder_blob_t* b = @@ -362,7 +359,8 @@ //printf( "%u l %u t%u r %u b %u\n", s, b->left, b->top, b->right, b->bottom ); PopColor(); } - + + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); glPopMatrix(); } Modified: code/stage/trunk/libstage/model_camera.cc =================================================================== --- code/stage/trunk/libstage/model_camera.cc 2008-07-24 18:45:15 UTC (rev 6922) +++ code/stage/trunk/libstage/model_camera.cc 2008-07-24 19:22:12 UTC (rev 6923) @@ -285,7 +285,6 @@ } glDisable( GL_CULL_FACE ); - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); //glBegin( GL_QUADS ); Modified: code/stage/trunk/libstage/model_fiducial.cc =================================================================== --- code/stage/trunk/libstage/model_fiducial.cc 2008-07-24 18:45:15 UTC (rev 6922) +++ code/stage/trunk/libstage/model_fiducial.cc 2008-07-24 19:22:12 UTC (rev 6923) @@ -259,7 +259,7 @@ // PushColor( 0,0,0,0.2 ); // gluQuadricDrawStyle( quadric, GLU_SILHOUETTE ); - // //glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + // gluPartialDisk( quadric, // 0, // max_range_anon, @@ -293,7 +293,7 @@ glPushMatrix(); gl_coord_shift( dx,dy,0,fid.geom.a ); - glPolygonMode( GL_FRONT, GL_LINE ); + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); glRectf( -fid.geom.x/2.0, -fid.geom.y/2.0, fid.geom.x/2.0, fid.geom.y/2.0 ); @@ -305,6 +305,7 @@ gl_draw_string( 0,0,0, idstr ); PopColor(); + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); glPopMatrix(); PopColor(); Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2008-07-24 18:45:15 UTC (rev 6922) +++ code/stage/trunk/libstage/model_laser.cc 2008-07-24 19:22:12 UTC (rev 6923) @@ -353,7 +353,6 @@ PopColor(); glDepthMask( GL_FALSE ); - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); // draw the filled polygon in transparent blue PushColor( 0, 0, 1, 0.1 ); Modified: code/stage/trunk/libstage/model_ranger.cc =================================================================== --- code/stage/trunk/libstage/model_ranger.cc 2008-07-24 18:45:15 UTC (rev 6922) +++ code/stage/trunk/libstage/model_ranger.cc 2008-07-24 19:22:12 UTC (rev 6923) @@ -328,7 +328,6 @@ if( showRangerTransducers ) { - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); PushColor( 0,0,0,1 ); for( unsigned int s=0; s<sensor_count; s++ ) @@ -384,7 +383,6 @@ // draw the filled triangles in transparent blue glDepthMask( GL_FALSE ); - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); PushColor( 0, 1, 0, 0.1 ); // transparent pale green //glEnableClientState( GL_VERTEX_ARRAY ); glVertexPointer( 3, GL_FLOAT, 0, pts ); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-07-25 14:53:30
|
Revision: 6928 http://playerstage.svn.sourceforge.net/playerstage/?rev=6928&view=rev Author: jeremy_asher Date: 2008-07-25 21:53:39 +0000 (Fri, 25 Jul 2008) Log Message: ----------- stage: fixed blobfinder visualization after previous changes Modified Paths: -------------- code/stage/trunk/libstage/model_blobfinder.cc code/stage/trunk/libstageplugin/p_blobfinder.cc Modified: code/stage/trunk/libstage/model_blobfinder.cc =================================================================== --- code/stage/trunk/libstage/model_blobfinder.cc 2008-07-25 21:27:59 UTC (rev 6927) +++ code/stage/trunk/libstage/model_blobfinder.cc 2008-07-25 21:53:39 UTC (rev 6928) @@ -327,10 +327,10 @@ // return to global rotation frame stg_pose_t gpose = GetGlobalPose(); - glRotatef( 180 + rtod(-gpose.a),0,0,1 ); + glRotatef( rtod(-gpose.a),0,0,1 ); // place the "screen" a little away from the robot - glTranslatef( 0.5, 0.5, 1.0 ); + glTranslatef( -3.0, -2.0, 1.0 ); // convert blob pixels to meters scale - arbitrary glScalef( 0.025, 0.025, 1 ); Modified: code/stage/trunk/libstageplugin/p_blobfinder.cc =================================================================== --- code/stage/trunk/libstageplugin/p_blobfinder.cc 2008-07-25 21:27:59 UTC (rev 6927) +++ code/stage/trunk/libstageplugin/p_blobfinder.cc 2008-07-25 21:53:39 UTC (rev 6928) @@ -73,7 +73,7 @@ for( b=0; b<bcount; b++ ) { // useful debug - leave in - + /* cout << "blob " << " left: " << blobs[b].left << " right: " << blobs[b].right @@ -81,8 +81,8 @@ << " bottom: " << blobs[b].bottom << " color: " << hex << blobs[b].color << dec << endl; + */ - int dx = blobs[b].right - blobs[b].left; int dy = blobs[b].top - blobs[b].bottom; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-07-25 16:16:52
|
Revision: 6929 http://playerstage.svn.sourceforge.net/playerstage/?rev=6929&view=rev Author: jeremy_asher Date: 2008-07-25 23:17:00 +0000 (Fri, 25 Jul 2008) Log Message: ----------- stage: changed blobfinder visualization to rotate to face screen, fixed up other code with the same functionality Modified Paths: -------------- code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_blinkenlight.cc code/stage/trunk/libstage/model_blobfinder.cc code/stage/trunk/libstage/model_camera.cc code/stage/trunk/libstage/model_fiducial.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/model_ranger.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstageplugin/p_driver.cc Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-07-25 21:53:39 UTC (rev 6928) +++ code/stage/trunk/libstage/canvas.cc 2008-07-25 23:17:00 UTC (rev 6929) @@ -663,14 +663,14 @@ if( showData ) { if ( visualizeAll ) { for( GList* it = world->StgWorld::children; it; it=it->next ) - ((StgModel*)it->data)->DataVisualizeTree(); + ((StgModel*)it->data)->DataVisualizeTree( current_camera ); } else if ( selected_models ) { for( GList* it = selected_models; it; it=it->next ) - ((StgModel*)it->data)->DataVisualizeTree(); + ((StgModel*)it->data)->DataVisualizeTree( current_camera ); } else if ( last_selection ) { - last_selection->DataVisualizeTree(); + last_selection->DataVisualizeTree( current_camera ); } } @@ -698,7 +698,7 @@ //ensure two icons can't be in the exact same plane if( camera.pitch() == 0 && !pCamOn ) glTranslatef( 0, 0, 0.1 ); - i->second->DrawStatusTree( this ); + i->second->DrawStatusTree( current_camera ); } glPopMatrix(); } Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-07-25 21:53:39 UTC (rev 6928) +++ code/stage/trunk/libstage/model.cc 2008-07-25 23:17:00 UTC (rev 6929) @@ -1001,21 +1001,21 @@ glPopMatrix(); } -void StgModel::DrawStatusTree( StgCanvas* canvas ) +void StgModel::DrawStatusTree( StgCamera* cam ) { PushLocalCoords(); - DrawStatus( canvas ); - LISTMETHODARG( children, StgModel*, DrawStatusTree, canvas ); + DrawStatus( cam ); + LISTMETHODARG( children, StgModel*, DrawStatusTree, cam ); PopCoords(); } -void StgModel::DrawStatus( StgCanvas* canvas ) +void StgModel::DrawStatus( StgCamera* cam ) { if( say_string ) { float yaw, pitch; - pitch = - canvas->current_camera->pitch(); - yaw = - canvas->current_camera->yaw(); + pitch = - cam->pitch(); + yaw = - cam->yaw(); float robotAngle = -rtod(pose.a); glPushMatrix(); @@ -1074,21 +1074,16 @@ if( stall ) { - DrawImage( TextureManager::getInstance()._stall_texture_id, canvas, 0.85 ); + DrawImage( TextureManager::getInstance()._stall_texture_id, cam, 0.85 ); } } -void StgModel::DrawImage( uint32_t texture_id, Stg::StgCanvas* canvas, float alpha ) +void StgModel::DrawImage( uint32_t texture_id, Stg::StgCamera* cam, float alpha ) { - float stheta = dtor( canvas->current_camera->pitch() ); - float sphi = - dtor( canvas->current_camera->yaw() ); - if( canvas->pCamOn == true ) { - sphi = atan2( - ( pose.x - canvas->current_camera->x() ) - , - ( pose.y - canvas->current_camera->y() ) - ); - } + float yaw, pitch; + pitch = - cam->pitch(); + yaw = - cam->yaw(); + float robotAngle = -rtod(pose.a); glEnable(GL_TEXTURE_2D); glBindTexture( GL_TEXTURE_2D, texture_id ); @@ -1098,14 +1093,10 @@ glTranslatef( 0.0, 0.0, 0.75 ); - //orient 2d sprites to face the camera (left-right) - float a = rtod( sphi + pose.a ); - glRotatef( -a, 0.0, 0.0, 1.0 ); + // rotate to face screen + glRotatef( robotAngle - yaw, 0,0,1 ); + glRotatef( -pitch - 90, 1,0,0 ); - //orient to face camera (from top-front) - a = rtod( stheta ); - glRotatef( -(90.0-a), 1.0, 0.0, 0.0 ); - //draw a square, with the textured image glBegin(GL_QUADS); glTexCoord2f(0.0f, 0.0f); glVertex3f(-0.25f, 0, -0.25f ); @@ -1222,18 +1213,18 @@ PopCoords(); } -void StgModel::DataVisualize( void ) +void StgModel::DataVisualize( StgCamera* cam ) { // do nothing } -void StgModel::DataVisualizeTree( void ) +void StgModel::DataVisualizeTree( StgCamera* cam ) { PushLocalCoords(); - DataVisualize(); // virtual function overridden by most model types + DataVisualize( cam ); // virtual function overridden by most model types // and draw the children - LISTMETHOD( children, StgModel*, DataVisualizeTree ); + LISTMETHODARG( children, StgModel*, DataVisualizeTree, cam ); PopCoords(); } Modified: code/stage/trunk/libstage/model_blinkenlight.cc =================================================================== --- code/stage/trunk/libstage/model_blinkenlight.cc 2008-07-25 21:53:39 UTC (rev 6928) +++ code/stage/trunk/libstage/model_blinkenlight.cc 2008-07-25 23:17:00 UTC (rev 6929) @@ -106,7 +106,7 @@ } -void StgModelBlinkenlight::DataVisualize() +void StgModelBlinkenlight::DataVisualize( StgCamera* cam ) { if( on && showBlinkenData ) { Modified: code/stage/trunk/libstage/model_blobfinder.cc =================================================================== --- code/stage/trunk/libstage/model_blobfinder.cc 2008-07-25 21:53:39 UTC (rev 6928) +++ code/stage/trunk/libstage/model_blobfinder.cc 2008-07-25 23:17:00 UTC (rev 6929) @@ -295,7 +295,7 @@ StgModel::Shutdown(); } -void StgModelBlobfinder::DataVisualize( void ) +void StgModelBlobfinder::DataVisualize( StgCamera* cam ) { if ( !showBlobData ) return; @@ -330,8 +330,16 @@ glRotatef( rtod(-gpose.a),0,0,1 ); // place the "screen" a little away from the robot - glTranslatef( -3.0, -2.0, 1.0 ); - + glTranslatef( -2.5, -1.5, 0.5 ); + + // rotate to face screen + float yaw, pitch; + pitch = - cam->pitch(); + yaw = - cam->yaw(); + float robotAngle = -rtod(pose.a); + glRotatef( robotAngle - yaw, 0,0,1 ); + glRotatef( -pitch, 1,0,0 ); + // convert blob pixels to meters scale - arbitrary glScalef( 0.025, 0.025, 1 ); Modified: code/stage/trunk/libstage/model_camera.cc =================================================================== --- code/stage/trunk/libstage/model_camera.cc 2008-07-25 21:53:39 UTC (rev 6928) +++ code/stage/trunk/libstage/model_camera.cc 2008-07-25 23:17:00 UTC (rev 6929) @@ -235,7 +235,7 @@ } //TODO create lines outlining camera frustrum, then iterate over each depth measurement and create a square -void StgModelCamera::DataVisualize( void ) +void StgModelCamera::DataVisualize( StgCamera* cam ) { if( _frame_data == NULL || !showCameraData ) Modified: code/stage/trunk/libstage/model_fiducial.cc =================================================================== --- code/stage/trunk/libstage/model_fiducial.cc 2008-07-25 21:53:39 UTC (rev 6928) +++ code/stage/trunk/libstage/model_fiducial.cc 2008-07-25 23:17:00 UTC (rev 6929) @@ -248,7 +248,7 @@ } -void StgModelFiducial::DataVisualize() +void StgModelFiducial::DataVisualize( StgCamera* cam ) { if ( !showFiducialData ) return; Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2008-07-25 21:53:39 UTC (rev 6928) +++ code/stage/trunk/libstage/model_laser.cc 2008-07-25 23:17:00 UTC (rev 6929) @@ -301,7 +301,7 @@ } -void StgModelLaser::DataVisualize( void ) +void StgModelLaser::DataVisualize( StgCamera* cam ) { if( ! (samples && sample_count) ) return; Modified: code/stage/trunk/libstage/model_ranger.cc =================================================================== --- code/stage/trunk/libstage/model_ranger.cc 2008-07-25 21:53:39 UTC (rev 6928) +++ code/stage/trunk/libstage/model_ranger.cc 2008-07-25 23:17:00 UTC (rev 6929) @@ -321,7 +321,7 @@ puts( " ]" ); } -void StgModelRanger::DataVisualize( void ) +void StgModelRanger::DataVisualize( StgCamera* cam ) { if( ! (samples && sensors && sensor_count) ) return; Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-07-25 21:53:39 UTC (rev 6928) +++ code/stage/trunk/libstage/stage.hh 2008-07-25 23:17:00 UTC (rev 6929) @@ -79,6 +79,7 @@ class StgModel; class FileManager; class OptionsDlg; + class StgCamera; /** Initialize the Stage library */ void Init( int* argc, char** argv[] ); @@ -1296,8 +1297,8 @@ void DrawBlocksTree(); virtual void DrawBlocks(); - virtual void DrawStatus( StgCanvas* canvas ); - void DrawStatusTree( StgCanvas* canvas ); + virtual void DrawStatus( StgCamera* cam ); + void DrawStatusTree( StgCamera* cam ); void DrawOriginTree(); void DrawOrigin(); @@ -1306,7 +1307,7 @@ void PopCoords(); ///Draw the image stored in texture_id above the model - void DrawImage( uint32_t texture_id, Stg::StgCanvas* canvas, float alpha ); + void DrawImage( uint32_t texture_id, Stg::StgCamera* cam, float alpha ); // static wrapper for DrawBlocks() @@ -1315,7 +1316,7 @@ void* arg ); virtual void DrawPicker(); - virtual void DataVisualize(); + virtual void DataVisualize( StgCamera* cam ); virtual void DrawSelected(void); @@ -1348,7 +1349,7 @@ stg_model_type_t type; static const char* typestr; - void DataVisualizeTree(); + void DataVisualizeTree( StgCamera* cam ); virtual void PushColor( stg_color_t col ) { world->PushColor( col ); } @@ -2071,7 +2072,7 @@ static Option showBlobData; - virtual void DataVisualize(); + virtual void DataVisualize( StgCamera* cam ); public: unsigned int scan_width; @@ -2203,7 +2204,7 @@ virtual void Update(); virtual void Load(); virtual void Print( char* prefix ); - virtual void DataVisualize(); + virtual void DataVisualize( StgCamera* cam ); uint32_t GetSampleCount(){ return sample_count; } @@ -2352,10 +2353,10 @@ // static wrapper function can be used as a function pointer static int AddModelIfVisibleStatic( StgModel* him, StgModelFiducial* me ) - { if( him != me ) me->AddModelIfVisible( him ); }; + { if( him != me ) me->AddModelIfVisible( him ); return 0; } virtual void Update(); - virtual void DataVisualize(); + virtual void DataVisualize( StgCamera* cam ); GArray* data; @@ -2403,7 +2404,7 @@ virtual void Startup(); virtual void Shutdown(); virtual void Update(); - virtual void DataVisualize(); + virtual void DataVisualize( StgCamera* cam ); public: static const char* typestr; @@ -2446,7 +2447,7 @@ virtual void Load(); virtual void Update(); - virtual void DataVisualize(); + virtual void DataVisualize( StgCamera* cam ); }; // CAMERA MODEL ---------------------------------------------------- @@ -2499,7 +2500,7 @@ //virtual void Draw( uint32_t flags, StgCanvas* canvas ); ///Draw camera visualization - virtual void DataVisualize(); + virtual void DataVisualize( StgCamera* cam ); ///width of captured image inline int getWidth( void ) const { return _width; } Modified: code/stage/trunk/libstageplugin/p_driver.cc =================================================================== --- code/stage/trunk/libstageplugin/p_driver.cc 2008-07-25 21:53:39 UTC (rev 6928) +++ code/stage/trunk/libstageplugin/p_driver.cc 2008-07-25 23:17:00 UTC (rev 6929) @@ -333,8 +333,21 @@ case PLAYER_SPEECH_CODE: ifsrc = new InterfaceSpeech( player_addr, this, cf, section ); - break; + break; + // case PLAYER_CAMERA_CODE: + // ifsrc = new InterfaceCamera( player_addr, this, cf, section ); + // break; + + // case PLAYER_GRAPHICS2D_CODE: + // ifsrc = new InterfaceGraphics2d( player_addr, this, cf, section ); + // break; + + // case PLAYER_GRAPHICS3D_CODE: + // ifsrc = new InterfaceGraphics3d( player_addr, this, cf, section ); + // break; + + // case PLAYER_LOCALIZE_CODE: // ifsrc = new InterfaceLocalize( player_addr, this, cf, section ); @@ -343,15 +356,7 @@ // case PLAYER_MAP_CODE: // ifsrc = new InterfaceMap( player_addr, this, cf, section ); // break; - -// case PLAYER_GRAPHICS2D_CODE: -// ifsrc = new InterfaceGraphics2d( player_addr, this, cf, section ); -// break; - -// case PLAYER_GRAPHICS3D_CODE: - // ifsrc = new InterfaceGraphics3d( player_addr, this, cf, section ); - //break; - + // case PLAYER_GRIPPER_CODE: // ifsrc = new InterfaceGripper( player_addr, this, cf, section ); // break; @@ -359,10 +364,6 @@ // case PLAYER_WIFI_CODE: // ifsrc = new InterfaceWifi( player_addr, this, cf, section ); // break; - -// case PLAYER_CAMERA_CODE: -// ifsrc = new InterfaceCamera( player_addr, this, cf, section ); -// break; // case PLAYER_POWER_CODE: // ifsrc = new InterfacePower( player_addr, this, cf, section ); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <jer...@us...> - 2008-08-01 14:03:06
|
Revision: 6942 http://playerstage.svn.sourceforge.net/playerstage/?rev=6942&view=rev Author: jeremy_asher Date: 2008-08-01 21:03:15 +0000 (Fri, 01 Aug 2008) Log Message: ----------- stage: fixed speech bubble and clock string background sizing issues Modified Paths: -------------- code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model.cc Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-08-01 19:48:20 UTC (rev 6941) +++ code/stage/trunk/libstage/canvas.cc 2008-08-01 21:03:15 UTC (rev 6942) @@ -739,17 +739,22 @@ std::string clockstr = world->ClockString(); if( showFollow == true && last_selection ) clockstr.append( " [ FOLLOW MODE ]" ); - - int margin = 3; - float width = gl_width( clockstr.c_str() ) + 2 * margin; - float height = gl_height() + 2 * margin; + fl_font( FL_HELVETICA, 12 ); + float txtWidth = gl_width( clockstr.c_str() ); + int txtHeight = gl_height(); + + int width, height; + width = int( txtWidth / 10 ) * 10; + height = ( txtHeight / 5 + 1 ) * 5; + float margin = ( height - txtHeight ) * 0.75; + colorstack.Push( 0.8,0.8,1.0 ); // pale blue glRectf( 0, 0, width, height ); colorstack.Pop(); colorstack.Push( 0,0,0 ); // black - gl_draw_string( margin, margin, 5, clockstr.c_str() ); + gl_draw_string( margin, margin, 0, clockstr.c_str() ); colorstack.Pop(); glEnable( GL_DEPTH_TEST ); Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-08-01 19:48:20 UTC (rev 6941) +++ code/stage/trunk/libstage/model.cc 2008-08-01 21:03:15 UTC (rev 6942) @@ -1020,6 +1020,7 @@ float robotAngle = -rtod(pose.a); glPushMatrix(); + fl_font( FL_HELVETICA, 12 ); float w = gl_width( this->say_string ); // scaled text width float h = gl_height(); // scaled text height @@ -1079,7 +1080,7 @@ PushColor( BUBBLE_TEXT ); // draw text inside the bubble - gl_draw_string( 2*m, 2*m, 0, this->say_string ); + gl_draw_string( 2.5*m, 2.5*m, 0, this->say_string ); PopColor(); glPopMatrix(); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <al...@us...> - 2008-10-06 20:36:13
|
Revision: 7067 http://playerstage.svn.sourceforge.net/playerstage/?rev=7067&view=rev Author: alexcb Date: 2008-10-06 20:33:32 +0000 (Mon, 06 Oct 2008) Log Message: ----------- fixed stall image to draw on top of all (really tall) robots Modified Paths: -------------- code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/stage.hh Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-10-06 01:32:59 UTC (rev 7066) +++ code/stage/trunk/libstage/model.cc 2008-10-06 20:33:32 UTC (rev 7067) @@ -1094,6 +1094,21 @@ } } +stg_meters_t StgModel::ModelHeight() +{ + stg_meters_t m_child = 0; //max size of any child + for( GList* it=this->children; it; it=it->next ) + { + StgModel* child = (StgModel*)it->data; + stg_meters_t tmp_h = child->ModelHeight(); + if( tmp_h > m_child ) + m_child = tmp_h; + } + + //height of model + max( child height ) + return geom.size.z + m_child; +} + void StgModel::DrawImage( uint32_t texture_id, Stg::StgCamera* cam, float alpha ) { float yaw, pitch; @@ -1107,7 +1122,8 @@ glColor4f( 1.0, 1.0, 1.0, alpha ); glPushMatrix(); - glTranslatef( 0.0, 0.0, 0.75 ); + //position image above the robot + glTranslatef( 0.0, 0.0, ModelHeight() + 0.3 ); // rotate to face screen glRotatef( robotAngle - yaw, 0,0,1 ); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-10-06 01:32:59 UTC (rev 7066) +++ code/stage/trunk/libstage/stage.hh 2008-10-06 20:33:32 UTC (rev 7067) @@ -1310,6 +1310,8 @@ ///Draw the image stored in texture_id above the model void DrawImage( uint32_t texture_id, Stg::StgCamera* cam, float alpha ); + ///Returns the max height of the model + stg_meters_t ModelHeight(); // static wrapper for DrawBlocks() static void DrawBlocks( gpointer dummykey, This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2008-11-15 02:46:32
|
Revision: 7146 http://playerstage.svn.sourceforge.net/playerstage/?rev=7146&view=rev Author: rtv Date: 2008-11-15 02:46:31 +0000 (Sat, 15 Nov 2008) Log Message: ----------- major changes for 3.1 Modified Paths: -------------- code/stage/trunk/libstage/region.hh code/stage/trunk/libstage/stage_internal.hh Property Changed: ---------------- code/stage/trunk/libstage/file_manager.hh code/stage/trunk/libstage/option.hh code/stage/trunk/libstage/options_dlg.hh code/stage/trunk/libstage/region.hh code/stage/trunk/libstage/stage_internal.hh code/stage/trunk/libstage/texture_manager.hh code/stage/trunk/libstage/worldfile.hh Property changes on: code/stage/trunk/libstage/file_manager.hh ___________________________________________________________________ Added: svn:eol + native Property changes on: code/stage/trunk/libstage/option.hh ___________________________________________________________________ Added: svn:eol + native Property changes on: code/stage/trunk/libstage/options_dlg.hh ___________________________________________________________________ Added: svn:eol + native Modified: code/stage/trunk/libstage/region.hh =================================================================== --- code/stage/trunk/libstage/region.hh 2008-11-15 02:46:11 UTC (rev 7145) +++ code/stage/trunk/libstage/region.hh 2008-11-15 02:46:31 UTC (rev 7146) @@ -8,11 +8,6 @@ const uint32_t SBITS = 5; // superregions contain (2^SBITS)^2 regions const uint32_t SRBITS = RBITS+SBITS; -typedef struct -{ - GSList* list; -} stg_cell_t; - class Stg::Region { friend class SuperRegion; @@ -21,17 +16,24 @@ static const uint32_t REGIONWIDTH = 1<<RBITS; static const uint32_t REGIONSIZE = REGIONWIDTH*REGIONWIDTH; - stg_cell_t cells[REGIONSIZE]; - + //stg_cell_t cells[REGIONSIZE]; + stg_cell_t* cells; + public: uint32_t count; // number of blocks rendered into these cells - Region() + Region() + : cells( new stg_cell_t[REGIONSIZE] ), + count(0) + { + /* do nothing */ + } + + ~Region() { - bzero( cells, REGIONSIZE * sizeof(stg_cell_t)); - count = 0; + delete[] cells; } - + stg_cell_t* GetCell( int32_t x, int32_t y ) { uint32_t index = x + (y*REGIONWIDTH); @@ -39,17 +41,50 @@ assert( index >=0 ); assert( index < REGIONSIZE ); #endif + + printf("GET CELL [%d %d] index %d gives %p\n", + x, y, index, &cells[index] ); + return &cells[index]; } // add a block to a region cell specified in REGION coordinates - void AddBlock( StgBlock* block, int32_t x, int32_t y, unsigned int* count2 ) + void AddBlock( StgModel* mod, + stg_color_t col, + stg_bounds_t zbounds, + int32_t x, int32_t y, + unsigned int* count2 ) { + assert( mod ); + stg_cell_t* cell = GetCell( x, y ); - cell->list = g_slist_prepend( cell->list, block ); - block->RecordRenderPoint( &cell->list, cell->list, &this->count, count2 ); - count++; + assert( cell ); + + // put a new empty pointer on the front of the list to use below + cell->list = g_slist_prepend( cell->list, NULL ); + + // this is the data to be stored in this cell + stg_list_entry_t el; + el.cell = cell; + el.link = cell->list; + el.counter1 = &count; + el.counter2 = count2; + el.mod = mod; + el.color = col; + el.zbounds = zbounds; + + // copy this struct onto the end of the model's array + g_array_append_val( mod->rendered_points, el ); + + // stash this record at the head of the list + cell->list->data = + & g_array_index( mod->rendered_points, + stg_list_entry_t, + mod->rendered_points->len-1 ); + + count++; } + }; @@ -89,16 +124,19 @@ } // add a block to a cell specified in superregion coordinates - void AddBlock( StgBlock* block, int32_t x, int32_t y ) + void AddBlock( StgModel* mod, + stg_color_t col, + stg_bounds_t zbounds, + int32_t x, int32_t y ) { GetRegion( x>>RBITS, y>>RBITS ) - ->AddBlock( block, - x - ((x>>RBITS)<<RBITS), - y - ((y>>RBITS)<<RBITS), + ->AddBlock( mod, col, zbounds, + x - ((x>>RBITS)<<RBITS), + y - ((y>>RBITS)<<RBITS), &count); count++; } - + // callback wrapper for Draw() static void Draw_cb( gpointer dummykey, SuperRegion* sr, Property changes on: code/stage/trunk/libstage/region.hh ___________________________________________________________________ Added: svn:eol + native Modified: code/stage/trunk/libstage/stage_internal.hh =================================================================== --- code/stage/trunk/libstage/stage_internal.hh 2008-11-15 02:46:11 UTC (rev 7145) +++ code/stage/trunk/libstage/stage_internal.hh 2008-11-15 02:46:31 UTC (rev 7146) @@ -11,10 +11,223 @@ // external definitions for libstage users #include "stage.hh" -// internal-only definitions and includes go here +// internal-only definitions and includes go below + #include "worldfile.hh" + // implementation files use our namespace by default using namespace Stg; -#endif + +namespace Stg +{ + + +// COLOR STACK CLASS +class GlColorStack +{ + public: + GlColorStack(); + ~GlColorStack(); + + void Push( GLdouble col[4] ); + void Push( double r, double g, double b, double a ); + void Push( double r, double g, double b ); + void Push( stg_color_t col ); + + void Pop(); + + unsigned int Length() + { return g_queue_get_length( colorstack ); } + + private: + GQueue* colorstack; +}; + + +class StgCanvas : public Fl_Gl_Window +{ + friend class StgWorldGui; // allow access to private members + friend class StgModel; + +private: + GlColorStack colorstack; + + Camera* current_camera; + OrthoCamera camera; + PerspectiveCamera perspective_camera; + bool dirty_buffer; + Worldfile* wf; + + int startx, starty; + bool selectedModel; + bool clicked_empty_space; + int empty_space_startx, empty_space_starty; + GList* selected_models; ///< a list of models that are currently + ///selected by the user + StgModel* last_selection; ///< the most recently selected model + ///(even if it is now unselected). + + stg_msec_t interval; // window refresh interval in ms + + GList* ray_list; + void RecordRay( double x1, double y1, double x2, double y2 ); + void DrawRays(); + void ClearRays(); + void DrawGlobalGrid(); + + Option + showBlinken, + showBlocks, + showClock, + showData, + showFlags, + showFollow, + showFootprints, + showGrid, + showOccupancy, + showScreenshots, + showStatus, + showTrailArrows, + showTrailRise, + showTrails, + showTree, + pCamOn, + visualizeAll; + +public: + StgCanvas( StgWorldGui* world, int x, int y, int W,int H); + ~StgCanvas(); + + bool graphics; + StgWorldGui* world; + + void Screenshot(); + + void createMenuItems( Fl_Menu_Bar* menu, std::string path ); + + void FixViewport(int W,int H); + void DrawFloor(); //simpler floor compared to grid + void DrawBlocks(); + void resetCamera(); + virtual void renderFrame(); + virtual void draw(); + virtual int handle( int event ); + void resize(int X,int Y,int W,int H); + + void CanvasToWorld( int px, int py, + double *wx, double *wy, double* wz ); + + StgModel* getModel( int x, int y ); + bool selected( StgModel* mod ); + void select( StgModel* mod ); + void unSelect( StgModel* mod ); + void unSelectAll(); + + inline void setDirtyBuffer( void ) { dirty_buffer = true; } + inline bool dirtyBuffer( void ) const { return dirty_buffer; } + + inline void PushColor( stg_color_t col ) + { colorstack.Push( col ); } + + void PushColor( double r, double g, double b, double a ) + { colorstack.Push( r,g,b,a ); } + + void PopColor(){ colorstack.Pop(); } + + void InvertView( uint32_t invertflags ); + + static void TimerCallback( StgCanvas* canvas ); + static void perspectiveCb( Fl_Widget* w, void* p ); + + void Load( Worldfile* wf, int section ); + void Save( Worldfile* wf, int section ); +}; + + +class Cell +{ + friend class Region; + friend class SuperRegion; + friend class StgWorld; + friend class StgBlock; + +private: + Region* region; + GSList* list; +public: + Cell() + : region( NULL), + list(NULL) + { /* do nothing */ } + + void RemoveBlock( StgBlock* b ); + void AddBlock( StgBlock* b ); + void AddBlockNoRecord( StgBlock* b ); +}; + +// a bit of experimenting suggests that these values are fast. YMMV. +#define RBITS 4 // regions contain (2^RBITS)^2 pixels +#define SBITS 4 // superregions contain (2^SBITS)^2 regions +#define SRBITS (RBITS+SBITS) + +#define REGIONWIDTH (1<<RBITS) +#define REGIONSIZE REGIONWIDTH*REGIONWIDTH + +#define SUPERREGIONWIDTH (1<<SBITS) +#define SUPERREGIONSIZE SUPERREGIONWIDTH*SUPERREGIONWIDTH + +class Region +{ + friend class SuperRegion; + +private: + static const uint32_t WIDTH; + static const uint32_t SIZE; + + Cell cells[REGIONSIZE]; + SuperRegion* superregion; + +public: + unsigned long count; // number of blocks rendered into these cells + + Region(); + ~Region(); + Cell* GetCell( int32_t x, int32_t y ); + + void DecrementOccupancy(); + void IncrementOccupancy(); +}; + + +class SuperRegion +{ + friend class StgWorld; + friend class StgModel; + +private: + static const uint32_t WIDTH; + static const uint32_t SIZE; + + Region regions[SUPERREGIONSIZE]; + unsigned long count; // number of blocks rendered into these regions + + stg_point_int_t origin; + +public: + + SuperRegion( int32_t x, int32_t y ); + ~SuperRegion(); + + Region* GetRegion( int32_t x, int32_t y ); + + void Draw( bool drawall ); + void Floor(); + void DecrementOccupancy(); + void IncrementOccupancy(); +}; + +}; // namespace Stg + +#endif // STG_INTERNAL_H Property changes on: code/stage/trunk/libstage/stage_internal.hh ___________________________________________________________________ Added: svn:eol + native Property changes on: code/stage/trunk/libstage/texture_manager.hh ___________________________________________________________________ Added: svn:eol + native Property changes on: code/stage/trunk/libstage/worldfile.hh ___________________________________________________________________ Added: svn:eol + native This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2008-11-15 03:01:01
|
Revision: 7148 http://playerstage.svn.sourceforge.net/playerstage/?rev=7148&view=rev Author: rtv Date: 2008-11-15 03:00:57 +0000 (Sat, 15 Nov 2008) Log Message: ----------- major changes for 3.1 Modified Paths: -------------- code/stage/trunk/libstage/model_blinkenlight.cc code/stage/trunk/libstage/model_blobfinder.cc code/stage/trunk/libstage/model_camera.cc code/stage/trunk/libstage/model_fiducial.cc code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/model_ranger.cc Property Changed: ---------------- code/stage/trunk/libstage/model_blinkenlight.cc code/stage/trunk/libstage/model_blobfinder.cc code/stage/trunk/libstage/model_callbacks.cc code/stage/trunk/libstage/model_camera.cc code/stage/trunk/libstage/model_fiducial.cc code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/model_props.cc code/stage/trunk/libstage/model_ranger.cc code/stage/trunk/libstage/model_wifi.cc Modified: code/stage/trunk/libstage/model_blinkenlight.cc =================================================================== --- code/stage/trunk/libstage/model_blinkenlight.cc 2008-11-15 02:48:12 UTC (rev 7147) +++ code/stage/trunk/libstage/model_blinkenlight.cc 2008-11-15 03:00:57 UTC (rev 7148) @@ -106,7 +106,7 @@ } -void StgModelBlinkenlight::DataVisualize( StgCamera* cam ) +void StgModelBlinkenlight::DataVisualize( Camera* cam ) { if( on && showBlinkenData ) { @@ -115,7 +115,8 @@ gl_pose_shift( &this->pose ); gl_pose_shift( &this->geom.pose ); glLineWidth( 3 ); - LISTMETHOD( this->blocks, StgBlock*, Draw ); + // TODO + //LISTMETHOD( this->blocks, StgBlock*, Draw ); glLineWidth( 1 ); glPopMatrix(); // drop out of local coords } Property changes on: code/stage/trunk/libstage/model_blinkenlight.cc ___________________________________________________________________ Added: svn:eol + native Modified: code/stage/trunk/libstage/model_blobfinder.cc =================================================================== --- code/stage/trunk/libstage/model_blobfinder.cc 2008-11-15 02:48:12 UTC (rev 7147) +++ code/stage/trunk/libstage/model_blobfinder.cc 2008-11-15 03:00:57 UTC (rev 7148) @@ -108,11 +108,11 @@ g_array_free( colors, true ); } -static bool blob_match( StgBlock* testblock, - StgModel* finder, - const void* dummy ) +static bool blob_match( StgModel* candidate, + StgModel* finder, + const void* dummy ) { - return( ! finder->IsRelated( testblock->Model() )); + return( ! finder->IsRelated( candidate )); } @@ -178,9 +178,9 @@ StgModel::Update(); // generate a scan for post-processing into a blob image + + stg_raytrace_result_t* samples = new stg_raytrace_result_t[scan_width]; - stg_raytrace_sample_t* samples = new stg_raytrace_sample_t[scan_width]; - Raytrace( pan, range, fov, blob_match, NULL, samples, scan_width, false ); // now the colors and ranges are filled in - time to do blob detection @@ -191,22 +191,22 @@ // scan through the samples looking for color blobs for(unsigned int s=0; s < scan_width; s++ ) { - if( samples[s].block == NULL ) + if( samples[s].mod == NULL ) continue; // we saw nothing //if( samples[s].block->Color() != 0xFFFF0000 ) //continue; // we saw nothing - + unsigned int right = s; - stg_color_t blobcol = samples[s].block->Color(); + stg_color_t blobcol = samples[s].color; //printf( "blob start %d color %X\n", blobleft, blobcol ); // loop until we hit the end of the blob // there has to be a gap of >1 pixel to end a blob // this avoids getting lots of crappy little blobs - while( s < scan_width && samples[s].block && - ColorMatchIgnoreAlpha( samples[s].block->Color(), blobcol) ) + while( s < scan_width && samples[s].mod && + ColorMatchIgnoreAlpha( samples[s].color, blobcol) ) { //printf( "%u blobcol %X block %p %s color %X\n", s, blobcol, samples[s].block, samples[s].block->Model()->Token(), samples[s].block->Color() ); s++; @@ -295,7 +295,7 @@ StgModel::Shutdown(); } -void StgModelBlobfinder::DataVisualize( StgCamera* cam ) +void StgModelBlobfinder::DataVisualize( Camera* cam ) { if ( !showBlobData ) return; Property changes on: code/stage/trunk/libstage/model_blobfinder.cc ___________________________________________________________________ Added: svn:eol + native Property changes on: code/stage/trunk/libstage/model_callbacks.cc ___________________________________________________________________ Added: svn:eol + native Modified: code/stage/trunk/libstage/model_camera.cc =================================================================== --- code/stage/trunk/libstage/model_camera.cc 2008-11-15 02:48:12 UTC (rev 7147) +++ code/stage/trunk/libstage/model_camera.cc 2008-11-15 03:00:57 UTC (rev 7148) @@ -19,7 +19,7 @@ Option StgModelCamera::showCameraData( "Show Camera Data", "show_camera", "", true ); -static const stg_size_t DEFAULT_SIZE = { 0.1, 0.07, 0.05 }; +static const stg_size_t DEFAULT_SIZE( 0.1, 0.07, 0.05 ); static const char DEFAULT_GEOM_COLOR[] = "black"; static const float DEFAULT_HFOV = 70; static const float DEFAULT_VFOV = 40; @@ -235,7 +235,7 @@ } //TODO create lines outlining camera frustrum, then iterate over each depth measurement and create a square -void StgModelCamera::DataVisualize( StgCamera* cam ) +void StgModelCamera::DataVisualize( Camera* cam ) { if( _frame_data == NULL || !showCameraData ) Property changes on: code/stage/trunk/libstage/model_camera.cc ___________________________________________________________________ Added: svn:eol + native Modified: code/stage/trunk/libstage/model_fiducial.cc =================================================================== --- code/stage/trunk/libstage/model_fiducial.cc 2008-11-15 02:48:12 UTC (rev 7147) +++ code/stage/trunk/libstage/model_fiducial.cc 2008-11-15 03:00:57 UTC (rev 7148) @@ -71,6 +71,9 @@ PRINT_DEBUG2( "Constructing StgModelFiducial %d (%s)\n", id, typestr ); + // assert that Update() is reentrant for this derived model + thread_safe = true; + // sensible fiducial defaults // interval = 200; // common for a SICK LMS200 @@ -100,9 +103,11 @@ g_array_free( data, true ); } -static bool fiducial_raytrace_match( StgBlock* testblock, StgModel* finder, const void* dummy ) +static bool fiducial_raytrace_match( StgModel* candidate, + StgModel* finder, + const void* dummy ) { - return( ! finder->IsRelated( testblock->Model() ) ); + return( ! finder->IsRelated( candidate ) ); } @@ -160,18 +165,15 @@ //printf( "bearing %.2f\n", RTOD(bearing) ); - - stg_raytrace_sample_t ray; - - Raytrace( dtheta, - max_range_anon, - fiducial_raytrace_match, - NULL, - &ray, - false ); - + + stg_raytrace_result_t ray = Raytrace( dtheta, + max_range_anon, + fiducial_raytrace_match, + NULL, + false ); + range = ray.range; - StgModel* hitmod = ray.block->Model(); + StgModel* hitmod = ray.mod; // printf( "ray hit %s and was seeking LOS to %s\n", //hitmod ? hitmod->Token() : "null", @@ -248,7 +250,7 @@ } -void StgModelFiducial::DataVisualize( StgCamera* cam ) +void StgModelFiducial::DataVisualize( Camera* cam ) { if ( !showFiducialData ) return; Property changes on: code/stage/trunk/libstage/model_fiducial.cc ___________________________________________________________________ Added: svn:eol + native Modified: code/stage/trunk/libstage/model_load.cc =================================================================== --- code/stage/trunk/libstage/model_load.cc 2008-11-15 02:48:12 UTC (rev 7147) +++ code/stage/trunk/libstage/model_load.cc 2008-11-15 03:00:57 UTC (rev 7148) @@ -39,9 +39,9 @@ else PRINT_ERR1( "Name blank for model %s. Check your worldfile\n", this->token ); } - + //PRINT_WARN1( "%s::Load", token ); - + if( wf->PropertyExists( wf_entity, "origin" ) ) { stg_geom_t geom = GetGeom(); @@ -79,17 +79,8 @@ vel.z = wf->ReadTupleLength(wf_entity, "velocity", 2, vel.z ); vel.a = wf->ReadTupleAngle(wf_entity, "velocity", 3, vel.a ); this->SetVelocity( vel ); - - if( vel.x || vel.y || vel.z || vel.a ) - world->StartUpdatingModel( this ); } - - if( wf->PropertyExists( wf_entity, "boundary" )) - { - this->SetBoundary( wf->ReadInt(wf_entity, "boundary", this->boundary )); - } - if( wf->PropertyExists( wf_entity, "color" )) { stg_color_t col = 0xFFFF0000; // red; @@ -118,148 +109,51 @@ this->SetColor( stg_color_pack( red, green, blue, alpha )); } - + if( wf->PropertyExists( wf_entity, "bitmap" ) ) { const char* bitmapfile = wf->ReadString( wf_entity, "bitmap", NULL ); assert( bitmapfile ); - char full[_POSIX_PATH_MAX]; + if( has_default_block ) + { + blockgroup.Clear(); + has_default_block = false; + } - if( bitmapfile[0] == '/' ) - strcpy( full, bitmapfile ); - else - { - 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 ); - - stg_rotrect_t* rects = NULL; - 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; - } - - this->UnMap(); - this->ClearBlocks(); - - //printf( "found %d rects\n", rect_count ); - - if( rects && (rect_count > 0) ) - { - //puts( "loading rects" ); - for( unsigned int r=0; r<rect_count; r++ ) - this->AddBlockRect( rects[r].pose.x, rects[r].pose.y, - rects[r].size.x, rects[r].size.y ); - - if( this->boundary ) - { - // add thin bounding blocks - double epsilon = 0.01; - this->AddBlockRect(0,0, epsilon, height ); - this->AddBlockRect(0,0, width, epsilon ); - this->AddBlockRect(0, height-epsilon, width, epsilon ); - this->AddBlockRect(width-epsilon,0, epsilon, height ); - } - - StgBlock::ScaleList( this->blocks, &this->geom.size ); - this->Map(); - this->NeedRedraw(); - - g_free( rects ); - } - - //printf( "model %s block count %d\n", - // token, g_list_length( blocks )); + //puts( "clearing blockgroup" ); + //blockgroup.Clear(); + //puts( "loading bitmap" ); + blockgroup.LoadBitmap( this, bitmapfile, wf ); } - - if( wf->PropertyExists( wf_entity, "blocks" ) ) + + if( wf->PropertyExists( wf_entity, "boundary" )) { - int blockcount = wf->ReadInt( wf_entity, "blocks", 0 ); + this->SetBoundary( wf->ReadInt(wf_entity, "boundary", this->boundary )); - this->UnMap(); - this->ClearBlocks(); + if( boundary ) + { + //PRINT_WARN1( "setting boundary for %s\n", token ); + + blockgroup.CalcSize(); - //printf( "expecting %d blocks\n", blockcount ); + double epsilon = 0.01; + double width = blockgroup.size.x; + double height = blockgroup.size.y; + double dx = width/2.0; + double dy = height/2.0; + double dz = blockgroup.size.z; - char key[256]; - for( int l=0; l<blockcount; l++ ) - { - snprintf(key, sizeof(key), "block[%d].points", l); - int pointcount = wf->ReadInt(wf_entity,key,0); + // add thin bounding blocks + AddBlockRect(-dx,-dy, epsilon, height, dz ); + AddBlockRect(-dx,-dy, width, epsilon, dz ); + AddBlockRect(-dx, -dy+height-epsilon, width, epsilon, dz ); + AddBlockRect(-dx+width-epsilon,-dy, epsilon, height, dz ); + } + } - //printf( "expecting %d points in block %d\n", - //pointcount, l ); - - stg_point_t* pts = stg_points_create( pointcount ); - - int p; - for( p=0; p<pointcount; p++ ) { - snprintf(key, sizeof(key), "block[%d].point[%d]", l, p ); - - pts[p].x = wf->ReadTupleLength(wf_entity, key, 0, 0); - pts[p].y = wf->ReadTupleLength(wf_entity, key, 1, 0); - - //printf( "key %s x: %.2f y: %.2f\n", - // key, pt.x, pt.y ); - } - - // block Z axis - snprintf(key, sizeof(key), "block[%d].z", l); - - stg_meters_t zmin = - wf->ReadTupleLength(wf_entity, key, 0, 0.0 ); - - stg_meters_t zmax = - wf->ReadTupleLength(wf_entity, key, 1, 1.0 ); - - // block color - stg_color_t blockcol = this->color; - bool inherit_color = true; - - snprintf(key, sizeof(key), "block[%d].color", l); - - const char* colorstr = wf->ReadString( wf_entity, key, NULL ); - if( colorstr ) - { - blockcol = stg_lookup_color( colorstr ); - inherit_color = false; - } - - this->AddBlock( pts, pointcount, zmin, zmax, blockcol, inherit_color ); - - stg_points_destroy( pts ); - } - - StgBlock::ScaleList( this->blocks, &this->geom.size ); - - if( this->boundary ) - { - // add thin bounding blocks - double epsilon = 0.001; - double width = geom.size.x; - double height = geom.size.y; - this->AddBlockRect(-width/2.0, -height/2.0, epsilon, height ); - this->AddBlockRect(-width/2.0, -height/2.0, width, epsilon ); - this->AddBlockRect(-width/2.0, height/2.0-epsilon, width, epsilon ); - this->AddBlockRect(width/2.0-epsilon, -height/2.0, epsilon, height ); - } - - this->Map(); - } - - if( wf->PropertyExists( wf_entity, "mass" )) + + if( wf->PropertyExists( wf_entity, "mass" )) this->SetMass( wf->ReadFloat(wf_entity, "mass", this->mass )); if( wf->PropertyExists( wf_entity, "fiducial_return" )) @@ -301,13 +195,13 @@ if( wf->PropertyExists( wf_entity, "ctrl" )) { char* lib = (char*)wf->ReadString(wf_entity, "ctrl", NULL ); - + if( !lib ) - puts( "Error - NULL library name" ); + puts( "Error - NULL library name" ); else - LoadControllerModule( lib ); + LoadControllerModule( lib ); } - + if( wf->PropertyExists( wf_entity, "say" )) this->Say( wf->ReadString(wf_entity, "say", NULL )); @@ -318,8 +212,10 @@ if( wf->PropertyExists( wf_entity, "alwayson" )) { if( wf->ReadInt( wf_entity, "alwayson", 0) > 0 ) - Startup(); + Startup(); } + + MapWithChildren(); if( this->debug ) printf( "Model \"%s\" is in debug mode\n", token ); @@ -406,6 +302,9 @@ } fflush(stdout); + + // as we now have a controller, the world needs to call our update function + StartUpdating(); } Property changes on: code/stage/trunk/libstage/model_load.cc ___________________________________________________________________ Added: svn:eol + native Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2008-11-15 02:48:12 UTC (rev 7147) +++ code/stage/trunk/libstage/model_position.cc 2008-11-15 03:00:57 UTC (rev 7148) @@ -89,6 +89,9 @@ PRINT_DEBUG2( "Constructing StgModelPosition %d (%s)\n", id, typestr ); + // assert that Update() is reentrant for this derived model + thread_safe = true; + // no power consumed until we're subscribed this->SetWatts( 0 ); @@ -434,6 +437,7 @@ PRINT_DEBUG3( " READING POSITION: [ %.4f %.4f %.4f ]\n", est_pose.x, est_pose.y, est_pose.a ); + StgModel::Update(); } Property changes on: code/stage/trunk/libstage/model_position.cc ___________________________________________________________________ Added: svn:eol + native Property changes on: code/stage/trunk/libstage/model_props.cc ___________________________________________________________________ Added: svn:eol + native Modified: code/stage/trunk/libstage/model_ranger.cc =================================================================== --- code/stage/trunk/libstage/model_ranger.cc 2008-11-15 02:48:12 UTC (rev 7147) +++ code/stage/trunk/libstage/model_ranger.cc 2008-11-15 03:00:57 UTC (rev 7148) @@ -108,6 +108,9 @@ // Set up sensible defaults + // assert that Update() is reentrant for this derived model + thread_safe = true; + stg_color_t col = stg_lookup_color( RANGER_CONFIG_COLOR ); this->SetColor( col ); @@ -255,14 +258,15 @@ } } -static bool ranger_match( StgBlock* block, StgModel* finder, const void* dummy ) +static bool ranger_match( StgModel* candidate, + StgModel* finder, const void* dummy ) { //printf( "ranger match sees %s %p %d \n", // block->Model()->Token(), block->Model(), block->Model()->LaserReturn() ); // Ignore myself, my children, and my ancestors. - return( block->Model()->GetRangerReturn() && - !block->Model()->IsRelated( finder ) ); + return( candidate->GetRangerReturn() && + !candidate->IsRelated( finder ) ); } void StgModelRanger::Update( void ) @@ -285,13 +289,11 @@ // TODO - reinstate multi-ray rangers //for( int r=0; r<sensors[t].ray_count; r++ ) //{ - stg_raytrace_sample_t ray; - Raytrace( sensors[t].pose, - sensors[t].bounds_range.max, - ranger_match, - NULL, - &ray ); - + stg_raytrace_result_t ray = Raytrace( sensors[t].pose, + sensors[t].bounds_range.max, + ranger_match, + NULL ); + samples[t] = MAX( ray.range, sensors[t].bounds_range.min ); //sensors[t].error = TODO; } @@ -321,7 +323,7 @@ puts( " ]" ); } -void StgModelRanger::DataVisualize( StgCamera* cam ) +void StgModelRanger::DataVisualize( Camera* cam ) { if( ! (samples && sensors && sensor_count) ) return; @@ -381,16 +383,14 @@ } } - // draw the filled triangles in transparent blue + // draw the filled triangles in transparent pale green glDepthMask( GL_FALSE ); - PushColor( 0, 1, 0, 0.1 ); // transparent pale green - //glEnableClientState( GL_VERTEX_ARRAY ); + PushColor( 0, 1, 0, 0.1 ); glVertexPointer( 3, GL_FLOAT, 0, pts ); glDrawArrays( GL_TRIANGLES, 0, 3 * sensor_count ); // restore state glDepthMask( GL_TRUE ); - //glDisableClientState( GL_VERTEX_ARRAY ); PopColor(); } Property changes on: code/stage/trunk/libstage/model_ranger.cc ___________________________________________________________________ Added: svn:eol + native Property changes on: code/stage/trunk/libstage/model_wifi.cc ___________________________________________________________________ Added: svn:eol + native This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2008-11-15 03:11:26
|
Revision: 7151 http://playerstage.svn.sourceforge.net/playerstage/?rev=7151&view=rev Author: rtv Date: 2008-11-15 03:11:20 +0000 (Sat, 15 Nov 2008) Log Message: ----------- major changes for 3.1 Modified Paths: -------------- code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/camera.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/main.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/stage.cc code/stage/trunk/libstage/test.cc code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldgui.cc Added Paths: ----------- code/stage/trunk/libstage/blockgroup.cc Property Changed: ---------------- code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/ancestor.cc code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/blockgrid.cc code/stage/trunk/libstage/camera.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/draw.cc code/stage/trunk/libstage/file_manager.cc code/stage/trunk/libstage/file_manager.hh code/stage/trunk/libstage/gl.cc code/stage/trunk/libstage/glcolorstack.cc code/stage/trunk/libstage/glutgraphics.cc code/stage/trunk/libstage/logo.cc code/stage/trunk/libstage/main.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_blinkenlight.cc code/stage/trunk/libstage/model_blobfinder.cc code/stage/trunk/libstage/model_callbacks.cc code/stage/trunk/libstage/model_camera.cc code/stage/trunk/libstage/model_fiducial.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/model_props.cc code/stage/trunk/libstage/model_ranger.cc code/stage/trunk/libstage/model_wifi.cc code/stage/trunk/libstage/option.cc code/stage/trunk/libstage/option.hh code/stage/trunk/libstage/options_dlg.cc code/stage/trunk/libstage/options_dlg.hh code/stage/trunk/libstage/region.hh code/stage/trunk/libstage/resource.cc code/stage/trunk/libstage/rgb.txt code/stage/trunk/libstage/stage.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/stage_internal.hh code/stage/trunk/libstage/stagecpp.cc code/stage/trunk/libstage/test.cc code/stage/trunk/libstage/texture_manager.cc code/stage/trunk/libstage/texture_manager.hh code/stage/trunk/libstage/typetable.cc code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldfile.cc code/stage/trunk/libstage/worldfile.hh code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2008-11-15 03:10:01 UTC (rev 7150) +++ code/stage/trunk/libstage/CMakeLists.txt 2008-11-15 03:11:20 UTC (rev 7151) @@ -2,6 +2,7 @@ set( stageSrcs ancestor.cc block.cc + blockgroup.cc camera.cc canvas.cc file_manager.cc @@ -23,6 +24,7 @@ option.hh options_dlg.cc options_dlg.hh + region.cc resource.cc stage.cc stage.hh Property changes on: code/stage/trunk/libstage/CMakeLists.txt ___________________________________________________________________ Added: svn:eol + native Added: svn:eol-style + native Property changes on: code/stage/trunk/libstage/ancestor.cc ___________________________________________________________________ Added: svn:eol + native Added: svn:eol-style + native Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2008-11-15 03:10:01 UTC (rev 7150) +++ code/stage/trunk/libstage/block.cc 2008-11-15 03:11:20 UTC (rev 7151) @@ -1,281 +1,328 @@ -//#define DEBUG 1 + #include "stage_internal.hh" -typedef struct +//GPtrArray* StgBlock::global_verts = g_ptr_array_sized_new( 1024 ); + +/** Create a new block. A model's body is a list of these + blocks. The point data is copied, so pts can safely be freed + after calling this.*/ +StgBlock::StgBlock( StgModel* mod, + stg_point_t* pts, + size_t pt_count, + stg_meters_t zmin, + stg_meters_t zmax, + stg_color_t color, + bool inherit_color + ) : + mod( mod ), + pt_count( pt_count ), + 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) ) + // _gpts( NULL ) { - GSList** head; - GSList* link; - unsigned int* counter1; - unsigned int* counter2; -} stg_list_entry_t; + assert( mod ); + assert( pt_count > 0 ); + assert( pts ); + local_z.min = zmin; + local_z.max = zmax; + + // add this block's global coords array to a global list + //g_ptr_array_add( global_verts, this ); +} -/** Create a new block. A model's body is a list of these - blocks. The point data is copied, so pts can safely be freed - after calling this.*/ -StgBlock::StgBlock( StgModel* mod, - stg_point_t* pts, - size_t pt_count, - stg_meters_t zmin, - stg_meters_t zmax, - stg_color_t color, - bool inherit_color ) -{ - this->mod = mod; - this->pt_count = pt_count; - this->pts = (stg_point_t*)g_memdup( pts, pt_count * sizeof(stg_point_t)); - // allocate space for the integer version of the block vertices - this->pts_global_pixels = new stg_point_int_t[pt_count]; +/** A from-file constructor */ +StgBlock::StgBlock( StgModel* mod, + Worldfile* wf, + int entity) + : mod( mod ), + pts(NULL), + pt_count(0), + color(0), + inherit_color(true), + rendered_cells( g_ptr_array_sized_new(32) ), + candidate_cells( g_ptr_array_sized_new(32) ) + // _gpts( NULL ) +{ + assert(mod); + assert(wf); + assert(entity); + + Load( wf, entity ); - this->zmin = zmin; - this->zmax = zmax; - this->color = color; - this->inherit_color = inherit_color; - this->rendered_points = - g_array_new( FALSE, FALSE, sizeof(stg_list_entry_t)); - - // flag these as unset until StgBlock::Map() is called. - this->global_zmin = -1; - this->global_zmax = -1; + // add this block's global coords array to a global list + //g_ptr_array_add( global_verts, this ); } + StgBlock::~StgBlock() { - this->UnMap(); - g_free( pts ); - g_array_free( rendered_points, TRUE ); + if( mapped ) UnMap(); + + stg_points_destroy( pts ); + + g_ptr_array_free( rendered_cells, TRUE ); + g_ptr_array_free( candidate_cells, TRUE ); - //delete[] edge_indices; + //free( _gpts ); + //g_ptr_array_remove( global_verts, this ); } -void Stg::stg_block_list_destroy( GList* list ) +stg_color_t StgBlock::GetColor() { - GList* it; - for( it=list; it; it=it->next ) - delete (StgBlock*)it->data; - g_list_free( list ); + return( inherit_color ? mod->color : color ); } -void StgBlock::DrawTop() + +StgModel* StgBlock::TestCollision() { - // draw a top that fits over the side strip - glPushMatrix(); - glTranslatef( 0,0,zmax); - glVertexPointer( 2, GL_DOUBLE, 0, pts ); - glDrawArrays( GL_POLYGON, 0, pt_count ); - glPopMatrix(); -} + //printf( "model %s block %p test collision...\n", mod->Token(), this ); -//TODO FIXME - this is really SLOW -void StgBlock::DrawSides() -{ - // construct a strip that wraps around the polygon - glBegin(GL_QUAD_STRIP); - for( unsigned int p=0; p<pt_count; p++) - { - glVertex3f( pts[p].x, pts[p].y, zmax ); - glVertex3f( pts[p].x, pts[p].y, zmin ); - } - // close the strip - glVertex3f( pts[0].x, pts[0].y, zmax ); - glVertex3f( pts[0].x, pts[0].y, zmin ); - glEnd(); + // find the set of cells we would render into given the current global pose + GenerateCandidateCells(); + + // for every cell we may be rendered into + for( int i=0; i<candidate_cells->len; i++ ) + { + Cell* cell = (Cell*)g_ptr_array_index(candidate_cells, i); + + // for every rendered into that cell + for( GSList* it = cell->list; it; it=it->next ) + { + StgBlock* testblock = (StgBlock*)it->data; + StgModel* testmod = testblock->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->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 } -void StgBlock::DrawFootPrint() -{ - glBegin(GL_POLYGON); - for( unsigned int p=0; p<pt_count; p++ ) - glVertex2f( pts[p].x, pts[p].y ); +void StgBlock::RemoveFromCellArray( GPtrArray* ptrarray ) +{ + for( unsigned int i=0; i<ptrarray->len; i++ ) + ((Cell*)g_ptr_array_index(ptrarray, i))->RemoveBlock( this ); +} - glEnd(); +void StgBlock::AddToCellArray( GPtrArray* ptrarray ) +{ + for( unsigned int i=0; i<ptrarray->len; i++ ) + ((Cell*)g_ptr_array_index(ptrarray, i))->AddBlock( this ); } -void StgBlock::Draw() +// used as a callback to gather an array of cells in a polygon +void AppendCellToPtrArray( Cell* c, GPtrArray* a ) { - // draw filled color polygons - stg_color_t color = Color(); - - PushColor( color ); - 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( color, &r, &g, &b, &a ); - 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 ); - - PopColor(); - PopColor(); + g_ptr_array_add( a, c ); } -void StgBlock::DrawSolid( void ) +// used as a callback to gather an array of cells in a polygon +void AddBlockToCell( Cell* c, StgBlock* block ) { - DrawSides(); - DrawTop(); + c->AddBlock( block ); } void StgBlock::Map() { - PRINT_DEBUG2( "%s mapping block with %d points", - mod->Token(), - (int)pt_count ); + // TODO - if called often, we may not need to generate each time + GenerateCandidateCells(); + SwitchToTestedCells(); + return; - // update the global coordinate list - stg_point3_t global; - stg_point3_t local; - - for( unsigned int p=0; p<pt_count; p++ ) - { - local.x = pts[p].x; - local.y = pts[p].y; - local.z = zmin; - - global = mod->LocalToGlobal( local ); - //global = local; - - pts_global_pixels[p].x = mod->GetWorld()->MetersToPixels( global.x ); - pts_global_pixels[p].y = mod->GetWorld()->MetersToPixels( global.y ); - - PRINT_DEBUG2("loc [%.2f %.2f]", - pts[p].x, - pts[p].y ); - - PRINT_DEBUG2("glb [%d %d]", - pts_global_pixels[p].x, - pts_global_pixels[p].y ); - } - - // store the block's global vertical bounds for inspection by the - // raytracer - global_zmin = global.z; - global_zmax = global.z + (zmax-zmin); - - stg_render_info_t render_info; - render_info.world = mod->GetWorld(); - render_info.block = this; - - stg_polygon_3d( pts_global_pixels, pt_count, - (stg_line3d_func_t)StgWorld::AddBlockPixel, - (void*)&render_info ); - + mapped = true; } void StgBlock::UnMap() { - PRINT_DEBUG2( "UnMapping block of model %s with %d points", - mod->Token(), - (int)pt_count); + RemoveFromCellArray( rendered_cells ); + + g_ptr_array_set_size( rendered_cells, 0 ); + mapped = false; +} - for( unsigned int p=0; p<rendered_points->len; p++ ) - { - stg_list_entry_t* pt = - &g_array_index( rendered_points, stg_list_entry_t, p); +void StgBlock::SwitchToTestedCells() +{ + RemoveFromCellArray( rendered_cells ); + AddToCellArray( candidate_cells ); + + // switch current and candidate cell pointers + GPtrArray* tmp = rendered_cells; + rendered_cells = candidate_cells; + candidate_cells = tmp; - *pt->head = g_slist_delete_link( *pt->head, pt->link ); - - // decrement the region and superregion render counts - (*pt->counter1)--; - (*pt->counter2)--; - } - - // forget the points we have unrendered (but keep their storage) - g_array_set_size( rendered_points,0 ); -} - -void StgBlock::RecordRenderPoint( GSList** head, - GSList* link, - unsigned int* c1, - unsigned int* c2 ) -{ - // store this index in the block for later fast deletion - stg_list_entry_t el; - el.head = head; - el.link = link; - el.counter1 = c1; - el.counter2 = c2; - g_array_append_val( rendered_points, el ); + mapped = true; } +void StgBlock::GenerateCandidateCells() -void StgBlock::ScaleList( GList* blocks, - stg_size_t* size ) { - if( g_list_length( blocks ) < 1 ) - return; + stg_pose_t gpose = mod->GetGlobalPose(); - // assuming the blocks currently fit in a square +/- one billion units - double minx, miny, maxx, maxy; - minx = miny = billion; - maxx = maxy = -billion; + stg_point3_t scale; + scale.x = mod->geom.size.x / mod->blockgroup.size.x; + scale.y = mod->geom.size.y / mod->blockgroup.size.y; + scale.z = mod->geom.size.z / mod->blockgroup.size.z; - double maxz = 0; + g_ptr_array_set_size( candidate_cells, 0 ); + + // compute the global location of the first point + stg_pose_t local( pts[0].x * scale.x, + pts[0].y * scale.y, + 0, 0 ); + stg_pose_t first_gpose, last_gpose; + first_gpose = last_gpose = pose_sum( gpose, local ); + + // store the block's absolute z bounds at this rendering + global_z.min = (scale.z * local_z.min) + last_gpose.z; + global_z.max = (scale.z * local_z.max) + last_gpose.z; + + // now loop from the the second to the last + for( int p=1; p<pt_count; p++ ) + { + stg_pose_t local( pts[p].x * scale.x, + pts[p].y * scale.y, + 0, 0 ); + + stg_pose_t gpose2 = pose_sum( gpose, local ); + + // and render the shape of the block into the global cells + mod->world->ForEachCellInLine( last_gpose.x, last_gpose.y, + gpose2.x, gpose2.y, + (stg_cell_callback_t)AppendCellToPtrArray, + candidate_cells ); + last_gpose = gpose2; + } + + // close the polygon + mod->world->ForEachCellInLine( last_gpose.x, last_gpose.y, + first_gpose.x, first_gpose.y, + (stg_cell_callback_t)AppendCellToPtrArray, + candidate_cells ); + + mapped = true; +} - GList* it; - for( it=blocks; it; it=it->next ) // examine all the blocks - { - // examine all the points in the polygon - StgBlock* block = (StgBlock*)it->data; - block->UnMap(); // just in case +void StgBlock::DrawTop() +{ + // draw the top of the block - a polygon at the highest vertical + // extent + glBegin( GL_POLYGON); + for( int i=0; i<pt_count; i++ ) + glVertex3f( pts[i].x, pts[i].y, local_z.max ); + glEnd(); +} - 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; +void StgBlock::DrawSides() +{ + // construct a strip that wraps around the polygon + glBegin(GL_QUAD_STRIP); + for( unsigned int p=0; p<pt_count; p++) + { + glVertex3f( pts[p].x, pts[p].y, local_z.max ); + glVertex3f( pts[p].x, pts[p].y, local_z.min ); + } + // close the strip + glVertex3f( pts[0].x, pts[0].y, local_z.max ); + glVertex3f( pts[0].x, pts[0].y, local_z.min ); + glEnd(); +} - assert( ! isnan( pt->x ) ); - assert( ! isnan( pt->y ) ); - } +void StgBlock::DrawFootPrint() +{ + glBegin(GL_POLYGON); + for( unsigned int p=0; p<pt_count; p++ ) + glVertex2f( pts[p].x, pts[p].y ); + glEnd(); +} +void StgBlock::Draw( StgModel* mod ) +{ + // 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); + + // // 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(); +} - if( block->zmax > maxz ) - maxz = block->zmax; - } +void StgBlock::DrawSolid() +{ + DrawSides(); + DrawTop(); +} - // now normalize all lengths so that the lines all fit inside - // the specified rectangle - double scalex = (maxx - minx); - double scaley = (maxy - miny); - double scalez = size->z / maxz; +//#define DEBUG 1 - for( it=blocks; it; it=it->next ) // examine all the blocks again - { - StgBlock* block = (StgBlock*)it->data; - // scale all the points in the block - // TODO - scaling data in the block instead? - for( unsigned int p=0; p < block->pt_count; p++ ) - { - stg_point_t* pt = &block->pts[p]; - - pt->x = ((pt->x - minx) / scalex * size->x) - size->x/2.0; - pt->y = ((pt->y - miny) / scaley * size->y) - size->y/2.0; - - assert( ! isnan( pt->x ) ); - assert( ! isnan( pt->y ) ); - } - - // todo - scale min properly - block->zmax *= scalez; - block->zmin *= scalez; - } +void StgBlock::Load( Worldfile* wf, int entity ) +{ + //printf( "StgBlock::Load entity %d\n", entity ); + + if( pts ) + stg_points_destroy( pts ); + + pt_count = wf->ReadInt( entity, "points", 0); + pts = stg_points_create( pt_count ); + + //printf( "reading %d points\n", + // pt_count ); + + char key[128]; + int p; + for( 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 ) + { + color = stg_lookup_color( colorstr ); + inherit_color = false; + } } + + Property changes on: code/stage/trunk/libstage/block.cc ___________________________________________________________________ Added: svn:eol + native Added: svn:eol-style + native Property changes on: code/stage/trunk/libstage/blockgrid.cc ___________________________________________________________________ Added: svn:eol + native Added: svn:eol-style + native Added: code/stage/trunk/libstage/blockgroup.cc =================================================================== --- code/stage/trunk/libstage/blockgroup.cc (rev 0) +++ code/stage/trunk/libstage/blockgroup.cc 2008-11-15 03:11:20 UTC (rev 7151) @@ -0,0 +1,277 @@ + +#include "stage_internal.hh" +#include <libgen.h> // for dirname(3) + +#undef DEBUG + +using namespace Stg; + +BlockGroup::BlockGroup() + : blocks(NULL), + count(0), + displaylist(0) +{ /* empty */ } + +BlockGroup::~BlockGroup() +{ + Clear(); +} + +void BlockGroup::AppendBlock( StgBlock* block ) +{ + blocks = g_list_append( blocks, block ); + ++count; + + block->mod->map_caches_are_invalid = true; +} + +void BlockGroup::Clear() +{ + while( blocks ) + { + delete (StgBlock*)blocks->data; + blocks = blocks->next; + } + + g_list_free( blocks ); + blocks = NULL; +} + + +void BlockGroup::SwitchToTestedCells() +{ + // confirm the tentative pose for all blocks + LISTMETHOD( blocks, StgBlock*, SwitchToTestedCells ); +} + +StgModel* BlockGroup::TestCollision() +{ + //printf( "blockgroup %p test collision...\n", this ); + + StgModel* hitmod = NULL; + + for( GList* it=blocks; it; it = it->next ) + if( hitmod = ((StgBlock*)it->data)->TestCollision()) + break; // bail on the earliest collision + + //printf( "blockgroup %p test collision done.\n", this ); + + return hitmod; +} + + +// establish the min and max of all the blocks, so we can scale this +// group later +void BlockGroup::CalcSize() +{ + // assuming the blocks currently fit in a square +/- one billion units + double minx, miny, maxx, maxy, minz, maxz; + minx = miny = billion; + maxx = maxy = -billion; + + size.z = 0.0; // grow to largest z we see + + if( blocks ) + ((StgBlock*)blocks->data)->mod->map_caches_are_invalid = true; + + for( GList* it=blocks; it; it=it->next ) // examine all the blocks + { + // examine all the points in the polygon + StgBlock* block = (StgBlock*)it->data; + + 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 scaling purposes + size.x = maxx-minx; + size.y = maxy-miny; +} + + +void BlockGroup::Map() +{ + LISTMETHOD( blocks, StgBlock*, Map ); +} + +void BlockGroup::UnMap() +{ + LISTMETHOD( blocks, StgBlock*, UnMap ); +} + +void BlockGroup::DrawSolid() +{ + LISTMETHOD( blocks, StgBlock*, DrawSolid ); +} + +void BlockGroup::DrawFootPrint() +{ + LISTMETHOD( blocks, StgBlock*, DrawFootPrint); +} + +void BlockGroup::BuildDisplayList( StgModel* mod ) +{ + //printf( "display list for model %s\n", mod->token ); + + if( displaylist == 0 ) + displaylist = glGenLists(1); + + glNewList( displaylist, GL_COMPILE ); + + stg_geom_t geom = mod->GetGeom(); + glScalef( geom.size.x / size.x, + geom.size.y / size.y, + geom.size.z / size.z ); + + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + glEnable(GL_POLYGON_OFFSET_FILL); + glPolygonOffset(1.0, 1.0); + + mod->PushColor( mod->color ); + + for( GList* it=blocks; it; it=it->next ) + { + StgBlock* blk = (StgBlock*)it->data; + + if( (!blk->inherit_color) && (blk->color != mod->color) ) + { + mod->PushColor( blk->color ); + blk->DrawSolid(); + mod->PopColor(); + } + else + blk->DrawSolid(); + } + + mod->PopColor(); + + glDisable(GL_POLYGON_OFFSET_FILL); + + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + glDepthMask(GL_FALSE); + + double r,g,b,a; + 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 ) + { + StgBlock* blk = (StgBlock*)it->data; + + 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(); + } + + // LISTMETHOD( blocks, StgBlock*, DrawSolid ); + + glDepthMask(GL_TRUE); + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + + mod->PopColor(); + + glEndList(); +} + +void BlockGroup::CallDisplayList( StgModel* mod ) +{ + if( displaylist == 0 ) + BuildDisplayList( mod ); + + //gl_pose_shift( &geom.pose ); + + glCallList( displaylist ); +} + +void BlockGroup::LoadBlock( StgModel* mod, Worldfile* wf, int entity ) +{ + AppendBlock( new StgBlock( mod, wf, entity )); +} + +void BlockGroup::LoadBitmap( StgModel* mod, const char* bitmapfile, Worldfile* wf ) +{ + PRINT_DEBUG1( "attempting to load bitmap \"%s\n", bitmapfile ); + + char full[_POSIX_PATH_MAX]; + + if( bitmapfile[0] == '/' ) + strcpy( full, bitmapfile ); + else + { + 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 ); + + stg_rotrect_t* rects = NULL; + 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; + } + + //printf( "found %d rects in \"%s\" at %p\n", + // rect_count, full, rects ); + + if( rects && (rect_count > 0) ) + { + // shift the origin from bottom-left to center of the image + double dx = width/2.0; + double dy = height/2.0; + + //puts( "loading rects" ); + 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; + + pts[0].x = x - dx; + pts[0].y = y - dy; + pts[1].x = x + w - dx; + pts[1].y = y -dy; + pts[2].x = x + w -dx; + pts[2].y = y + h -dy; + pts[3].x = x - dx; + pts[3].y = y + h -dy; + + // TODO fix this + stg_color_t col = stg_color_pack( 1.0, 0,0,1.0 ); + + AppendBlock( new StgBlock( mod, + pts,4, + 0,1, + col, + true ) ); + } + free( rects ); + } + + CalcSize(); +} Property changes on: code/stage/trunk/libstage/blockgroup.cc ___________________________________________________________________ Added: svn:eol + native Added: svn:eol-style + native Modified: code/stage/trunk/libstage/camera.cc =================================================================== --- code/stage/trunk/libstage/camera.cc 2008-11-15 03:10:01 UTC (rev 7150) +++ code/stage/trunk/libstage/camera.cc 2008-11-15 03:11:20 UTC (rev 7151) @@ -12,8 +12,8 @@ #include <iostream> // Perspective Camera -StgPerspectiveCamera::StgPerspectiveCamera( void ) : - StgCamera(), +PerspectiveCamera::PerspectiveCamera( void ) : + Camera(), _z_near( 0.2 ), _z_far( 40.0 ), _vert_fov( 70 ), _horiz_fov( 70 ), _aspect( 1.0 ) @@ -21,7 +21,7 @@ setPitch( 70.0 ); } -void StgPerspectiveCamera::move( float x, float y, float z ) +void PerspectiveCamera::move( float x, float y, float z ) { //scale relative to zoom level x *= _z / 100.0; @@ -35,7 +35,7 @@ _y += cos( dtor( _yaw ) ) * y; } -void StgPerspectiveCamera::Draw( void ) const +void PerspectiveCamera::Draw( void ) const { glMatrixMode (GL_MODELVIEW); glLoadIdentity (); @@ -45,10 +45,9 @@ glTranslatef( - _x, - _y, - _z ); //zooming needs to happen in the Projection code (don't use glScale for zoom) - } -void StgPerspectiveCamera::SetProjection( void ) const +void PerspectiveCamera::SetProjection( void ) const { // SetProjection( pixels_width/pixels_height ); @@ -69,24 +68,24 @@ } -void StgPerspectiveCamera::update( void ) +void PerspectiveCamera::update( void ) { } -void StgPerspectiveCamera::strafe( float amount ) +void PerspectiveCamera::strafe( float amount ) { _x += cos( dtor( _yaw ) ) * amount; _y += sin( dtor( _yaw ) ) * amount; } -void StgPerspectiveCamera::forward( float amount ) +void PerspectiveCamera::forward( float amount ) { _x += -sin( dtor( _yaw ) ) * amount; _y += cos( dtor( _yaw ) ) * amount; } -void StgPerspectiveCamera::Load( Worldfile* wf, int sec ) { +void PerspectiveCamera::Load( Worldfile* wf, int sec ) { float x_pos = wf->ReadTupleLength(sec, "pcam_loc", 0, x() ); float y_pos = wf->ReadTupleLength(sec, "pcam_loc", 1, y() ); float z_pos = wf->ReadTupleLength(sec, "pcam_loc", 2, z() ); @@ -95,7 +94,7 @@ setYaw( wf->ReadTupleFloat( sec, "pcam_angle", 1, yaw() ) ); } -void StgPerspectiveCamera::Save( Worldfile* wf, int sec ) { +void PerspectiveCamera::Save( Worldfile* wf, int sec ) { wf->WriteTupleFloat( sec, "pcam_loc", 0, x() ); wf->WriteTupleFloat( sec, "pcam_loc", 1, y() ); wf->WriteTupleFloat( sec, "pcam_loc", 2, z() ); @@ -109,7 +108,7 @@ ////////////////////////////////////////////////////////////////////////////////////////////////////////////// //Ortho camera ////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void StgOrthoCamera::Draw( void ) const +void OrthoCamera::Draw( void ) const { glMatrixMode (GL_MODELVIEW); glLoadIdentity (); @@ -122,7 +121,7 @@ } -void StgOrthoCamera::SetProjection( void ) const +void OrthoCamera::SetProjection( void ) const { glMatrixMode (GL_PROJECTION); glLoadIdentity (); @@ -134,7 +133,7 @@ glMatrixMode (GL_MODELVIEW); } -void StgOrthoCamera::SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) +void OrthoCamera::SetProjection( float pixels_width, float pixels_height, float y_min, float y_max ) { _pixels_width = pixels_width; _pixels_height = pixels_height; @@ -143,7 +142,7 @@ SetProjection(); } -void StgOrthoCamera::move( float x, float y ) { +void OrthoCamera::move( float x, float y ) { //convert screen points into world points x = x / ( _scale ); y = y / ( _scale ); @@ -167,7 +166,7 @@ } //TODO re-evaluate the way the camera is shifted when the mouse zooms - it might be possible to simplify -void StgOrthoCamera::scale( float scale, float shift_x, float w, float shift_y, float h ) +void OrthoCamera::scale( float scale, float shift_x, float w, float shift_y, float h ) { float to_scale = -scale; const float old_scale = _scale; @@ -205,7 +204,7 @@ } } -void StgOrthoCamera::Load( Worldfile* wf, int sec ) { +void OrthoCamera::Load( Worldfile* wf, int sec ) { float x_pos = wf->ReadTupleLength(sec, "center", 0, x() ); float y_pos = wf->ReadTupleLength(sec, "center", 1, y() ); setPose( x_pos, y_pos ); @@ -214,7 +213,7 @@ setScale( wf->ReadFloat(sec, "scale", scale() ) ); } -void StgOrthoCamera::Save( Worldfile* wf, int sec ) { +void OrthoCamera::Save( Worldfile* wf, int sec ) { wf->WriteTupleFloat( sec, "center", 0, x() ); wf->WriteTupleFloat( sec, "center", 1, y() ); wf->WriteTupleFloat( sec, "rotate", 0, pitch() ); Property changes on: code/stage/trunk/libstage/camera.cc ___________________________________________________________________ Added: svn:eol + native Added: svn:eol-style + native Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-11-15 03:10:01 UTC (rev 7150) +++ code/stage/trunk/libstage/canvas.cc 2008-11-15 03:11:20 UTC (rev 7151) @@ -1,8 +1,8 @@ /** canvas.cc Implement the main world viewing area in FLTK and OpenGL. Authors: Richard Vaughan (va...@sf...) - Alex Couture-Beil (as...@sf...) - Jeremy Asher (jr...@sf...) + Alex Couture-Beil (as...@sf...) + Jeremy Asher (jr...@sf...) $Id$ */ @@ -14,6 +14,7 @@ #include <map> #include <sstream> #include <png.h> +#include <GLUT/glut.h> #include "file_manager.hh" #include "options_dlg.hh" @@ -30,11 +31,14 @@ c->redraw(); Fl::repeat_timeout(((double)c->interval/1000), - (Fl_Timeout_Handler)StgCanvas::TimerCallback, - c); + (Fl_Timeout_Handler)StgCanvas::TimerCallback, + c); } + + + StgCanvas::StgCanvas( StgWorldGui* world, int x, int y, int w, int h) : Fl_Gl_Window(x,y,w,h), // initialize Option objects @@ -74,11 +78,17 @@ interval = 50; //msec between redraws graphics = true; - + // // start the timer that causes regular redraws Fl::add_timeout( ((double)interval/1000), - (Fl_Timeout_Handler)StgCanvas::TimerCallback, - this); + (Fl_Timeout_Handler)StgCanvas::TimerCallback, + this); + + + GLenum status; + + + } StgCanvas::~StgCanvas() @@ -89,7 +99,7 @@ { // render all models in a unique color make_current(); // make sure the GL context is current - glClearColor ( 1,1,1,1 ); // white + glClearColor( 1,1,1,1 ); // white glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); glLoadIdentity(); current_camera->SetProjection(); @@ -106,19 +116,19 @@ StgModel* mod = (StgModel*)it->data; if( mod->GuiMask() & (STG_MOVE_TRANS | STG_MOVE_ROT )) - { - 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 @@ -130,13 +140,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; @@ -162,65 +172,65 @@ } bool StgCanvas::selected( StgModel* mod ) { - if( g_list_find( selected_models, mod ) ) - return true; - else - return false; + if( g_list_find( selected_models, mod ) ) + return true; + else + return false; } void StgCanvas::select( StgModel* mod ) { - if( mod ) + if( mod ) { last_selection = mod; selected_models = g_list_prepend( selected_models, mod ); -// mod->Disable(); + // mod->Disable(); } } void StgCanvas::unSelect( StgModel* mod ) { - if( mod ) - { + if( mod ) + { if ( GList* link = g_list_find( selected_models, mod ) ) - { - // remove it from the selected list - selected_models = - g_list_remove_link( selected_models, link ); -// mod->Enable(); - } - } + { + // remove it from the selected list + selected_models = + g_list_remove_link( selected_models, link ); + // mod->Enable(); + } + } } void StgCanvas::unSelectAll() { -// for( GList* it=selected_models; it; it=it->next ) -// ((StgModel*)it->data)->Enable(); + // for( GList* it=selected_models; it; it=it->next ) + // ((StgModel*)it->data)->Enable(); - g_list_free( selected_models ); - selected_models = NULL; + g_list_free( selected_models ); + selected_models = NULL; } // convert from 2d window pixel to 3d world coordinates void StgCanvas::CanvasToWorld( int px, int py, - double *wx, double *wy, double* wz ) + double *wx, double *wy, double* wz ) { - if( px <= 0 ) - px = 1; - else if( px >= w() ) - px = w() - 1; - if( py <= 0 ) - py = 1; - else if( py >= h() ) - py = h() - 1; + if( px <= 0 ) + px = 1; + else if( px >= w() ) + px = w() - 1; + if( py <= 0 ) + py = 1; + else if( py >= h() ) + py = h() - 1; - //redraw the screen only if the camera model isn't active. - //TODO new selection technique will simply use drawfloor to result in z = 0 always and prevent strange behaviours near walls - //TODO refactor, so glReadPixels reads (then caches) the whole screen only when the camera changes. - if( true || dirtyBuffer() ) { - glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); - current_camera->SetProjection(); - current_camera->Draw(); - DrawFloor(); //call this rather than renderFrame for speed - this won't give correct z values - dirty_buffer = false; - } + //redraw the screen only if the camera model isn't active. + //TODO new selection technique will simply use drawfloor to result in z = 0 always and prevent strange behaviours near walls + //TODO refactor, so glReadPixels reads (then caches) the whole screen only when the camera changes. + if( true || dirtyBuffer() ) { + glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); + current_camera->SetProjection(); + current_camera->Draw(); + DrawFloor(); //call this rather than renderFrame for speed - this won't give correct z values + dirty_buffer = false; + } int viewport[4]; glGetIntegerv(GL_VIEWPORT, viewport); @@ -239,214 +249,214 @@ int StgCanvas::handle(int event) { - switch(event) - { - case FL_MOUSEWHEEL: + switch(event) + { + 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(); return 1; - case FL_MOVE: // moused moved while no button was pressed + case FL_MOVE: // moused moved while no button was pressed if ( startx >=0 ) { - // mouse pointing to valid value + // mouse pointing to valid value - if( Fl::event_state( FL_CTRL ) ) - { + 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 ); + 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 ) ); + 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 ) ) - { + } + 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 ); + perspective_camera.move( -dx, dy, 0.0 ); } else { - camera.move( -dx, dy ); + camera.move( -dx, dy ); } invalidate(); - } + } } startx = Fl::event_x(); starty = Fl::event_y(); return 1; - case FL_PUSH: // button pressed - { - StgModel* 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 - } - } + case FL_PUSH: // button pressed + { + StgModel* 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 + } + } - return 1; - case 3: - { - // leave selections alone - // rotating handled within FL_DRAG - return 1; + return 1; + case 3: + { + // leave selections alone + // rotating handled within FL_DRAG + return 1; + } + default: + return 0; + } } - default: - return 0; - } - } - case FL_DRAG: // mouse moved while button was pressed - { - int dx = Fl::event_x() - startx; - int dy = Fl::event_y() - starty; + case FL_DRAG: // mouse moved while button was pressed + { + 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 ) { + 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 ); + &sx, &sy, &sz ); double x,y,z; CanvasToWorld( Fl::event_x(), Fl::event_y(), - &x, &y, &z ); + &x, &y, &z ); // move all selected models to the mouse pointer for( GList* it = selected_models; it; it=it->next ) - { - StgModel* mod = (StgModel*)it->data; - mod->AddToPose( x-sx, y-sy, 0, 0 ); - } - } - else { + { + StgModel* mod = (StgModel*)it->data; + 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 ); + perspective_camera.move( -dx, dy, 0.0 ); } else { - camera.move( -dx, dy ); + 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( GList* it = selected_models; it; it=it->next ) + } + } + else if ( Fl::event_state( FL_BUTTON3 ) || ( Fl::event_state( FL_BUTTON1 ) && Fl::event_state( FL_CTRL ) ) ) { + // rotate all selected models + for( GList* it = selected_models; it; it=it->next ) { - StgModel* mod = (StgModel*)it->data; - mod->AddToPose( 0,0,0, 0.05*(dx+dy) ); + StgModel* mod = (StgModel*)it->data; + 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; - } // end case FL_DRAG + 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(); - } + 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(); + } return 1; - case FL_FOCUS: - case FL_UNFOCUS: + case FL_FOCUS: + case FL_UNFOCUS: //.... Return 1 if you want keyboard events, 0 otherwise return 1; - case FL_KEYBOARD: + case FL_KEYBOARD: switch( Fl::event_key() ) - { - case 'p': // pause - world->TogglePause(); - break; - case ' ': // space bar + { + case 'p': // pause + world->TogglePause(); + break; + case ' ': // space bar - // if the worldfile doesn't have the fields you need, you get - // a weird view. need to think this through a bit before - // eliminating the old behaviour - rtv + // if the worldfile doesn't have the fields you need, you get + // a weird view. need to think this through a bit before + // eliminating the old behaviour - rtv - //if ( wf ) - //current_camera->Load( wf, wf->LookupEntity( "window" ) ); - //else - current_camera->reset(); + //if ( wf ) + //current_camera->Load( wf, wf->LookupEntity( "window" ) ); + //else + current_camera->reset(); - //invalidate(); - if( Fl::event_state( FL_CTRL ) ) { + //invalidate(); + if( Fl::event_state( FL_CTRL ) ) { resetCamera(); - } - redraw(); - break; - 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: - return 0; // keypress unhandled - } + } + redraw(); + break; + 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: + return 0; // keypress unhandled + } invalidate(); // update projection return 1; -// case FL_SHORTCUT: -// //... shortcut, key is in Fl::event_key(), ascii in Fl::event_text() -// //... Return 1 if you understand/use the shortcut event, 0 otherwise... -// return 1; - default: + // case FL_SHORTCUT: + // //... shortcut, key is in Fl::event_key(), ascii in Fl::event_text() + // //... Return 1 if you understand/use the shortcut event, 0 otherwise... + // return 1; + default: // pass other events to the base class... //printf( "EVENT %d\n", event ); return Fl_Gl_Window::handle(event); @@ -482,6 +492,8 @@ } PopColor(); + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + glEnable(GL_POLYGON_OFFSET_FILL); glPolygonOffset(2.0, 2.0); glDisable(GL_BLEND); @@ -493,20 +505,19 @@ glBegin(GL_QUADS); glTexCoord2f( bounds.x.min/2.0, bounds.y.min/2.0 ); - glVertex3f( bounds.x.min, bounds.y.min, 0 ); + glVertex2f( bounds.x.min, bounds.y.min ); glTexCoord2f( bounds.x.max/2.0, bounds.y.min/2.0); - glVertex3f( bounds.x.max, bounds.y.min, 0 ); + glVertex2f( bounds.x.max, bounds.y.min ); glTexCoord2f( bounds.x.max/2.0, bounds.y.max/2.0 ); - glVertex3f( bounds.x.max, bounds.y.max, 0 ); + glVertex2f( bounds.x.max, bounds.y.max ); glTexCoord2f( bounds.x.min/2.0, bounds.y.max/2.0 ); - glVertex3f( bounds.x.min, bounds.y.max, 0 ); + glVertex2f( bounds.x.min, bounds.y.max ); glEnd(); glDisable(GL_TEXTURE_2D); glEnable(GL_BLEND); - glDisable(GL_POLYGON_OFFSET_FILL ); } @@ -514,17 +525,17 @@ void StgCanvas::DrawFloor() { stg_bounds3d_t bounds = world->GetExtent(); - float z = 0; glEnable(GL_POLYGON_OFFSET_FILL); glPolygonOffset(2.0, 2.0); glColor4f( 1.0, 1.0, 1.0, 1.0 ); + glBegin(GL_QUADS); - glVertex3f( bounds.x.min, bounds.y.min, z ); - glVertex3f( bounds.x.max, bounds.y.min, z ); - glVertex3f( bounds.x.max, bounds.y.max, z ); - glVertex3f( bounds.x.min, bounds.y.max, z ); + glVertex2f( bounds.x.min, bounds.y.min ); + glVertex2f( bounds.x.max, bounds.y.min ); + glVertex2f( bounds.x.max, bounds.y.max ); + glVertex2f( bounds.x.min, bounds.y.max ); glEnd(); glEnd(); @@ -537,109 +548,108 @@ void StgCanvas::resetCamera() { - float max_x = 0, max_y = 0, min_x = 0, min_y = 0; + 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->StgWorld::children; it; it=it->next ) { - StgModel* ptr = (StgModel*) it->data; - stg_pose_t pose = ptr->GetPose(); - stg_geom_t geom = ptr->GetGeom(); + //TODO take orrientation ( `a' ) and geom.pose offset into consideration + for( GList* it=world->StgWorld::children; it; it=it->next ) { + StgModel* ptr = (StgModel*) it->data; + stg_pose_t pose = ptr->GetPose(); + stg_geom_t geom = ptr->GetGeom(); - float tmp_min_x = pose.x - geom.size.x / 2.0; - float tmp_max_x = pose.x + geom.size.x / 2.0; - float tmp_min_y = pose.y - geom.size.y / 2.0; - float tmp_max_y = pose.y + geom.size.y / 2.0; + float tmp_min_x = pose.x - geom.size.x / 2.0; + float tmp_max_x = pose.x + geom.size.x / 2.0; + float tmp_min_y = pose.y - geom.size.y / 2.0; + float tmp_max_y = pose.y + geom.size.y / 2.0; - if( tmp_min_x < min_x ) min_x = tmp_min_x; - if( tmp_max_x > max_x ) max_x = tmp_max_x; - if( tmp_min_y < min_y ) min_y = tmp_min_y; - if( tmp_max_y > max_y ) max_y = tmp_max_y; - } + if( tmp_min_x < min_x ) min_x = tmp_min_x; + if( tmp_max_x > max_x ) max_x = tmp_max_x; + if( tmp_min_y < min_y ) min_y = tmp_min_y; + if( tmp_max_y > max_y ) max_y = tmp_max_y; + } - //do a complete reset - float x = ( min_x + max_x ) / 2.0; - float y = ( min_y + max_y ) / 2.0; - camera.setPose( x, y ); - float scale_x = w() / (max_x - min_x) * 0.9; - float scale_y = h() / (max_y - min_y) * 0.9; - camera.setScale( scale_x < scale_y ? scale_x : scale_y ); + //do a complete reset + float x = ( min_x + max_x ) / 2.0; + float y = ( min_y + max_y ) / 2.0; + camera.setPose( x, y ); + float scale_x = w() / (max_x - min_x) * 0.9; + float scale_y = h() / (max_y - min_y) * 0.9; + camera.setScale( scale_x < scale_y ? scale_x : scale_y ); - //TODO reset perspective cam + //TODO reset perspective cam } void StgCanvas::renderFrame() { - //before drawing, order all models based on distance from camera - float x = current_camera->x(); - float y = current_camera->y(); - float sphi = -dtor( current_camera->yaw() ); + //before drawing, order all models based on distance from camera + float x = current_camera->x(); + float y = current_camera->y(); + float sphi = -dtor( current_camera->yaw() ); - //estimate point of camera location - hard to do with orthogonal mode - x += -sin( sphi ) * 100; - y += -cos( sphi ) * 100; + //estimate point of camera location - hard to do with orthogonal mode + x += -sin( sphi ) * 100; + y += -cos( sphi ) * 100; - // TODO - keep this map around in between frames, because the order - // changes slowly, and sorting an already-sorted list is - // usually very fast (rtv) + // TODO - keep this map around in between frames, because the order + // changes slowly, and sorting an already-sorted list is + // usually very fast (rtv) - //store all models in a sorted multimap - std::multimap< float, StgModel* > ordered; - for( GList* it=world->StgWorld::children; it; it=it->next ) { - StgModel* ptr = (StgModel*) it->data; - stg_pose_t pose = ptr->GetPose(); + //store all models in a sorted multimap + std::multimap< float, StgModel* > ordered; + for( GList* it=world->StgWorld::children; it; it=it->next ) { + StgModel* ptr = (StgModel*) it->data; + stg_pose_t pose = ptr->GetPose(); - float dist = sqrt( ( x - pose.x ) * ( x - pose.x ) + ( y - pose.y ) * ( y - pose.y ) ); - ordered.insert( std::pair< float, StgModel* >( dist, (StgModel*)it->data ) ); - } + float dist = sqrt( ( x - pose.x ) * ( x - pose.x ) + ( y - pose.y ) * ( y - pose.y ) ); + ordered.insert( std::pair< float, StgModel* >( dist, (StgModel*)it->data ) ); + } - //now the models can be iterated over with: - // for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) + //now the models can be iterated over with: + // for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) + glEnable( GL_DEPTH_TEST ); + if( ! showTrails ) - glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); + glClear( GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT ); - if( showGrid ) - DrawGlobalGrid(); - else - DrawFloor(); - if( showTree || showOccupancy ) { glPushMatrix(); - glScalef( 1.0/world->Resolution(), 1.0/world->Resolution(), 0 ); + + GLfloat scale = 1.0/world->Resolution(); + glScalef( scale, scale, 1.0 ); // XX TODO - this seems slightly + // out for Z. look into it. - glLineWidth( 1 ); - glPolygonMode( GL_FRONT, GL_LINE ); - colorstack.Push(1,0,0); - if( showOccupancy ) - ((StgWorldGui*)world)->DrawTree( false ); + ((StgWorldGui*)world)->DrawTree( false ); if( showTree ) - ((StgWorldGui*)world)->DrawTree( true ); + ((StgWorldGui*)world)->DrawTree( true ); - colorstack.Pop(); - glPolygonMode( GL_FRONT, GL_FILL ); glPopMatrix(); } + + if( showGrid ) + DrawGlobalGrid(); + else + DrawFloor(); - for( GList* it=selected_models; it; it=it->next ) - ((StgModel*)it->data)->DrawSelected(); - if( showFootprints ) { glDisable( GL_DEPTH_TEST ); for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { - i->second->DrawTrailFootprint(); + i->second->DrawTrailFootprint(); } glEnable( GL_DEPTH_TEST ); } + + if( showBlocks ) + DrawBlocks(); if( showTrailRise ) { for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { - i->second->DrawTrailBlocks(); + i->second->DrawTrailBlocks(); } } @@ -647,74 +657,76 @@ { glEnable( GL_DEPTH_TEST ); for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { - i->second->DrawTrailArrows(); + i->second->DrawTrailArrows(); } } - if( showBlocks ) - DrawBlocks(); + + for( GList* it=selected_models; it; it=it->next ) + ((StgModel*)it->data)->DrawSelected(); + // useful debug - puts a point at the origin of each model //for( GList* it = world->StgWorld::children; it; it=it->next ) // ((StgModel*)it->data)->DrawOriginTree(); // draw the model-specific visualizations - if( showData ) { - if ( visualizeAll ) { - for( GList* it = world->StgWorld::children; it; it=it->next ) - ((StgModel*)it->data)->DataVisualizeTree( current_camera ); - } - else if ( selected_models ) { - for( GList* it = selected_models; it; it=it->next ) - ((StgModel*)it->data)->DataVisualizeTree( current_camera ); - } - else if ( last_selection ) { - last_selection->DataVisualizeTree( current_camera ); - } - } + if( showData ) { + if ( visualizeAll ) { + for( GList* it = world->StgWorld::children; it; it=it->next ) + ((StgModel*)it->data)->DataVisualizeTree( current_camera ); + } + else if ( selected_models ) { + for( GList* it = selected_models; it; it=it->next ) + ((StgModel*)it->data)->DataVisualizeTree( current_camera ); + } + else if ( last_selection ) { + last_selection->DataVisualizeTree( current_camera ); + } + } if( showGrid ) - for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { - i->second->DrawGrid(); - } + for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { + i->second->DrawGrid(); + } if( showFlags ) - for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { - i->second->DrawFlagList(); - } + for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { + i->second->DrawFlagList(); + } if( showBlinken ) - for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { + for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { i->second->DrawBlinkenlights(); - } + } - if ( showStatus ) { - glPushMatrix(); - for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { - //ensure two icons can't be in the exact same plane - if( camera.pitch() == 0 && !pCamOn ) - glTranslatef( 0, 0, 0.1 ); - i->second->DrawStatusTree( current_camera ); - } - glPopMatrix(); - } + if ( showStatus ) { + glPushMatrix(); + for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { + //ensure two icons can't be in the exact same plane + if( camera.pitch() == 0 && !pCamOn ) + glTranslatef( 0, 0, 0.1 ); + i->second->DrawStatusTree( current_camera ); + } + glPopMatrix(); + } if( world->GetRayList() ) { glDisable( GL_DEPTH_TEST ); PushColor( 0,0,0,0.5 ); for( GList* it = world->GetRayList(); it; it=it->next ) - { - float* pts = (float*)it->data; - glBegin( GL_LINES ); - glVertex2f( pts[0], pts[1] ); - glVertex2f( pts[2], pts[3] ); - glEnd(); - } + { + float* pts = (float*)it->data; + glBegin( GL_LINES ); + glVertex2f( pts[0], pts[1] ); + glVertex2f( pts[2], pts[3] ); + glEnd(); + } PopColor(); glEnable( GL_DEPTH_TEST ); @@ -738,7 +750,7 @@ std::string clockstr = world->ClockString(); if( showFollow == true && last_selection ) - clockstr.append( " [ FOLLOW MODE ]" ); + clockstr.append( " [ FOLLOW MODE ]" ); fl_font( FL_HELVETICA, 12 ); float txtWidth = gl_width( clockstr.c_str() ); @@ -819,11 +831,11 @@ //png_set_compression_level(pp, Z_DEFAULT_COMPRESSION); 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 ); @@ -837,23 +849,23 @@ void StgCanvas::perspectiveCb( Fl_Widget* w, void* p ) { - StgCanvas* canvas = static_cast<StgCanvas*>( w ); - Option* opt = static_cast<Option*>( p ); // pCamOn - if ( opt ) { - // Perspective mode is on, change camera - canvas->current_camera = &canvas->perspective_camera; - } - else { - canvas->current_camera = &canvas->camera; - } + StgCanvas* canvas = static_cast<StgCanvas*>( w ); + Option* opt = static_cast<Option*>( p ); // pCamOn + if ( opt ) { + // Perspective mode is on, change camera + canvas->current_camera = &canvas->perspective_camera; + } + else { + canvas->current_camera = &canvas->camera; + } - canvas->invalidate(); + canvas->invalidate(); } void StgCanvas::createMenuItems( Fl_Menu_Bar* menu, std::string path ) { showData.createMenuItem( menu, path ); -// visualizeAll.createMenuItem( menu, path ); + // visualizeAll.createMenuItem( menu, path ); showBlocks.createMenuItem( menu, path ); showFlags.createMenuItem( menu, path ); showClock.createMenuItem( menu, path ); @@ -935,10 +947,9 @@ { valid(1); FixViewport(w(), h()); - + // set gl state that won't change every redraw glClearColor ( 0.7, 0.7, 0.8, 1.0); - //glClearColor ( 1,1,1,1 ); glDisable( GL_LIGHTING ); glEnable( GL_DEPTH_TEST ); glDepthFunc( GL_LESS ); @@ -950,84 +961,88 @@ glHint( GL_LINE_SMOOTH_HINT, GL_FASTEST ); glDepthMask( GL_TRUE ); glEnable( GL_TEXTURE_2D ); - glEnableClientState( GL_VERTEX_ARRAY ); - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + glEnableClientState( GL_VERTEX_ARRAY ); + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); //TODO find a better home for loading textures - if( loaded_texture == false ) { - std::string fullpath = FileManager::findFile( "assets/stall.png" ); - if ( fullpath == "" ) { - PRINT_DEBUG( "Unable to load texture.\n" ); - } - - GLuint stall_id = TextureManager::getInstance().loadTexture( fullpath.c_str() ); - TextureManager::getInstance()._stall_texture_id = stall_id; - - //create floor texture - { - //TODO merge this code into the textureManager - int i, j; - for (i = 0; i < checkImageHeight; i++) - for (j = 0; j < checkImageWidth; j+... [truncated message content] |
From: <rt...@us...> - 2008-11-18 02:48:10
|
Revision: 7157 http://playerstage.svn.sourceforge.net/playerstage/?rev=7157&view=rev Author: rtv Date: 2008-11-18 02:48:05 +0000 (Tue, 18 Nov 2008) Log Message: ----------- small speedups and const correctness additions Modified Paths: -------------- code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/region.cc code/stage/trunk/libstage/stage.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/stage_internal.hh code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2008-11-17 23:06:46 UTC (rev 7156) +++ code/stage/trunk/libstage/model.cc 2008-11-18 02:48:05 UTC (rev 7157) @@ -361,11 +361,11 @@ } -stg_raytrace_result_t StgModel::Raytrace( stg_pose_t pose, - stg_meters_t range, - stg_ray_test_func_t func, +stg_raytrace_result_t StgModel::Raytrace( const stg_pose_t &pose, + const stg_meters_t range, + const stg_ray_test_func_t func, const void* arg, - bool ztest ) + const bool ztest ) { return world->Raytrace( LocalToGlobal(pose), range, @@ -375,11 +375,11 @@ ztest ); } -stg_raytrace_result_t StgModel::Raytrace( stg_radians_t bearing, - stg_meters_t range, - stg_ray_test_func_t func, +stg_raytrace_result_t StgModel::Raytrace( const stg_radians_t bearing, + const stg_meters_t range, + const stg_ray_test_func_t func, const void* arg, - bool ztest ) + const bool ztest ) { stg_pose_t raystart; bzero( &raystart, sizeof(raystart)); @@ -394,14 +394,14 @@ } -void StgModel::Raytrace( stg_radians_t bearing, - stg_meters_t range, - stg_radians_t fov, - stg_ray_test_func_t func, +void StgModel::Raytrace( const stg_radians_t bearing, + const stg_meters_t range, + const stg_radians_t fov, + const stg_ray_test_func_t func, const void* arg, stg_raytrace_result_t* samples, - uint32_t sample_count, - bool ztest ) + const uint32_t sample_count, + const bool ztest ) { stg_pose_t raystart; bzero( &raystart, sizeof(raystart)); @@ -550,22 +550,17 @@ // get the model's position in the global frame stg_pose_t StgModel::GetGlobalPose() { - //printf( "model %s global pose ", token ); - - stg_pose_t parent_pose; + // if I'm a top level model, my global pose is my local pose + if( parent == NULL ) + return pose; - // find my parent's pose - if( this->parent ) - { - parent_pose = parent->GetGlobalPose(); - stg_pose_sum( &global_pose, &parent_pose, &pose ); - - // we are on top of our parent - global_pose.z += parent->geom.size.z; - } - else - memcpy( &global_pose, &pose, sizeof(stg_pose_t)); + // otherwise + stg_pose_t global_pose = pose_sum( parent->GetGlobalPose(), pose ); + + // we are on top of our parent + global_pose.z += parent->geom.size.z; + // PRINT_DEBUG4( "GET GLOBAL POSE [x:%.2f y:%.2f z:%.2f a:%.2f]", // global_pose.x, // global_pose.y, @@ -579,27 +574,27 @@ // convert a pose in this model's local coordinates into global // coordinates // should one day do all this with affine transforms for neatness? -stg_pose_t StgModel::LocalToGlobal( stg_pose_t pose ) +inline stg_pose_t StgModel::LocalToGlobal( stg_pose_t pose ) { return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose ); } -stg_point3_t StgModel::LocalToGlobal( stg_point3_t point ) -{ - stg_pose_t pose; - pose.x = point.x; - pose.y = point.y; - pose.z = point.z; - pose.a = 0; +// stg_point3_t StgModel::LocalToGlobal( stg_point3_t point ) +// { +// stg_pose_t pose; +// pose.x = point.x; +// pose.y = point.y; +// pose.z = point.z; +// pose.a = 0; - pose = LocalToGlobal( pose ); +// pose = LocalToGlobal( pose ); - point.x = pose.x; - point.y = pose.y; - point.z = pose.z; +// point.x = pose.x; +// point.y = pose.y; +// point.z = pose.z; - return point; -} +// return point; +// } void StgModel::MapWithChildren() { @@ -623,28 +618,28 @@ // given an input point array in model local coordinates, return // an array with the same points in global coordinates. caller must // delete[] the points. -stg_point_t* StgModel::LocalToGlobal( double scalex, - double scaley, - stg_point_t pts[], - uint32_t pt_count ) -{ - stg_point_t* glob = new stg_point_t[pt_count]; +// stg_point_t* StgModel::LocalToGlobal( double scalex, +// double scaley, +// stg_point_t pts[], +// uint32_t pt_count ) +// { +// stg_point_t* glob = new stg_point_t[pt_count]; - stg_pose_t global_pose = GetGlobalPose(); +// stg_pose_t global_pose = GetGlobalPose(); - for( int p=0; p<pt_count; p++ ) - { - stg_pose_t local( pts[p].x * scalex, - pts[p].y * scaley, - 0, 0 ); - stg_pose_t global = pose_sum( global_pose, local ); +// for( int p=0; p<pt_count; p++ ) +// { +// stg_pose_t local( pts[p].x * scalex, +// pts[p].y * scaley, +// 0, 0 ); +// stg_pose_t global = pose_sum( global_pose, local ); - glob[p].x = global.x; - glob[p].y = global.y; - } +// glob[p].x = global.x; +// glob[p].y = global.y; +// } - return glob; -} +// return glob; +// } void StgModel::Map() Modified: code/stage/trunk/libstage/region.cc =================================================================== --- code/stage/trunk/libstage/region.cc 2008-11-17 23:06:46 UTC (rev 7156) +++ code/stage/trunk/libstage/region.cc 2008-11-18 02:48:05 UTC (rev 7157) @@ -12,33 +12,7 @@ const uint32_t SuperRegion::WIDTH = SUPERREGIONWIDTH; const uint32_t SuperRegion::SIZE = SUPERREGIONSIZE; -//inline -void Cell::RemoveBlock( StgBlock* b ) -{ - // linear time removal, but these lists should be very short. - list = g_slist_remove( list, b ); - - assert( region ); - region->DecrementOccupancy(); -} -void Cell::AddBlock( StgBlock* b ) -{ - // constant time prepend - list = g_slist_prepend( list, b ); - - region->IncrementOccupancy(); - b->RecordRendering( this ); -} - -void Cell::AddBlockNoRecord( StgBlock* b ) -{ - list = g_slist_prepend( list, b ); - - // don't add this cell to the block - we assume it's already there -} - - Region::Region() : count(0) { @@ -51,30 +25,7 @@ delete[] cells; } -inline void Region::DecrementOccupancy() -{ - assert( superregion ); - superregion->DecrementOccupancy(); - --count; -} -inline void Region::IncrementOccupancy() -{ - assert( superregion ); - superregion->IncrementOccupancy(); - ++count; -} - -Cell* Region::GetCell( int32_t x, int32_t y ) -{ - uint32_t index = x + (y*Region::WIDTH); - assert( x < Region::WIDTH ); - assert( index >=0 ); - assert( index < Region::SIZE ); - return &cells[index]; -} - - SuperRegion::SuperRegion( int32_t x, int32_t y ) : count(0) { @@ -91,25 +42,6 @@ // nothing to do } -// get the region x,y from the region array -Region* SuperRegion::GetRegion( int32_t x, int32_t y ) -{ - int32_t index = x + (y*SuperRegion::WIDTH); - assert( index >=0 ); - assert( index < (int)SuperRegion::SIZE ); - return ®ions[ index ]; -} - -inline void SuperRegion::DecrementOccupancy() -{ - --count; -} - -inline void SuperRegion::IncrementOccupancy() -{ - ++count; -} - void SuperRegion::Draw( bool drawall ) { glEnable( GL_DEPTH_TEST ); Modified: code/stage/trunk/libstage/stage.cc =================================================================== --- code/stage/trunk/libstage/stage.cc 2008-11-17 23:06:46 UTC (rev 7156) +++ code/stage/trunk/libstage/stage.cc 2008-11-18 02:48:05 UTC (rev 7157) @@ -197,22 +197,9 @@ } } -// sets [result] to the pose of [p2] in [p1]'s coordinate system -void Stg::stg_pose_sum( stg_pose_t* result, stg_pose_t* p1, stg_pose_t* p2 ) -{ - double cosa = cos(p1->a); - double sina = sin(p1->a); - 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); - - // printf( "pose z %.2f\n", result->z ); -} - -// returns the resultant of vector [p1] and [p2] -stg_pose_t Stg::pose_sum( stg_pose_t p1, stg_pose_t p2 ) +// returns the pose of p2 in p1's coordinate system +stg_pose_t Stg::pose_sum( const stg_pose_t& p1, const stg_pose_t& p2 ) { double cosa = cos(p1.a); double sina = sin(p1.a); @@ -227,12 +214,15 @@ } // returns the resultant of vector [p1] and [p2] -stg_pose_t Stg::pose_scale( stg_pose_t p1, double sx, double sy, double sz ) +stg_pose_t Stg::pose_scale( const stg_pose_t& p1, const double sx, const double sy, const double sz ) { - p1.x *= sx; - p1.y *= sy; - p1.z *= sz; - return p1; + stg_pose_t scaled; + scaled.x = p1.x * sx; + scaled.y = p1.y * sy; + scaled.z = p1.z * sz; + scaled.a = p1.a; + + return scaled; } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-11-17 23:06:46 UTC (rev 7156) +++ code/stage/trunk/libstage/stage.hh 2008-11-18 02:48:05 UTC (rev 7157) @@ -545,15 +545,11 @@ */ stg_color_t stg_lookup_color(const char *name); - /** calculate the sum of [p1] and [p2], in [p1]'s coordinate system, and - copy the result into result. */ - void stg_pose_sum( stg_pose_t* result, stg_pose_t* p1, stg_pose_t* p2 ); - /** returns the sum of [p1] + [p2], in [p1]'s coordinate system */ - stg_pose_t pose_sum( stg_pose_t p1, stg_pose_t p2 ); + stg_pose_t pose_sum( const stg_pose_t& p1, const stg_pose_t& p2 ); /** returns a new pose, with each axis scaled */ - stg_pose_t pose_scale( stg_pose_t p1, double x, double y, double z ); + stg_pose_t pose_scale( const stg_pose_t& p1, const double x, const double y, const double z ); // PRETTY PRINTING ------------------------------------------------- @@ -929,22 +925,22 @@ SuperRegion* CreateSuperRegion( int32_t x, int32_t y ); void DestroySuperRegion( SuperRegion* sr ); - stg_raytrace_result_t Raytrace( stg_pose_t pose, - stg_meters_t range, - stg_ray_test_func_t func, - StgModel* finder, + stg_raytrace_result_t Raytrace( const stg_pose_t& pose, + const stg_meters_t range, + const stg_ray_test_func_t func, + const StgModel* finder, const void* arg, - bool ztest ); + const bool ztest ); - void Raytrace( stg_pose_t pose, - stg_meters_t range, - stg_radians_t fov, - stg_ray_test_func_t func, - StgModel* finder, + void Raytrace( const stg_pose_t &pose, + const stg_meters_t range, + const stg_radians_t fov, + const stg_ray_test_func_t func, + const StgModel* finder, const void* arg, stg_raytrace_result_t* samples, - uint32_t sample_count, - bool ztest ); + const uint32_t sample_count, + const bool ztest ); protected: @@ -1057,9 +1053,9 @@ /** Return the number of times the world has been updated. */ long unsigned int GetUpdateCount() { return updates; } - stg_point_t* LocalToGlobal( double scalex, double scaley, - stg_point_t pts[], - uint32_t pt_count ); +// stg_point_t* LocalToGlobal( double scalex, double scaley, +// stg_point_t pts[], +// uint32_t pt_count ); }; class StgBlock @@ -1333,37 +1329,37 @@ /** raytraces a single ray from the point and heading identified by pose, in local coords */ - stg_raytrace_result_t Raytrace( stg_pose_t pose, - stg_meters_t range, - stg_ray_test_func_t func, + stg_raytrace_result_t Raytrace( const stg_pose_t &pose, + const stg_meters_t range, + const stg_ray_test_func_t func, const void* arg, - bool ztest = true ); + const bool ztest = true ); /** raytraces multiple rays around the point and heading identified by pose, in local coords */ - void Raytrace( stg_pose_t pose, - stg_meters_t range, - stg_radians_t fov, - stg_ray_test_func_t func, + void Raytrace( const stg_pose_t &pose, + const stg_meters_t range, + const stg_radians_t fov, + const stg_ray_test_func_t func, const void* arg, stg_raytrace_result_t* samples, - uint32_t sample_count, - bool ztest = true ); + const uint32_t sample_count, + const bool ztest = true ); - stg_raytrace_result_t Raytrace( stg_radians_t bearing, - stg_meters_t range, - stg_ray_test_func_t func, + stg_raytrace_result_t Raytrace( const stg_radians_t bearing, + const stg_meters_t range, + const stg_ray_test_func_t func, const void* arg, - bool ztest = true ); + const bool ztest = true ); - void Raytrace( stg_radians_t bearing, - stg_meters_t range, - stg_radians_t fov, - stg_ray_test_func_t func, + void Raytrace( const stg_radians_t bearing, + const stg_meters_t range, + const stg_radians_t fov, + const stg_ray_test_func_t func, const void* arg, stg_raytrace_result_t* samples, - uint32_t sample_count, - bool ztest = true ); + const uint32_t sample_count, + const bool ztest = true ); /** Causes this model and its children to recompute their global @@ -1430,10 +1426,10 @@ virtual void PopColor(){ world->PopColor(); } void DrawFlagList(); - stg_point_t* LocalToGlobal( double scalex, - double scaley, - stg_point_t pts[], - uint32_t pt_count ); +// stg_point_t* LocalToGlobal( double scalex, +// double scaley, +// stg_point_t pts[], +// uint32_t pt_count ); void DrawPose( stg_pose_t pose ); Modified: code/stage/trunk/libstage/stage_internal.hh =================================================================== --- code/stage/trunk/libstage/stage_internal.hh 2008-11-17 23:06:46 UTC (rev 7156) +++ code/stage/trunk/libstage/stage_internal.hh 2008-11-18 02:48:05 UTC (rev 7157) @@ -135,12 +135,12 @@ { colorstack.Push( r,g,b,a ); } void PopColor(){ colorstack.Pop(); } - - void InvertView( uint32_t invertflags ); - - static void TimerCallback( StgCanvas* canvas ); - static void perspectiveCb( Fl_Widget* w, void* p ); + void InvertView( uint32_t invertflags ); + + static void TimerCallback( StgCanvas* canvas ); + static void perspectiveCb( Fl_Widget* w, void* p ); + void Load( Worldfile* wf, int section ); void Save( Worldfile* wf, int section ); }; @@ -152,7 +152,7 @@ friend class SuperRegion; friend class StgWorld; friend class StgBlock; - + private: Region* region; GSList* list; @@ -161,10 +161,10 @@ : region( NULL), list(NULL) { /* do nothing */ } - - void RemoveBlock( StgBlock* b ); - void AddBlock( StgBlock* b ); - void AddBlockNoRecord( StgBlock* b ); + + inline void RemoveBlock( StgBlock* b ); + inline void AddBlock( StgBlock* b ); + inline void AddBlockNoRecord( StgBlock* b ); }; // a bit of experimenting suggests that these values are fast. YMMV. @@ -188,14 +188,18 @@ Cell cells[REGIONSIZE]; SuperRegion* superregion; - + public: unsigned long count; // number of blocks rendered into these cells Region(); ~Region(); - Cell* GetCell( int32_t x, int32_t y ); + Cell* GetCell( int32_t x, int32_t y ) + { + return( &cells[x + (y*Region::WIDTH)] ); + }; + void DecrementOccupancy(); void IncrementOccupancy(); }; @@ -220,14 +224,58 @@ SuperRegion( int32_t x, int32_t y ); ~SuperRegion(); - Region* GetRegion( int32_t x, int32_t y ); + Region* GetRegion( int32_t x, int32_t y ) + { + return( ®ions[ x + (y*SuperRegion::WIDTH) ] ); + }; void Draw( bool drawall ); void Floor(); - void DecrementOccupancy(); - void IncrementOccupancy(); + + void DecrementOccupancy(){ --count; }; + void IncrementOccupancy(){ ++count; }; }; + +// INLINE METHOD DEFITIONS + +inline void Region::DecrementOccupancy() +{ + assert( superregion ); + superregion->DecrementOccupancy(); + --count; +} + +inline void Region::IncrementOccupancy() +{ + assert( superregion ); + superregion->IncrementOccupancy(); + ++count; +} + +inline void Cell::RemoveBlock( StgBlock* b ) +{ + // linear time removal, but these lists should be very short. + list = g_slist_remove( list, b ); + region->DecrementOccupancy(); +} + +inline void Cell::AddBlock( StgBlock* b ) +{ + // constant time prepend + list = g_slist_prepend( list, b ); + region->IncrementOccupancy(); + b->RecordRendering( this ); +} + +inline void Cell::AddBlockNoRecord( StgBlock* b ) +{ + list = g_slist_prepend( list, b ); + // don't add this cell to the block - we assume it's already there +} + }; // namespace Stg + + #endif // STG_INTERNAL_H Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2008-11-17 23:06:46 UTC (rev 7156) +++ code/stage/trunk/libstage/world.cc 2008-11-18 02:48:05 UTC (rev 7157) @@ -143,10 +143,10 @@ PRINT_WARN( "Stg::Init() must be called before a StgWorld is created." ); exit(-1); } + + bzero( &extent, sizeof(extent) ); StgWorld::world_list = g_list_append( StgWorld::world_list, this ); - - bzero( &this->extent, sizeof(this->extent)); } @@ -483,24 +483,28 @@ } -void StgWorld::Raytrace( stg_pose_t pose, // global pose - stg_meters_t range, - stg_radians_t fov, - stg_ray_test_func_t func, - StgModel* model, - const void* arg, - stg_raytrace_result_t* samples, // preallocated storage for samples - uint32_t sample_count, - bool ztest ) // number of samples +void StgWorld::Raytrace( const stg_pose_t &pose, // global pose + const stg_meters_t range, + const stg_radians_t fov, + const stg_ray_test_func_t func, + const StgModel* model, + const void* arg, + stg_raytrace_result_t* samples, // preallocated storage for samples + const uint32_t sample_count, + const bool ztest ) // number of samples { - pose.a -= fov/2.0; // direction of first ray - stg_radians_t angle_incr = fov/(double)sample_count; - - for( uint32_t s=0; s < sample_count; s++ ) - { - samples[s] = Raytrace( pose, range, func, model, arg, ztest ); - pose.a += angle_incr; - } + // find the direction of the first ray + stg_pose_t raypose = pose; + raypose.a -= fov/2.0; + + // increment the ray direction by this much for each sample + stg_radians_t angle_incr = fov/(double)sample_count; + + for( uint32_t s=0; s < sample_count; s++ ) + { + samples[s] = Raytrace( raypose, range, func, model, arg, ztest ); + raypose.a += angle_incr; + } } @@ -548,12 +552,12 @@ return c; } -stg_raytrace_result_t StgWorld::Raytrace( stg_pose_t gpose, - stg_meters_t range, - stg_ray_test_func_t func, - StgModel* mod, +stg_raytrace_result_t StgWorld::Raytrace( const stg_pose_t &gpose, + const stg_meters_t range, + const stg_ray_test_func_t func, + const StgModel* mod, const void* arg, - bool ztest ) + const bool ztest ) { stg_raytrace_result_t sample; @@ -603,14 +607,9 @@ // x,y, dx,dy, n ); // superregion coords - stg_point_int_t lastsup; - lastsup.x = INT_MAX; // an unlikely first raytrace - lastsup.y = INT_MAX; + stg_point_int_t lastsup = {INT_MAX, INT_MAX }; + stg_point_int_t lastreg = {INT_MAX, INT_MAX }; - stg_point_int_t lastreg; - lastreg.x = INT_MAX; // an unlikely first raytrace - lastreg.y = INT_MAX; - SuperRegion* sr = NULL; Region* r = NULL; bool nonempty_region = false; @@ -637,8 +636,7 @@ sr = GetSuperRegionCached( sup ); // possibly NULL, but unlikely while ( n-- ) - { - + { //printf( "pixel [%d %d]\tS[ %d %d ]\t", // x, y, sup.x, sup.y ); @@ -690,7 +688,7 @@ // mod, mod->Token(), ent->mod, ent->mod->Token(), x, y ); // test the predicate we were passed - if( (*func)( block->mod, mod, arg )) // TODO + if( (*func)( block->mod, (StgModel*)mod, arg )) // TODO { // 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...> - 2008-11-19 01:24:43
|
Revision: 7158 http://playerstage.svn.sourceforge.net/playerstage/?rev=7158&view=rev Author: rtv Date: 2008-11-19 01:24:40 +0000 (Wed, 19 Nov 2008) Log Message: ----------- added persistent sorted list for in-order rendering Modified Paths: -------------- code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/stage_internal.hh code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-11-18 02:48:05 UTC (rev 7157) +++ code/stage/trunk/libstage/canvas.cc 2008-11-19 01:24:40 UTC (rev 7158) @@ -1,8 +1,11 @@ /** canvas.cc Implement the main world viewing area in FLTK and OpenGL. - Authors: Richard Vaughan (va...@sf...) - Alex Couture-Beil (as...@sf...) - Jeremy Asher (jr...@sf...) + + Authors: + Richard Vaughan (va...@sf...) + Alex Couture-Beil (as...@sf...) + Jeremy Asher (jr...@sf...) + $Id$ */ @@ -11,7 +14,7 @@ #include "replace.h" #include <string> -#include <map> +//#include <map> #include <sstream> #include <png.h> #include <GLUT/glut.h> @@ -85,21 +88,11 @@ assert( can_do( FL_ACCUM ) ); graphics = true; - -// // // start the timer that causes regular redraws -// Fl::add_timeout( ((double)interval/1000), -// (Fl_Timeout_Handler)StgCanvas::TimerCallback, -// this); - - - GLenum status; - - - } StgCanvas::~StgCanvas() -{ +{ + // nothing to do } StgModel* StgCanvas::getModel( int x, int y ) @@ -469,6 +462,7 @@ 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 } @@ -493,9 +487,11 @@ glViewport(0,0,W,H); } +void StgCanvas::AddModel( StgModel* mod ) +{ + models_sorted = g_list_append( models_sorted, mod ); +} - - void StgCanvas::DrawGlobalGrid() { stg_bounds3d_t bounds = world->GetExtent(); @@ -564,10 +560,10 @@ void StgCanvas::DrawBlocks() { - LISTMETHOD( world->StgWorld::children, StgModel*, DrawBlocksTree ); + LISTMETHOD( models_sorted, StgModel*, DrawBlocksTree ); } -void StgCanvas::resetCamera() +inline void StgCanvas::resetCamera() { float max_x = 0, max_y = 0, min_x = 0, min_y = 0; @@ -599,6 +595,27 @@ //TODO reset perspective cam } +// used to sort a list of models by inverse distance from the x,y pose in [coords] +gint compare_distance( StgModel* a, StgModel* b, double coords[2] ) +{ + stg_pose_t a_pose = a->GetGlobalPose(); + stg_pose_t b_pose = b->GetGlobalPose(); + + double a_dist = hypot( coords[1] - a_pose.y, + coords[0] - a_pose.x ); + + double b_dist = hypot( coords[1] - b_pose.y, + coords[0] - b_pose.x ); + + if( a_dist < b_dist ) + return 1; + + if( a_dist > b_dist ) + return -1; + + return 0; // must be the same +} + void StgCanvas::renderFrame() { //before drawing, order all models based on distance from camera @@ -610,23 +627,15 @@ x += -sin( sphi ) * 100; y += -cos( sphi ) * 100; - // TODO - keep this map around in between frames, because the order - // changes slowly, and sorting an already-sorted list is - // usually very fast (rtv) - - //store all models in a sorted multimap - std::multimap< float, StgModel* > ordered; - for( GList* it=world->StgWorld::children; it; it=it->next ) { - StgModel* ptr = (StgModel*) it->data; - stg_pose_t pose = ptr->GetPose(); - - float dist = sqrt( ( x - pose.x ) * ( x - pose.x ) + ( y - pose.y ) * ( y - pose.y ) ); - ordered.insert( std::pair< float, StgModel* >( dist, (StgModel*)it->data ) ); - } - - //now the models can be iterated over with: - // for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) - + double coords[2]; + coords[0] = x; + coords[1] = y; + + // sort the list of models by inverse distance from the camera - + // probably doesn't change too much between frames so this is + // usually fast + models_sorted = g_list_sort_with_data( models_sorted, (GCompareDataFunc)compare_distance, coords ); + glEnable( GL_DEPTH_TEST ); if( ! showTrails ) @@ -635,7 +644,7 @@ 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. @@ -648,27 +657,22 @@ glPopMatrix(); } - if( showFootprints ) - { - glDisable( GL_DEPTH_TEST ); - - for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { - i->second->DrawTrailFootprint(); - } - glEnable( GL_DEPTH_TEST ); - } + { + glDisable( GL_DEPTH_TEST ); + LISTMETHOD( models_sorted, StgModel*, DrawTrailFootprint ); + glEnable( GL_DEPTH_TEST ); + } if( showGrid ) DrawGlobalGrid(); else DrawFloor(); - + if( showBlocks ) - DrawBlocks(); + DrawBlocks(); - // MOTION BLUR // if( showBlocks ) // { @@ -748,21 +752,21 @@ // } // } - if( showTrailRise ) - { - for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { - i->second->DrawTrailBlocks(); - } - } +// if( showTrailRise ) +// { +// for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { +// i->second->DrawTrailBlocks(); +// } +// } - if( showTrailArrows ) - { - glEnable( GL_DEPTH_TEST ); - for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { - i->second->DrawTrailArrows(); - } +// if( showTrailArrows ) +// { +// glEnable( GL_DEPTH_TEST ); +// for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { +// i->second->DrawTrailArrows(); +// } - } +// } for( GList* it=selected_models; it; it=it->next ) @@ -789,34 +793,29 @@ } if( showGrid ) - for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { - i->second->DrawGrid(); - } - - + LISTMETHOD( models_sorted, StgModel*, DrawGrid ); + if( showFlags ) - for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { - i->second->DrawFlagList(); - } - - + LISTMETHOD( models_sorted, StgModel*, DrawFlagList ); + if( showBlinken ) - for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { - i->second->DrawBlinkenlights(); - } - - - if ( showStatus ) { - glPushMatrix(); - for( std::multimap< float, StgModel* >::reverse_iterator i = ordered.rbegin(); i != ordered.rend(); i++ ) { + LISTMETHOD( models_sorted, StgModel*, DrawBlinkenlights ); + + if( showStatus ) + { + glDisable( GL_DEPTH_TEST ); + + glPushMatrix(); //ensure two icons can't be in the exact same plane if( camera.pitch() == 0 && !pCamOn ) glTranslatef( 0, 0, 0.1 ); - i->second->DrawStatusTree( current_camera ); - } - glPopMatrix(); - } + LISTMETHODARG( models_sorted, StgModel*, DrawStatusTree, &camera ); + + glEnable( GL_DEPTH_TEST ); + glPopMatrix(); + } + if( world->GetRayList() ) { glDisable( GL_DEPTH_TEST ); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-11-18 02:48:05 UTC (rev 7157) +++ code/stage/trunk/libstage/stage.hh 2008-11-19 01:24:40 UTC (rev 7158) @@ -1899,6 +1899,8 @@ // Quit time pause bool pause_time; + virtual void AddModel( StgModel* mod ); + protected: virtual void PushColor( stg_color_t col ); virtual void PushColor( double r, double g, double b, double a ); Modified: code/stage/trunk/libstage/stage_internal.hh =================================================================== --- code/stage/trunk/libstage/stage_internal.hh 2008-11-18 02:48:05 UTC (rev 7157) +++ code/stage/trunk/libstage/stage_internal.hh 2008-11-19 01:24:40 UTC (rev 7158) @@ -54,6 +54,8 @@ private: GlColorStack colorstack; + GList* models_sorted; + Camera* current_camera; OrthoCamera camera; PerspectiveCamera perspective_camera; @@ -77,6 +79,8 @@ void ClearRays(); void DrawGlobalGrid(); + void AddModel( StgModel* mod ); + Option showBlinken, showBlocks, Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-11-18 02:48:05 UTC (rev 7157) +++ code/stage/trunk/libstage/worldgui.cc 2008-11-19 01:24:40 UTC (rev 7158) @@ -330,7 +330,15 @@ return val; } +void StgWorldGui::AddModel( StgModel* mod ) +{ + if( mod->parent == NULL ) + canvas->AddModel( mod ); + + StgWorld::AddModel( mod ); +} + std::string StgWorldGui::ClockString() { const uint32_t usec_per_hour = 3600000000U; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2008-11-20 00:52:35
|
Revision: 7161 http://playerstage.svn.sourceforge.net/playerstage/?rev=7161&view=rev Author: rtv Date: 2008-11-20 00:52:29 +0000 (Thu, 20 Nov 2008) Log Message: ----------- comments and todos Modified Paths: -------------- code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/model_load.cc =================================================================== --- code/stage/trunk/libstage/model_load.cc 2008-11-19 20:30:37 UTC (rev 7160) +++ code/stage/trunk/libstage/model_load.cc 2008-11-20 00:52:29 UTC (rev 7161) @@ -267,6 +267,7 @@ } +// todo - use GLib's portable module code here void StgModel::LoadControllerModule( char* lib ) { //printf( "[Ctrl \"%s\"", lib ); Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2008-11-19 20:30:37 UTC (rev 7160) +++ code/stage/trunk/libstage/world.cc 2008-11-20 00:52:29 UTC (rev 7161) @@ -372,6 +372,7 @@ stg_usec_t StgWorld::RealTimeNow() { + // TODO - move to GLib's portable timing code, so we can port to Windows more easily? 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...> - 2008-11-25 22:47:11
|
Revision: 7170 http://playerstage.svn.sourceforge.net/playerstage/?rev=7170&view=rev Author: rtv Date: 2008-11-25 22:47:00 +0000 (Tue, 25 Nov 2008) Log Message: ----------- fixed header file bug Modified Paths: -------------- code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/stage.hh Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2008-11-25 19:18:23 UTC (rev 7169) +++ code/stage/trunk/libstage/canvas.cc 2008-11-25 22:47:00 UTC (rev 7170) @@ -17,7 +17,6 @@ //#include <map> #include <sstream> #include <png.h> -#include <GLUT/glut.h> #include "file_manager.hh" #include "options_dlg.hh" Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-11-25 19:18:23 UTC (rev 7169) +++ code/stage/trunk/libstage/stage.hh 2008-11-25 22:47:00 UTC (rev 7170) @@ -61,8 +61,10 @@ // except GLU for some reason #ifdef __APPLE__ #include <OpenGL/glu.h> +#include <GLUT/glut.h> #else #include <GL/glu.h> +#include <GL/glut.h> #endif #include "option.hh" This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2008-12-06 21:25:26
|
Revision: 7190 http://playerstage.svn.sourceforge.net/playerstage/?rev=7190&view=rev Author: rtv Date: 2008-12-06 21:25:20 +0000 (Sat, 06 Dec 2008) Log Message: ----------- on-demand cell allocation in a Region gives drastic memory use improvement for minor CPU overhead. WARNING - mysterious lock-up bug exists (not related to this commit) Modified Paths: -------------- code/stage/trunk/libstage/region.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/stage_internal.hh code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/region.cc =================================================================== --- code/stage/trunk/libstage/region.cc 2008-12-05 22:31:14 UTC (rev 7189) +++ code/stage/trunk/libstage/region.cc 2008-12-06 21:25:20 UTC (rev 7190) @@ -14,24 +14,25 @@ Region::Region() - : count(0) + : count(0), cells(NULL) { - for( unsigned int i=0; i<Region::SIZE; i++ ) - cells[i].region = this; + //for( unsigned int i=0; i<Region::SIZE; i++ ) + //cells[i].region = this; } Region::~Region() { - delete[] cells; + if(cells) + delete[] cells; } -SuperRegion::SuperRegion( int32_t x, int32_t y ) - : count(0) +SuperRegion::SuperRegion( StgWorld* world, stg_point_int_t origin ) + : count(0), origin(origin), world(world) { - origin.x = x; - origin.y = y; - + //static int srcount=0; + //printf( "created SR number %d\n", ++srcount ); + // initialize the parent pointer for all my child regions for( unsigned int i=0; i<SuperRegion::SIZE; i++ ) regions[i].superregion = this; @@ -39,7 +40,7 @@ SuperRegion::~SuperRegion() { - // nothing to do + //printf( "deleting SR %p at [%d,%d]\n", this, origin.x, origin.y ); } void SuperRegion::Draw( bool drawall ) @@ -55,13 +56,20 @@ glColor3f( 0,1,0 ); for( unsigned int x=0; x<SuperRegion::WIDTH; x++ ) for( unsigned int y=0; y<SuperRegion::WIDTH; y++ ) - glRecti( x<<RBITS, y<<RBITS, - (x+1)<<RBITS, (y+1)<<RBITS ); - + glRecti( x<<RBITS, y<<RBITS, + (x+1)<<RBITS, (y+1)<<RBITS ); + // outline superregion - glColor3f( 0,0,1 ); + glColor3f( 0,0,1 ); + glRecti( 0,0, 1<<SRBITS, 1<<SRBITS ); + char buf[32]; + snprintf( buf, 15, "%lu", count ); + gl_draw_string( 1<<SBITS, 1<<SBITS, 0, buf ); + + glColor3f( 1.0,0,0 ); + for( unsigned int x=0; x<SuperRegion::WIDTH; x++ ) for( unsigned int y=0; y<SuperRegion::WIDTH; y++ ) { @@ -70,7 +78,6 @@ if( r->count < 1 ) continue; - char buf[16]; snprintf( buf, 15, "%lu", r->count ); gl_draw_string( x<<RBITS, y<<RBITS, 0, buf ); @@ -84,7 +91,6 @@ if( ! drawall ) // draw a rectangle on the floor { - glColor3f( 1.0,0,0 ); glRecti( xx, yy, xx+1, yy+1); } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2008-12-05 22:31:14 UTC (rev 7189) +++ code/stage/trunk/libstage/stage.hh 2008-12-06 21:25:20 UTC (rev 7190) @@ -170,10 +170,11 @@ */ inline double normalize( double a ) { - //optimized version of return( atan2(sin(a), cos(a))); - while( a < -M_PI ) a += 2.0 * M_PI; - while( a > M_PI ) a -= 2.0 * M_PI; - return a; + return( atan2(sin(a), cos(a))); + // faster than return( atan2(sin(a), cos(a))); + //while( a < -M_PI ) a += 2.0 * M_PI; + //while( a > M_PI ) a -= 2.0 * M_PI; + //return a; } @@ -927,7 +928,8 @@ SuperRegion* AddSuperRegion( const stg_point_int_t& coord ); SuperRegion* GetSuperRegion( const stg_point_int_t& coord ); SuperRegion* GetSuperRegionCached( const stg_point_int_t& coord); - + void ExpireSuperRegion( SuperRegion* sr ); + inline Cell* GetCell( const int32_t x, const int32_t y ); void ForEachCellInPolygonGLfloat( const GLfloat pts[], @@ -960,7 +962,7 @@ virtual void PushColor( double r, double g, double b, double a ) { /* do nothing */ }; virtual void PopColor(){ /* do nothing */ }; - SuperRegion* CreateSuperRegion( int32_t x, int32_t y ); + SuperRegion* CreateSuperRegion( stg_point_int_t origin ); void DestroySuperRegion( SuperRegion* sr ); stg_raytrace_result_t Raytrace( const stg_pose_t& pose, Modified: code/stage/trunk/libstage/stage_internal.hh =================================================================== --- code/stage/trunk/libstage/stage_internal.hh 2008-12-05 22:31:14 UTC (rev 7189) +++ code/stage/trunk/libstage/stage_internal.hh 2008-12-06 21:25:20 UTC (rev 7190) @@ -192,7 +192,8 @@ static const uint32_t WIDTH; static const uint32_t SIZE; - Cell cells[REGIONSIZE]; + //Cell cells[REGIONSIZE]; + Cell* cells; SuperRegion* superregion; public: @@ -203,7 +204,14 @@ Cell* GetCell( int32_t x, int32_t y ) { - return( &cells[x + (y*Region::WIDTH)] ); + if( ! cells ) + { + cells = new Cell[REGIONSIZE]; + for( unsigned int i=0; i<Region::SIZE; i++ ) + cells[i].region = this; + } + return( &cells[x + (y*Region::WIDTH)] ); + }; void DecrementOccupancy(); @@ -224,10 +232,11 @@ unsigned long count; // number of blocks rendered into these regions stg_point_int_t origin; - + StgWorld* world; + public: - SuperRegion( int32_t x, int32_t y ); + SuperRegion( StgWorld* world, stg_point_int_t origin ); ~SuperRegion(); Region* GetRegion( int32_t x, int32_t y ) @@ -237,8 +246,8 @@ void Draw( bool drawall ); void Floor(); - - void DecrementOccupancy(){ --count; }; + + void DecrementOccupancy(){ --count; }; void IncrementOccupancy(){ ++count; }; }; @@ -250,7 +259,7 @@ assert( superregion ); superregion->DecrementOccupancy(); --count; -} +}; inline void Region::IncrementOccupancy() { Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2008-12-05 22:31:14 UTC (rev 7189) +++ code/stage/trunk/libstage/world.cc 2008-12-06 21:25:20 UTC (rev 7190) @@ -121,9 +121,9 @@ } -SuperRegion* StgWorld::CreateSuperRegion( int32_t x, int32_t y ) +SuperRegion* StgWorld::CreateSuperRegion( stg_point_int_t origin ) { - SuperRegion* sr = new SuperRegion( x, y ); + SuperRegion* sr = new SuperRegion( this, origin ); g_hash_table_insert( superregions, &sr->origin, sr ); dirty = true; // force redraw @@ -397,6 +397,7 @@ return( (quit_time > 0) && (sim_time >= quit_time) ); } + bool StgWorld::Update() { PRINT_DEBUG( "StgWorld::Update()" ); @@ -406,7 +407,7 @@ if( IsGUI() == false ) return true; } - + // upate all positions first LISTMETHOD( velocity_list, StgModel*, UpdatePose ); @@ -783,7 +784,7 @@ SuperRegion* StgWorld::AddSuperRegion( const stg_point_int_t& sup ) { //printf( "Creating super region [ %d %d ]\n", sup.x, sup.y ); - SuperRegion* sr = CreateSuperRegion( sup.x, sup.y ); + SuperRegion* sr = CreateSuperRegion( sup ); // the bounds of the world have changed stg_point3_t pt; @@ -800,6 +801,7 @@ return sr; } + inline SuperRegion* StgWorld::GetSuperRegionCached( const stg_point_int_t& sup ) { // around 99% of the time the SR is the same as last Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2008-12-05 22:31:14 UTC (rev 7189) +++ code/stage/trunk/libstage/worldgui.cc 2008-12-06 21:25:20 UTC (rev 7190) @@ -325,7 +325,7 @@ } } while( interval < interval_real ); - + //printf( "\r \t\t timenow %lu", timenow ); //printf( "interval_real %.20f\n", interval_real ); // if( paused ) // gentle on the CPU when paused This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <al...@us...> - 2009-01-15 23:15:19
|
Revision: 7274 http://playerstage.svn.sourceforge.net/playerstage/?rev=7274&view=rev Author: alexcb Date: 2009-01-15 23:15:17 +0000 (Thu, 15 Jan 2009) Log Message: ----------- added framework for user supplied custom visializations Modified Paths: -------------- code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model.hh Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-01-15 23:13:38 UTC (rev 7273) +++ code/stage/trunk/libstage/model.cc 2009-01-15 23:15:17 UTC (rev 7274) @@ -137,6 +137,7 @@ data_fresh(false), disabled(false), flag_list(NULL), + custom_visual_list( NULL ), geom(), has_default_block( true ), id( Model::count++ ), @@ -896,6 +897,19 @@ glPopMatrix(); } +void Model::AddCustomVisualizer( CustomVisualizer* custom_visual ) +{ + if( custom_visual ) + custom_visual_list = g_list_append(custom_visual_list, custom_visual ); +} + +void Model::RemoveCustomVisualizer( CustomVisualizer* custom_visual ) +{ + if( custom_visual ) + custom_visual_list = g_list_remove(custom_visual_list, custom_visual ); +} + + void Model::DrawStatusTree( Camera* cam ) { PushLocalCoords(); @@ -1143,6 +1157,11 @@ PushLocalCoords(); DataVisualize( cam ); // virtual function overridden by most model types + for( GList* item = custom_visual_list; item; item = item->next ) { + static_cast< CustomVisualizer* >( item->data )->DataVisualize( cam ); + } + + // and draw the children LISTMETHODARG( children, Model*, DataVisualizeTree, cam ); Modified: code/stage/trunk/libstage/model.hh =================================================================== --- code/stage/trunk/libstage/model.hh 2009-01-15 23:13:38 UTC (rev 7273) +++ code/stage/trunk/libstage/model.hh 2009-01-15 23:15:17 UTC (rev 7274) @@ -34,7 +34,15 @@ } }; +/** Abstract class for adding visualizations to models. DataVisualize must be overloaded, and is then called in the models local coord system */ +class CustomVisualizer { +public: + //TODO allow user to specify name - which will show up in display filter + virtual ~CustomVisualizer( void ) { } + virtual void DataVisualize( Camera* cam ) = 0; +}; + /* Hooks for attaching special callback functions (not used as variables - we just need unique addresses for them.) */ class CallbackHooks @@ -116,6 +124,7 @@ instead of adding a data callback. */ bool data_fresh; stg_bool_t disabled; //< if non-zero, the model is disabled + GList* custom_visual_list; GList* flag_list; Geom geom; Pose global_pose; @@ -343,6 +352,11 @@ virtual ~Model(); void Say( const char* str ); + /** Attach a user supplied visualization to a model */ + void AddCustomVisualizer( CustomVisualizer* custom_visual ); + /** remove user supplied visualization to a model - supply the same ptr passed to AddCustomVisualizer */ + void RemoveCustomVisualizer( CustomVisualizer* custom_visual ); + void Load( Worldfile* wf, int wf_entity ) { This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <al...@us...> - 2009-01-21 23:07:50
|
Revision: 7282 http://playerstage.svn.sourceforge.net/playerstage/?rev=7282&view=rev Author: alexcb Date: 2009-01-21 23:07:45 +0000 (Wed, 21 Jan 2009) Log Message: ----------- added data visualization filter support for custom visualization Modified Paths: -------------- code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model.hh code/stage/trunk/libstage/option.hh code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/stage_internal.hh Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-01-20 05:39:31 UTC (rev 7281) +++ code/stage/trunk/libstage/model.cc 2009-01-21 23:07:45 UTC (rev 7282) @@ -165,7 +165,8 @@ watts(0), wf(NULL), wf_entity(0), - world(world) + world(world), + world_gui( dynamic_cast< WorldGui* >( world ) ) { assert( modelsbyid ); assert( world ); @@ -926,14 +927,34 @@ void Model::AddCustomVisualizer( CustomVisualizer* custom_visual ) { - if( custom_visual ) - custom_visual_list = g_list_append(custom_visual_list, custom_visual ); + if( !custom_visual ) + return; + + //Visualizations can only be added to stage when run in a GUI + if( world_gui == NULL ) { + printf( "Unable to add custom visualization - it must be run with a GUI world\n" ); + return; + } + + //save visual instance + custom_visual_list = g_list_append(custom_visual_list, custom_visual ); + + //register option for all instances which share the same name + Canvas* canvas = world_gui->GetCanvas(); + std::map< std::string, Option* >::iterator i = canvas->_custom_options.find( custom_visual->name() ); + if( i == canvas->_custom_options.end() ) { + Option* op = new Option( custom_visual->name(), custom_visual->name(), "", true ); + canvas->_custom_options[ custom_visual->name() ] = op; + registerOption( op ); + } } void Model::RemoveCustomVisualizer( CustomVisualizer* custom_visual ) { if( custom_visual ) custom_visual_list = g_list_remove(custom_visual_list, custom_visual ); + + //TODO unregister option - tricky because there might still be instances attached to different models which have the same name } @@ -1218,8 +1239,11 @@ PushLocalCoords(); DataVisualize( cam ); // virtual function overridden by most model types + CustomVisualizer* vis; for( GList* item = custom_visual_list; item; item = item->next ) { - static_cast< CustomVisualizer* >( item->data )->DataVisualize( cam ); + vis = static_cast< CustomVisualizer* >( item->data ); + if( world_gui->GetCanvas()->_custom_options[ vis->name() ]->isEnabled() ) + vis->DataVisualize( cam ); } Modified: code/stage/trunk/libstage/model.hh =================================================================== --- code/stage/trunk/libstage/model.hh 2009-01-20 05:39:31 UTC (rev 7281) +++ code/stage/trunk/libstage/model.hh 2009-01-21 23:07:45 UTC (rev 7282) @@ -68,6 +68,7 @@ //TODO allow user to specify name - which will show up in display filter virtual ~CustomVisualizer( void ) { } virtual void DataVisualize( Camera* cam ) = 0; + virtual const std::string& name() = 0; //must return a name for visualization (careful not to return stack-memory) }; @@ -207,6 +208,7 @@ Worldfile* wf; int wf_entity; World* world; // pointer to the world in which this model exists + WorldGui* world_gui; //pointer to the GUI world - NULL if running in non-gui mode public: Modified: code/stage/trunk/libstage/option.hh =================================================================== --- code/stage/trunk/libstage/option.hh 2009-01-20 05:39:31 UTC (rev 7281) +++ code/stage/trunk/libstage/option.hh 2009-01-21 23:07:45 UTC (rev 7282) @@ -33,6 +33,7 @@ Option( std::string n, std::string tok, std::string key, bool v ); const std::string name() const { return optName; } + inline bool isEnabled() const { return value; } inline bool val() const { return value; } inline operator bool() { return val(); } inline bool operator<( const Option& rhs ) const Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-01-20 05:39:31 UTC (rev 7281) +++ code/stage/trunk/libstage/stage.hh 2009-01-21 23:07:45 UTC (rev 7282) @@ -1300,6 +1300,7 @@ { friend class Canvas; friend class ModelCamera; + friend class Model; private: Modified: code/stage/trunk/libstage/stage_internal.hh =================================================================== --- code/stage/trunk/libstage/stage_internal.hh 2009-01-20 05:39:31 UTC (rev 7281) +++ code/stage/trunk/libstage/stage_internal.hh 2009-01-21 23:07:45 UTC (rev 7282) @@ -8,6 +8,9 @@ #ifndef STG_INTERNAL_H #define STG_INTERNAL_H +#include <map> +#include <string> + // external definitions for libstage users #include "stage.hh" @@ -100,6 +103,8 @@ showBlur, pCamOn, visualizeAll; + + std::map< std::string, Option* > _custom_options; public: Canvas( WorldGui* world, int x, int y, int width, int height); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <al...@us...> - 2009-01-21 23:57:06
|
Revision: 7283 http://playerstage.svn.sourceforge.net/playerstage/?rev=7283&view=rev Author: alexcb Date: 2009-01-21 23:57:00 +0000 (Wed, 21 Jan 2009) Log Message: ----------- started to fix bug when changes to data filter doesnt redraw screen - fixed for custom visualizations Modified Paths: -------------- code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_blinkenlight.cc code/stage/trunk/libstage/model_blobfinder.cc code/stage/trunk/libstage/model_camera.cc code/stage/trunk/libstage/model_fiducial.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/model_ranger.cc code/stage/trunk/libstage/option.cc code/stage/trunk/libstage/option.hh code/stage/trunk/libstage/stage.hh Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-01-21 23:07:45 UTC (rev 7282) +++ code/stage/trunk/libstage/canvas.cc 2009-01-21 23:57:00 UTC (rev 7283) @@ -56,25 +56,25 @@ last_selection( NULL ), interval( 50 ), //msec between redraws // initialize Option objects - showBlinken( "Blinkenlights", "show_blinkenlights", "", true ), - showBlocks( "Blocks", "show_blocks", "b", true ), - showClock( "Clock", "show_clock", "c", true ), - showData( "Data", "show_data", "d", false ), - showFlags( "Flags", "show_flags", "l", true ), - showFollow( "Follow", "show_follow", "f", false ), - showFootprints( "Footprints", "show_footprints", "o", false ), - showGrid( "Grid", "show_grid", "g", true ), - showOccupancy( "Debug/Occupancy", "show_occupancy", "^o", false ), - showScreenshots( "Save screenshots", "screenshots", "", false ), - showStatus( "Status", "show_status", "s", true ), - showTrailArrows( "Trails/Rising Arrows", "show_trailarrows", "^a", false ), - showTrailRise( "Trails/Rising blocks", "show_trailrise", "^r", false ), - showTrails( "Trails/Fast", "show_trailfast", "^f", false ), - showTree( "Debug/Tree", "show_tree", "^t", false ), - showBBoxes( "Debug/Bounding boxes", "show_boundingboxes", "^b", false ), - showBlur( "Trails/Blur", "show_trailblur", "^d", false ), - pCamOn( "Perspective camera", "pcam_on", "r", false ), - visualizeAll( "Selected only", "vis_all", "^v", false ), + showBlinken( "Blinkenlights", "show_blinkenlights", "", true, world ), + showBlocks( "Blocks", "show_blocks", "b", true, world ), + showClock( "Clock", "show_clock", "c", true, world ), + showData( "Data", "show_data", "d", false, world ), + showFlags( "Flags", "show_flags", "l", true, world ), + showFollow( "Follow", "show_follow", "f", false, world ), + showFootprints( "Footprints", "show_footprints", "o", false, world ), + showGrid( "Grid", "show_grid", "g", true, world ), + showOccupancy( "Debug/Occupancy", "show_occupancy", "^o", false, world ), + showScreenshots( "Save screenshots", "screenshots", "", false, world ), + showStatus( "Status", "show_status", "s", true, world ), + 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 ), + pCamOn( "Perspective camera", "pcam_on", "r", false, world ), + visualizeAll( "Selected only", "vis_all", "^v", false, world ), // and the rest graphics( true ), world( world ), Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-01-21 23:07:45 UTC (rev 7282) +++ code/stage/trunk/libstage/model.cc 2009-01-21 23:57:00 UTC (rev 7283) @@ -943,7 +943,7 @@ Canvas* canvas = world_gui->GetCanvas(); std::map< std::string, Option* >::iterator i = canvas->_custom_options.find( custom_visual->name() ); if( i == canvas->_custom_options.end() ) { - Option* op = new Option( custom_visual->name(), custom_visual->name(), "", true ); + Option* op = new Option( custom_visual->name(), custom_visual->name(), "", true, world ); canvas->_custom_options[ custom_visual->name() ] = op; registerOption( op ); } Modified: code/stage/trunk/libstage/model_blinkenlight.cc =================================================================== --- code/stage/trunk/libstage/model_blinkenlight.cc 2009-01-21 23:07:45 UTC (rev 7282) +++ code/stage/trunk/libstage/model_blinkenlight.cc 2009-01-21 23:57:00 UTC (rev 7283) @@ -51,7 +51,8 @@ #include "stage_internal.hh" #include "option.hh" -Option ModelBlinkenlight::showBlinkenData( "Show Blink", "show_blinken", "", true ); +//TODO make instance attempt to register an option (as customvisualizations do) +Option ModelBlinkenlight::showBlinkenData( "Show Blink", "show_blinken", "", true, NULL ); ModelBlinkenlight::ModelBlinkenlight( World* world, Modified: code/stage/trunk/libstage/model_blobfinder.cc =================================================================== --- code/stage/trunk/libstage/model_blobfinder.cc 2009-01-21 23:07:45 UTC (rev 7282) +++ code/stage/trunk/libstage/model_blobfinder.cc 2009-01-21 23:57:00 UTC (rev 7283) @@ -25,7 +25,8 @@ static const unsigned int DEFAULT_BLOBFINDERSCANWIDTH = 80; static const unsigned int DEFAULT_BLOBFINDERSCANHEIGHT = 60; -Option ModelBlobfinder::showBlobData( "Show Blobfinder", "show_blob", "", true ); +//TODO make instance attempt to register an option (as customvisualizations do) +Option ModelBlobfinder::showBlobData( "Show Blobfinder", "show_blob", "", true, NULL ); /** @ingroup model Modified: code/stage/trunk/libstage/model_camera.cc =================================================================== --- code/stage/trunk/libstage/model_camera.cc 2009-01-21 23:07:45 UTC (rev 7282) +++ code/stage/trunk/libstage/model_camera.cc 2009-01-21 23:57:00 UTC (rev 7283) @@ -17,7 +17,8 @@ #include <sstream> #include <iomanip> -Option ModelCamera::showCameraData( "Show Camera Data", "show_camera", "", true ); +//TODO make instance attempt to register an option (as customvisualizations do) +Option ModelCamera::showCameraData( "Show Camera Data", "show_camera", "", true, NULL ); static const Size DEFAULT_SIZE( 0.1, 0.07, 0.05 ); static const char DEFAULT_GEOM_COLOR[] = "black"; Modified: code/stage/trunk/libstage/model_fiducial.cc =================================================================== --- code/stage/trunk/libstage/model_fiducial.cc 2009-01-21 23:07:45 UTC (rev 7282) +++ code/stage/trunk/libstage/model_fiducial.cc 2009-01-21 23:57:00 UTC (rev 7283) @@ -23,7 +23,8 @@ const stg_radians_t DEFAULT_FIDUCIAL_FOV = M_PI; const stg_watts_t DEFAULT_FIDUCIAL_WATTS = 10.0; -Option ModelFiducial::showFiducialData( "Show Fiducial", "show_fiducial", "", true ); +//TODO make instance attempt to register an option (as customvisualizations do) +Option ModelFiducial::showFiducialData( "Show Fiducial", "show_fiducial", "", true, NULL ); /** @ingroup model @@ -38,9 +39,9 @@ @par Summary and default values @verbatim -fiducialfinder +fiducial ( - # fiducialfinder properties + # fiducial properties range_min 0.0 range_max 8.0 range_max_id 5.0 @@ -68,8 +69,8 @@ Model* parent ) : Model( world, parent, MODEL_TYPE_FIDUCIAL ) { - PRINT_DEBUG2( "Constructing ModelFiducial %d (%s)\n", - id, typestr ); + //PRINT_DEBUG2( "Constructing ModelFiducial %d (%s)\n", + // id, typestr ); // assert that Update() is reentrant for this derived model thread_safe = true; Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2009-01-21 23:07:45 UTC (rev 7282) +++ code/stage/trunk/libstage/model_laser.cc 2009-01-21 23:57:00 UTC (rev 7283) @@ -28,8 +28,9 @@ static const unsigned int DEFAULT_RESOLUTION = 1; static const char* DEFAULT_COLOR = "blue"; -Option ModelLaser::showLaserData( "Laser scans", "show_laser", "", true ); -Option ModelLaser::showLaserStrikes( "Laser strikes", "show_laser_strikes", "", false ); +//TODO make instance attempt to register an option (as customvisualizations do) +Option ModelLaser::showLaserData( "Laser scans", "show_laser", "", true, NULL ); +Option ModelLaser::showLaserStrikes( "Laser strikes", "show_laser_strikes", "", false, NULL ); /** @ingroup model Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2009-01-21 23:07:45 UTC (rev 7282) +++ code/stage/trunk/libstage/model_position.cc 2009-01-21 23:57:00 UTC (rev 7283) @@ -82,8 +82,9 @@ const stg_position_localization_mode_t POSITION_LOCALIZATION_DEFAULT = STG_POSITION_LOCALIZATION_GPS; const stg_position_drive_mode_t POSITION_DRIVE_DEFAULT = STG_POSITION_DRIVE_DIFFERENTIAL; -Option ModelPosition::showCoords( "Position Coordinates", "show_coords", "", false ); -Option ModelPosition::showWaypoints( "Position Waypoints", "show_waypoints", "", false ); +//TODO make instance attempt to register an option (as customvisualizations do) +Option ModelPosition::showCoords( "Position Coordinates", "show_coords", "", false, NULL ); +Option ModelPosition::showWaypoints( "Position Waypoints", "show_waypoints", "", false, NULL ); ModelPosition::ModelPosition( World* world, Modified: code/stage/trunk/libstage/model_ranger.cc =================================================================== --- code/stage/trunk/libstage/model_ranger.cc 2009-01-21 23:07:45 UTC (rev 7282) +++ code/stage/trunk/libstage/model_ranger.cc 2009-01-21 23:57:00 UTC (rev 7283) @@ -95,8 +95,9 @@ static const char RANGER_CONFIG_COLOR[] = "gray90"; static const char RANGER_GEOM_COLOR[] = "orange"; -Option ModelRanger::showRangerData( "Ranger ranges", "show_ranger", "", true ); -Option ModelRanger::showRangerTransducers( "Ranger transducers", "show_ranger_transducers", "", false ); +//TODO make instance attempt to register an option (as customvisualizations do) +Option ModelRanger::showRangerData( "Ranger ranges", "show_ranger", "", true, NULL ); +Option ModelRanger::showRangerTransducers( "Ranger transducers", "show_ranger_transducers", "", false, NULL ); ModelRanger::ModelRanger( World* world, Modified: code/stage/trunk/libstage/option.cc =================================================================== --- code/stage/trunk/libstage/option.cc 2009-01-21 23:07:45 UTC (rev 7282) +++ code/stage/trunk/libstage/option.cc 2009-01-21 23:57:00 UTC (rev 7283) @@ -3,13 +3,14 @@ using namespace Stg; -Option::Option( std::string n, std::string tok, std::string key, bool v ) : +Option::Option( std::string n, std::string tok, std::string key, bool v, World* world ) : optName( n ), value( v ), wf_token( tok ), shortcut( key ), menu( NULL ), -menuCb( NULL ) +menuCb( NULL ), +_world( world ) { } Fl_Menu_Item* getMenuItem( Fl_Menu_* menu, int i ) { @@ -61,4 +62,12 @@ Fl_Menu_Item* item = getMenuItem( menu, menuIndex ); value ? item->set() : item->clear(); } + + if( _world ) { + WorldGui* wg = dynamic_cast< WorldGui* >( _world ); + if( wg == NULL ) return; + Canvas* canvas = wg->GetCanvas(); + canvas->invalidate(); + canvas->redraw(); + } } Modified: code/stage/trunk/libstage/option.hh =================================================================== --- code/stage/trunk/libstage/option.hh 2009-01-21 23:07:45 UTC (rev 7282) +++ code/stage/trunk/libstage/option.hh 2009-01-21 23:57:00 UTC (rev 7283) @@ -8,6 +8,7 @@ #include <FL/Fl_Menu_Item.H> namespace Stg { + class World; /** option.hh Class that encapsulates a boolean and pairs it with a string description Used to pass settings between the GUI and the drawing classes @@ -28,9 +29,10 @@ int menuIndex; Fl_Callback* menuCb; Fl_Widget* menuCbWidget; + World* _world; public: - Option( std::string n, std::string tok, std::string key, bool v ); + Option( std::string n, std::string tok, std::string key, bool v, World *world ); const std::string name() const { return optName; } inline bool isEnabled() const { return value; } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-01-21 23:07:45 UTC (rev 7282) +++ code/stage/trunk/libstage/stage.hh 2009-01-21 23:57:00 UTC (rev 7283) @@ -1301,6 +1301,7 @@ friend class Canvas; friend class ModelCamera; friend class Model; + friend class Option; private: This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-01-22 23:59:06
|
Revision: 7287 http://playerstage.svn.sourceforge.net/playerstage/?rev=7287&view=rev Author: rtv Date: 2009-01-22 23:58:58 +0000 (Thu, 22 Jan 2009) Log Message: ----------- header clean up Modified Paths: -------------- code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/ancestor.cc code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/blockgroup.cc code/stage/trunk/libstage/camera.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/charger.cc code/stage/trunk/libstage/gl.cc code/stage/trunk/libstage/glcolorstack.cc code/stage/trunk/libstage/main.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_blinkenlight.cc code/stage/trunk/libstage/model_blobfinder.cc code/stage/trunk/libstage/model_callbacks.cc code/stage/trunk/libstage/model_camera.cc code/stage/trunk/libstage/model_fiducial.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/model_props.cc code/stage/trunk/libstage/model_ranger.cc code/stage/trunk/libstage/option.cc code/stage/trunk/libstage/options_dlg.hh code/stage/trunk/libstage/powerpack.cc code/stage/trunk/libstage/region.cc code/stage/trunk/libstage/region.hh code/stage/trunk/libstage/resource.cc code/stage/trunk/libstage/stage.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/texture_manager.cc code/stage/trunk/libstage/texture_manager.hh code/stage/trunk/libstage/typetable.cc code/stage/trunk/libstage/waypoint.cc code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldfile.cc code/stage/trunk/libstage/worldfile.hh code/stage/trunk/libstage/worldgui.cc Added Paths: ----------- code/stage/trunk/libstage/canvas.hh code/stage/trunk/libstage/gl.hh Removed Paths: ------------- code/stage/trunk/libstage/model.hh code/stage/trunk/libstage/stage_internal.hh Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2009-01-22 21:36:27 UTC (rev 7286) +++ code/stage/trunk/libstage/CMakeLists.txt 2009-01-22 23:58:58 UTC (rev 7287) @@ -25,7 +25,6 @@ model_props.cc model_ranger.cc option.cc - option.hh options_dlg.cc options_dlg.hh powerpack.cc @@ -82,7 +81,7 @@ LIBRARY DESTINATION lib ) -INSTALL(FILES stage.hh option.hh worldfile.hh +INSTALL(FILES stage.hh DESTINATION include/${PROJECT_NAME}-${V_MAJOR}.${V_MINOR}) ADD_TEST( test1 ${EXECUTABLE_OUTPUT_PATH}stagetest ) Modified: code/stage/trunk/libstage/ancestor.cc =================================================================== --- code/stage/trunk/libstage/ancestor.cc 2009-01-22 21:36:27 UTC (rev 7286) +++ code/stage/trunk/libstage/ancestor.cc 2009-01-22 23:58:58 UTC (rev 7287) @@ -1,4 +1,5 @@ -#include "stage_internal.hh" +#include "stage.hh" +using namespace Stg; Ancestor::Ancestor() { Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2009-01-22 21:36:27 UTC (rev 7286) +++ code/stage/trunk/libstage/block.cc 2009-01-22 23:58:58 UTC (rev 7287) @@ -1,7 +1,8 @@ -#include "stage_internal.hh" +#include "region.hh" +#include "worldfile.hh" -//GPtrArray* Block::global_verts = g_ptr_array_sized_new( 1024 ); +using namespace Stg; /** Create a new block. A model's body is a list of these blocks. The point data is copied, so pts can safely be freed Modified: code/stage/trunk/libstage/blockgroup.cc =================================================================== --- code/stage/trunk/libstage/blockgroup.cc 2009-01-22 21:36:27 UTC (rev 7286) +++ code/stage/trunk/libstage/blockgroup.cc 2009-01-22 23:58:58 UTC (rev 7287) @@ -1,5 +1,8 @@ -#include "stage_internal.hh" +#include "stage.hh" +#include "gl.hh" +#include "worldfile.hh" + #include <libgen.h> // for dirname(3) #undef DEBUG @@ -123,7 +126,7 @@ { glPushMatrix(); - gl_pose_shift( geom.pose ); + Gl::pose_shift( geom.pose ); glScalef( geom.size.x / size.x, geom.size.y / size.y, @@ -166,7 +169,7 @@ Geom geom = mod->GetGeom(); - gl_pose_shift( geom.pose ); + Gl::pose_shift( geom.pose ); glScalef( geom.size.x / size.x, geom.size.y / size.y, Modified: code/stage/trunk/libstage/camera.cc =================================================================== --- code/stage/trunk/libstage/camera.cc 2009-01-22 21:36:27 UTC (rev 7286) +++ code/stage/trunk/libstage/camera.cc 2009-01-22 23:58:58 UTC (rev 7287) @@ -7,7 +7,9 @@ */ -#include "stage_internal.hh" +#include "stage.hh" +#include "worldfile.hh" +using namespace Stg; #include <iostream> Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-01-22 21:36:27 UTC (rev 7286) +++ code/stage/trunk/libstage/canvas.cc 2009-01-22 23:58:58 UTC (rev 7287) @@ -9,15 +9,18 @@ $Id$ */ -#include "stage_internal.hh" +#include "stage.hh" +#include "gl.hh" +#include "canvas.hh" +#include "worldfile.hh" #include "texture_manager.hh" #include "replace.h" #include <string> -//#include <map> #include <sstream> #include <png.h> + #include "file_manager.hh" #include "options_dlg.hh" @@ -49,6 +52,12 @@ int x, int y, int width, int height) : Fl_Gl_Window( x, y, width, height ), + colorstack(), + models_sorted( NULL ), + current_camera( NULL ), + camera(), + perspective_camera(), + dirty_buffer( false ), wf( NULL ), startx( -1 ), starty( -1 ), @@ -79,8 +88,7 @@ graphics( true ), world( world ), frames_rendered_count( 0 ), - screenshot_frame_skip( 1 ), - models_sorted( NULL ) + screenshot_frame_skip( 1 ) { end(); @@ -608,13 +616,13 @@ for( double i = floor(bounds.x.min); i < bounds.x.max; i++) { snprintf( str, 16, "%d", (int)i ); - gl_draw_string( i, 0, 0.00, str ); + Gl::draw_string( i, 0, 0.00, str ); } for( double i = floor(bounds.y.min); i < bounds.y.max; i++) { snprintf( str, 16, "%d", (int)i ); - gl_draw_string( 0, i, 0.00, str ); + Gl::draw_string( 0, i, 0.00, str ); } PopColor(); @@ -996,7 +1004,7 @@ colorstack.Pop(); colorstack.Push( 0,0,0 ); // black - gl_draw_string( margin, margin, 0, clockstr.c_str() ); + Gl::draw_string( margin, margin, 0, clockstr.c_str() ); colorstack.Pop(); glEnable( GL_DEPTH_TEST ); Added: code/stage/trunk/libstage/canvas.hh =================================================================== --- code/stage/trunk/libstage/canvas.hh (rev 0) +++ code/stage/trunk/libstage/canvas.hh 2009-01-22 23:58:58 UTC (rev 7287) @@ -0,0 +1,140 @@ + +#include "stage.hh" +#include "option.hh" + +#include <map> + +namespace Stg +{ +// COLOR STACK CLASS +class GlColorStack +{ + public: + GlColorStack(); + ~GlColorStack(); + + void Push( GLdouble col[4] ); + void Push( double r, double g, double b, double a ); + void Push( double r, double g, double b ); + void Push( stg_color_t col ); + + void Pop(); + + unsigned int Length() + { return g_queue_get_length( colorstack ); } + + private: + GQueue* colorstack; +}; + + +class Canvas : public Fl_Gl_Window +{ + friend class WorldGui; // allow access to private members + friend class Model; + +private: + GlColorStack colorstack; + + GList* models_sorted; + + Camera* current_camera; + OrthoCamera camera; + PerspectiveCamera perspective_camera; + bool dirty_buffer; + Worldfile* wf; + + int startx, starty; + bool selectedModel; + bool clicked_empty_space; + int empty_space_startx, empty_space_starty; + GList* selected_models; ///< a list of models that are currently + ///selected by the user + Model* last_selection; ///< the most recently selected model + ///(even if it is now unselected). + + stg_msec_t interval; // window refresh interval in ms + + GList* ray_list; + void RecordRay( double x1, double y1, double x2, double y2 ); + void DrawRays(); + void ClearRays(); + void DrawGlobalGrid(); + + void AddModel( Model* mod ); + + Option showBlinken, + showBlocks, + showClock, + showData, + showFlags, + showFollow, + showFootprints, + showGrid, + showOccupancy, + showScreenshots, + showStatus, + showTrailArrows, + showTrailRise, + showTrails, + showTree, + showBBoxes, + showBlur, + pCamOn, + visualizeAll; + +public: + Canvas( WorldGui* world, int x, int y, int width, int height); + ~Canvas(); + + bool graphics; + WorldGui* world; + unsigned long frames_rendered_count; + int screenshot_frame_skip; + + std::map< std::string, Option* > _custom_options; + + void Screenshot(); + void InitGl(); + void createMenuItems( Fl_Menu_Bar* menu, std::string path ); + + void FixViewport(int W,int H); + void DrawFloor(); //simpler floor compared to grid + void DrawBlocks(); + void DrawBoundingBoxes(); + void resetCamera(); + virtual void renderFrame(); + virtual void draw(); + virtual int handle( int event ); + void resize(int X,int Y,int W,int H); + + void CanvasToWorld( int px, int py, + double *wx, double *wy, double* wz ); + + Model* getModel( int x, int y ); + bool selected( Model* mod ); + void select( Model* mod ); + void unSelect( Model* mod ); + void unSelectAll(); + + inline void setDirtyBuffer( void ) { dirty_buffer = true; } + inline bool dirtyBuffer( void ) const { return dirty_buffer; } + + inline void PushColor( stg_color_t col ) + { colorstack.Push( col ); } + + void PushColor( double r, double g, double b, double a ) + { colorstack.Push( r,g,b,a ); } + + void PopColor(){ colorstack.Pop(); } + + void InvertView( uint32_t invertflags ); + + static void TimerCallback( Canvas* canvas ); + static void perspectiveCb( Fl_Widget* w, void* p ); + + void Load( Worldfile* wf, int section ); + void Save( Worldfile* wf, int section ); +}; + +} // namespace Stg Modified: code/stage/trunk/libstage/charger.cc =================================================================== --- code/stage/trunk/libstage/charger.cc 2009-01-22 21:36:27 UTC (rev 7286) +++ code/stage/trunk/libstage/charger.cc 2009-01-22 23:58:58 UTC (rev 7287) @@ -6,7 +6,9 @@ */ -#include "stage_internal.hh" +#include "stage.hh" +#include "worldfile.hh" +using namespace Stg; Charger::Charger( World* world ) : world( world ), watts( 1000.0 ) Modified: code/stage/trunk/libstage/gl.cc =================================================================== --- code/stage/trunk/libstage/gl.cc 2009-01-22 21:36:27 UTC (rev 7286) +++ code/stage/trunk/libstage/gl.cc 2009-01-22 23:58:58 UTC (rev 7287) @@ -1,41 +1,42 @@ -#include "stage_internal.hh" +#include "gl.hh" +using namespace Stg; // transform the current coordinate frame by the given pose -void Stg::gl_coord_shift( double x, double y, double z, double a ) +void Stg::Gl::coord_shift( double x, double y, double z, double a ) { glTranslatef( x,y,z ); glRotatef( rtod(a), 0,0,1 ); } // transform the current coordinate frame by the given pose -void Stg::gl_pose_shift( const Pose &pose ) +void Stg::Gl::pose_shift( const Pose &pose ) { - gl_coord_shift( pose.x, pose.y, pose.z, pose.a ); + coord_shift( pose.x, pose.y, pose.z, pose.a ); } -void Stg::gl_pose_inverse_shift( const Pose &pose ) +void Stg::Gl::pose_inverse_shift( const Pose &pose ) { - gl_coord_shift( 0,0,0, -pose.a ); - gl_coord_shift( -pose.x, -pose.y, -pose.z, 0 ); + coord_shift( 0,0,0, -pose.a ); + coord_shift( -pose.x, -pose.y, -pose.z, 0 ); } -void Stg::gl_draw_string( float x, float y, float z, const char *str ) +void Stg::Gl::draw_string( float x, float y, float z, const char *str ) { glRasterPos3f( x, y, z ); //printf( "[%.2f %.2f %.2f] string %u %s\n", x,y,z,(unsigned int)strlen(str), str ); gl_draw(str); } -void Stg::gl_speech_bubble( float x, float y, float z, const char* str ) +void Stg::Gl::draw_speech_bubble( float x, float y, float z, const char* str ) { - gl_draw_string( x, y, z, str ); + draw_string( x, y, z, str ); } // draw an octagon with center rectangle dimensions w/h // and outside margin m -void Stg::gl_draw_octagon( float w, float h, float m ) +void Stg::Gl::draw_octagon( float w, float h, float m ) { glBegin(GL_POLYGON); glVertex2f( m+w, 0 ); @@ -49,7 +50,7 @@ glEnd(); } -void Stg::gl_draw_vector( double x, double y, double z ) +void Stg::Gl::draw_vector( double x, double y, double z ) { glBegin( GL_LINES ); glVertex3f( 0,0,0 ); @@ -57,14 +58,14 @@ glEnd(); } -void Stg::gl_draw_origin( double len ) +void Stg::Gl::draw_origin( double len ) { - gl_draw_vector( len,0,0 ); - gl_draw_vector( 0,len,0 ); - gl_draw_vector( 0,0,len ); + draw_vector( len,0,0 ); + draw_vector( 0,len,0 ); + draw_vector( 0,0,len ); } -void Stg::gl_draw_grid( stg_bounds3d_t vol ) +void Stg::Gl::draw_grid( stg_bounds3d_t vol ) { glBegin(GL_LINES); @@ -87,13 +88,13 @@ for( double i = floor(vol.x.min); i < vol.x.max; i++) { snprintf( str, 16, "%d", (int)i ); - gl_draw_string( i, 0, 0.00, str ); + draw_string( i, 0, 0.00, str ); } for( double i = floor(vol.y.min); i < vol.y.max; i++) { snprintf( str, 16, "%d", (int)i ); - gl_draw_string( 0, i, 0.00, str ); + draw_string( 0, i, 0.00, str ); } } Added: code/stage/trunk/libstage/gl.hh =================================================================== --- code/stage/trunk/libstage/gl.hh (rev 0) +++ code/stage/trunk/libstage/gl.hh 2009-01-22 23:58:58 UTC (rev 7287) @@ -0,0 +1,20 @@ + +#include "stage.hh" + +namespace Stg +{ + /** @brief Internal low-level drawing convenience routines. */ + namespace Gl + { + void pose_shift( const Pose &pose ); + void pose_inverse_shift( const Pose &pose ); + void coord_shift( double x, double y, double z, double a ); + void draw_grid( stg_bounds3d_t vol ); + /** Render a string at [x,y,z] in the current color */ + void draw_string( float x, float y, float z, const char *string); + void draw_speech_bubble( float x, float y, float z, const char* str ); + void draw_octagon( float w, float h, float m ); + void draw_vector( double x, double y, double z ); + void draw_origin( double len ); + } +} Modified: code/stage/trunk/libstage/glcolorstack.cc =================================================================== --- code/stage/trunk/libstage/glcolorstack.cc 2009-01-22 21:36:27 UTC (rev 7286) +++ code/stage/trunk/libstage/glcolorstack.cc 2009-01-22 23:58:58 UTC (rev 7287) @@ -1,5 +1,7 @@ -#include "stage_internal.hh" +#include "canvas.hh" +using namespace Stg; + GlColorStack::GlColorStack() { colorstack = g_queue_new(); Modified: code/stage/trunk/libstage/main.cc =================================================================== --- code/stage/trunk/libstage/main.cc 2009-01-22 21:36:27 UTC (rev 7286) +++ code/stage/trunk/libstage/main.cc 2009-01-22 23:58:58 UTC (rev 7287) @@ -6,8 +6,9 @@ #include <getopt.h> -#include "stage_internal.hh" +#include "stage.hh" #include "config.h" +using namespace Stg; /* options descriptor */ static struct option longopts[] = { Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-01-22 21:36:27 UTC (rev 7286) +++ code/stage/trunk/libstage/model.cc 2009-01-22 23:58:58 UTC (rev 7287) @@ -109,8 +109,15 @@ #endif //#define DEBUG 0 -#include "stage_internal.hh" +#include "stage.hh" +#include "gl.hh" + +#include <map> + +#include "worldfile.hh" +#include "canvas.hh" #include "texture_manager.hh" +using namespace Stg; // speech bubble colors static const stg_color_t BUBBLE_FILL = 0xFFC8C8FF; // light blue/grey @@ -121,7 +128,43 @@ uint32_t Model::count = 0; GHashTable* Model::modelsbyid = g_hash_table_new( NULL, NULL ); +Visibility::Visibility() : + blob_return( true ), + fiducial_key( 0 ), + fiducial_return( 0 ), + gripper_return( false ), + laser_return( LaserVisible ), + obstacle_return( true ), + ranger_return( true ) +{ /* nothing do do */ } +void Visibility::Load( Worldfile* wf, int wf_entity ) +{ + blob_return = wf->ReadInt( wf_entity, "blob_return", blob_return); + fiducial_key = wf->ReadInt( wf_entity, "fiducial_key", fiducial_key); + fiducial_return = wf->ReadInt( wf_entity, "fiducial_return", fiducial_return); + gripper_return = wf->ReadInt( wf_entity, "gripper_return", gripper_return); + 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); +} + +GuiState:: GuiState() : + grid( false ), + mask( 0 ), + nose( false ), + outline( false ) +{ /* nothing to do */} + +void GuiState::Load( Worldfile* wf, int wf_entity ) +{ + nose = wf->ReadInt( wf_entity, "gui_nose", nose); + grid = wf->ReadInt( wf_entity, "gui_grid", grid); + outline = wf->ReadInt( wf_entity, "gui_outline", outline); + mask = wf->ReadInt( wf_entity, "gui_movemask", mask); +} + + // constructor Model::Model( World* world, Model* parent, @@ -136,8 +179,8 @@ color( 0xFFFF0000 ), // red data_fresh(false), disabled(false), + custom_visual_list( NULL ), flag_list(NULL), - custom_visual_list( NULL ), geom(), has_default_block( true ), id( Model::count++ ), @@ -166,7 +209,7 @@ wf(NULL), wf_entity(0), world(world), - world_gui( dynamic_cast< WorldGui* >( world ) ) + world_gui( dynamic_cast< WorldGui* >( world ) ) { assert( modelsbyid ); assert( world ); @@ -467,6 +510,18 @@ return( (this == mod2) || IsAntecedent( mod2 ) || IsDescendent( mod2 ) ); } + +// bool Model::IsRelated( Model* that ) +// { +// if( this == that ) +// return true; + +// for( GList* it = children; it; it=it->next ) +// { +// if( +// } + + // get the model's velocity in the global frame Velocity Model::GetGlobalVelocity() { @@ -702,11 +757,11 @@ token, gpose.x, gpose.y, gpose.z, rtod(gpose.a) ); PushColor( 0,0,0,1 ); // text color black - gl_draw_string( 0.5,0.5,0.5, buf ); + Gl::draw_string( 0.5,0.5,0.5, buf ); glRotatef( rtod(pose.a), 0,0,1 ); - gl_pose_shift( geom.pose ); + Gl::pose_shift( geom.pose ); double dx = geom.size.x / 2.0 * 1.6; double dy = geom.size.y / 2.0 * 1.6; @@ -737,8 +792,8 @@ stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t, i ); glPushMatrix(); - gl_pose_shift( checkpoint->pose ); - gl_pose_shift( geom.pose ); + Gl::pose_shift( checkpoint->pose ); + Gl::pose_shift( geom.pose ); stg_color_unpack( checkpoint->color, &r, &g, &b, &a ); PushColor( r, g, b, 0.1 ); @@ -872,7 +927,7 @@ void Model::DrawBoundingBox() { - gl_pose_shift( geom.pose ); + Gl::pose_shift( geom.pose ); PushColor( color ); @@ -917,7 +972,7 @@ if( parent ) glTranslatef( 0,0, parent->geom.size.z ); - gl_pose_shift( pose ); + Gl::pose_shift( pose ); } void Model::PopCoords() @@ -943,7 +998,7 @@ Canvas* canvas = world_gui->GetCanvas(); std::map< std::string, Option* >::iterator i = canvas->_custom_options.find( custom_visual->name() ); if( i == canvas->_custom_options.end() ) { - Option* op = new Option( custom_visual->name(), custom_visual->name(), "", true, world ); + Option* op = new Option( custom_visual->name(), custom_visual->name(), "", true, world_gui ); canvas->_custom_options[ custom_visual->name() ] = op; registerOption( op ); } @@ -1038,7 +1093,7 @@ glPolygonMode( GL_FRONT, GL_FILL ); glEnable( GL_POLYGON_OFFSET_FILL ); glPolygonOffset( 1.0, 1.0 ); - gl_draw_octagon( w, h, m ); + Gl::draw_octagon( w, h, m ); glDisable( GL_POLYGON_OFFSET_FILL ); PopColor(); @@ -1047,13 +1102,13 @@ glLineWidth( 1 ); glEnable( GL_LINE_SMOOTH ); glPolygonMode( GL_FRONT, GL_LINE ); - gl_draw_octagon( w, h, m ); + Gl::draw_octagon( w, h, m ); glPopAttrib(); PopColor(); PushColor( BUBBLE_TEXT ); // draw text inside the bubble - gl_draw_string( 2.5*m, 2.5*m, 0, this->say_string ); + Gl::draw_string( 2.5*m, 2.5*m, 0, this->say_string ); PopColor(); } } @@ -1268,7 +1323,7 @@ vol.z.max = geom.size.z; PushColor( 0,0,1,0.4 ); - gl_draw_grid(vol); + Gl::draw_grid(vol); PopColor(); PopCoords(); } Deleted: code/stage/trunk/libstage/model.hh =================================================================== --- code/stage/trunk/libstage/model.hh 2009-01-22 21:36:27 UTC (rev 7286) +++ code/stage/trunk/libstage/model.hh 2009-01-22 23:58:58 UTC (rev 7287) @@ -1,1190 +0,0 @@ -#ifndef MODEL_H -#define MODEL_H - -class Camera; - -/** energy data packet */ -class PowerPack -{ -public: - PowerPack( Model* mod ); - - /** The model that owns this object */ - Model* mod; - - /** Energy stored */ - stg_joules_t stored; - - /** Energy capacity */ - stg_joules_t capacity; - - /** TRUE iff the device is receiving energy from a charger */ - bool charging; - - /** OpenGL visualization of the powerpack state */ - void Visualize( Camera* cam ); - - /** Print human-readable status on stdout, prefixed with the - argument string */ - void Print( char* prefix ); -}; - -class Visibility -{ -public: - bool blob_return; - int fiducial_key; - int fiducial_return; - bool gripper_return; - stg_laser_return_t laser_return; - bool obstacle_return; - bool ranger_return; - - Visibility() : - blob_return( true ), - fiducial_key( 0 ), - fiducial_return( 0 ), - gripper_return( false ), - laser_return( LaserVisible ), - obstacle_return( true ), - ranger_return( true ) - { /* nothing do do */ } - - void Load( Worldfile* wf, int wf_entity ) - { - blob_return = wf->ReadInt( wf_entity, "blob_return", blob_return); - fiducial_key = wf->ReadInt( wf_entity, "fiducial_key", fiducial_key); - fiducial_return = wf->ReadInt( wf_entity, "fiducial_return", fiducial_return); - gripper_return = wf->ReadInt( wf_entity, "gripper_return", gripper_return); - 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); - } -}; - -/** Abstract class for adding visualizations to models. DataVisualize must be overloaded, and is then called in the models local coord system */ -class CustomVisualizer { -public: - //TODO allow user to specify name - which will show up in display filter - virtual ~CustomVisualizer( void ) { } - virtual void DataVisualize( Camera* cam ) = 0; - virtual const std::string& name() = 0; //must return a name for visualization (careful not to return stack-memory) -}; - - -/* Hooks for attaching special callback functions (not used as - variables - we just need unique addresses for them.) */ -class CallbackHooks -{ -public: - char load; - char save; - char shutdown; - char startup; - char update; -}; - -/** Records model state and functionality in the GUI, if used */ -class GuiState -{ -public: - bool grid; - unsigned int mask; - bool nose; - bool outline; - - GuiState() : - grid( false ), - mask( 0 ), - nose( false ), - outline( false ) - { /* nothing to do */} - - void Load( Worldfile* wf, int wf_entity ) - { - nose = wf->ReadInt( wf_entity, "gui_nose", nose); - grid = wf->ReadInt( wf_entity, "gui_grid", grid); - outline = wf->ReadInt( wf_entity, "gui_outline", outline); - mask = wf->ReadInt( wf_entity, "gui_movemask", mask); - } - -}; - -/// %Model class -class Model : public Ancestor -{ - friend class Ancestor; - friend class World; - friend class WorldGui; - friend class Canvas; - friend class Block; - friend class Region; - friend class BlockGroup; - -private: - /** the number of models instatiated - used to assign unique IDs */ - static uint32_t count; - static GHashTable* modelsbyid; - std::vector<Option*> drawOptions; - const std::vector<Option*>& getOptions() const { return drawOptions; } - -protected: - GMutex* access_mutex; - GPtrArray* blinkenlights; - BlockGroup blockgroup; - /** OpenGL display list identifier for the blockgroup */ - int blocks_dl; - - /** Iff true, 4 thin blocks are automatically added to the model, - forming a solid boundary around the bounding box of the - model. */ - int boundary; - - /** callback functions can be attached to any field in this - structure. When the internal function model_change(<mod>,<field>) - is called, the callback is triggered */ - GHashTable* callbacks; - - /** Default color of the model's blocks.*/ - stg_color_t color; - - /** This can be set to indicate that the model has new data that - may be of interest to users. This allows polling the model - instead of adding a data callback. */ - bool data_fresh; - stg_bool_t disabled; //< if non-zero, the model is disabled - GList* custom_visual_list; - GList* flag_list; - Geom geom; - Pose global_pose; - bool gpose_dirty; //< set this to indicate that global pose may have changed - /** Controls our appearance and functionality in the GUI, if used */ - GuiState gui; - - bool has_default_block; - - /* hooks for attaching special callback functions (not used as - variables - we just need unique addresses for them.) */ - CallbackHooks hooks; - - /** unique process-wide identifier for this model */ - uint32_t id; - ctrlinit_t* initfunc; - stg_usec_t interval; //< time between updates in us - stg_usec_t last_update; //< time of last update in us - bool map_caches_are_invalid; - stg_meters_t map_resolution; - stg_kg_t mass; - bool on_update_list; - bool on_velocity_list; - - /** Pointer to the parent of this model, possibly NULL. */ - Model* parent; - - /** The pose of the model in it's parents coordinate frame, or the - global coordinate frame is the parent is NULL. */ - Pose pose; - - /** Optional attached PowerPack, defaults to NULL */ - PowerPack* power_pack; - - /** GData datalist can contain arbitrary named data items. Can be used - by derived model types to store properties, and for user code - to associate arbitrary items with a model. */ - GData* props; - bool rebuild_displaylist; //< iff true, regenerate block display list before redraw - char* say_string; //< if non-null, this string is displayed in the GUI - - stg_bool_t stall; - /** Thread safety flag. Iff true, Update() may be called in - parallel with other models. Defaults to false for safety */ - int subs; //< the number of subscriptions to this model - bool thread_safe; - GArray* trail; - stg_model_type_t type; - bool used; //< TRUE iff this model has been returned by GetUnusedModelOfType() - Velocity velocity; - stg_watts_t watts; //< power consumed by this model - Worldfile* wf; - int wf_entity; - World* world; // pointer to the world in which this model exists - WorldGui* world_gui; //pointer to the GUI world - NULL if running in non-gui mode - -public: - - Visibility vis; - - void Lock() - { - if( access_mutex == NULL ) - access_mutex = g_mutex_new(); - - assert( access_mutex ); - g_mutex_lock( access_mutex ); - } - - void Unlock() - { - assert( access_mutex ); - g_mutex_unlock( access_mutex ); - } - - -private: - /** Private copy constructor declared but not defined, to make it - impossible to copy models. */ - explicit Model(const Model& original); - - /** Private assignment operator declared but not defined, to make it - impossible to copy models */ - Model& operator=(const Model& original); - -protected: - - /// Register an Option for pickup by the GUI - void registerOption( Option* opt ) - { drawOptions.push_back( opt ); } - - /** Check to see if the current pose will yield a collision with - obstacles. Returns a pointer to the first entity we are in - collision with, or NULL if no collision exists. */ - Model* TestCollision(); - - void CommitTestedPose(); - - void Map(); - void UnMap(); - - void MapWithChildren(); - void UnMapWithChildren(); - - int TreeToPtrArray( GPtrArray* array ); - - /** raytraces a single ray from the point and heading identified by - pose, in local coords */ - stg_raytrace_result_t Raytrace( const Pose &pose, - const stg_meters_t range, - const stg_ray_test_func_t func, - const void* arg, - const bool ztest = true ); - - /** raytraces multiple rays around the point and heading identified - by pose, in local coords */ - void Raytrace( const Pose &pose, - const stg_meters_t range, - const stg_radians_t fov, - const stg_ray_test_func_t func, - const void* arg, - stg_raytrace_result_t* samples, - const uint32_t sample_count, - const bool ztest = true ); - - stg_raytrace_result_t Raytrace( const stg_radians_t bearing, - const stg_meters_t range, - const stg_ray_test_func_t func, - const void* arg, - const bool ztest = true ); - - void Raytrace( const stg_radians_t bearing, - const stg_meters_t range, - const stg_radians_t fov, - const stg_ray_test_func_t func, - const void* arg, - stg_raytrace_result_t* samples, - const uint32_t sample_count, - const bool ztest = true ); - - - /** Causes this model and its children to recompute their global - position instead of using a cached pose in - Model::GetGlobalPose()..*/ - void GPoseDirtyTree(); - - virtual void Startup(); - virtual void Shutdown(); - virtual void Update(); - virtual void UpdatePose(); - - void StartUpdating(); - void StopUpdating(); - - Model* ConditionalMove( Pose newpose ); - - stg_meters_t ModelHeight(); - - bool UpdateDue( void ); - void UpdateIfDue(); - - void DrawBlocksTree(); - virtual void DrawBlocks(); - void DrawBoundingBox(); - void DrawBoundingBoxTree(); - virtual void DrawStatus( Camera* cam ); - void DrawStatusTree( Camera* cam ); - - void DrawOriginTree(); - void DrawOrigin(); - - void PushLocalCoords(); - void PopCoords(); - - /** Draw the image stored in texture_id above the model */ - void DrawImage( uint32_t texture_id, Camera* cam, float alpha ); - - - /** static wrapper for DrawBlocks() */ - static void DrawBlocks( gpointer dummykey, - Model* mod, - void* arg ); - - virtual void DrawPicker(); - virtual void DataVisualize( Camera* cam ); - - virtual void DrawSelected(void); - - void DrawTrailFootprint(); - void DrawTrailBlocks(); - void DrawTrailArrows(); - void DrawGrid(); - - - void DrawBlinkenlights(); - - void DataVisualizeTree( Camera* cam ); - - virtual void PushColor( stg_color_t col ) - { world->PushColor( col ); } - - virtual void PushColor( double r, double g, double b, double a ) - { world->PushColor( r,g,b,a ); } - - virtual void PopColor(){ world->PopColor(); } - - void DrawFlagList(); - - void DrawPose( Pose pose ); - -public: - void RecordRenderPoint( GSList** head, GSList* link, - unsigned int* c1, unsigned int* c2 ); - - void PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax, - stg_meters_t ymin, stg_meters_t ymax ); - - /** 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 ); } - - /** Constructor */ - Model( World* world, - Model* parent, - stg_model_type_t type = MODEL_TYPE_PLAIN ); - - /** Destructor */ - virtual ~Model(); - - void Say( const char* str ); - /** Attach a user supplied visualization to a model */ - void AddCustomVisualizer( CustomVisualizer* custom_visual ); - /** remove user supplied visualization to a model - supply the same ptr passed to AddCustomVisualizer */ - void RemoveCustomVisualizer( CustomVisualizer* custom_visual ); - - - void Load( Worldfile* wf, int wf_entity ) - { - /** Set the worldfile and worldfile entity ID - must be called - before Load() */ - SetWorldfile( wf, wf_entity ); - Load(); // call virtual load - } - - /** Set the worldfile and worldfile entity ID */ - void SetWorldfile( Worldfile* wf, int wf_entity ) - { this->wf = wf; this->wf_entity = wf_entity; } - - /** configure a model by reading from the current world file */ - virtual void Load(); - - /** save the state of the model to the current world file */ - virtual void Save(); - - /** Should be called after all models are loaded, to do any last-minute setup */ - void Init(); - void InitRecursive(); - - void AddFlag( Flag* flag ); - void RemoveFlag( Flag* flag ); - - void PushFlag( Flag* flag ); - Flag* PopFlag(); - - int GetFlagCount(){ return g_list_length( flag_list ); } - - /** Add a pointer to a blinkenlight to the model. */ - void AddBlinkenlight( stg_blinkenlight_t* b ) - { g_ptr_array_add( this->blinkenlights, b ); } - - /** Clear all blinkenlights from the model. Does not destroy the - blinkenlight objects. */ - void ClearBlinkenlights() - { g_ptr_array_set_size( this->blinkenlights, 0 ); } - - /** Disable the model. Its pose will not change due to velocity - until re-enabled using Enable(). This is used for example when - dragging a model with the mouse pointer. The model is enabled by - default. */ - void Disable(){ disabled = true; }; - - /** Enable the model, so that non-zero velocities will change the - model's pose. Models are enabled by default. */ - void Enable(){ disabled = false; }; - - /** Load a control program for this model from an external - library */ - void LoadControllerModule( char* lib ); - - /** Sets the redraw flag, so this model will be redrawn at the - earliest opportunity */ - void NeedRedraw(); - - /** Add a block to this model by loading it from a worldfile - entity */ - void LoadBlock( Worldfile* wf, int entity ); - - /** Add a block to this model centered at [x,y] with extent [dx, dy, - dz] */ - void AddBlockRect( stg_meters_t x, stg_meters_t y, - stg_meters_t dx, stg_meters_t dy, - stg_meters_t dz ); - - /** remove all blocks from this model, freeing their memory */ - void ClearBlocks(); - - /** Returns a pointer to this model's parent model, or NULL if this - model has no parent */ - Model* Parent(){ return this->parent; } - - Model* GetModel( const char* name ); - //int GuiMask(){ return this->gui_mask; }; - - /** Returns a pointer to the world that contains this model */ - World* GetWorld(){ return this->world; } - - /** return the root model of the tree containing this model */ - Model* Root(){ return( parent ? parent->Root() : this ); } - - bool IsAntecedent( Model* testmod ); - - /** returns true if model [testmod] is a descendent of this model */ - bool IsDescendent( Model* testmod ); - - /** returns true if model [testmod] is a descendent or antecedent of this model */ - bool IsRelated( Model* testmod ); - - /** get the pose of a model in the global CS */ - Pose GetGlobalPose(); - - /** get the velocity of a model in the global CS */ - Velocity GetGlobalVelocity(); - - /* set the velocity of a model in the global coordinate system */ - void SetGlobalVelocity( Velocity gvel ); - - /** subscribe to a model's data */ - void Subscribe(); - - /** unsubscribe from a model's data */ - void Unsubscribe(); - - /** set the pose of model in global coordinates */ - void SetGlobalPose( Pose gpose ); - - /** set a model's velocity in its parent's coordinate system */ - void SetVelocity( Velocity vel ); - - /** set a model's pose in its parent's coordinate system */ - void SetPose( Pose pose ); - - /** add values to a model's pose in its parent's coordinate system */ - void AddToPose( Pose pose ); - - /** add values to a model's pose in its parent's coordinate system */ - void AddToPose( double dx, double dy, double dz, double da ); - - /** set a model's geometry (size and center offsets) */ - void SetGeom( Geom src ); - - /** set a model's geometry (size and center offsets) */ - void SetFiducialReturn( int fid ); - - /** set a model's fiducial key: only fiducial finders with a - matching key can detect this model as a fiducial. */ - void SetFiducialKey( int key ); - - stg_color_t GetColor(){ return color; } - - // stg_laser_return_t GetLaserReturn(){ return laser_return; } - - /** Change a model's parent - experimental*/ - int SetParent( Model* newparent); - - /** Get a model's geometry - it's size and local pose (offset from - origin in local coords) */ - Geom GetGeom(){ return geom; } - - /** Get the pose of a model in its parent's coordinate system */ - Pose GetPose(){ return pose; } - - /** Get a model's velocity (in its local reference frame) */ - Velocity GetVelocity(){ return velocity; } - - // guess what these do? - void SetColor( stg_color_t col ); - void SetMass( stg_kg_t mass ); - void SetStall( stg_bool_t stall ); - void SetGripperReturn( int val ); - void SetLaserReturn( stg_laser_return_t val ); - void SetObstacleReturn( int val ); - void SetBlobReturn( int val ); - void SetRangerReturn( int val ); - void SetBoundary( int val ); - void SetGuiNose( int val ); - void SetGuiMask( int val ); - void SetGuiGrid( int val ); - void SetGuiOutline( int val ); - void SetWatts( stg_watts_t watts ); - void SetMapResolution( stg_meters_t res ); - - bool DataIsFresh(){ return this->data_fresh; } - - /* attach callback functions to data members. The function gets - called when the member is changed using SetX() accessor method */ - - void AddCallback( void* address, - stg_model_callback_t cb, - void* user ); - - int RemoveCallback( void* member, - stg_model_callback_t callback ); - - void CallCallbacks( void* address ); - - /* wrappers for the generic callback add & remove functions that hide - some implementation detail */ - - void AddStartupCallback( stg_model_callback_t cb, void* user ) - { AddCallback( &hooks.startup, cb, user ); }; - - void RemoveStartupCallback( stg_model_callback_t cb ) - { RemoveCallback( &hooks.startup, cb ); }; - - void AddShutdownCallback( stg_model_callback_t cb, void* user ) - { AddCallback( &hooks.shutdown, cb, user ); }; - - void RemoveShutdownCallback( stg_model_callback_t cb ) - { RemoveCallback( &hooks.shutdown, cb ); } - - void AddLoadCallback( stg_model_callback_t cb, void* user ) - { AddCallback( &hooks.load, cb, user ); } - - void RemoveLoadCallback( stg_model_callback_t cb ) - { RemoveCallback( &hooks.load, cb ); } - - void AddSaveCallback( stg_model_callback_t cb, void* user ) - { AddCallback( &hooks.save, cb, user ); } - - void RemoveSaveCallback( stg_model_callback_t cb ) - { RemoveCallback( &hooks.save, cb ); } - - void AddUpdateCallback( stg_model_callback_t cb, void* user ) - { AddCallback( &hooks.update, cb, user ); } - - void RemoveUpdateCallback( stg_model_callback_t cb ) - { RemoveCallback( &hooks.update, cb ); } - - /** named-property interface - */ - void* GetProperty( char* key ); - - /** @brief Set a named property of a Stage model. - - Set a property of a Stage model. - - This function can set both predefined and user-defined - properties of a model. Predefined properties are intrinsic to - every model, such as pose and color. Every supported predefined - properties has its identifying string defined as a preprocessor - macro in stage.h. Users should use the macro instead of a - hard-coded string, so that the compiler can help you to avoid - mis-naming properties. - - User-defined properties allow the user to attach arbitrary data - pointers to a model. User-defined property data is not copied, - so the original pointer must remain valid. User-defined property - names are simple strings. Names beginning with an underscore - ('_') are reserved for internal libstage use: users should not - use names beginning with underscore (at risk of causing very - weird behaviour). - - Any callbacks registered for the named property will be called. - - Returns 0 on success, or a positive error code on failure. - - *CAUTION* The caller is responsible for making sure the pointer - points to data of the correct type for the property, so use - carefully. Check the docs or the equivalent - stg_model_set_<property>() function definition to see the type - of data required for each property. - */ - int SetProperty( char* key, void* data ); - void UnsetProperty( char* key ); - - virtual void Print( char* prefix ); - virtual const char* PrintWithPose(); - - /** Convert a pose in the world coordinate system into a model's - local coordinate system. Overwrites [pose] with the new - coordinate. */ - Pose GlobalToLocal( const Pose pose ); - - /** 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 ); - - /** Return the 3d point in world coordinates of a 3d point - specified in the model's local coordinate system */ - stg_point3_t LocalToGlobal( const stg_point3_t local ); - - /** returns the first descendent of this model that is unsubscribed - and has the type indicated by the string */ - Model* GetUnsubscribedModelOfType( const stg_model_type_t type ); - - /** returns the first descendent of this model that is unused - 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(){ return this->stall; } -}; - - -// BLOBFINDER MODEL -------------------------------------------------------- - -/** blobfinder data packet - */ -typedef struct -{ - stg_color_t color; - uint32_t left, top, right, bottom; - stg_meters_t range; -} stg_blobfinder_blob_t; - -/// %ModelBlobfinder class -class ModelBlobfinder : public Model -{ -private: - GArray* colors; - GArray* blobs; - - // predicate for ray tracing - static bool BlockMatcher( Block* testblock, Model* finder ); - - static Option showBlobData; - - virtual void DataVisualize( Camera* cam ); - -public: - unsigned int scan_width; - unsigned int scan_height; - stg_meters_t range; - stg_radians_t fov; - stg_radians_t pan; - - static const char* typestr; - - // constructor - ModelBlobfinder( World* world, - Model* parent ); - // destructor - ~ModelBlobfinder(); - - virtual void Startup(); - virtual void Shutdown(); - virtual void Update(); - virtual void Load(); - - stg_blobfinder_blob_t* GetBlobs( unsigned int* count ) - { - if( count ) *count = blobs->len; - return (stg_blobfinder_blob_t*)blobs->data; - } - - /** Start finding blobs with this color.*/ - void AddColor( stg_color_t col ); - - /** Stop tracking blobs with this color */ - void RemoveColor( stg_color_t col ); - - /** Stop tracking all colors. Call this to clear the defaults, then - add colors individually with AddColor(); */ - void RemoveAllColors(); -}; - - - - -// LASER MODEL -------------------------------------------------------- - -/** laser sample packet - */ -typedef struct -{ - stg_meters_t range; ///< range to laser hit in meters - double reflectance; ///< intensity of the reflection 0.0 to 1.0 -} stg_laser_sample_t; - -typedef struct -{ - uint32_t sample_count; - uint32_t resolution; - Bounds range_bounds; - stg_radians_t fov; - stg_usec_t interval; -} stg_laser_cfg_t; - -/// %ModelLaser class -class ModelLaser : public Model -{ -private: - /** OpenGL displaylist for laser data */ - int data_dl; - bool data_dirty; - - stg_laser_sample_t* samples; - uint32_t sample_count; - stg_meters_t range_min, range_max; - stg_radians_t fov; - uint32_t resolution; - - static Option showLaserData; - static Option showLaserStrikes; - -public: - static const char* typestr; - // constructor - ModelLaser( World* world, - Model* parent ); - - // destructor - ~ModelLaser(); - - virtual void Startup(); - virtual void Shutdown(); - virtual void Update(); - virtual void Load(); - virtual void Print( char* prefix ); - virtual void DataVisualize( Camera* cam ); - - uint32_t GetSampleCount(){ return sample_count; } - - stg_laser_sample_t* GetSamples( uint32_t* count=NULL); - - void SetSamples( stg_laser_sample_t* samples, uint32_t count); - - // Get the user-tweakable configuration of the laser - stg_laser_cfg_t GetConfig( ); - - // Set the user-tweakable configuration of the laser - void SetConfig( stg_laser_cfg_t cfg ); -}; - -// \todo GRIPPER MODEL -------------------------------------------------------- - -// typedef enum { -// STG_GRIPPER_PADDLE_OPEN = 0, // default state -// STG_GRIPPER_PADDLE_CLOSED, -// STG_GRIPPER_PADDLE_OPENING, -// STG_GRIPPER_PADDLE_CLOSING, -// } stg_gripper_paddle_state_t; - -// typedef enum { -// STG_GRIPPER_LIFT_DOWN = 0, // default state -// STG_GRIPPER_LIFT_UP, -// STG_GRIPPER_LIFT_UPPING, // verbed these to match the paddle state -// STG_GRIPPER_LIFT_DOWNING, -// } stg_gripper_lift_state_t; - -// typedef enum { -// STG_GRIPPER_CMD_NOP = 0, // default state -// STG_GRIPPER_CMD_OPEN, -// STG_GRIPPER_CMD_CLOSE, -// STG_GRIPPER_CMD_UP, -// STG_GRIPPER_CMD_DOWN -// } stg_gripper_cmd_type_t; - -// /** gripper configuration packet -// */ -// typedef struct -// { -// Size paddle_size; ///< paddle dimensions - -// stg_gripper_paddle_state_t paddles; -// stg_gripper_lift_state_t lift; - -// double paddle_position; ///< 0.0 = full open, 1.0 full closed -// double lift_position; ///< 0.0 = full down, 1.0 full up - -// stg_meters_t inner_break_beam_inset; ///< distance from the end of the paddle -// stg_meters_t outer_break_beam_inset; ///< distance from the end of the paddle -// stg_bool_t paddles_stalled; // true iff some solid object stopped -// // the paddles closing or opening - -// GSList *grip_stack; ///< stack of items gripped -// int grip_stack_size; ///< maximum number of objects in stack, or -1 for unlimited - -// double close_limit; ///< How far the gripper can close. If < 1.0, the gripper has its mouth full. - -// } stg_gripper_config_t; - -// /** gripper command packet -// */ -// typedef struct -// { -// stg_gripper_cmd_type_t cmd; -// int arg; -// } stg_gripper_cmd_t; - - -// /** gripper data packet -// */ -// typedef struct -// { -// stg_gripper_paddle_state_t paddles; -// stg_gripper_lift_state_t lift; - -// double paddle_position; ///< 0.0 = full open, 1.0 full closed -// double lift_position; ///< 0.0 = full down, 1.0 full up - -// stg_bool_t inner_break_beam; ///< non-zero iff beam is broken -// stg_bool_t outer_break_beam; ///< non-zero iff beam is broken - -// stg_bool_t paddle_contacts[2]; ///< non-zero iff paddles touch something - -// stg_bool_t paddles_stalled; // true iff some solid object stopped -// // the paddles closing or opening - -// int stack_count; ///< number of objects in stack - - -// } stg_gripper_data_t; - - -// \todo BUMPER MODEL -------------------------------------------------------- - -// typedef struct -// { -// Pose pose; -// stg_meters_t length; -// } stg_bumper_config_t; - -// typedef struct -// { -// Model* hit; -// stg_point_t hit_point; -// } stg_bumper_sample_t; - - -// FIDUCIAL MODEL -------------------------------------------------------- - -/** fiducial config packet - */ -typedef struct -{ - stg_meters_t max_range_anon; //< maximum detection range - stg_meters_t max_range_id; ///< maximum range at which the ID can be read - stg_meters_t min_range; ///< minimum detection range - stg_radians_t fov; ///< field of view - stg_radians_t heading; ///< center of field of view - - /// only detects fiducials with a key string that matches this one - /// (defaults to NULL) - int key; -} stg_fiducial_config_t; - -/** fiducial data packet - */ -typedef struct -{ - stg_meters_t range; ///< range to the target - stg_radians_t bearing; ///< bearing to the target - Pose geom; ///< size and relative angle of the target - Pose pose; ///< Absolute accurate position of the target in world coordinates (it's cheating to use this in robot controllers!) - int id; ///< the identifier of the target, or -1 if none can be detected. - -} stg_fiducial_t; - -/// %ModelFiducial class -class ModelFiducial : public Model -{ -private: - // if neighbor is visible, add him to the fiducial scan - void AddModelIfVisible( Model* him ); - - // static wrapper function can be used as a function pointer - static int AddModelIfVisibleStatic( Model* him, ModelFiducial* me ) - { if( him != me ) me->AddModelIfVisible( him ); return 0; } - - virtual void Update(); - virtual void DataVisualize( Camera* cam ); - - GArray* data; - - static Option showFiducialData; - -public: - static const char* typestr; - // constructor - ModelFiducial( World* world, - Model* parent ); - // destructor - virtual ~ModelFiducial(); - - virtual void Load(); - void Shutdown( void ); - - stg_meters_t max_range_anon; //< maximum detection range - stg_meters_t max_range_id; ///< maximum range at which the ID can be read - stg_meters_t min_range; ///< minimum detection range - stg_radians_t fov; ///< field of view - stg_radians_t heading; ///< center of field of view - int key; ///< /// only detect fiducials with a key that matches this one (defaults 0) - - stg_fiducial_t* fiducials; ///< array of detected fiducials - uint32_t fiducial_count; ///< the number of fiducials detected -}; - - -// RANGER MODEL -------------------------------------------------------- - -typedef struct -{ - Pose pose; - Size size; - Bounds bounds_range; - stg_radians_t fov; - int ray_count; -} stg_ranger_sensor_t; - -/// %ModelRanger class -class ModelRanger : public Model -{ -protected: - - virtual void Startup(); - virtual void Shutdown(); - virtual void Update(); - virtual void DataVisualize( Camera* cam ); - -public: - static const char* typestr; - // constructor - ModelRanger( World* world, - Model* parent ); - // destructor - virtual ~ModelRanger(); - - virtual void Load(); - virtual void Print( char* prefix ); - - uint32_t sensor_count; - stg_ranger_sensor_t* sensors; - stg_meters_t* samples; - -private: - static Option showRangerData; - static Option showRangerTransducers; - -}; - -// BLINKENLIGHT MODEL ---------------------------------------------------- -class ModelBlinkenlight : public Model -{ -private: - double dutycycle; - bool enabled; - stg_msec_t period; - bool on; - - static Option showBlinkenData; -public: - - static const char* typestr; - ModelBlinkenlight( World* world, - Model* parent ); - - ~ModelBlinkenlight(); - - virtual void Load(); - virtual void Update(); - virtual void DataVisualize( Camera* cam ); -}; - -// CAMERA MODEL ---------------------------------------------------- -typedef struct { - // GL_V3F - GLfloat x, y, z; -} ColoredVertex; - -/// %ModelCamera class -class ModelCamera : public Model -{ -private: - Canvas* _canvas; - - GLfloat* _frame_data; //opengl read buffer - GLubyte* _frame_color_data; //opengl read buffer - - bool _valid_vertexbuf_cache; - ColoredVertex* _vertexbuf_cache; //cached unit vectors with appropriate rotations (these must be scalled by z-buffer length) - - int _width; //width of buffer - int _height; //height of buffer - static const int _depth = 4; - - int _camera_quads_size; - GLfloat* _camera_quads; - GLubyte* _camera_colors; - - static Option showCameraData; - - PerspectiveCamera _camera; - float _yaw_offset; //position camera is mounted at - float _pitch_offset; - - ///Take a screenshot from the camera's perspective. return: true for sucess, and data is available via FrameDepth() / FrameColor() - bool GetFrame(); - -public: - ModelCamera( World* world, - Model* parent ); - - ~ModelCamera(); - - virtual void Load(); - - ///Capture a new frame ( calls GetFrame ) - virtual void Update(); - - ///Draw Camera Model - TODO - //virtual void Draw( uint32_t flags, Canvas* canvas ); - - ///Draw camera visualization - virtual void DataVisualize( Camera* cam ); - - ///width of captured image - inline int getWidth( void ) const { return _width; } - - ///height of captured image - inline int getHeight( void ) const { return _height; } - - ///get reference to camera used - inline const PerspectiveCamera& getCamera( void ) const { return _camera; } - - ///get a reference to camera depth buffer - inline const GLfloat* FrameDepth() const { return _frame_data; } - - ///get a reference to camera color image. 3 bytes (RGB) per pixel - inline const GLubyte* FrameColor() const { return _frame_color_data; } - - ///change the pitch - inline void setPitch( float pitch ) { _pitch_offset = pitch; _valid_vertexbuf_cache = false; } - - ///change the yaw - inline void setYaw( float yaw ) { _yaw_offset = yaw; _valid_vertexbuf_cache = false; } -}; - -// POSITION MODEL -------------------------------------------------------- - -/** Define a position control method */ -typedef enum - { STG_POSITION_CONTROL_VELOCITY, - STG_POSITION_CONTROL_POSITION - } stg_position_control_mode_t; - -/** Define a localization method */ -typedef enum - { STG_POSITION_LOCALIZATION_GPS, - STG_POSITION_LOCALIZATION_ODOM - } stg_position_localization_mode_t; - -/** Define a driving method */ -typedef enum - { STG_POSITION_DRIVE_DIFFERENTIAL, - STG_POSITION_DRIVE_OMNI, - STG_POSITION_DRIVE_CAR - } stg_position_drive_mode_t; - - -/// %ModelPosition class -class ModelPosition : public Model -{ - friend class Canvas; - -private: - Pose goal; //< the current velocity or pose to reach, depending on the value of control_mode - stg_position_control_mode_t control_mode; - stg_position_drive_mode_t drive_mode; - stg_position_localization_mode_t localization_mode; ///< global or local mode - Velocity integration_error; ///< errors to apply in simple odometry model - - Waypoint* waypoints; - uint32_t waypoint_count; - void DrawWaypoints(); - -private: - static Option showCoords; - static Option showWaypoints; - -public: - static const char* typestr; - // constructor - ModelPosition( World* world, - Model* parent ); - // destructor - ~ModelPosition(); - - virtual void Startup(); - virtual void Shutdown(); - virtual void Update(); - virtual void Load(); - - virtual void DataVisualize( Camera* cam ); - - /** Set the waypoint array pointer. Returns the old pointer, in case you need to free/delete[] it */ - Waypoint* SetWaypoints( Waypoint* wps, uint32_t count ); - - /** Set the current pose estimate.*/ - void SetOdom( Pose odom ); - - /** Sets the control_mode to STG_POSITION_CONTROL_VELOCITY and sets - the goal velocity. */ - void SetSpeed( double x, double y, double a ); - void SetXSpeed( double x ); - void SetYSpeed( double y ); - void SetZSpeed( double z ); - void SetTurnSpeed( double a ); - void SetSpeed( Velocity vel ); - - /** Sets the control mode to STG_POSITION_CONTROL_POSITION and sets - the goal pose */ - void GoTo( double x, double y, double a ); - void GoTo( Pose pose ); - - // localization state - Pose est_pose; ///< position estimate in local coordinates - Pose est_pose_error; ///< estimated error in position estimate - Pose est_origin; ///< global origin of the local coordinate system -}; - -#endif // MODEL_H Modified: code/stage/trunk/libstage/model_blinkenlight.cc =================================================================== --- code/stage/trunk/libstage/model_blinkenlight.cc 2009-01-22 21:36:27 UTC (rev 7286) +++ code/stage/trunk/libstage/model_blinkenlight.cc 2009-01-22 23:58:58 UTC (rev 7287) @@ -48,8 +48,10 @@ */ //#define DEBUG 1 -#include "stage_internal.hh" +#include "stage.hh" +#include "worldfile.hh" #include "option.hh" +using namespace Stg; //TODO make instance attempt to register an option (as customvisualizations do) Option ModelBlinkenlight::showBlinkenData( "Show Blink", "show_blinken", "", true, NULL ); Modified: code/stage/trunk/libstage/model_blobfinder.cc =================================================================== --- code/stage/trunk/libstage/model_blobfinder.cc 2009-01-22 21:36:27 UTC (rev 7286) +++ code/stage/trunk/libstage/model_blobfinder.cc 2009-01-22 23:58:58 UTC (rev 7287) @@ -14,8 +14,12 @@ //#define DEBUG #include <sys/time.h> -#include "stage_internal.hh" +#include "stage.hh" +#include "option.hh" +#include "worldfile.hh" +using namespace Stg; + static const stg_watts_t DEFAULT_BLOBFINDERWATTS = 2.0; static const stg_meters_t DEFAULT_BLOBFINDERRANGE = 12.0; static const stg_radians_t DEFAULT_BLOBFINDERFOV = M_PI/3.0; Modified: code/stage/trunk/libstage/model_callbacks.cc =================================================================== --- code/stage/trunk/libstage/model_callbacks.cc 2009-01-22 21:36:27 UTC (rev 7286) +++ code/stage/trunk/libstage/model_callbacks.cc 2009-01-22 23:58:58 UTC (rev 7287) @@ -1,4 +1,5 @@ -#include "stage_internal.hh" +#include "stage.hh" +using namespace Stg; int key_gen( Model* mod, void* address ) { Modified: code/stage/trunk/libstage/model_camera.cc =================================================================== --- code/stage/trunk/libstage/model_camera.cc 2009-01-22 21:36:27 UTC (rev 7286) +++ code/stage/trunk/libstage/model_camera.cc 2009-01-22 23:58:58 UTC (rev 7287) @@ -13,7 +13,11 @@ #define CAMERA_FAR_CLIP 8.0 //#define DEBUG 1 -#include "stage_internal.hh" +#include "canvas.hh" +#include "worldfile.hh" + +using namespace Stg; + #include <sstream> #include <iomanip> Modified: code/stage/trunk/libstage/model_fiducial.cc =================================================================== --- code/stage/trunk/libstage/model_fiducial.cc 2009-01-22 21:36:27 UTC (rev 7286) +++ code/stage/trunk/libstage/model_fiducial.cc 2009-01-22 23:58:58 UTC (rev 7287) @@ -15,8 +15,13 @@ #include <assert.h> #include <math.h> -#include "stage_internal.hh" +#include "stage.hh" +#include "gl.hh" +#include "option.hh" +#include "worldfile.hh" +using namespace Stg; + const stg_meters_t DEFAULT_FIDUCIAL_RANGEMIN = 0.0; const stg_meters_t DEFAULT_FIDUCIAL_RANGEMAXID = 5.0; const stg_meters_t DEFAULT_FIDUCIAL_RANGEMAXANON = 8.0; @@ -176,10 +181,12 @@ range = ray.range; Model* hitmod = ray.mod; - // printf( "ray hit %s and was seeking LOS to %s\n", - //hitmod ? hitmod->Token() : "null", - //him->Token() ); +// printf( "ray hit %s and was seeking LOS to %s\n", +// hitmod ? hitmod->Token() : "null", +// him->Token() ); + //assert( ! (hitmod == this) ); + // if it was him, we can see him if( hitmod == him ) { @@ -296,7 +303,7 @@ glEnd(); glPushMatrix(); - gl_coord_shift( dx,dy,0,fid.geom.a ); + Gl::coord_shift( dx,dy,0,fid.geom.a ); glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); glRectf( -fid.geom.x/2.0, -fid.geom.y/2.0, @@ -307,7 +314,7 @@ snprintf(idstr, 31, "%d", fid.id ); PushColor( 0,0,0,1 ); // black - gl_draw_string( 0,0,0, idstr ); + Gl::draw_string( 0,0,0, idstr ); PopColor(); glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2009-01-22 21:36:27 UTC (rev 7286) +++ code/stage/trunk/libstage/model_laser.cc 2009-01-22 23:58:58 UTC (rev 7287) @@ -14,8 +14,12 @@ //#define DEBUG #include <sys/time.h> -#include "stage_internal.hh" +#include "stage.hh" +#include "worldfile.... [truncated message content] |
From: <rt...@us...> - 2009-01-23 23:39:12
|
Revision: 7291 http://playerstage.svn.sourceforge.net/playerstage/?rev=7291&view=rev Author: rtv Date: 2009-01-23 23:39:04 +0000 (Fri, 23 Jan 2009) Log Message: ----------- moved Stg::Gl namespace into stage.hh and removed gl.hh Modified Paths: -------------- code/stage/trunk/libstage/blockgroup.cc code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/gl.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_fiducial.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/region.cc code/stage/trunk/libstage/stage.hh Removed Paths: ------------- code/stage/trunk/libstage/gl.hh Modified: code/stage/trunk/libstage/blockgroup.cc =================================================================== --- code/stage/trunk/libstage/blockgroup.cc 2009-01-23 21:49:08 UTC (rev 7290) +++ code/stage/trunk/libstage/blockgroup.cc 2009-01-23 23:39:04 UTC (rev 7291) @@ -1,6 +1,5 @@ #include "stage.hh" -#include "gl.hh" #include "worldfile.hh" #include <libgen.h> // for dirname(3) Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-01-23 21:49:08 UTC (rev 7290) +++ code/stage/trunk/libstage/canvas.cc 2009-01-23 23:39:04 UTC (rev 7291) @@ -10,7 +10,6 @@ */ #include "stage.hh" -#include "gl.hh" #include "canvas.hh" #include "worldfile.hh" #include "texture_manager.hh" Modified: code/stage/trunk/libstage/gl.cc =================================================================== --- code/stage/trunk/libstage/gl.cc 2009-01-23 21:49:08 UTC (rev 7290) +++ code/stage/trunk/libstage/gl.cc 2009-01-23 23:39:04 UTC (rev 7291) @@ -1,5 +1,5 @@ -#include "gl.hh" +#include "stage.hh" using namespace Stg; // transform the current coordinate frame by the given pose Deleted: code/stage/trunk/libstage/gl.hh =================================================================== --- code/stage/trunk/libstage/gl.hh 2009-01-23 21:49:08 UTC (rev 7290) +++ code/stage/trunk/libstage/gl.hh 2009-01-23 23:39:04 UTC (rev 7291) @@ -1,20 +0,0 @@ - -#include "stage.hh" - -namespace Stg -{ - /** @brief Internal low-level drawing convenience routines. */ - namespace Gl - { - void pose_shift( const Pose &pose ); - void pose_inverse_shift( const Pose &pose ); - void coord_shift( double x, double y, double z, double a ); - void draw_grid( stg_bounds3d_t vol ); - /** Render a string at [x,y,z] in the current color */ - void draw_string( float x, float y, float z, const char *string); - void draw_speech_bubble( float x, float y, float z, const char* str ); - void draw_octagon( float w, float h, float m ); - void draw_vector( double x, double y, double z ); - void draw_origin( double len ); - } -} Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-01-23 21:49:08 UTC (rev 7290) +++ code/stage/trunk/libstage/model.cc 2009-01-23 23:39:04 UTC (rev 7291) @@ -108,12 +108,10 @@ #define _GNU_SOURCE #endif -//#define DEBUG 0 -#include "stage.hh" -#include "gl.hh" - #include <map> +//#define DEBUG 0 +#include "stage.hh" #include "worldfile.hh" #include "canvas.hh" #include "texture_manager.hh" Modified: code/stage/trunk/libstage/model_fiducial.cc =================================================================== --- code/stage/trunk/libstage/model_fiducial.cc 2009-01-23 21:49:08 UTC (rev 7290) +++ code/stage/trunk/libstage/model_fiducial.cc 2009-01-23 23:39:04 UTC (rev 7291) @@ -17,7 +17,6 @@ #include <math.h> #include "stage.hh" -#include "gl.hh" #include "option.hh" #include "worldfile.hh" using namespace Stg; Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2009-01-23 21:49:08 UTC (rev 7290) +++ code/stage/trunk/libstage/model_position.cc 2009-01-23 23:39:04 UTC (rev 7291) @@ -19,7 +19,6 @@ //#define DEBUG #include "stage.hh" -#include "gl.hh" #include "option.hh" #include "worldfile.hh" using namespace Stg; Modified: code/stage/trunk/libstage/region.cc =================================================================== --- code/stage/trunk/libstage/region.cc 2009-01-23 21:49:08 UTC (rev 7290) +++ code/stage/trunk/libstage/region.cc 2009-01-23 23:39:04 UTC (rev 7291) @@ -5,7 +5,6 @@ */ #include "region.hh" -#include "gl.hh" using namespace Stg; const uint32_t Region::WIDTH = REGIONWIDTH; Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-01-23 21:49:08 UTC (rev 7290) +++ code/stage/trunk/libstage/stage.hh 2009-01-23 23:39:04 UTC (rev 7291) @@ -236,8 +236,8 @@ class Pose { public: - stg_meters_t x, y, z; //< location in 3 axes - stg_radians_t a; //< rotation about the z axis. + stg_meters_t x, y, z;///< location in 3 axes + stg_radians_t a;///< rotation about the z axis. Pose( stg_meters_t x, stg_meters_t y, @@ -262,6 +262,9 @@ normalize( drand48() * (2.0 * M_PI) )); } + /** 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", @@ -270,10 +273,15 @@ }; - /** specify a 3 axis velocity in x, y and heading. */ + /** Specify a 3 axis velocity in x, y and heading. */ class Velocity : public Pose { public: + /** @param x x position in meters + @param y y position in meters + @param z z position in meters + @param a heading in radians + */ Velocity( stg_meters_t x, stg_meters_t y, stg_meters_t z, @@ -283,6 +291,9 @@ Velocity() { /*empty*/ } + /** Print velocity in human-readable format on stdout + @param prefix Character string to prepend to pose output + */ virtual void Print( const char* prefix ) { printf( "%s velocity [x:%.3f y:%.3f z:%3.f a:%.3f]\n", @@ -290,13 +301,13 @@ } }; - /** specify an object's basic geometry: position and rectangular + /** Specify an object's basic geometry: position and rectangular size. */ class Geom { public: - Pose pose; //< position - Size size; //< extent + Pose pose;///< position + Size size;///< extent void Print( const char* prefix ) { @@ -310,6 +321,8 @@ } }; + /** Specify a point in space. Arrays of Waypoints can be attached to + Models and visualized. */ class Waypoint { public: @@ -321,27 +334,32 @@ stg_color_t color; }; - /** bound a range of values, from min to max. min and max are initialized to zero. */ + /** Bound a range of values, from min to max. min and max are initialized to zero. */ class Bounds { public: - double max; //< largest value in range - double min; //< smallest value in range + /// largest value in range, initially zero + double max; + /// smallest value in range, initially zero + double min; Bounds() : max(0), min(0) { /* empty*/ }; }; - /** bound a volume along the x,y,z axes. All bounds initialized to zero. */ + /** Bound a volume along the x,y,z axes. All bounds initialized to zero. */ typedef struct { - Bounds x; //< volume extent along x axis - Bounds y; //< volume extent along y axis - Bounds z; //< volume extent along z axis + /// volume extent along x axis, intially zero + Bounds x; + /// volume extent along y axis, initially zero + Bounds y; + /// volume extent along z axis, initially zero + Bounds z; } stg_bounds3d_t; - /** define a three-dimensional bounding box, initialized to zero */ + /** Define a three-dimensional bounding box, initialized to zero */ typedef struct { Bounds x, y, z; @@ -350,8 +368,8 @@ /** define a field-of-view: an angle and range bounds */ typedef struct { - Bounds range; //< min and max range of sensor - stg_radians_t angle; //< width of viewing angle of sensor + Bounds range; ///< min and max range of sensor + stg_radians_t angle; ///< width of viewing angle of sensor } stg_fov_t; /** define a point on a 2d plane */ @@ -398,9 +416,9 @@ typedef uint32_t stg_movemask_t; - const uint32_t STG_MOVE_TRANS = (1 << 0); //< bitmask for stg_movemask_t - const uint32_t STG_MOVE_ROT = (1 << 1); //< bitmask for stg_movemask_t - const uint32_t STG_MOVE_SCALE = (1 << 2); //< bitmask for stg_movemask_t + const uint32_t STG_MOVE_TRANS = (1 << 0); ///< bitmask for stg_movemask_t + const uint32_t STG_MOVE_ROT = (1 << 1); ///< bitmask for stg_movemask_t + const uint32_t STG_MOVE_SCALE = (1 << 2); ///< bitmask for stg_movemask_t const char STG_MP_PREFIX[] = "_mp_"; const char STG_MP_POSE[] = "_mp_pose"; @@ -421,10 +439,27 @@ { LaserTransparent=0, ///<not detected by laser model LaserVisible, ///< detected by laser with a reflected intensity of 0 - LaserBright ////< detected by laser with a reflected intensity of 1 + LaserBright ///< detected by laser with a reflected intensity of 1 } stg_laser_return_t; + + /** Convenient OpenGL drawing routines, used by visualization + code. */ + namespace Gl + { + void pose_shift( const Pose &pose ); + void pose_inverse_shift( const Pose &pose ); + void coord_shift( double x, double y, double z, double a ); + void draw_grid( stg_bounds3d_t vol ); + /** Render a string at [x,y,z] in the current color */ + void draw_string( float x, float y, float z, const char *string); + void draw_speech_bubble( float x, float y, float z, const char* str ); + void draw_octagon( float w, float h, float m ); + void draw_vector( double x, double y, double z ); + void draw_origin( double len ); + } + /** @brief Interface to the internal drawing system. */ namespace Draw { @@ -1068,15 +1103,14 @@ stg_color_t GetColor(); private: - Model* mod; //< model to which this block belongs + Model* mod; ///< model to which this block belongs - size_t pt_count; //< the number of points - stg_point_t* pts; //< points defining a polygon + size_t pt_count; ///< the number of points + stg_point_t* pts; ///< points defining a polygon Size size; - - Bounds local_z; //< z extent in local coords + Bounds local_z; ///< z extent in local coords stg_color_t color; bool inherit_color; @@ -1524,12 +1558,12 @@ may be of interest to users. This allows polling the model instead of adding a data callback. */ bool data_fresh; - stg_bool_t disabled; //< if non-zero, the model is disabled + stg_bool_t disabled; ///< if non-zero, the model is disabled GList* custom_visual_list; GList* flag_list; Geom geom; Pose global_pose; - bool gpose_dirty; //< set this to indicate that global pose may have changed + bool gpose_dirty; ///< set this to indicate that global pose may have changed /** Controls our appearance and functionality in the GUI, if used */ GuiState gui; @@ -1542,8 +1576,8 @@ /** unique process-wide identifier for this model */ uint32_t id; ctrlinit_t* initfunc; - stg_usec_t interval; //< time between updates in us - stg_usec_t last_update; //< time of last update in us + stg_usec_t interval; ///< time between updates in us + stg_usec_t last_update; ///< time of last update in us bool map_caches_are_invalid; stg_meters_t map_resolution; stg_kg_t mass; @@ -1564,19 +1598,19 @@ by derived model types to store properties, and for user code to associate arbitrary items with a model. */ GData* props; - bool rebuild_displaylist; //< iff true, regenerate block display list before redraw - char* say_string; //< if non-null, this string is displayed in the GUI + bool rebuild_displaylist; ///< iff true, regenerate block display list before redraw + char* say_string; ///< if non-null, this string is displayed in the GUI stg_bool_t stall; /** Thread safety flag. Iff true, Update() may be called in parallel with other models. Defaults to false for safety */ - int subs; //< the number of subscriptions to this model + int subs; ///< the number of subscriptions to this model bool thread_safe; GArray* trail; stg_model_type_t type; - bool used; //< TRUE iff this model has been returned by GetUnusedModelOfType() + bool used; ///< TRUE iff this model has been returned by GetUnusedModelOfType() Velocity velocity; - stg_watts_t watts; //< power consumed by this model + stg_watts_t watts;///< power consumed by this model Worldfile* wf; int wf_entity; World* world; // pointer to the world in which this model exists @@ -2273,7 +2307,7 @@ */ typedef struct { - stg_meters_t max_range_anon; //< maximum detection range + stg_meters_t max_range_anon;///< maximum detection range stg_meters_t max_range_id; ///< maximum range at which the ID can be read stg_meters_t min_range; ///< minimum detection range stg_radians_t fov; ///< field of view @@ -2325,7 +2359,7 @@ virtual void Load(); void Shutdown( void ); - stg_meters_t max_range_anon; //< maximum detection range + stg_meters_t max_range_anon;///< maximum detection range stg_meters_t max_range_id; ///< maximum range at which the ID can be read stg_meters_t min_range; ///< minimum detection range stg_radians_t fov; ///< field of view @@ -2504,7 +2538,7 @@ friend class Canvas; private: - Pose goal; //< the current velocity or pose to reach, depending on the value of control_mode + Pose goal;///< the current velocity or pose to reach, depending on the value of control_mode stg_position_control_mode_t control_mode; stg_position_drive_mode_t drive_mode; stg_position_localization_mode_t localization_mode; ///< global or local mode This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-01-27 04:02:21
|
Revision: 7306 http://playerstage.svn.sourceforge.net/playerstage/?rev=7306&view=rev Author: rtv Date: 2009-01-27 04:02:15 +0000 (Tue, 27 Jan 2009) Log Message: ----------- bugfixes Modified Paths: -------------- code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_props.cc code/stage/trunk/libstage/option.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstageplugin/p_driver.cc code/stage/trunk/libstageplugin/p_fiducial.cc code/stage/trunk/libstageplugin/p_position.cc Removed Paths: ------------- code/stage/trunk/libstage/draw.cc Deleted: code/stage/trunk/libstage/draw.cc =================================================================== --- code/stage/trunk/libstage/draw.cc 2009-01-27 01:18:55 UTC (rev 7305) +++ code/stage/trunk/libstage/draw.cc 2009-01-27 04:02:15 UTC (rev 7306) @@ -1,4 +0,0 @@ -/* Author: Richard Vaughan - Date -#include "stage_internal.hh" - Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-01-27 01:18:55 UTC (rev 7305) +++ code/stage/trunk/libstage/model.cc 2009-01-27 04:02:15 UTC (rev 7306) @@ -615,18 +615,20 @@ { subs++; world->total_subs++; - + world->dirty = true; // need redraw + //printf( "subscribe to %s %d\n", token, subs ); - + // if this is the first sub, call startup - if( this->subs == 1 ) - this->Startup(); + if( subs == 1 ) + Startup(); } void Model::Unsubscribe( void ) { subs--; world->total_subs--; + world->dirty = true; // need redraw printf( "unsubscribe from %s %d\n", token, subs ); @@ -1683,9 +1685,7 @@ } Model* Model::GetUnsubscribedModelOfType( stg_model_type_t type ) -{ - printf( "searching for type %d in model %s type %d\n", type, token, this->type ); - +{ if( (this->type == type) && (this->subs == 0) ) return this; @@ -1705,7 +1705,7 @@ Model* Model::GetUnusedModelOfType( stg_model_type_t type ) { - printf( "searching for type %d in model %s type %d\n", type, token, this->type ); + //printf( "searching for type %d in model %s type %d\n", type, token, this->type ); if( (this->type == type) && (!this->used ) ) { Modified: code/stage/trunk/libstage/model_props.cc =================================================================== --- code/stage/trunk/libstage/model_props.cc 2009-01-27 01:18:55 UTC (rev 7305) +++ code/stage/trunk/libstage/model_props.cc 2009-01-27 04:02:15 UTC (rev 7306) @@ -7,16 +7,16 @@ void* Model::GetProperty( char* key ) { // see if the key has the predefined-property prefix - if( strncmp( key, STG_MP_PREFIX, strlen(STG_MP_PREFIX)) == 0 ) + if( strncmp( key, MP_PREFIX, strlen(MP_PREFIX)) == 0 ) { - if( MATCH( key, STG_MP_COLOR)) return (void*)&color; - if( MATCH( key, STG_MP_MASS)) return (void*)&mass; - if( MATCH( key, STG_MP_WATTS)) return (void*)&watts; - if( MATCH( key, STG_MP_FIDUCIAL_RETURN)) return (void*)&vis.fiducial_return; - if( MATCH( key, STG_MP_LASER_RETURN)) return (void*)&vis.laser_return; - if( MATCH( key, STG_MP_OBSTACLE_RETURN)) return (void*)&vis.obstacle_return; - if( MATCH( key, STG_MP_RANGER_RETURN)) return (void*)&vis.ranger_return; - if( MATCH( key, STG_MP_GRIPPER_RETURN)) return (void*)&vis.gripper_return; + if( MATCH( key, MP_COLOR)) return (void*)&color; + if( MATCH( key, MP_MASS)) return (void*)&mass; + if( MATCH( key, MP_WATTS)) return (void*)&watts; + if( MATCH( key, MP_FIDUCIAL_RETURN)) return (void*)&vis.fiducial_return; + if( MATCH( key, MP_LASER_RETURN)) return (void*)&vis.laser_return; + if( MATCH( key, MP_OBSTACLE_RETURN)) return (void*)&vis.obstacle_return; + if( MATCH( key, MP_RANGER_RETURN)) return (void*)&vis.ranger_return; + if( MATCH( key, MP_GRIPPER_RETURN)) return (void*)&vis.gripper_return; PRINT_WARN1( "Requested non-existent model core property \"%s\"", key ); return NULL; @@ -30,46 +30,46 @@ void* data ) { // see if the key has the predefined-property prefix - if( strncmp( key, STG_MP_PREFIX, strlen(STG_MP_PREFIX)) == 0 ) + if( strncmp( key, MP_PREFIX, strlen(MP_PREFIX)) == 0 ) { PRINT_DEBUG1( "Looking up model core property \"%s\"\n", key ); - if( MATCH(key,STG_MP_FIDUCIAL_RETURN) ) + if( MATCH(key,MP_FIDUCIAL_RETURN) ) { this->SetFiducialReturn( *(int*)data ); return 0; } - if( MATCH( key, STG_MP_LASER_RETURN ) ) + if( MATCH( key, MP_LASER_RETURN ) ) { this->SetLaserReturn( *(stg_laser_return_t*)data ); return 0; } - if( MATCH( key, STG_MP_OBSTACLE_RETURN ) ) + if( MATCH( key, MP_OBSTACLE_RETURN ) ) { this->SetObstacleReturn( *(int*)data ); return 0; } - if( MATCH( key, STG_MP_RANGER_RETURN ) ) + if( MATCH( key, MP_RANGER_RETURN ) ) { this->SetRangerReturn( *(int*)data ); return 0; } - if( MATCH( key, STG_MP_GRIPPER_RETURN ) ) + if( MATCH( key, MP_GRIPPER_RETURN ) ) { this->SetGripperReturn( *(int*)data ); return 0; } - if( MATCH( key, STG_MP_COLOR ) ) + if( MATCH( key, MP_COLOR ) ) { this->SetColor( *(int*)data ); return 0; } - if( MATCH( key, STG_MP_MASS ) ) + if( MATCH( key, MP_MASS ) ) { this->SetMass( *(double*)data ); return 0; } - if( MATCH( key, STG_MP_WATTS ) ) + if( MATCH( key, MP_WATTS ) ) { this->SetWatts( *(double*)data ); return 0; @@ -87,7 +87,7 @@ void Model::UnsetProperty( char* key ) { - if( strncmp( key, STG_MP_PREFIX, strlen(STG_MP_PREFIX)) == 0 ) + if( strncmp( key, MP_PREFIX, strlen(MP_PREFIX)) == 0 ) PRINT_WARN1( "Attempt to unset a model core property \"%s\" has no effect", key ); else g_datalist_remove_data( &this->props, key ); Modified: code/stage/trunk/libstage/option.cc =================================================================== --- code/stage/trunk/libstage/option.cc 2009-01-27 01:18:55 UTC (rev 7305) +++ code/stage/trunk/libstage/option.cc 2009-01-27 04:02:15 UTC (rev 7306) @@ -24,7 +24,7 @@ void Option::Load( Worldfile* wf, int section ) { - printf( "loading wf key %s\n", wf_token.c_str() ); + //printf( "loading wf key %s\n", wf_token.c_str() ); set( (bool)wf->ReadInt( section, wf_token.c_str(), value )); } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-01-27 01:18:55 UTC (rev 7305) +++ code/stage/trunk/libstage/stage.hh 2009-01-27 04:02:15 UTC (rev 7306) @@ -102,7 +102,7 @@ /// Copyright string const char COPYRIGHT[] = - "Copyright Richard Vaughan and contributors 2000-2008"; + "Copyright Richard Vaughan and contributors 2000-2009"; /// Author string const char AUTHORS[] = @@ -420,18 +420,18 @@ const uint32_t STG_MOVE_ROT = (1 << 1); ///< bitmask for stg_movemask_t const uint32_t STG_MOVE_SCALE = (1 << 2); ///< bitmask for stg_movemask_t - const char STG_MP_PREFIX[] = "_mp_"; - const char STG_MP_POSE[] = "_mp_pose"; - const char STG_MP_VELOCITY[] = "_mp_velocity"; - const char STG_MP_GEOM[] = "_mp_geom"; - const char STG_MP_COLOR[] = "_mp_color"; - const char STG_MP_WATTS[] = "_mp_watts"; - const char STG_MP_FIDUCIAL_RETURN[] = "_mp_fiducial_return"; - const char STG_MP_LASER_RETURN[] = "_mp_laser_return"; - const char STG_MP_OBSTACLE_RETURN[] = "_mp_obstacle_return"; - const char STG_MP_RANGER_RETURN[] = "_mp_ranger_return"; - const char STG_MP_GRIPPER_RETURN[] = "_mp_gripper_return"; - const char STG_MP_MASS[] = "_mp_mass"; + const char MP_PREFIX[] = "_mp_"; + const char MP_POSE[] = "_mp_pose"; + const char MP_VELOCITY[] = "_mp_velocity"; + const char MP_GEOM[] = "_mp_geom"; + const char MP_COLOR[] = "_mp_color"; + const char MP_WATTS[] = "_mp_watts"; + const char MP_FIDUCIAL_RETURN[] = "_mp_fiducial_return"; + const char MP_LASER_RETURN[] = "_mp_laser_return"; + const char MP_OBSTACLE_RETURN[] = "_mp_obstacle_return"; + const char MP_RANGER_RETURN[] = "_mp_ranger_return"; + const char MP_GRIPPER_RETURN[] = "_mp_gripper_return"; + const char MP_MASS[] = "_mp_mass"; /// laser return value @@ -472,20 +472,20 @@ } bounds3_t; typedef enum { - STG_D_DRAW_POINTS, - STG_D_DRAW_LINES, - STG_D_DRAW_LINE_STRIP, - STG_D_DRAW_LINE_LOOP, - STG_D_DRAW_TRIANGLES, - STG_D_DRAW_TRIANGLE_STRIP, - STG_D_DRAW_TRIANGLE_FAN, - STG_D_DRAW_QUADS, - STG_D_DRAW_QUAD_STRIP, - STG_D_DRAW_POLYGON, - STG_D_PUSH, - STG_D_POP, - STG_D_ROTATE, - STG_D_TRANSLATE, + DRAW_POINTS, + DRAW_LINES, + DRAW_LINE_STRIP, + DRAW_LINE_LOOP, + DRAW_TRIANGLES, + DRAW_TRIANGLE_STRIP, + DRAW_TRIANGLE_FAN, + DRAW_QUADS, + DRAW_QUAD_STRIP, + DRAW_POLYGON, + PUSH, + POP, + ROTATE, + TRANSLATE, } type_t; /** the start of all stg_d structures looks like this */ @@ -1919,10 +1919,15 @@ /** set a model's geometry (size and center offsets) */ void SetGeom( Geom src ); - - /** set a model's geometry (size and center offsets) */ + + /** Set a model's fiducial return value. Values less than zero + are not detected by the fiducial sensor. */ void SetFiducialReturn( int fid ); - + + /** Get a model's fiducial return value. */ + int GetFiducialReturn() + { return vis.fiducial_return; } + /** set a model's fiducial key: only fiducial finders with a matching key can detect this model as a fiducial. */ void SetFiducialKey( int key ); Modified: code/stage/trunk/libstageplugin/p_driver.cc =================================================================== --- code/stage/trunk/libstageplugin/p_driver.cc 2009-01-27 01:18:55 UTC (rev 7305) +++ code/stage/trunk/libstageplugin/p_driver.cc 2009-01-27 04:02:15 UTC (rev 7306) @@ -137,7 +137,6 @@ */ - // TODO - configs I should implement // - PLAYER_SONAR_POWER_REQ // - PLAYER_BLOBFINDER_SET_COLOR_REQ @@ -148,15 +147,14 @@ #include <unistd.h> #include <string.h> #include <math.h> - +#include "config.h" #include "p_driver.h" using namespace Stg; const char* copyright_notice = "\n * Part of the Player Project [http://playerstage.sourceforge.net]\n" -" * Copyright 2000-2007 Richard Vaughan, Brian Gerkey and contributors.\n" -" * Released under the GNU General Public License v2.\n" -" **\n"; +" * Copyright 2000-2009 Richard Vaughan, Brian Gerkey and contributors.\n" +" * Released under the GNU General Public License v2.\n"; #define STG_DEFAULT_WORLDFILE "default.world" #define DRIVER_ERROR(X) printf( "Stage driver error: %s\n", X ) @@ -195,7 +193,7 @@ // driver can support and how to create a driver instance. void StgDriver_Register(DriverTable* table) { - printf( "\n ** Stage plugin v%s **", "3.0dev" ); // XX TODO + printf( "\n ** %s plugin v%s **", PROJECT, VERSION ); // XX TODO if( !player_quiet_startup ) { @@ -417,8 +415,8 @@ player_devaddr_t* addr, stg_model_type_t type ) { - printf( "attempting to find a model under model \"%s\" of type [%d]\n", - basename, type ); + //printf( "attempting to find a model under model \"%s\" of type [%d]\n", + // basename, type ); Model* base_model = world->GetModel( basename ); Modified: code/stage/trunk/libstageplugin/p_fiducial.cc =================================================================== --- code/stage/trunk/libstageplugin/p_fiducial.cc 2009-01-27 01:18:55 UTC (rev 7305) +++ code/stage/trunk/libstageplugin/p_fiducial.cc 2009-01-27 04:02:15 UTC (rev 7306) @@ -164,11 +164,10 @@ this->addr)) { // fill in the data formatted player-like -#warning disabled this bit - player_fiducial_id_t pid; -// pid.id = mod->FiducialReturn(); - pid.id = 0; + player_fiducial_id_t pid; + pid.id = mod->GetFiducialReturn(); + // acknowledge, including the new ID this->driver->Publish(this->addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, Modified: code/stage/trunk/libstageplugin/p_position.cc =================================================================== --- code/stage/trunk/libstageplugin/p_position.cc 2009-01-27 01:18:55 UTC (rev 7305) +++ code/stage/trunk/libstageplugin/p_position.cc 2009-01-27 04:02:15 UTC (rev 7306) @@ -224,22 +224,15 @@ { if(hdr->size == sizeof(player_position2d_position_mode_req_t)) { - PRINT_WARN( "set control mode not implemented") ; - player_position2d_position_mode_req_t* req = - (player_position2d_position_mode_req_t*)data; + //player_position2d_position_mode_req_t* req = + // (player_position2d_position_mode_req_t*)data; - stg_position_control_mode_t mode = (stg_position_control_mode_t)req->state; + //stg_position_control_mode_t mode = (stg_position_control_mode_t)req->state; - // XX should this be in cfg instead? - //cmd->mode = mode; - //model_change( mod, &mod->cmd ); - - - //stg_model_set_property( mod, "position_control", &mode, sizeof(mode)); - PRINT_WARN2( "Put model %s into %s control mode", this->mod->Token(), mod ? "POSITION" : "VELOCITY" ); + PRINT_WARN( "set control mode not yet implemented") ; this->driver->Publish( this->addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-02-09 03:58:37
|
Revision: 7321 http://playerstage.svn.sourceforge.net/playerstage/?rev=7321&view=rev Author: rtv Date: 2009-02-09 03:58:32 +0000 (Mon, 09 Feb 2009) Log Message: ----------- powerpacks now set charging flag correctly Modified Paths: -------------- code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/blockgroup.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/powerpack.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc Added Paths: ----------- code/stage/trunk/libstage/model_draw.cc code/stage/trunk/libstage/model_getset.cc Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2009-02-07 02:18:31 UTC (rev 7320) +++ code/stage/trunk/libstage/CMakeLists.txt 2009-02-09 03:58:32 UTC (rev 7321) @@ -16,8 +16,10 @@ model_blinkenlight.cc model_blobfinder.cc model_callbacks.cc + model_draw.cc model_camera.cc model_fiducial.cc + model_getset.cc model_laser.cc model_load.cc model_position.cc Modified: code/stage/trunk/libstage/blockgroup.cc =================================================================== --- code/stage/trunk/libstage/blockgroup.cc 2009-02-07 02:18:31 UTC (rev 7320) +++ code/stage/trunk/libstage/blockgroup.cc 2009-02-09 03:58:32 UTC (rev 7321) @@ -27,8 +27,6 @@ { blocks = g_list_append( blocks, block ); ++count; - - block->mod->map_caches_are_invalid = true; } void BlockGroup::Clear() @@ -84,9 +82,6 @@ maxx = maxy = -billion; size.z = 0.0; // grow to largest z we see - - if( blocks ) - ((Block*)blocks->data)->mod->map_caches_are_invalid = true; for( GList* it=blocks; it; it=it->next ) // examine all the blocks { Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-02-07 02:18:31 UTC (rev 7320) +++ code/stage/trunk/libstage/model.cc 2009-02-09 03:58:32 UTC (rev 7321) @@ -113,15 +113,8 @@ //#define DEBUG 0 #include "stage.hh" #include "worldfile.hh" -#include "canvas.hh" -#include "texture_manager.hh" using namespace Stg; -// speech bubble colors -static const stg_color_t BUBBLE_FILL = 0xFFC8C8FF; // light blue/grey -static const stg_color_t BUBBLE_BORDER = 0xFF000000; // black -static const stg_color_t BUBBLE_TEXT = 0xFF000000; // black - // static members uint32_t Model::count = 0; GHashTable* Model::modelsbyid = g_hash_table_new( NULL, NULL ); @@ -216,7 +209,6 @@ initfunc(NULL), interval((stg_usec_t)1e4), // 10msec last_update(0), - map_caches_are_invalid( true ), map_resolution(0.1), mass(0), on_update_list( false ), @@ -318,7 +310,6 @@ blockgroup.CalcSize(); UnMap(); // remove any old cruft rendered during startup - map_caches_are_invalid = true; Map(); if( initfunc ) @@ -366,8 +357,6 @@ { UnMap(); blockgroup.Clear(); - map_caches_are_invalid = true; - //no need to Map() - we have no blocks NeedRedraw(); } @@ -550,66 +539,7 @@ } -// get the model's velocity in the global frame -Velocity Model::GetGlobalVelocity() -{ - Pose gpose = GetGlobalPose(); - double cosa = cos( gpose.a ); - double sina = sin( gpose.a ); - - Velocity gv; - gv.x = velocity.x * cosa - velocity.y * sina; - gv.y = velocity.x * sina + velocity.y * cosa; - gv.a = velocity.a; - - //printf( "local velocity %.2f %.2f %.2f\nglobal velocity %.2f %.2f %.2f\n", - // mod->velocity.x, mod->velocity.y, mod->velocity.a, - // gvel->x, gvel->y, gvel->a ); - - return gv; -} - -// set the model's velocity in the global frame -void Model::SetGlobalVelocity( Velocity gv ) -{ - Pose gpose = GetGlobalPose(); - - double cosa = cos( gpose.a ); - double sina = sin( gpose.a ); - - Velocity lv; - lv.x = gv.x * cosa + gv.y * sina; - lv.y = -gv.x * sina + gv.y * cosa; - lv.a = gv.a; - - this->SetVelocity( lv ); -} - -// get the model's position in the global frame -Pose Model::GetGlobalPose() -{ - // if I'm a top level model, my global pose is my local pose - if( parent == NULL ) - return pose; - - // otherwise - - Pose global_pose = pose_sum( parent->GetGlobalPose(), pose ); - - // we are on top of our parent - global_pose.z += parent->geom.size.z; - - // PRINT_DEBUG4( "GET GLOBAL POSE [x:%.2f y:%.2f z:%.2f a:%.2f]", - // global_pose.x, - // global_pose.y, - // global_pose.z, - // global_pose.a ); - - return global_pose; -} - - inline Pose Model::LocalToGlobal( Pose pose ) { return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose ); @@ -640,7 +570,6 @@ // render all blocks in the group at my global pose and size blockgroup.Map(); - map_caches_are_invalid = false; } @@ -770,390 +699,7 @@ last_update = world->sim_time; } -void Model::DrawSelected() -{ - glPushMatrix(); - - glTranslatef( pose.x, pose.y, pose.z+0.01 ); // tiny Z offset raises rect above grid - - Pose gpose = GetGlobalPose(); - - char buf[64]; - snprintf( buf, 63, "%s [%.2f %.2f %.2f %.2f]", - token, gpose.x, gpose.y, gpose.z, rtod(gpose.a) ); - - PushColor( 0,0,0,1 ); // text color black - Gl::draw_string( 0.5,0.5,0.5, buf ); - - glRotatef( rtod(pose.a), 0,0,1 ); - - Gl::pose_shift( geom.pose ); - - double dx = geom.size.x / 2.0 * 1.6; - double dy = geom.size.y / 2.0 * 1.6; - - PopColor(); - PushColor( 0,1,0,0.4 ); // highlight color blue - glRectf( -dx, -dy, dx, dy ); - PopColor(); - - PushColor( 0,1,0,0.8 ); // highlight color blue - glLineWidth( 1 ); - glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); - glRectf( -dx, -dy, dx, dy ); - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); - PopColor(); - - glPopMatrix(); -} - - -void Model::DrawTrailFootprint() -{ - double r,g,b,a; - - for( int i=trail->len-1; i>=0; i-- ) - { - stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t, i ); - - glPushMatrix(); - Gl::pose_shift( checkpoint->pose ); - Gl::pose_shift( geom.pose ); - - stg_color_unpack( checkpoint->color, &r, &g, &b, &a ); - PushColor( r, g, b, 0.1 ); - - blockgroup.DrawFootPrint( geom ); - - glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); - PushColor( r/2, g/2, b/2, 0.1 ); - - blockgroup.DrawFootPrint( geom ); - - PopColor(); - PopColor(); - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); - glPopMatrix(); - } -} - -void Model::DrawTrailBlocks() -{ - double timescale = 0.0000001; - - for( int i=trail->len-1; i>=0; i-- ) - { - stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t, i ); - - Pose pose; - memcpy( &pose, &checkpoint->pose, sizeof(pose)); - pose.z = (world->sim_time - checkpoint->time) * timescale; - - PushLocalCoords(); - //blockgroup.Draw( this ); - - DrawBlocksTree(); - PopCoords(); - } -} - -void Model::DrawTrailArrows() -{ - double r,g,b,a; - - double dx = 0.2; - double dy = 0.07; - double timescale = 0.0000001; - - for( unsigned int i=0; i<trail->len; i++ ) - { - stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t, i ); - - Pose pose; - memcpy( &pose, &checkpoint->pose, sizeof(pose)); - pose.z = (world->sim_time - checkpoint->time) * timescale; - - PushLocalCoords(); - - // set the height proportional to age - - PushColor( checkpoint->color ); - - glEnable(GL_POLYGON_OFFSET_FILL); - glPolygonOffset(1.0, 1.0); - - glBegin( GL_TRIANGLES ); - glVertex3f( 0, -dy, 0); - glVertex3f( dx, 0, 0 ); - glVertex3f( 0, +dy, 0 ); - glEnd(); - glDisable(GL_POLYGON_OFFSET_FILL); - - glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); - - stg_color_unpack( checkpoint->color, &r, &g, &b, &a ); - PushColor( r/2, g/2, b/2, 1 ); // darker color - - glDepthMask(GL_FALSE); - glBegin( GL_TRIANGLES ); - glVertex3f( 0, -dy, 0); - glVertex3f( dx, 0, 0 ); - glVertex3f( 0, +dy, 0 ); - glEnd(); - glDepthMask(GL_TRUE); - - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); - PopColor(); - PopColor(); - PopCoords(); - } -} - -void Model::DrawOriginTree() -{ - DrawPose( GetGlobalPose() ); - for( GList* it=children; it; it=it->next ) - ((Model*)it->data)->DrawOriginTree(); -} - - -void Model::DrawBlocksTree( ) -{ - PushLocalCoords(); - LISTMETHOD( children, Model*, DrawBlocksTree ); - DrawBlocks(); - PopCoords(); -} - -void Model::DrawPose( Pose pose ) -{ - PushColor( 0,0,0,1 ); - glPointSize( 4 ); - - glBegin( GL_POINTS ); - glVertex3f( pose.x, pose.y, pose.z ); - glEnd(); - - PopColor(); -} - -void Model::DrawBlocks( ) -{ - blockgroup.CallDisplayList( this ); -} - -void Model::DrawBoundingBoxTree() -{ - PushLocalCoords(); - LISTMETHOD( children, Model*, DrawBoundingBoxTree ); - DrawBoundingBox(); - PopCoords(); -} - -void Model::DrawBoundingBox() -{ - Gl::pose_shift( geom.pose ); - - PushColor( color ); - - glBegin( GL_QUAD_STRIP ); - - glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, geom.size.z ); - glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, 0 ); - - glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, geom.size.z ); - glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, 0 ); - - glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, geom.size.z ); - glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, 0 ); - - glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, geom.size.z ); - glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, 0 ); - - glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, geom.size.z ); - glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, 0 ); - - glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, geom.size.z ); - glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, 0 ); - - glEnd(); - - glBegin( GL_LINES ); - glVertex2f( -0.02, 0 ); - glVertex2f( +0.02, 0 ); - - glVertex2f( 0, -0.02 ); - glVertex2f( 0, +0.02 ); - glEnd(); - - PopColor(); -} - -// move into this model's local coordinate frame -void Model::PushLocalCoords() -{ - glPushMatrix(); - - if( parent ) - glTranslatef( 0,0, parent->geom.size.z ); - - Gl::pose_shift( pose ); -} - -void Model::PopCoords() -{ - glPopMatrix(); -} - -void Model::AddCustomVisualizer( CustomVisualizer* custom_visual ) -{ - if( !custom_visual ) - return; - - //Visualizations can only be added to stage when run in a GUI - if( world_gui == NULL ) { - printf( "Unable to add custom visualization - it must be run with a GUI world\n" ); - return; - } - - //save visual instance - custom_visual_list = g_list_append(custom_visual_list, custom_visual ); - - //register option for all instances which share the same name - Canvas* canvas = world_gui->GetCanvas(); - std::map< std::string, Option* >::iterator i = canvas->_custom_options.find( custom_visual->name() ); - if( i == canvas->_custom_options.end() ) { - Option* op = new Option( custom_visual->name(), custom_visual->name(), "", true, world_gui ); - canvas->_custom_options[ custom_visual->name() ] = op; - registerOption( op ); - } -} - -void Model::RemoveCustomVisualizer( CustomVisualizer* custom_visual ) -{ - if( custom_visual ) - custom_visual_list = g_list_remove(custom_visual_list, custom_visual ); - - //TODO unregister option - tricky because there might still be instances attached to different models which have the same name -} - - -void Model::DrawStatusTree( Camera* cam ) -{ - PushLocalCoords(); - DrawStatus( cam ); - LISTMETHODARG( children, Model*, DrawStatusTree, cam ); - PopCoords(); -} - -void Model::DrawStatus( Camera* cam ) -{ - // quick hack -// if( power_pack && power_pack->stored < 0.0 ) -// { -// glPushMatrix(); -// glTranslatef( 0.3, 0, 0.0 ); -// DrawImage( TextureManager::getInstance()._mains_texture_id, cam, 0.85 ); -// glPopMatrix(); -// } - - if( say_string || power_pack ) - { - float yaw, pitch; - pitch = - cam->pitch(); - yaw = - cam->yaw(); - - Pose gpz = GetGlobalPose(); - - float robotAngle = -rtod(gpz.a); - glPushMatrix(); - - - // move above the robot - glTranslatef( 0, 0, 0.5 ); - - // rotate to face screen - glRotatef( robotAngle - yaw, 0,0,1 ); - glRotatef( -pitch, 1,0,0 ); - - - //if( ! parent ) - // glRectf( 0,0,1,1 ); - - //if( power_pack->stored > 0.0 ) - power_pack->Visualize( cam ); - - if( say_string ) - { - //get raster positition, add gl_width, then project back to world coords - glRasterPos3f( 0, 0, 0 ); - GLfloat pos[ 4 ]; - glGetFloatv(GL_CURRENT_RASTER_POSITION, pos); - - GLboolean valid; - glGetBooleanv( GL_CURRENT_RASTER_POSITION_VALID, &valid ); - - if( valid ) - { - - fl_font( FL_HELVETICA, 12 ); - float w = gl_width( this->say_string ); // scaled text width - float h = gl_height(); // scaled text height - - GLdouble wx, wy, wz; - GLint viewport[4]; - glGetIntegerv(GL_VIEWPORT, viewport); - - GLdouble modelview[16]; - glGetDoublev(GL_MODELVIEW_MATRIX, modelview); - - GLdouble projection[16]; - glGetDoublev(GL_PROJECTION_MATRIX, projection); - - //get width and height in world coords - gluUnProject( pos[0] + w, pos[1], pos[2], modelview, projection, viewport, &wx, &wy, &wz ); - w = wx; - gluUnProject( pos[0], pos[1] + h, pos[2], modelview, projection, viewport, &wx, &wy, &wz ); - h = wy; - - // calculate speech bubble margin - const float m = h/10; - - // draw inside of bubble - PushColor( BUBBLE_FILL ); - glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT ); - glPolygonMode( GL_FRONT, GL_FILL ); - glEnable( GL_POLYGON_OFFSET_FILL ); - glPolygonOffset( 1.0, 1.0 ); - Gl::draw_octagon( w, h, m ); - glDisable( GL_POLYGON_OFFSET_FILL ); - PopColor(); - - // draw outline of bubble - PushColor( BUBBLE_BORDER ); - glLineWidth( 1 ); - glEnable( GL_LINE_SMOOTH ); - glPolygonMode( GL_FRONT, GL_LINE ); - Gl::draw_octagon( w, h, m ); - glPopAttrib(); - PopColor(); - - PushColor( BUBBLE_TEXT ); - // draw text inside the bubble - Gl::draw_string( 2.5*m, 2.5*m, 0, this->say_string ); - PopColor(); - } - } - glPopMatrix(); - } - - if( stall ) - { - DrawImage( TextureManager::getInstance()._stall_texture_id, cam, 0.85 ); - } -} - stg_meters_t Model::ModelHeight() { stg_meters_t m_child = 0; //max size of any child @@ -1170,272 +716,8 @@ } -void Model::DrawImage( uint32_t texture_id, Camera* cam, float alpha, double width, double height ) -{ - float yaw, pitch; - pitch = - cam->pitch(); - yaw = - cam->yaw(); - float robotAngle = -rtod(pose.a); - glEnable(GL_TEXTURE_2D); - glBindTexture( GL_TEXTURE_2D, texture_id ); - glColor4f( 1.0, 1.0, 1.0, alpha ); - glPushMatrix(); - - //position image above the robot - glTranslatef( 0.0, 0.0, ModelHeight() + 0.3 ); - - // rotate to face screen - glRotatef( robotAngle - yaw, 0,0,1 ); - glRotatef( -pitch - 90, 1,0,0 ); - - //draw a square, with the textured image - glBegin(GL_QUADS); - glTexCoord2f(0.0f, 0.0f); glVertex3f(-0.25f, 0, -0.25f ); - glTexCoord2f(width, 0.0f); glVertex3f( 0.25f, 0, -0.25f ); - glTexCoord2f(width, height); glVertex3f( 0.25f, 0, 0.25f ); - glTexCoord2f(0.0f, height); glVertex3f(-0.25f, 0, 0.25f ); - glEnd(); - - glPopMatrix(); - glBindTexture( GL_TEXTURE_2D, 0 ); - glDisable(GL_TEXTURE_2D); -} - - -void Model::DrawFlagList( void ) -{ - if( flag_list == NULL ) - return; - - PushLocalCoords(); - - glPolygonMode( GL_FRONT, GL_FILL ); - - GLUquadric* quadric = gluNewQuadric(); - glTranslatef(0,0,1); // jump up - Pose gpose = GetGlobalPose(); - glRotatef( 180 + rtod(-gpose.a),0,0,1 ); - - - GList* list = g_list_copy( flag_list ); - list = g_list_reverse(list); - - for( GList* item = list; item; item = item->next ) - { - - Flag* flag = (Flag*)item->data; - - glTranslatef( 0, 0, flag->size/2.0 ); - - PushColor( flag->color ); - - - glEnable(GL_POLYGON_OFFSET_FILL); - glPolygonOffset(1.0, 1.0); - gluQuadricDrawStyle( quadric, GLU_FILL ); - gluSphere( quadric, flag->size/2.0, 4,2 ); - glDisable(GL_POLYGON_OFFSET_FILL); - - // draw the edges darker version of the same color - double r,g,b,a; - stg_color_unpack( flag->color, &r, &g, &b, &a ); - PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a )); - - gluQuadricDrawStyle( quadric, GLU_LINE ); - gluSphere( quadric, flag->size/2.0, 4,2 ); - - PopColor(); - PopColor(); - - glTranslatef( 0, 0, flag->size/2.0 ); - } - - g_list_free( list ); - - gluDeleteQuadric( quadric ); - - PopCoords(); -} - - -void Model::DrawBlinkenlights() -{ - PushLocalCoords(); - - GLUquadric* quadric = gluNewQuadric(); - //glTranslatef(0,0,1); // jump up - //Pose gpose = GetGlobalPose(); - //glRotatef( 180 + rtod(-gpose.a),0,0,1 ); - - for( unsigned int i=0; i<blinkenlights->len; i++ ) - { - stg_blinkenlight_t* b = - (stg_blinkenlight_t*)g_ptr_array_index( blinkenlights, i ); - assert(b); - - glTranslatef( b->pose.x, b->pose.y, b->pose.z ); - - PushColor( b->color ); - - if( b->enabled ) - gluQuadricDrawStyle( quadric, GLU_FILL ); - else - gluQuadricDrawStyle( quadric, GLU_LINE ); - - gluSphere( quadric, b->size/2.0, 8,8 ); - - PopColor(); - } - - gluDeleteQuadric( quadric ); - - PopCoords(); -} - -void Model::DrawPicker( void ) -{ - //PRINT_DEBUG1( "Drawing %s", token ); - PushLocalCoords(); - - // draw the boxes - blockgroup.DrawSolid( geom ); - - // recursively draw the tree below this model - LISTMETHOD( this->children, Model*, DrawPicker ); - - PopCoords(); -} - -void Model::DataVisualize( Camera* cam ) -{ -// if( power_pack ) -// { -// // back into global coords to get rid of my rotation -// glPushMatrix(); -// gl_pose_inverse_shift( GetGlobalPose() ); - -// // shift to the top left corner of the model (roughly) -// glTranslatef( pose.x - geom.size.x/2.0, -// pose.y + geom.size.y/2.0, -// pose.z + geom.size.z ); - -// power_pack->Visualize( cam ); -// glPopMatrix(); -// } -} - -void Model::DataVisualizeTree( Camera* cam ) -{ - PushLocalCoords(); - DataVisualize( cam ); // virtual function overridden by most model types - - CustomVisualizer* vis; - for( GList* item = custom_visual_list; item; item = item->next ) { - vis = static_cast< CustomVisualizer* >( item->data ); - if( world_gui->GetCanvas()->_custom_options[ vis->name() ]->isEnabled() ) - vis->DataVisualize( cam ); - } - - - // and draw the children - LISTMETHODARG( children, Model*, DataVisualizeTree, cam ); - - PopCoords(); -} - -void Model::DrawGrid( void ) -{ - if ( gui.grid ) - { - PushLocalCoords(); - - stg_bounds3d_t vol; - vol.x.min = -geom.size.x/2.0; - vol.x.max = geom.size.x/2.0; - vol.y.min = -geom.size.y/2.0; - vol.y.max = geom.size.y/2.0; - vol.z.min = 0; - vol.z.max = geom.size.z; - - PushColor( 0,0,1,0.4 ); - Gl::draw_grid(vol); - PopColor(); - PopCoords(); - } -} - -inline bool velocity_is_zero( Velocity& v ) -{ - return( !(v.x || v.y || v.z || v.a) ); -} - -void Model::SetVelocity( Velocity vel ) -{ - // printf( "SetVelocity %.2f %.2f %.2f %.2f\n", - // vel.x, - // vel.y, - // vel.z, - // vel.a ); - - assert( ! isnan(vel.x) ); - assert( ! isnan(vel.y) ); - assert( ! isnan(vel.z) ); - assert( ! isnan(vel.a) ); - - this->velocity = vel; - - if( on_velocity_list && velocity_is_zero( vel ) ) - { - world->StopUpdatingModelPose( this ); - on_velocity_list = false; - } - - if( (!on_velocity_list) && (!velocity_is_zero( vel )) ) - { - world->StartUpdatingModelPose( this ); - on_velocity_list = true; - } - - CallCallbacks( &this->velocity ); -} - -void Model::NeedRedraw( void ) -{ - this->rebuild_displaylist = true; - - if( parent ) - parent->NeedRedraw(); - else - world->NeedRedraw(); -} - -void Model::SetPose( Pose newpose ) -{ - //PRINT_DEBUG5( "%s.SetPose(%.2f %.2f %.2f %.2f)", - // this->token, pose->x, pose->y, pose->z, pose->a ); - - // if the pose has changed, we need to do some work - if( memcmp( &pose, &newpose, sizeof(Pose) ) != 0 ) - { - pose = newpose; - pose.a = normalize(pose.a); - - if( isnan( pose.a ) ) - printf( "SetPose bad angle %s [%.2f %.2f %.2f %.2f]\n", - token, pose.x, pose.y, pose.z, pose.a ); - - NeedRedraw(); - map_caches_are_invalid = true; - MapWithChildren(); - world->dirty = true; - } - - // register a model change even if the pose didn't actually change - CallCallbacks( &this->pose ); -} - void Model::AddToPose( double dx, double dy, double dz, double da ) { if( dx || dy || dz || da ) @@ -1455,155 +737,8 @@ this->AddToPose( pose.x, pose.y, pose.z, pose.a ); } -void Model::SetGeom( Geom geom ) -{ - //printf( "MODEL \"%s\" SET GEOM (%.2f %.2f %.2f)[%.2f %.2f]\n", - // this->token, - // geom->pose.x, geom->pose.y, geom->pose.a, - // geom->size.x, geom->size.y ); - UnMapWithChildren(); - - this->geom = geom; - - blockgroup.CalcSize(); - - NeedRedraw(); - MapWithChildren(); - - CallCallbacks( &geom ); -} - -void Model::SetColor( stg_color_t col ) -{ - this->color = col; - NeedRedraw(); - CallCallbacks( &color ); -} - -void Model::SetMass( stg_kg_t mass ) -{ - this->mass = mass; - CallCallbacks( &mass ); -} - -void Model::SetStall( stg_bool_t stall ) -{ - this->stall = stall; - CallCallbacks( &stall ); -} - -void Model::SetGripperReturn( int val ) -{ - vis.gripper_return = val; - CallCallbacks( &vis.gripper_return ); -} - -void Model::SetFiducialReturn( int val ) -{ - vis.fiducial_return = val; - CallCallbacks( &vis.fiducial_return ); -} - -void Model::SetFiducialKey( int key ) -{ - vis.fiducial_key = key; - CallCallbacks( &vis.fiducial_key ); -} - -void Model::SetLaserReturn( stg_laser_return_t val ) -{ - vis.laser_return = val; - CallCallbacks( &vis.laser_return ); -} - -void Model::SetObstacleReturn( int val ) -{ - vis.obstacle_return = val; - CallCallbacks( &vis.obstacle_return ); -} - -void Model::SetBlobReturn( int val ) -{ - vis.blob_return = val; - CallCallbacks( &vis.blob_return ); -} - -void Model::SetRangerReturn( int val ) -{ - vis.ranger_return = val; - CallCallbacks( &vis.ranger_return ); -} - -void Model::SetBoundary( int val ) -{ - boundary = val; - CallCallbacks( &boundary ); -} - -void Model::SetGuiNose( int val ) -{ - gui.nose = val; - CallCallbacks( &gui.nose ); -} - -void Model::SetGuiMask( int val ) -{ - gui.mask = val; - CallCallbacks( &gui.mask ); -} - -void Model::SetGuiGrid( int val ) -{ - gui.grid = val; - CallCallbacks( &this->gui.grid ); -} - -void Model::SetGuiOutline( int val ) -{ - gui.outline = val; - CallCallbacks( &gui.outline ); -} - -void Model::SetWatts( stg_watts_t watts ) -{ - watts = watts; - CallCallbacks( &watts ); -} - -void Model::SetMapResolution( stg_meters_t res ) -{ - map_resolution = res; - CallCallbacks( &map_resolution ); -} - -// set the pose of model in global coordinates -void Model::SetGlobalPose( Pose gpose ) -{ - SetPose( parent ? parent->GlobalToLocal( gpose ) : gpose ); - - //printf( "setting global pose %.2f %.2f %.2f = local pose %.2f %.2f %.2f\n", - // gpose->x, gpose->y, gpose->a, lpose.x, lpose.y, lpose.a ); -} - -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( newparent ) - newparent->children = g_list_append( newparent->children, this ); - - // link from the model to its new parent - this->parent = newparent; - - CallCallbacks( &this->parent ); - return 0; //ok -} - - void Model::PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax, stg_meters_t ymin, stg_meters_t ymax ) { @@ -1617,6 +752,7 @@ return blockgroup.AppendTouchingModels( list ); } + Model* Model::TestCollision() { //printf( "mod %s test collision...\n", token ); @@ -1641,21 +777,22 @@ void Model::UpdateCharge() { - //printf( "model %s is a charger. testing to see if anyone is touching\n", - // token );x - assert( watts_give > 0 ); PowerPack* mypp = FindPowerPack(); assert( mypp ); + // make sure we only charge a powerpack once + GList* ppacks_serviced = NULL; + + // run through and update all appropriate touchers for( GList* touchers = AppendTouchingModels( NULL ); touchers; touchers = touchers->next ) { - Model* toucher = (Model*)touchers->data; - - PowerPack* hispp =toucher-> FindPowerPack(); + Model* toucher = (Model*)touchers->data; + PowerPack* hispp =toucher->FindPowerPack(); + if( hispp && toucher->watts_take > 0.0) { //printf( " toucher %s can take up to %.2f wats\n", @@ -1672,7 +809,6 @@ hispp->charging = true; } } - } void Model::CommitTestedPose() @@ -1792,6 +928,16 @@ return NULL; } +void Model::NeedRedraw( void ) +{ + this->rebuild_displaylist = true; + + if( parent ) + parent->NeedRedraw(); + else + world->NeedRedraw(); +} + Model* Model::GetUnusedModelOfType( stg_model_type_t type ) { //printf( "searching for type %d in model %s type %d\n", type, token, this->type ); Added: code/stage/trunk/libstage/model_draw.cc =================================================================== --- code/stage/trunk/libstage/model_draw.cc (rev 0) +++ code/stage/trunk/libstage/model_draw.cc 2009-02-09 03:58:32 UTC (rev 7321) @@ -0,0 +1,590 @@ +#include "stage.hh" +#include "worldfile.hh" +#include "canvas.hh" +#include "texture_manager.hh" +using namespace Stg; + +// speech bubble colors +static const stg_color_t BUBBLE_FILL = 0xFFC8C8FF; // light blue/grey +static const stg_color_t BUBBLE_BORDER = 0xFF000000; // black +static const stg_color_t BUBBLE_TEXT = 0xFF000000; // black + +void Model::DrawSelected() +{ + glPushMatrix(); + + glTranslatef( pose.x, pose.y, pose.z+0.01 ); // tiny Z offset raises rect above grid + + Pose gpose = GetGlobalPose(); + + char buf[64]; + snprintf( buf, 63, "%s [%.2f %.2f %.2f %.2f]", + token, gpose.x, gpose.y, gpose.z, rtod(gpose.a) ); + + PushColor( 0,0,0,1 ); // text color black + Gl::draw_string( 0.5,0.5,0.5, buf ); + + glRotatef( rtod(pose.a), 0,0,1 ); + + Gl::pose_shift( geom.pose ); + + double dx = geom.size.x / 2.0 * 1.6; + double dy = geom.size.y / 2.0 * 1.6; + + PopColor(); + + PushColor( 0,1,0,0.4 ); // highlight color blue + glRectf( -dx, -dy, dx, dy ); + PopColor(); + + PushColor( 0,1,0,0.8 ); // highlight color blue + glLineWidth( 1 ); + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + glRectf( -dx, -dy, dx, dy ); + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + PopColor(); + + glPopMatrix(); +} + + +void Model::DrawTrailFootprint() +{ + double r,g,b,a; + + for( int i=trail->len-1; i>=0; i-- ) + { + stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t, i ); + + glPushMatrix(); + Gl::pose_shift( checkpoint->pose ); + Gl::pose_shift( geom.pose ); + + stg_color_unpack( checkpoint->color, &r, &g, &b, &a ); + PushColor( r, g, b, 0.1 ); + + blockgroup.DrawFootPrint( geom ); + + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + PushColor( r/2, g/2, b/2, 0.1 ); + + blockgroup.DrawFootPrint( geom ); + + PopColor(); + PopColor(); + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + glPopMatrix(); + } +} + +void Model::DrawTrailBlocks() +{ + double timescale = 0.0000001; + + for( int i=trail->len-1; i>=0; i-- ) + { + stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t, i ); + + Pose pose; + memcpy( &pose, &checkpoint->pose, sizeof(pose)); + pose.z = (world->sim_time - checkpoint->time) * timescale; + + PushLocalCoords(); + //blockgroup.Draw( this ); + + DrawBlocksTree(); + PopCoords(); + } +} + +void Model::DrawTrailArrows() +{ + double r,g,b,a; + + double dx = 0.2; + double dy = 0.07; + double timescale = 0.0000001; + + for( unsigned int i=0; i<trail->len; i++ ) + { + stg_trail_item_t* checkpoint = & g_array_index( trail, stg_trail_item_t, i ); + + Pose pose; + memcpy( &pose, &checkpoint->pose, sizeof(pose)); + pose.z = (world->sim_time - checkpoint->time) * timescale; + + PushLocalCoords(); + + // set the height proportional to age + + PushColor( checkpoint->color ); + + glEnable(GL_POLYGON_OFFSET_FILL); + glPolygonOffset(1.0, 1.0); + + glBegin( GL_TRIANGLES ); + glVertex3f( 0, -dy, 0); + glVertex3f( dx, 0, 0 ); + glVertex3f( 0, +dy, 0 ); + glEnd(); + glDisable(GL_POLYGON_OFFSET_FILL); + + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + + stg_color_unpack( checkpoint->color, &r, &g, &b, &a ); + PushColor( r/2, g/2, b/2, 1 ); // darker color + + glDepthMask(GL_FALSE); + glBegin( GL_TRIANGLES ); + glVertex3f( 0, -dy, 0); + glVertex3f( dx, 0, 0 ); + glVertex3f( 0, +dy, 0 ); + glEnd(); + glDepthMask(GL_TRUE); + + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + PopColor(); + PopColor(); + PopCoords(); + } +} + +void Model::DrawOriginTree() +{ + DrawPose( GetGlobalPose() ); + for( GList* it=children; it; it=it->next ) + ((Model*)it->data)->DrawOriginTree(); +} + + +void Model::DrawBlocksTree( ) +{ + PushLocalCoords(); + LISTMETHOD( children, Model*, DrawBlocksTree ); + DrawBlocks(); + PopCoords(); +} + +void Model::DrawPose( Pose pose ) +{ + PushColor( 0,0,0,1 ); + glPointSize( 4 ); + + glBegin( GL_POINTS ); + glVertex3f( pose.x, pose.y, pose.z ); + glEnd(); + + PopColor(); +} + +void Model::DrawBlocks( ) +{ + blockgroup.CallDisplayList( this ); +} + +void Model::DrawBoundingBoxTree() +{ + PushLocalCoords(); + LISTMETHOD( children, Model*, DrawBoundingBoxTree ); + DrawBoundingBox(); + PopCoords(); +} + +void Model::DrawBoundingBox() +{ + Gl::pose_shift( geom.pose ); + + PushColor( color ); + + glBegin( GL_QUAD_STRIP ); + + glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, geom.size.z ); + glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, 0 ); + + glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, geom.size.z ); + glVertex3f( +geom.size.x/2.0, -geom.size.y/2.0, 0 ); + + glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, geom.size.z ); + glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, 0 ); + + glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, geom.size.z ); + glVertex3f( +geom.size.x/2.0, +geom.size.y/2.0, 0 ); + + glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, geom.size.z ); + glVertex3f( -geom.size.x/2.0, +geom.size.y/2.0, 0 ); + + glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, geom.size.z ); + glVertex3f( -geom.size.x/2.0, -geom.size.y/2.0, 0 ); + + glEnd(); + + glBegin( GL_LINES ); + glVertex2f( -0.02, 0 ); + glVertex2f( +0.02, 0 ); + + glVertex2f( 0, -0.02 ); + glVertex2f( 0, +0.02 ); + glEnd(); + + PopColor(); +} + +// move into this model's local coordinate frame +void Model::PushLocalCoords() +{ + glPushMatrix(); + + if( parent ) + glTranslatef( 0,0, parent->geom.size.z ); + + Gl::pose_shift( pose ); +} + +void Model::PopCoords() +{ + glPopMatrix(); +} + +void Model::AddCustomVisualizer( CustomVisualizer* custom_visual ) +{ + if( !custom_visual ) + return; + + //Visualizations can only be added to stage when run in a GUI + if( world_gui == NULL ) { + printf( "Unable to add custom visualization - it must be run with a GUI world\n" ); + return; + } + + //save visual instance + custom_visual_list = g_list_append(custom_visual_list, custom_visual ); + + //register option for all instances which share the same name + Canvas* canvas = world_gui->GetCanvas(); + std::map< std::string, Option* >::iterator i = canvas->_custom_options.find( custom_visual->name() ); + if( i == canvas->_custom_options.end() ) { + Option* op = new Option( custom_visual->name(), custom_visual->name(), "", true, world_gui ); + canvas->_custom_options[ custom_visual->name() ] = op; + registerOption( op ); + } +} + +void Model::RemoveCustomVisualizer( CustomVisualizer* custom_visual ) +{ + if( custom_visual ) + custom_visual_list = g_list_remove(custom_visual_list, custom_visual ); + + //TODO unregister option - tricky because there might still be instances attached to different models which have the same name +} + + +void Model::DrawStatusTree( Camera* cam ) +{ + PushLocalCoords(); + DrawStatus( cam ); + LISTMETHODARG( children, Model*, DrawStatusTree, cam ); + PopCoords(); +} + +void Model::DrawStatus( Camera* cam ) +{ + // quick hack +// if( power_pack && power_pack->stored < 0.0 ) +// { +// glPushMatrix(); +// glTranslatef( 0.3, 0, 0.0 ); +// DrawImage( TextureManager::getInstance()._mains_texture_id, cam, 0.85 ); +// glPopMatrix(); +// } + + if( say_string || power_pack ) + { + float yaw, pitch; + pitch = - cam->pitch(); + yaw = - cam->yaw(); + + Pose gpz = GetGlobalPose(); + + float robotAngle = -rtod(gpz.a); + glPushMatrix(); + + + // move above the robot + glTranslatef( 0, 0, 0.5 ); + + // rotate to face screen + glRotatef( robotAngle - yaw, 0,0,1 ); + glRotatef( -pitch, 1,0,0 ); + + + //if( ! parent ) + // glRectf( 0,0,1,1 ); + + //if( power_pack->stored > 0.0 ) + power_pack->Visualize( cam ); + + if( say_string ) + { + //get raster positition, add gl_width, then project back to world coords + glRasterPos3f( 0, 0, 0 ); + GLfloat pos[ 4 ]; + glGetFloatv(GL_CURRENT_RASTER_POSITION, pos); + + GLboolean valid; + glGetBooleanv( GL_CURRENT_RASTER_POSITION_VALID, &valid ); + + if( valid ) + { + + fl_font( FL_HELVETICA, 12 ); + float w = gl_width( this->say_string ); // scaled text width + float h = gl_height(); // scaled text height + + GLdouble wx, wy, wz; + GLint viewport[4]; + glGetIntegerv(GL_VIEWPORT, viewport); + + GLdouble modelview[16]; + glGetDoublev(GL_MODELVIEW_MATRIX, modelview); + + GLdouble projection[16]; + glGetDoublev(GL_PROJECTION_MATRIX, projection); + + //get width and height in world coords + gluUnProject( pos[0] + w, pos[1], pos[2], modelview, projection, viewport, &wx, &wy, &wz ); + w = wx; + gluUnProject( pos[0], pos[1] + h, pos[2], modelview, projection, viewport, &wx, &wy, &wz ); + h = wy; + + // calculate speech bubble margin + const float m = h/10; + + // draw inside of bubble + PushColor( BUBBLE_FILL ); + glPushAttrib( GL_POLYGON_BIT | GL_LINE_BIT ); + glPolygonMode( GL_FRONT, GL_FILL ); + glEnable( GL_POLYGON_OFFSET_FILL ); + glPolygonOffset( 1.0, 1.0 ); + Gl::draw_octagon( w, h, m ); + glDisable( GL_POLYGON_OFFSET_FILL ); + PopColor(); + + // draw outline of bubble + PushColor( BUBBLE_BORDER ); + glLineWidth( 1 ); + glEnable( GL_LINE_SMOOTH ); + glPolygonMode( GL_FRONT, GL_LINE ); + Gl::draw_octagon( w, h, m ); + glPopAttrib(); + PopColor(); + + PushColor( BUBBLE_TEXT ); + // draw text inside the bubble + Gl::draw_string( 2.5*m, 2.5*m, 0, this->say_string ); + PopColor(); + } + } + glPopMatrix(); + } + + if( stall ) + { + DrawImage( TextureManager::getInstance()._stall_texture_id, cam, 0.85 ); + } +} + +void Model::DrawImage( uint32_t texture_id, Camera* cam, float alpha, double width, double height ) +{ + float yaw, pitch; + pitch = - cam->pitch(); + yaw = - cam->yaw(); + float robotAngle = -rtod(pose.a); + + glEnable(GL_TEXTURE_2D); + glBindTexture( GL_TEXTURE_2D, texture_id ); + + glColor4f( 1.0, 1.0, 1.0, alpha ); + glPushMatrix(); + + //position image above the robot + glTranslatef( 0.0, 0.0, ModelHeight() + 0.3 ); + + // rotate to face screen + glRotatef( robotAngle - yaw, 0,0,1 ); + glRotatef( -pitch - 90, 1,0,0 ); + + //draw a square, with the textured image + glBegin(GL_QUADS); + glTexCoord2f(0.0f, 0.0f); glVertex3f(-0.25f, 0, -0.25f ); + glTexCoord2f(width, 0.0f); glVertex3f( 0.25f, 0, -0.25f ); + glTexCoord2f(width, height); glVertex3f( 0.25f, 0, 0.25f ); + glTexCoord2f(0.0f, height); glVertex3f(-0.25f, 0, 0.25f ); + glEnd(); + + glPopMatrix(); + glBindTexture( GL_TEXTURE_2D, 0 ); + glDisable(GL_TEXTURE_2D); +} + + +void Model::DrawFlagList( void ) +{ + if( flag_list == NULL ) + return; + + PushLocalCoords(); + + glPolygonMode( GL_FRONT, GL_FILL ); + + GLUquadric* quadric = gluNewQuadric(); + glTranslatef(0,0,1); // jump up + Pose gpose = GetGlobalPose(); + glRotatef( 180 + rtod(-gpose.a),0,0,1 ); + + + GList* list = g_list_copy( flag_list ); + list = g_list_reverse(list); + + for( GList* item = list; item; item = item->next ) + { + + Flag* flag = (Flag*)item->data; + + glTranslatef( 0, 0, flag->size/2.0 ); + + PushColor( flag->color ); + + + glEnable(GL_POLYGON_OFFSET_FILL); + glPolygonOffset(1.0, 1.0); + gluQuadricDrawStyle( quadric, GLU_FILL ); + gluSphere( quadric, flag->size/2.0, 4,2 ); + glDisable(GL_POLYGON_OFFSET_FILL); + + // draw the edges darker version of the same color + double r,g,b,a; + stg_color_unpack( flag->color, &r, &g, &b, &a ); + PushColor( stg_color_pack( r/2.0, g/2.0, b/2.0, a )); + + gluQuadricDrawStyle( quadric, GLU_LINE ); + gluSphere( quadric, flag->size/2.0, 4,2 ); + + PopColor(); + PopColor(); + + glTranslatef( 0, 0, flag->size/2.0 ); + } + + g_list_free( list ); + + gluDeleteQuadric( quadric ); + + PopCoords(); +} + + +void Model::DrawBlinkenlights() +{ + PushLocalCoords(); + + GLUquadric* quadric = gluNewQuadric(); + //glTranslatef(0,0,1); // jump up + //Pose gpose = GetGlobalPose(); + //glRotatef( 180 + rtod(-gpose.a),0,0,1 ); + + for( unsigned int i=0; i<blinkenlights->len; i++ ) + { + stg_blinkenlight_t* b = + (stg_blinkenlight_t*)g_ptr_array_index( blinkenlights, i ); + assert(b); + + glTranslatef( b->pose.x, b->pose.y, b->pose.z ); + + PushColor( b->color ); + + if( b->enabled ) + gluQuadricDrawStyle( quadric, GLU_FILL ); + else + gluQuadricDrawStyle( quadric, GLU_LINE ); + + gluSphere( quadric, b->size/2.0, 8,8 ); + + PopColor(); + } + + gluDeleteQuadric( quadric ); + + PopCoords(); +} + +void Model::DrawPicker( void ) +{ + //PRINT_DEBUG1( "Drawing %s", token ); + PushLocalCoords(); + + // draw the boxes + blockgroup.DrawSolid( geom ); + + // recursively draw the tree below this model + LISTMETHOD( this->children, Model*, DrawPicker ); + + PopCoords(); +} + +void Model::DataVisualize( Camera* cam ) +{ +// if( power_pack ) +// { +// // back into global coords to get rid of my rotation +// glPushMatrix(); +// gl_pose_inverse_shift( GetGlobalPose() ); + +// // shift to the top left corner of the model (roughly) +// glTranslatef( pose.x - geom.size.x/2.0, +// pose.y + geom.size.y/2.0, +// pose.z + geom.size.z ); + +// power_pack->Visualize( cam ); +// glPopMatrix(); +// } +} + +void Model::DataVisualizeTree( Camera* cam ) +{ + PushLocalCoords(); + DataVisualize( cam ); // virtual function overridden by most model types + + CustomVisualizer* vis; + for( GList* item = custom_visual_list; item; item = item->next ) { + vis = static_cast< CustomVisualizer* >( item->data ); + if( world_gui->GetCanvas()->_custom_options[ vis->name() ]->isEnabled() ) + vis->DataVisualize( cam ); + } + + + // and draw the children + LISTMETHODARG( children, Model*, DataVisualizeTree, cam ); + + PopCoords(); +} + +void Model::DrawGrid( void ) +{ + if ( gui.grid ) + { + PushLocalCoords(); + + stg_bounds3d_t vol; + vol.x.min = -geom.size.x/2.0; + vol.x.max = geom.size.x/2.0; + vol.y.min = -geom.size.y/2.0; + vol.y.max = geom.size.y/2.0; + vol.z.min = 0; + vol.z.max = geom.size.z; + + PushColor( 0,0,1,0.4 ); + Gl::draw_grid(vol); + PopColor(); + PopCoords(); + } +} Added: code/stage/trunk/libstage/model_getset.cc =================================================================== --- code/stage/trunk/libstage/model_getset.cc (rev 0) +++ code/stage/trunk/libstage/model_getset.cc 2009-02-09 03:58:32 UTC (rev 7321) @@ -0,0 +1,245 @@ +#include "stage.hh" +using namespace Stg; + +void Model::SetGeom( Geom geom ) +{ + UnMapWithChildren(); + + this->geom = geom; + + blockgroup.CalcSize(); + + NeedRedraw(); + + MapWithChildren(); + + CallCallbacks( &geom ); +} + +void Model::SetColor( stg_color_t col ) +{ + this->color = col; + NeedRedraw(); + CallCallbacks( &color ); +} + +void Model::SetMass( stg_kg_t mass ) +{ + this->mass = mass; + CallCallbacks( &mass ); +} + +void Model::SetStall( stg_bool_t stall ) +{ + this->stall = stall; + CallCallbacks( &stall ); +} + +void Model::SetGripperReturn( int val ) +{ + vis.gripper_return = val; + CallCallbacks( &vis.gripper_return ); +} + +void Model::SetFiducialReturn( int val ) +{ + vis.fiducial_return = val; + CallCallbacks( &vis.fiducial_return ); +} + +void Model::SetFiducialKey( int key ) +{ + vis.fiducial_key = key; + CallCallbacks( &vis.fiducial_key ); +} + +void Model::SetLaserReturn( stg_laser_return_t val ) +{ + vis.laser_return = val; + CallCallbacks( &vis.laser_return ); +} + +void Model::SetObstacleReturn( int val ) +{ + vis.obstacle_return = val; + CallCallbacks( &vis.obstacle_return ); +} + +void Model::SetBlobReturn( int val ) +{ + vis.blob_return = val; + CallCallbacks( &vis.blob_return ); +} + +void Model::SetRangerReturn( int val ) +{ + vis.ranger_return = val; + CallCallbacks( &vis.ranger_return ); +} + +void Model::SetBoundary( int val ) +{ + boundary = val; + CallCallbacks( &boundary ); +} + +void Model::SetGuiNose( int val ) +{ + gui.nose = val; + CallCallbacks( &gui.nose ); +} + +void Model::SetGuiMask( int val ) +{ + gui.mask = val; + CallCallbacks( &gui.mask ); +} + +void Model::SetGuiGrid( int val ) +{ + gui.grid = val; + CallCallbacks( &this->gui.grid ); +} + +void Model::SetGuiOutline( int val ) +{ + gui.outline = val; + CallCallbacks( &gui.outline ); +} + +void Model::SetWatts( stg_watts_t watts ) +{ + watts = watts; + CallCallbacks( &watts ); +} + +void Model::SetMapResolution( stg_meters_t res ) +{ + map_resolution = res; + CallCallbacks( &map_resolution ); +} + +// set the pose of model in global coordinates +void Model::SetGlobalPose( Pose gpose ) +{ + SetPose( parent ? parent->GlobalToLocal( gpose ) : gpose ); +} + +int Model::SetParent( Model* newparent) +{ + // remove the model from its old parent (if it has one) + if( this->parent ) + this->parent->children = g_list_remove( this->parent->children, this ); + + if( newparent ) + newparent->children = g_list_append( newparent->children, this ); + + // link from the model to its new parent + this->parent = newparent; + + CallCallbacks( &this->parent ); + return 0; //ok +} + +// get the model's velocity in the global frame +Velocity Model::GetGlobalVelocity() +{ + Pose gpose = GetGlobalPose(); + + double cosa = cos( gpose.a ); + double sina = sin( gpose.a ); + + Velocity gv; + gv.x = velocity.x * cosa - velocity.y * sina; + gv.y = velocity.x * sina + velocity.y * cosa; + gv.a = velocity.a; + + return gv; +} + +// set the model's velocity in the global frame +void Model::SetGlobalVelocity( Velocity gv ) +{ + Pose gpose = GetGlobalPose(); + + double cosa = cos( gpose.a ); + double sina = sin( gpose.a ); + + Velocity lv; + lv.x = gv.x * cosa + gv.y * sina; + lv.y = -gv.x * sina + gv.y * cosa; + lv.a = gv.a; + + this->SetVelocity( lv ); +} + +// get the model's position in the global frame +Pose Model::GetGlobalPose() +{ + // if I'm a top level model, my global pose is my local pose + if( parent == NULL ) + return pose; + + // otherwise + + Pose global_pose = pose_sum( parent->GetGlobalPose(), pose ); + + // we are on top of our parent + global_pose.z += parent->geom.size.z; + + // PRINT_DEBUG4( "GET GLOBAL POSE [x:%.2f y:%.2f z:%.2f a:%.2f]", + // global_pose.x, + // global_pose.y, + // global_pose.z, + // global_pose.a ); + + return global_pose; +} + + +void Model::SetVelocity( Velocity vel ) +{ +// assert( ! isnan(vel.x) ); +// assert( ! isnan(vel.y) ); +// assert( ! isnan(vel.z) ); +// assert( ! isnan(vel.a) ); + + this->velocity = vel; + + if( on_velocity_list && vel.IsZero() ) + { + world->StopUpdatingModelPose( this ); + on_velocity_list = false; + } + + if( (!on_velocity_list) && (!vel.IsZero()) ) + { + world->StartUpdatingModelPose( this ); + on_velocity_list = true; + } + + CallCallbacks( &this->velocity ); +} + + +// set the model's pose in the local frame +void Model::SetPose( Pose newpose ) +{ + // if the pose has changed, we need to do some work + if( memcmp( &pose, &newpose, sizeof(Pose) ) != 0 ) + { + pose = newpose; + pose.a = normalize(pose.a); + + if( isnan( pose.a ) ) + printf( "SetPose bad angle %s [%.2f %.2f %.2f %.2f]\n", + token, pose.x, pose.y, pose.z, pose.a ); + + NeedRedraw(); + MapWithChildren(); + world->dirty = true; + } + + CallCallbacks( &this->pose ); +} + Modified: code/stage/trunk/libstage/powerpack.cc =================================================================== --- code/stage/trunk/libstage/powerpack.cc 2009-02-07 02:18:31 UTC (rev 7320) +++ code/stage/trunk/libstage/powerpack.cc 2009-02-09 03:58:32 UTC (rev 7321) @@ -12,15 +12,29 @@ PowerPack::PowerPack( Model* mod ) : mod( mod), stored( 0.0 ), capacity( 0.0 ), charging( false ) { - // nothing to do + // tell the world about this new pp + mod->world->AddPowerPack( this ); }; +PowerPack::~PowerPack() +{ + // tell the world about this new pp + mod->world->RemovePowerPack( this ); +} + void PowerPack::Print( char* prefix ) { printf( "%s stored %.2f/%.2f joules\n", prefix, stored, capacity ); } +// this is called every world update cycle - can we get away without doing this? +void PowerPack::Update() +{ + charging = false; +} + + /** OpenGL visualization of the powerpack state */ void PowerPack::Visualize( Camera* cam ) { @@ -45,84 +59,83 @@ // glVertex2i( 5, 4); // glEnd(); //} - + double percent = stored/capacity * 100.0; - + const double alpha = 0.5; - if( percent > 50 ) - glColor4f( 0,1,0, alpha ); // green + if( percent > 50 ) + glColor4f( 0,1,0, alpha ); // green else if( percent > 25 ) glColor4f( 1,0,1, alpha ); // magenta else glColor4f( 1,0,0, alpha ); // red + + static char buf[6]; + snprintf( buf, 6, "%.0f", percent ); + + glTranslatef( -width, 0.0, 0.0 ); + + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + + GLfloat fullness = height * (percent * 0.01); + glRectf( 0,0,width, fullness); + + // outline the charge-o-meter + glTranslatef( 0,0,0.001 ); + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + + glColor4f( 0,0,0,0.7 ); + + glRectf( 0,0,width, height ); + + glBegin( GL_LINES ); + glVertex2f( 0, fullness ); + glVertex2f( width, fullness ); + glEnd(); + + if( stored < 0.0 ) // inifinite supply! + { + // draw an arrow toward the top + glBegin( GL_LINES ); + glVertex2f( width/3.0, height/3.0 ); + glVertex2f( 2.0 * width/3, height/3.0 ); - static char buf[6]; - snprintf( buf, 6, "%.0f", percent ); + glVertex2f( width/3.0, height/3.0 ); + glVertex2f( width/3.0, height - height/5.0 ); - glTranslatef( -width, 0.0, 0.0 ); + glVertex2f( width/3.0, height - height/5.0 ); + glVertex2f( 0, height - height/5.0 ); - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + glVertex2f( 0, height - height/5.0 ); + glVertex2f( width/2.0, height ); - GLfloat fullness = height * (percent * 0.01); - glRectf( 0,0,width, fullness); + glVertex2f( width/2.0, height ); + glVertex2f( width, height - height/5.0 ); - // outline the charge-o-meter - glTranslatef( 0,0,0.001 ); - glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + glVertex2f( width, height - height/5.0 ); + glVertex2f( 2.0 * width/3.0, height - height/5.0 ); - glColor4f( 0,0,0,0.7 ); + glVertex2f( 2.0 * width/3.0, height - height/5.0 ); + glVertex2f( 2.0 * width/3, height/3.0 ); + glEnd(); + } + + + if( charging ) + { + glLineWidth( 6.0 ); + glColor4f( 1,0,0,0.7 ); + glRectf( 0,0,width, height ); - glBegin( GL_LINES ); - glVertex2f( 0, fullness ); - glVertex2f( width, fullness ); - glEnd(); - - if( stored < 0.0 ) // inifinite supply! - { - // draw an arrow toward the top - glBegin( GL_LINES ); - glVertex2f( width/3.0, height/3.0 ); - glVertex2f( 2.0 * width/3, height/3.0 ); - - glVertex2f( width/3.0, height/3.0 ); - glVertex2f( width/3.0, height - height/5.0 ); - - glVertex2f( width/3.0, height - height/5.0 ); - glVertex2f( 0, height - height/5.0 ); - - glVertex2f( 0, height - height/5.0 ); - glVertex2f( width/2.0, height ); - - glVertex2f( width/2.0, height ); - glVertex2f( width, height - height/5.0 ); - - glVertex2f( width, height - height/5.0 ); - glVertex2f( 2.0 * width/3.0, height - height/5.0 ); - - glVertex2f( 2.0 * width/3.0, height - height/5.0 ); - glVertex2f( 2.0 * width/3, height/3.0 ); - - glEnd(); - - } - - - if( charging ) - { - glLineWidth( 6.0 ); - glColor4f( 1,0,0,0.7 ); - - glRectf( 0,0,width, height ); - - glLineWidth( 1.0 ); - } - - // draw the percentage - //gl_draw_string( -0.2, 0, 0, buf ); - + glLineWidth( 1.0 ); + } + + // draw the percentage + //gl_draw_string( -0.2, 0, 0, buf ); + // ? glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-02-07 02:18:31 UTC (rev 7320) +++ code/stage/trunk/libstage/stage.hh 2009-02-09 03:58:32 UTC (rev 7321) @@ -273,6 +273,8 @@ printf( "%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n", prefix, x,y,z,a ); } + + bool IsZero(){ return( !(x || y || z || a )); }; void Load( Worldfile* wf, int section, const char* keyword ); void Save( Worldfile* wf, int section, const char* keyword ); @@ -304,9 +306,7 @@ { printf( "%s velocity [x:%.3f y:%.3f z:%3.f a:%.3f]\n", prefix, x,y,z,a ); - } - - bool IsZero(){ return( !(x || y || z || a )); }; + } }; /** Specify an object's basic geometry: position and rectangular @@ -373,38 +373,38 @@ Bounds x, y, z; } stg_bbox3d_t; - /** define a field-of-view: an angle and range bounds */ + /** Define a field-of-view: an angle and range bounds */ typedef struct { Bounds range; ///< min and max range of sensor stg_radians_t angle; ///< width of viewing angle of sensor } stg_fov_t; - /** define a point on a 2d plane */ + /** Define a point on a 2d plane */ typedef struct { stg_meters_t x, y; } stg_point_t; - /** define a point in 3d space */ + /** Define a point in 3d space */ typedef struct { float x, y, z; } stg_vertex_t; - /** define vertex and its color */ + /** Define vertex and its color */ typedef struct { float x, y, z, r, g, b, a; } stg_colorvertex_t; - /** define a point in 3d space */ + /** Define a point in 3d space */ typedef struct { stg_meters_t x, y, z; } stg_point3_t; - /** define an integer point on the 2d plane */ + /** Define an integer point on the 2d plane */ typedef struct { int32_t x,y; @@ -639,7 +639,7 @@ stg_cb_t* cb_create( stg_model_callback_t callback, void* arg ); void cb_destroy( stg_cb_t* cb ); - /** defines a rectangle of [size] located at [pose] */ + /** Defines a rectangle of [size] located at [pose] */ typedef struct { Pose pose; @@ -846,6 +846,7 @@ 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 + 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 ms GHashTable* superregions; @@ -924,6 +925,9 @@ virtual void AddModel( Model* mod ); virtual void RemoveModel( Model* mod ); + + void AddPowerPack( PowerPack* pp ); + void RemovePowerPack( PowerPack* pp ); GList* GetRayList(){ return ray_list; }; void ClearRays(); @@ -1371,13 +1375,12 @@ }; - class Camera; - /** energy data packet */ class PowerPack { public: PowerPack( Model* mod ); + ~PowerPack(); /** The model that owns this object */ Model* mod; @@ -1415,6 +1418,10 @@ void Print( const char* prefix ) { printf( "%s PowerPack %.2f/%.2f J\n", prefix, stored, capacity ); } + + /** Called exactly once for each pack on every world update + cycle */ + void Update(); }; class Visibility @@ -1571,7 +1578,6 @@ ctrlinit_t* initfunc; stg_usec_t interval; ///< time between updates in us stg_usec_t last_update; ///< time of last update in us - bool map_caches_are_invalid; stg_meters_t map_resolution; stg_kg_t mass; bool on_update_list; Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-02-07 02:18:31 UTC (rev 7320) +++ code/stage/trunk/libstage/world.cc 2009-02-09 03:58:32 UTC (rev 7321) @@ -77,6 +77,7 @@ destroy( false ), dirty( true ), models_by_name( g_hash_table_new( g_str_hash, g_str_equal ) ), + powerpack_list( NULL ), ppm( ppm ), // raytrace resolution quit( false ), quit_time( 0 ), @@ -192,17 +193,14 @@ void World::LoadPuck( Worldfile* wf, int entity, GHashTable* entitytable ) { -// // lookup the group in which this was defined -// Ancestor* anc = (Ancestor*)g_hash_table_lookup( entitytable, -// (gpointer)wf->GetEntityParent( entity ) ); - - Puck* puck = new Puck(); puck->Load( wf, entity ); puck_list = g_list_prepend( puck_list, puck ); } + + void World::LoadModel( Worldfile* wf, int entity, GHashTable* entitytable ) { int parent_entity = wf->GetEntityParent( entity ); @@ -424,14 +422,17 @@ if( IsGUI() == false ) return true; } - + // upate all positions first LISTMETHOD( velocity_list, Model*, UpdatePose ); + // upate all powerpacks + LISTMETHOD( powerpack_list, PowerPack*, Update ); + // test all models that supply charge to see if they are touching // something that takes charge LISTMETHOD( charge_list, Model*, UpdateCharge ); - + // then update all sensors if( worker_threads == 0 ) // do all the work in this thread { @@ -441,31 +442,31 @@ { // push the update for every model that needs it into the thread pool for( GList* it = update_list; it; it=it->next ) - { - Model* mod = (Model*)it->data; - - if( mod->UpdateDue() ) - { - if( mod->thread_safe ) // do update in a worker thread - { - g_mutex_lock( thread_mutex ); - update_jobs_pending++; - g_mutex_unlock( thread_mutex ); - g_thread_pool_push( threadpool, mod, NULL ); - } - else - mod->Update(); // do update in this thread - } - } - + { + Model* mod = (Model*)it->data; + + if( mod->UpdateDue() ) + { + if( mod->thread_safe ) // do update in a worker thread + { + g_mutex_lock( thread_mutex ); + update_jobs_pending++; + g_mutex_unlock( thread_mutex ); + g_thread_pool_push( threadpool, mod, NULL ); + } + else + mod->Update(); // do update in this thread + } + } + // wait for all the last update job to complete - it will // signal the worker_threads_done condition var g_mutex_lock( thread_mutex ); while( update_jobs_pending ) - g_cond_wait( worker_threads_done, thread_mutex ); + g_cond_wait( worker_threads_done, thread_mutex ); g_mutex_unlock( thread_mutex ); } - + this->sim_time += this->interval_sim; this->updates++; @@ -938,4 +939,12 @@ } +void World::AddPowerPack( PowerPack* pp ) +{ + powerpack_list = g_list_append( powerpack_list, pp ); +} +void World::RemovePowerPack( PowerPack* pp ) +{ + powerpack_list = g_list_remove( powerpack_list, pp ); +} This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-02-09 18:53:56
|
Revision: 7322 http://playerstage.svn.sourceforge.net/playerstage/?rev=7322&view=rev Author: rtv Date: 2009-02-09 18:53:51 +0000 (Mon, 09 Feb 2009) Log Message: ----------- increased effciency of charging model Modified Paths: -------------- code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/powerpack.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-02-09 03:58:32 UTC (rev 7321) +++ code/stage/trunk/libstage/CMakeLists.txt 2009-02-09 18:53:51 UTC (rev 7322) @@ -29,7 +29,6 @@ options_dlg.cc options_dlg.hh powerpack.cc - puck.cc region.cc resource.cc stage.cc @@ -42,6 +41,10 @@ worldgui.cc ) +# TODO +# puck.cc + + add_library(stage SHARED ${stageSrcs}) set_source_files_properties( ${stageSrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" ) Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-02-09 03:58:32 UTC (rev 7321) +++ code/stage/trunk/libstage/canvas.cc 2009-02-09 18:53:51 UTC (rev 7322) @@ -829,7 +829,7 @@ DrawBoundingBoxes(); - LISTMETHOD( world->puck_list, Puck*, Draw ); + //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-02-09 03:58:32 UTC (rev 7321) +++ code/stage/trunk/libstage/model.cc 2009-02-09 18:53:51 UTC (rev 7322) @@ -216,6 +216,7 @@ parent(parent), pose(), power_pack( NULL ), + pps_charging(NULL), props(NULL), rebuild_displaylist(true), say_string(NULL), @@ -782,8 +783,11 @@ PowerPack* mypp = FindPowerPack(); assert( mypp ); - // make sure we only charge a powerpack once - GList* ppacks_serviced = NULL; + // detach charger from all the packs charged last time + for( GList* it = pps_charging; it; it = it->next ) + ((PowerPack*)it->data)->charging = false; + g_list_free( pps_charging ); + pps_charging = NULL; // run through and update all appropriate touchers for( GList* touchers = AppendTouchingModels( NULL ); @@ -807,6 +811,9 @@ // move some joules from me to him mypp->TransferTo( hispp, amount ); hispp->charging = true; + + // remember who we are charging so we can detatch next time + pps_charging = g_list_prepend( pps_charging, hispp ); } } } Modified: code/stage/trunk/libstage/powerpack.cc =================================================================== --- code/stage/trunk/libstage/powerpack.cc 2009-02-09 03:58:32 UTC (rev 7321) +++ code/stage/trunk/libstage/powerpack.cc 2009-02-09 18:53:51 UTC (rev 7322) @@ -28,13 +28,6 @@ printf( "%s stored %.2f/%.2f joules\n", prefix, stored, capacity ); } -// this is called every world update cycle - can we get away without doing this? -void PowerPack::Update() -{ - charging = false; -} - - /** OpenGL visualization of the powerpack state */ void PowerPack::Visualize( Camera* cam ) { Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-02-09 03:58:32 UTC (rev 7321) +++ code/stage/trunk/libstage/stage.hh 2009-02-09 18:53:51 UTC (rev 7322) @@ -732,25 +732,66 @@ record within a model and called whenever the record is set.*/ typedef int (*stg_model_callback_t)( Model* mod, void* user ); - class Puck - { - private: - void BuildDisplayList(); + // class Puck +// { +// private: +// void BuildDisplayList(); - public: - stg_color_t color; - int displaylist; - stg_meters_t height; - Pose pose; - stg_meters_t radius; +// public: +// stg_color_t color; +// int displaylist; +// stg_meters_t height; +// Pose pose; +// stg_meters_t radius; - Puck(); - void Load( Worldfile* wf, int section ); - void Save( Worldfile* wf, int section ); +// Puck(); +// void Load( Worldfile* wf, int section ); +// void Save( Worldfile* wf, int section ); - void Draw(); - }; +// void Draw(); +// }; + + // class EventQueue +// { +// private: +// GTree* future; + +// static gint InstantCompare( stg_usec_t a, stg_usec_t b ) +// { +// if( a < b ) +// return -1; + +// if( a > b ) +// return 1; + +// return 0; // they are equal +// } + +// public: +// EventQueue() +// { +// future = g_tree_new( InstantCompare ); +// } + +// RunInstant() ///< Updates all events due at the next instant +// { +// GList* instant_list = +// } + +// QueueModel( Model* mod ) ///< Adds the model to the event queue +// { +// g_tree_insert( mod->updatedue, g_list_prepend( g_tree_lookup( mod->updatedue ), mod ); ); +// } + +// DeQueueModel( Model* mod ) ///< removes the model from the event queue +// { +// g_tree_insert( mod->updatedue, g_list_remove( g_tree_lookup( mod->updatedue ), mod )); +// } + +// }; + + // ANCESTOR CLASS /** Base class for Model and World */ class Ancestor @@ -825,6 +866,7 @@ GList* charge_list; ///< Models which receive charge are listed here bool destroy; bool dirty; ///< iff true, a gui redraw would be required + GList* event_list; //< GHashTable* models_by_name; ///< the models that make up the world, indexed by name double ppm; ///< the resolution of the world model in pixels per meter bool quit; ///< quit this world ASAP @@ -1418,10 +1460,6 @@ void Print( const char* prefix ) { printf( "%s PowerPack %.2f/%.2f J\n", prefix, stored, capacity ); } - - /** Called exactly once for each pack on every world update - cycle */ - void Update(); }; class Visibility @@ -1593,6 +1631,10 @@ /** Optional attached PowerPack, defaults to NULL */ PowerPack* power_pack; + /** list of powerpacks that this model is currently charging, + initially NULL. */ + GList* pps_charging; + /** GData datalist can contain arbitrary named data items. Can be used by derived model types to store properties, and for user code to associate arbitrary items with a model. */ Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-02-09 03:58:32 UTC (rev 7321) +++ code/stage/trunk/libstage/world.cc 2009-02-09 18:53:51 UTC (rev 7322) @@ -191,12 +191,12 @@ 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 ); -} +// 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 ); +// } @@ -334,8 +334,8 @@ } else if( strcmp( typestr, "block" ) == 0 ) LoadBlock( wf, entity, entitytable ); - else if( strcmp( typestr, "puck" ) == 0 ) - LoadPuck( wf, entity, entitytable ); +// else if( strcmp( typestr, "puck" ) == 0 ) +// LoadPuck( wf, entity, entitytable ); else LoadModel( wf, entity, entitytable ); } @@ -426,9 +426,6 @@ // upate all positions first LISTMETHOD( velocity_list, Model*, UpdatePose ); - // upate all powerpacks - LISTMETHOD( powerpack_list, PowerPack*, Update ); - // test all models that supply charge to see if they are touching // something that takes charge LISTMETHOD( charge_list, Model*, UpdateCharge ); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-02-17 06:13:48
|
Revision: 7338 http://playerstage.svn.sourceforge.net/playerstage/?rev=7338&view=rev Author: rtv Date: 2009-02-17 06:13:41 +0000 (Tue, 17 Feb 2009) Log Message: ----------- cleaned up vis options and dialog Modified Paths: -------------- code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_blinkenlight.cc code/stage/trunk/libstage/model_blobfinder.cc code/stage/trunk/libstage/model_camera.cc code/stage/trunk/libstage/model_draw.cc code/stage/trunk/libstage/model_fiducial.cc code/stage/trunk/libstage/model_gripper.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/model_ranger.cc code/stage/trunk/libstage/options_dlg.cc code/stage/trunk/libstage/options_dlg.hh code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-02-17 00:56:38 UTC (rev 7337) +++ code/stage/trunk/libstage/model.cc 2009-02-17 06:13:41 UTC (rev 7338) @@ -1022,8 +1022,8 @@ void Model::RegisterOption( Option* opt ) { - drawOptions.push_back( opt ); + //drawOptions.push_back( opt ); - if( world->IsGUI() ) - world->RegisterOption( opt ); + //if( world->IsGUI() ) + world->RegisterOption( opt ); } Modified: code/stage/trunk/libstage/model_blinkenlight.cc =================================================================== --- code/stage/trunk/libstage/model_blinkenlight.cc 2009-02-17 00:56:38 UTC (rev 7337) +++ code/stage/trunk/libstage/model_blinkenlight.cc 2009-02-17 06:13:41 UTC (rev 7338) @@ -83,7 +83,7 @@ this->Startup(); - registerOption( &showBlinkenData ); + RegisterOption( &showBlinkenData ); } ModelBlinkenlight::~ModelBlinkenlight() Modified: code/stage/trunk/libstage/model_blobfinder.cc =================================================================== --- code/stage/trunk/libstage/model_blobfinder.cc 2009-02-17 00:56:38 UTC (rev 7337) +++ code/stage/trunk/libstage/model_blobfinder.cc 2009-02-17 06:13:41 UTC (rev 7338) @@ -100,7 +100,7 @@ // leave the color filter array empty initally - this tracks all colors colors = g_array_new( false, true, sizeof(stg_color_t) ); - registerOption( &showBlobData ); + RegisterOption( &showBlobData ); } Modified: code/stage/trunk/libstage/model_camera.cc =================================================================== --- code/stage/trunk/libstage/model_camera.cc 2009-02-17 00:56:38 UTC (rev 7337) +++ code/stage/trunk/libstage/model_camera.cc 2009-02-17 06:13:41 UTC (rev 7338) @@ -122,7 +122,7 @@ // set default color SetColor( stg_lookup_color(DEFAULT_GEOM_COLOR)); - registerOption( &showCameraData ); + RegisterOption( &showCameraData ); Startup(); } Modified: code/stage/trunk/libstage/model_draw.cc =================================================================== --- code/stage/trunk/libstage/model_draw.cc 2009-02-17 00:56:38 UTC (rev 7337) +++ code/stage/trunk/libstage/model_draw.cc 2009-02-17 06:13:41 UTC (rev 7338) @@ -269,7 +269,7 @@ true, world_gui ); canvas->_custom_options[ custom_visual->name() ] = op; - registerOption( op ); + RegisterOption( op ); } } Modified: code/stage/trunk/libstage/model_fiducial.cc =================================================================== --- code/stage/trunk/libstage/model_fiducial.cc 2009-02-17 00:56:38 UTC (rev 7337) +++ code/stage/trunk/libstage/model_fiducial.cc 2009-02-17 06:13:41 UTC (rev 7338) @@ -99,7 +99,7 @@ data = g_array_new( false, true, sizeof(stg_fiducial_t) ); - registerOption( &showFiducialData ); + RegisterOption( &showFiducialData ); } ModelFiducial::~ModelFiducial( void ) Modified: code/stage/trunk/libstage/model_gripper.cc =================================================================== --- code/stage/trunk/libstage/model_gripper.cc 2009-02-17 00:56:38 UTC (rev 7337) +++ code/stage/trunk/libstage/model_gripper.cc 2009-02-17 06:13:41 UTC (rev 7338) @@ -78,6 +78,10 @@ cfg.paddles_stalled = false; cfg.autosnatch = false; cfg.gripped = NULL; + cfg.beam[0] = 0; + cfg.beam[1] = 0; + cfg.contact[0] = 0; + cfg.contact[1] = 0; // place the break beam sensors at 1/4 and 3/4 the length of the paddle cfg.break_beam_inset[0] = 3.0/4.0 * cfg.paddle_size.x; @@ -101,7 +105,7 @@ PositionPaddles(); - registerOption( &showData ); + RegisterOption( &showData ); } ModelGripper::~ModelGripper() @@ -480,8 +484,8 @@ if( subs < 1 ) return; - if( ! showData ) - return; + //if( ! showData ) + //return; // outline the sensor lights in black PushColor( 0,0,0,1.0 ); // black Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2009-02-17 00:56:38 UTC (rev 7337) +++ code/stage/trunk/libstage/model_position.cc 2009-02-17 06:13:41 UTC (rev 7338) @@ -135,8 +135,8 @@ drand48() * STG_POSITION_INTEGRATION_ERROR_MAX_A - STG_POSITION_INTEGRATION_ERROR_MAX_A/2.0; - registerOption( &showCoords ); - registerOption( &showWaypoints ); + RegisterOption( &showCoords ); + RegisterOption( &showWaypoints ); } Modified: code/stage/trunk/libstage/model_ranger.cc =================================================================== --- code/stage/trunk/libstage/model_ranger.cc 2009-02-17 00:56:38 UTC (rev 7337) +++ code/stage/trunk/libstage/model_ranger.cc 2009-02-17 06:13:41 UTC (rev 7338) @@ -100,7 +100,6 @@ static const char RANGER_CONFIG_COLOR[] = "gray90"; static const char RANGER_GEOM_COLOR[] = "orange"; -//TODO make instance attempt to register an option (as customvisualizations do) Option ModelRanger::showRangerData( "Ranger ranges", "show_ranger", "", true, NULL ); Option ModelRanger::showRangerTransducers( "Ranger transducers", "show_ranger_transducers", "", false, NULL ); @@ -155,12 +154,14 @@ sensors[c].ray_count = RANGER_RAYCOUNT; } - registerOption( &showRangerData ); - registerOption( &showRangerTransducers ); + RegisterOption( &showRangerData ); + RegisterOption( &showRangerTransducers ); } ModelRanger::~ModelRanger() { + if( sensors ) + delete[] sensors; } void ModelRanger::Startup( void ) Modified: code/stage/trunk/libstage/options_dlg.cc =================================================================== --- code/stage/trunk/libstage/options_dlg.cc 2009-02-17 00:56:38 UTC (rev 7337) +++ code/stage/trunk/libstage/options_dlg.cc 2009-02-17 06:13:41 UTC (rev 7338) @@ -14,19 +14,19 @@ showAllCheck = new Fl_Check_Button( 0,0, w,boxH ); showAllCheck->callback( checkChanged, this ); - - scroll = new Fl_Scroll( 0,boxH+vm, w,h-boxH-btnH-3*vm ); - scroll->type( Fl_Scroll::VERTICAL ); + showAllCheck->box( FL_UP_FRAME ); + + //scroll = new Fl_Scroll( 0,boxH+vm, w,h-boxH-btnH-3*vm ); + scroll = new Fl_Scroll( 0,boxH+vm, w,h-boxH-3*vm ); + //scroll->box( FL_ENGRAVED_BOX ); + resizable( scroll ); + scroll->type( Fl_Scroll::VERTICAL ); scroll->end(); - - button = new Fl_Button( hm, h-btnH-vm, w-2*hm, btnH, "&Close" ); - button->callback( closePress, this ); this->end(); } OptionsDlg::~OptionsDlg() { - delete button; delete scroll; // deletes members delete showAllCheck; } @@ -55,19 +55,6 @@ } } - void OptionsDlg::closePress( Fl_Widget* w, void* p ) { - OptionsDlg* oDlg = static_cast<OptionsDlg*>( p ); - - oDlg->status = CLOSE; - oDlg->do_callback(); - oDlg->status = NO_EVENT; - } - -// int OptionsDlg::handle( int event ) { -// return Fl_Window::handle( event ); -// } - - void OptionsDlg::updateChecks() { if (scroll->children()) scroll->clear(); @@ -84,15 +71,11 @@ } void OptionsDlg::setOptions( const std::vector<Option*>& opts ) { + options.clear(); options.assign( opts.begin(), opts.end() ); updateChecks(); } - void OptionsDlg::setOptions( const std::set<Option*, Option::optComp>& opts ) { - options.clear(); - options.insert( options.begin(), opts.begin(), opts.end() ); - updateChecks(); - } void OptionsDlg::showAllOpt( Option* opt ) { showAll = opt; Modified: code/stage/trunk/libstage/options_dlg.hh =================================================================== --- code/stage/trunk/libstage/options_dlg.hh 2009-02-17 00:56:38 UTC (rev 7337) +++ code/stage/trunk/libstage/options_dlg.hh 2009-02-17 06:13:41 UTC (rev 7338) @@ -27,18 +27,14 @@ Option* showAll; event_t status; Fl_Scroll* scroll; - Fl_Button* button; Fl_Check_Button* showAllCheck; void updateChecks(); - //virtual int handle( int event ); static void checkChanged( Fl_Widget* w, void* p ); - static void closePress( Fl_Widget* w, void* p ); - + // constants static const int vm = 4; const int hm; - static const int btnH = 25; static const int boxH = 30; public: @@ -46,7 +42,6 @@ virtual ~OptionsDlg(); void setOptions( const std::vector<Option*>& opts ); - void setOptions( const std::set<Option*, Option::optComp>& 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-02-17 00:56:38 UTC (rev 7337) +++ code/stage/trunk/libstage/stage.hh 2009-02-17 06:13:41 UTC (rev 7338) @@ -1387,8 +1387,8 @@ static void fileSaveCb( Fl_Widget* w, void* p ); static void fileSaveAsCb( Fl_Widget* w, void* p ); static void fileExitCb( Fl_Widget* w, void* p ); - static void viewOptionsCb( Fl_Widget* w, void* p ); - static void optionsDlgCb( Fl_Widget* w, void* p ); + static void viewOptionsCb( OptionsDlg* oDlg, WorldGui* worldGui ); + static void optionsDlgCb( OptionsDlg* oDlg, WorldGui* worldGui ); static void helpAboutCb( Fl_Widget* w, void* p ); // GUI functions @@ -1684,7 +1684,6 @@ /// Register an Option for pickup by the GUI void RegisterOption( Option* opt ); - void registerOption( Option* opt ) { RegisterOption( opt) ; }; GList* AppendTouchingModels( GList* list ); //void AddTouchingModelsToList( GList* list ); Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2009-02-17 00:56:38 UTC (rev 7337) +++ code/stage/trunk/libstage/worldgui.cc 2009-02-17 06:13:41 UTC (rev 7338) @@ -192,7 +192,7 @@ mbar->add( "File/E&xit", FL_CTRL+'q', WorldGui::fileExitCb, this ); mbar->add( "&View", 0, 0, 0, FL_SUBMENU ); - mbar->add( "View/Filter data...", FL_SHIFT + 'd', WorldGui::viewOptionsCb, this ); + mbar->add( "View/Filter data...", FL_SHIFT + 'd', (Fl_Callback*)WorldGui::viewOptionsCb, this ); canvas->createMenuItems( mbar, "View" ); mbar->add( "&Help", 0, 0, 0, FL_SUBMENU ); @@ -258,7 +258,7 @@ } label( title.c_str() ); - UpdateOptions(); + //UpdateOptions(); show(); } @@ -518,40 +518,54 @@ } } +static void append_option( char* name, Option* opt, std::vector<Option*>* optv ) +{ + //printf( "adding option %s @ %p\n", name, opt ); + optv->push_back( opt ); +} -void list_option( char* name, Option* opt, void* dummy ) +static bool sort_option_pointer( Option* a, Option* b ) { - printf( "option %s @ %p\n", name, opt ); - + // Option class overloads operator<. Nasty nasty C++ makes code less + // readable IMHO. + return (*a) < (*b); } -void WorldGui::viewOptionsCb( Fl_Widget* w, void* p ) +void WorldGui::viewOptionsCb( OptionsDlg* oDlg, WorldGui* worldGui ) { - WorldGui* worldGui = static_cast<WorldGui*>( p ); + // the options dialog expects a std::vector of options (annoyingly) + std::vector<Option*> optvec; + // adds each option to the vector + 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 ); + if ( !worldGui->oDlg ) { int x = worldGui->w()+worldGui->x() + 10; int y = worldGui->y(); OptionsDlg* oDlg = new OptionsDlg( x,y, 180,250 ); - oDlg->callback( optionsDlgCb, worldGui ); - oDlg->setOptions( worldGui->drawOptions ); + oDlg->callback( (Fl_Callback*)optionsDlgCb, worldGui ); + + oDlg->setOptions( optvec ); oDlg->showAllOpt( &worldGui->canvas->visualizeAll ); worldGui->oDlg = oDlg; oDlg->show(); } else { - worldGui->oDlg->show(); // bring it to front + worldGui->oDlg->hide(); + delete worldGui->oDlg; + worldGui->oDlg = NULL; } - - //g_hash_table_foreach( worldGui->option_table, (GHFunc)list_option, NULL ); + } -void WorldGui::optionsDlgCb( Fl_Widget* w, void* p ) { - OptionsDlg* oDlg = static_cast<OptionsDlg*>( w ); - WorldGui* worldGui = static_cast<WorldGui*>( p ); - +void WorldGui::optionsDlgCb( OptionsDlg* oDlg, WorldGui* worldGui ) +{ // get event from dialog OptionsDlg::event_t event; event = oDlg->event(); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |