From: <rt...@us...> - 2009-02-17 07:31:13
|
Revision: 7339 http://playerstage.svn.sourceforge.net/playerstage/?rev=7339&view=rev Author: rtv Date: 2009-02-17 07:31:10 +0000 (Tue, 17 Feb 2009) Log Message: ----------- interface improvements Modified Paths: -------------- code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/options_dlg.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-02-17 06:13:41 UTC (rev 7338) +++ code/stage/trunk/libstage/canvas.cc 2009-02-17 07:31:10 UTC (rev 7339) @@ -508,63 +508,6 @@ case FL_KEYBOARD: switch( Fl::event_key() ) { - case 'p': // pause - world->TogglePause(); - - if( ! world->paused ) - { - // // start the timer that causes regular redraws - Fl::add_timeout( ((double)interval/1000), - (Fl_Timeout_Handler)Canvas::TimerCallback, - this); - } - else - { // remove the timeout - Fl::remove_timeout( (Fl_Timeout_Handler)Canvas::TimerCallback ); - } - - redraw(); // in case something happened that will never be - // drawn 'cos we cancelled the timeout - - 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 ( wf ) - //current_camera->Load( wf, wf->LookupEntity( "window" ) ); - //else - current_camera->reset(); - - //invalidate(); - if( Fl::event_state( FL_CTRL ) ) { - resetCamera(); - } - redraw(); - break; - - case '[': // slow down - if( world->interval_real == 0 ) - world->interval_real = 10; - else - { - world->interval_real *= 1.2; - } - break; // need the parens above - - case ']': // speed up - if( world->interval_real == 0 ) - putchar( 7 ); // bell! - else - { - world->interval_real *= 0.8; - if( world->interval_real < 10 ) - world->interval_real = 0; - } - break; - case FL_Left: if( pCamOn == false ) { camera.move( -10, 0 ); } else { perspective_camera.strafe( -0.5 ); } break; @@ -595,6 +538,8 @@ return Fl_Gl_Window::handle(event); } // end switch( event ) + + return 0; } void Canvas::FixViewport(int W,int H) @@ -715,7 +660,7 @@ glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); } -inline void Canvas::resetCamera() +void Canvas::resetCamera() { float max_x = 0, max_y = 0, min_x = 0, min_y = 0; Modified: code/stage/trunk/libstage/options_dlg.cc =================================================================== --- code/stage/trunk/libstage/options_dlg.cc 2009-02-17 06:13:41 UTC (rev 7338) +++ code/stage/trunk/libstage/options_dlg.cc 2009-02-17 07:31:10 UTC (rev 7339) @@ -16,9 +16,7 @@ showAllCheck->callback( checkChanged, this ); 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(); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-02-17 06:13:41 UTC (rev 7338) +++ code/stage/trunk/libstage/stage.hh 2009-02-17 07:31:10 UTC (rev 7339) @@ -1390,7 +1390,13 @@ static void viewOptionsCb( OptionsDlg* oDlg, WorldGui* worldGui ); static void optionsDlgCb( OptionsDlg* oDlg, WorldGui* worldGui ); static void helpAboutCb( Fl_Widget* w, void* p ); - + static void pauseCb( Fl_Widget* w, WorldGui* worldGui ); + static void fasterCb( Fl_Widget* w, WorldGui* worldGui ); + static void slowerCb( Fl_Widget* w, WorldGui* worldGui ); + static void realtimeCb( Fl_Widget* w, WorldGui* worldGui ); + static void fasttimeCb( Fl_Widget* w, WorldGui* worldGui ); + static void resetViewCb( Fl_Widget* w, WorldGui* worldGui ); + // GUI functions bool saveAsDialog(); bool closeWindowQuery(); Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2009-02-17 06:13:41 UTC (rev 7338) +++ code/stage/trunk/libstage/worldgui.cc 2009-02-17 07:31:10 UTC (rev 7339) @@ -179,6 +179,8 @@ label( PROJECT ); + // make this menu's shortcuts work whoever has focus + mbar->global(); mbar->textsize(12); mbar->add( "&File", 0, 0, 0, FL_SUBMENU ); @@ -192,8 +194,18 @@ mbar->add( "File/E&xit", FL_CTRL+'q', WorldGui::fileExitCb, this ); mbar->add( "&View", 0, 0, 0, FL_SUBMENU ); + + mbar->add( "View/Reset", ' ', (Fl_Callback*) WorldGui::resetViewCb, this ); + mbar->add( "View/Filter data...", FL_SHIFT + 'd', (Fl_Callback*)WorldGui::viewOptionsCb, this ); canvas->createMenuItems( mbar, "View" ); + + mbar->add( "Run", 0,0,0, FL_SUBMENU ); + mbar->add( "Run/Pause", 'p', (Fl_Callback*) WorldGui::pauseCb, this ); + mbar->add( "Run/Faster", ']', (Fl_Callback*) WorldGui::fasterCb, this ); + mbar->add( "Run/Slower", '[', (Fl_Callback*) WorldGui::slowerCb, this ); + mbar->add( "Run/Realtime", '{', (Fl_Callback*) WorldGui::realtimeCb, this ); + mbar->add( "Run/Fast", '}', (Fl_Callback*) WorldGui::fasttimeCb, this ); mbar->add( "&Help", 0, 0, 0, FL_SUBMENU ); mbar->add( "Help/&About Stage...", 0, WorldGui::helpAboutCb, this ); @@ -212,6 +224,7 @@ delete canvas; } + void WorldGui::Show() { show(); // fltk @@ -531,6 +544,69 @@ return (*a) < (*b); } +void WorldGui::resetViewCb( Fl_Widget* w, WorldGui* worldGui ) +{ + worldGui->canvas->current_camera->reset(); + + if( Fl::event_state( FL_CTRL ) ) + { + worldGui->canvas->resetCamera(); + } + worldGui->canvas->redraw(); +} + +void WorldGui::slowerCb( Fl_Widget* w, WorldGui* wg ) +{ + if( wg->interval_real == 0 ) + wg->interval_real = 10; + else + { + wg->interval_real *= 1.2; + } +} + +void WorldGui::fasterCb( Fl_Widget* w, WorldGui* wg ) +{ + if( wg->interval_real == 0 ) + putchar( 7 ); // bell! + else + { + wg->interval_real *= 0.8; + if( wg->interval_real < 10 ) + wg->interval_real = 0; + } +} + +void WorldGui::realtimeCb( Fl_Widget* w, WorldGui* wg ) +{ + wg->interval_real = wg->interval_sim; +} + +void WorldGui::fasttimeCb( Fl_Widget* w, WorldGui* wg ) +{ + wg->interval_real = 0; +} + +void WorldGui::pauseCb( Fl_Widget* w, WorldGui* worldGui ) +{ + worldGui->TogglePause(); + + if( ! worldGui->paused ) + { + // // start the timer that causes regular redraws + Fl::add_timeout( ((double)worldGui->canvas->interval/1000), + (Fl_Timeout_Handler)Canvas::TimerCallback, + worldGui->canvas ); + } + else + { // remove the timeout + Fl::remove_timeout( (Fl_Timeout_Handler)Canvas::TimerCallback ); + } + + worldGui->canvas->redraw(); // in case something happened that will never be + // drawn 'cos we cancelled the timeout +} + void WorldGui::viewOptionsCb( OptionsDlg* oDlg, WorldGui* worldGui ) { // the options dialog expects a std::vector of options (annoyingly) This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-02-17 08:39:49
|
Revision: 7340 http://playerstage.svn.sourceforge.net/playerstage/?rev=7340&view=rev Author: rtv Date: 2009-02-17 08:39:45 +0000 (Tue, 17 Feb 2009) Log Message: ----------- added help dialog Modified Paths: -------------- code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-02-17 07:31:10 UTC (rev 7339) +++ code/stage/trunk/libstage/stage.hh 2009-02-17 08:39:45 UTC (rev 7340) @@ -1396,6 +1396,7 @@ static void realtimeCb( Fl_Widget* w, WorldGui* worldGui ); static void fasttimeCb( Fl_Widget* w, WorldGui* worldGui ); static void resetViewCb( Fl_Widget* w, WorldGui* worldGui ); + static void moreHelptCb( Fl_Widget* w, WorldGui* wg ); // GUI functions bool saveAsDialog(); Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2009-02-17 07:31:10 UTC (rev 7339) +++ code/stage/trunk/libstage/worldgui.cc 2009-02-17 08:39:45 UTC (rev 7340) @@ -83,8 +83,7 @@ <h3>Saving the world</h3> <P>You can save the current pose of everything in the world, using the -File/Save menu item. <b>Warning: the saved poses overwrite the current -world file.</b> Make a copy of your world file before saving if you +File/Save menu item. <b>Warning: the saved poses overwrite the currentworld file.</b> Make a copy of your world file before saving if you want to keep the old poses. Alternatively the File/Save As menu item can be used to save to a new world file. @@ -156,6 +155,21 @@ "Distributed under the terms of the \n" "GNU General Public License v2"; +static const char* MoreHelpText = + "http://playerstage.org\n" + "\n" + "has these resources to help you:\n" + "\n" + "\t* A user manual including API documentation\n" + "\t* A bug and feature request tracking system\n" + "\t* Mailing lists for users and developers\n" + "\t* A Wiki" + "\n\n" + "The user manual is included with the Stage source code but\n" + "is not built by default. To build the manual, run \"make\"\n" + "in the directory \"docsrc\" to produce \"docsrc/stage/index.html\" .\n" + "(requires Doxygen and supporting programs to be installed first).\n"; + WorldGui::WorldGui(int W,int H,const char* L) : Fl_Window(W,H,L ), canvas( new Canvas( this,0,30,W,H-30 ) ), @@ -201,15 +215,15 @@ canvas->createMenuItems( mbar, "View" ); mbar->add( "Run", 0,0,0, FL_SUBMENU ); - mbar->add( "Run/Pause", 'p', (Fl_Callback*) WorldGui::pauseCb, this ); + mbar->add( "Run/Pause", 'p', (Fl_Callback*) WorldGui::pauseCb, this, FL_MENU_DIVIDER ); mbar->add( "Run/Faster", ']', (Fl_Callback*) WorldGui::fasterCb, this ); mbar->add( "Run/Slower", '[', (Fl_Callback*) WorldGui::slowerCb, this ); - mbar->add( "Run/Realtime", '{', (Fl_Callback*) WorldGui::realtimeCb, this ); + mbar->add( "Run/Realtime", '{', (Fl_Callback*) WorldGui::realtimeCb, this, FL_MENU_DIVIDER ); mbar->add( "Run/Fast", '}', (Fl_Callback*) WorldGui::fasttimeCb, this ); mbar->add( "&Help", 0, 0, 0, FL_SUBMENU ); - mbar->add( "Help/&About Stage...", 0, WorldGui::helpAboutCb, this ); - //mbar->add( "Help/HTML Documentation", FL_CTRL + 'g', (Fl_Callback *)dummy_cb ); + mbar->add( "Help/Getting help...", 0, (Fl_Callback*)WorldGui::moreHelptCb, this, FL_MENU_DIVIDER ); + mbar->add( "Help/&About Stage...", 0, (Fl_Callback*) WorldGui::helpAboutCb, this ); callback( WorldGui::windowCb, this ); @@ -271,8 +285,6 @@ } label( title.c_str() ); - //UpdateOptions(); - show(); } @@ -699,8 +711,6 @@ void WorldGui::helpAboutCb( Fl_Widget* w, void* p ) { - // WorldGui* worldGui = static_cast<WorldGui*>( p ); - fl_register_images(); const int Width = 420; @@ -745,6 +755,32 @@ win->show(); } +void WorldGui::moreHelptCb( Fl_Widget* w, WorldGui* wg ) +{ + const int Width = 500; + const int Height = 250; + const int Spc = 10; + + Fl_Window* win = new Fl_Window( Width, Height ); // make a window + win->label( "Getting help with Stage" ); + + Fl_Text_Display* textDisplay; + textDisplay = new Fl_Text_Display( Spc, Spc, + Width-2*Spc, Height-2*Spc ); + + win->resizable( textDisplay ); + textDisplay->box( FL_NO_BOX ); + textDisplay->color( win->color() ); + + Fl_Text_Buffer* tbuf = new Fl_Text_Buffer; + tbuf->append( MoreHelpText ); + // textDisplay->wrap_mode( true, 50 ); + textDisplay->buffer( tbuf ); + + win->show(); +} + + bool WorldGui::saveAsDialog() { const char* newFilename; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-27 07:18:48
|
Revision: 7548 http://playerstage.svn.sourceforge.net/playerstage/?rev=7548&view=rev Author: rtv Date: 2009-03-27 07:18:14 +0000 (Fri, 27 Mar 2009) Log Message: ----------- reworked CustomVisualizer => Visualizer - tweaked API. See ModelLaser for example Modified Paths: -------------- code/stage/trunk/libstage/model_draw.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/stage.hh Modified: code/stage/trunk/libstage/model_draw.cc =================================================================== --- code/stage/trunk/libstage/model_draw.cc 2009-03-27 02:13:08 UTC (rev 7547) +++ code/stage/trunk/libstage/model_draw.cc 2009-03-27 07:18:14 UTC (rev 7548) @@ -245,35 +245,36 @@ glPopMatrix(); } -void Model::AddCustomVisualizer( CustomVisualizer* custom_visual ) +void Model::AddVisualizer( Visualizer* 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; - } - + 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() ); + std::map< std::string, Option* >::iterator i = canvas->_custom_options.find( custom_visual->GetMenuName() ); 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; + Option* op = new Option( custom_visual->GetMenuName(), + custom_visual->GetWorldfileName(), + "", + true, + world_gui ); + canvas->_custom_options[ custom_visual->GetMenuName() ] = op; RegisterOption( op ); } } -void Model::RemoveCustomVisualizer( CustomVisualizer* custom_visual ) +void Model::RemoveVisualizer( Visualizer* custom_visual ) { if( custom_visual ) custom_visual_list = g_list_remove(custom_visual_list, custom_visual ); @@ -537,20 +538,6 @@ 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 ) @@ -558,14 +545,13 @@ PushLocalCoords(); DataVisualize( cam ); // virtual function overridden by most model types - CustomVisualizer* vis; + Visualizer* 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 ); + vis = static_cast<Visualizer* >( item->data ); + if( world_gui->GetCanvas()->_custom_options[ vis->GetMenuName() ]->isEnabled() ) + vis->Visualize( this, cam ); } - // and draw the children LISTMETHODARG( children, Model*, DataVisualizeTree, cam ); Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2009-03-27 02:13:08 UTC (rev 7547) +++ code/stage/trunk/libstage/model_laser.cc 2009-03-27 07:18:14 UTC (rev 7548) @@ -32,10 +32,10 @@ static const char* DEFAULT_COLOR = "blue"; //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 ); -Option ModelLaser::showLaserFov( "Laser FOV", "show_laser_fov", "", false, NULL ); -Option ModelLaser::showLaserBeams( "Laser beams", "show_laser_beams", "", false, NULL ); +Option ModelLaser::Vis::showArea( "Laser scans", "show_laser", "", true, NULL ); +Option ModelLaser::Vis::showStrikes( "Laser strikes", "show_laser_strikes", "", false, NULL ); +Option ModelLaser::Vis::showFov( "Laser FOV", "show_laser_fov", "", false, NULL ); +Option ModelLaser::Vis::showBeams( "Laser beams", "show_laser_beams", "", false, NULL ); /** @ingroup model @@ -75,9 +75,122 @@ Only calculate the true range of every nth laser sample. The missing samples are filled in with a linear interpolation. Generally it would be better to use fewer samples, but some (poorly implemented!) programs expect a fixed number of samples. Setting this number > 1 allows you to reduce the amount of computation required for your fixed-size laser vector. */ +// create static vis objects + +ModelLaser::Vis::Vis( World* world ) + : Visualizer( "Laser", "laser_vis" ) +{ + world->RegisterOption( &showArea ); + world->RegisterOption( &showStrikes ); + world->RegisterOption( &showFov ); + world->RegisterOption( &showBeams ); +} + +void ModelLaser::Vis::Visualize( Model* mod, Camera* cam ) +{ + ModelLaser* laser = dynamic_cast<ModelLaser*>(mod); + unsigned int sample_count = 0; + stg_laser_sample_t* samples = laser->GetSamples( &sample_count ); + + if( ! (samples && sample_count) ) + return; + + glPushMatrix(); + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + + glTranslatef( 0,0, laser->GetGeom().size.z/2.0 ); // shoot the laser beam out at the right height + + // pack the laser hit points into a vertex array for fast rendering + static float* pts = NULL; + pts = (float*)g_realloc( pts, 2 * (sample_count+1) * sizeof(float)); + + pts[0] = 0.0; + pts[1] = 0.0; + + laser->PushColor( 1, 0, 0, 0.5 ); + glDepthMask( GL_FALSE ); + glPointSize( 2 ); + + for( unsigned int s=0; s<sample_count; s++ ) + { + double ray_angle = (s * (laser->fov / (sample_count-1))) - laser->fov/2.0; + pts[2*s+2] = (float)(samples[s].range * cos(ray_angle) ); + pts[2*s+3] = (float)(samples[s].range * sin(ray_angle) ); + + // if the sample is unusually bright, draw a little blob + if( samples[s].reflectance > 0 ) + { + glBegin( GL_POINTS ); + glVertex2f( pts[2*s+2], pts[2*s+3] ); + glEnd(); + } + } + + glVertexPointer( 2, GL_FLOAT, 0, pts ); + + laser->PopColor(); + + if( showArea ) + { + // draw the filled polygon in transparent blue + laser->PushColor( 0, 0, 1, 0.1 ); + glDrawArrays( GL_POLYGON, 0, sample_count+1 ); + laser->PopColor(); + //glDepthMask( GL_TRUE ); + } + + glDepthMask( GL_TRUE ); + + if( showStrikes ) + { + // draw the beam strike points + laser->PushColor( 0, 0, 1, 0.8 ); + glDrawArrays( GL_POINTS, 0, sample_count+1 ); + laser->PopColor(); + } + + if( showFov ) + { + for( unsigned int s=0; s<sample_count; s++ ) + { + double ray_angle = (s * (laser->fov / (sample_count-1))) - laser->fov/2.0; + pts[2*s+2] = (float)(laser->range_max * cos(ray_angle) ); + pts[2*s+3] = (float)(laser->range_max * sin(ray_angle) ); + } + + glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + laser->PushColor( 0, 0, 1, 0.5 ); + glDrawArrays( GL_POLYGON, 0, sample_count+1 ); + laser->PopColor(); + // glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); + } + + if( showBeams ) + { + laser->PushColor( 0, 0, 1, 0.5 ); + glBegin( GL_LINES ); + + for( unsigned int s=0; s<sample_count; s++ ) + { + + glVertex2f( 0,0 ); + double ray_angle = (s * (laser->fov / (sample_count-1))) - laser->fov/2.0; + glVertex2f( samples[s].range * cos(ray_angle), + samples[s].range * sin(ray_angle) ); + + } + glEnd(); + laser->PopColor(); + } + + glPopMatrix(); +} + + ModelLaser::ModelLaser( World* world, Model* parent ) : Model( world, parent, MODEL_TYPE_LASER ), + vis( world ), data_dl(0), data_dirty( true ), samples( NULL ), // don't allocate sample buffer memory until Update() is called @@ -104,13 +217,8 @@ // set default color SetColor( stg_lookup_color(DEFAULT_COLOR)); - - RegisterOption( &showLaserData ); - RegisterOption( &showLaserStrikes ); - RegisterOption( &showLaserFov ); - RegisterOption( &showLaserBeams ); - - //AddCustomVisualizer( new LaserScanVis( this )); + + AddVisualizer( &vis ); } @@ -130,8 +238,8 @@ fov = wf->ReadAngle( wf_entity, "fov", fov ); resolution = wf->ReadInt( wf_entity, "resolution", resolution ); - showLaserData.Load( wf, wf_entity ); - showLaserStrikes.Load( wf, wf_entity ); + //showLaserData.Load( wf, wf_entity ); + //showLaserStrikes.Load( wf, wf_entity ); if( resolution < 1 ) { @@ -300,132 +408,5 @@ } -void ModelLaser::DataVisualize( Camera* cam ) -{ - if( ! (samples && sample_count) ) - return; - - if ( ! (showLaserData || showLaserStrikes || showLaserFov || showLaserBeams ) ) - return; - - glPushMatrix(); - - glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); - // we only regenerate the list if there's new data - if( 1 ) // data_dirty ) // TODO - hmm, why doesn't this work? - { - data_dirty = false; - if( data_dl < 1 ) - data_dl = glGenLists(1); - - glNewList( data_dl, GL_COMPILE ); - - glTranslatef( 0,0, geom.size.z/2.0 ); // shoot the laser beam out at the right height - - // DEBUG - draw the origin of the laser beams - PushColor( 0,0,0,1.0 ); - glPointSize( 4.0 ); - glBegin( GL_POINTS ); - glVertex2f( 0,0 ); - glEnd(); - PopColor(); - - // pack the laser hit points into a vertex array for fast rendering - static float* pts = NULL; - pts = (float*)g_realloc( pts, 2 * (sample_count+1) * sizeof(float)); - - pts[0] = 0.0; - pts[1] = 0.0; - - PushColor( 0, 0, 1, 0.5 ); - glDepthMask( GL_FALSE ); - glPointSize( 2 ); - - for( unsigned int s=0; s<sample_count; s++ ) - { - double ray_angle = (s * (fov / (sample_count-1))) - fov/2.0; - pts[2*s+2] = (float)(samples[s].range * cos(ray_angle) ); - pts[2*s+3] = (float)(samples[s].range * sin(ray_angle) ); - - // if the sample is unusually bright, draw a little blob - if( showLaserData && (samples[s].reflectance > 0) ) - { - glBegin( GL_POINTS ); - glVertex2f( pts[2*s+2], pts[2*s+3] ); - glEnd(); - } - } - - glVertexPointer( 2, GL_FLOAT, 0, pts ); - - PopColor(); - - if( showLaserData ) - { - // draw the filled polygon in transparent blue - PushColor( 0, 0, 1, 0.1 ); - glDrawArrays( GL_POLYGON, 0, sample_count+1 ); - PopColor(); - } - - if( showLaserStrikes ) - { - // draw the beam strike points - PushColor( 0, 0, 1, 0.8 ); - glDrawArrays( GL_POINTS, 0, sample_count+1 ); - PopColor(); - } - - if( showLaserFov ) - { - for( unsigned int s=0; s<sample_count; s++ ) - { - double ray_angle = (s * (fov / (sample_count-1))) - fov/2.0; - pts[2*s+2] = (float)(range_max * cos(ray_angle) ); - pts[2*s+3] = (float)(range_max * sin(ray_angle) ); - } - - glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); - PushColor( 0, 0, 1, 0.5 ); - glDrawArrays( GL_POLYGON, 0, sample_count+1 ); - PopColor(); - // glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); - - } - - if( showLaserBeams ) - { - PushColor( 0, 0, 1, 0.5 ); - glBegin( GL_LINES ); - - for( unsigned int s=0; s<sample_count; s++ ) - { - - glVertex2f( 0,0 ); - double ray_angle = (s * (fov / (sample_count-1))) - fov/2.0; - glVertex2f( samples[s].range * cos(ray_angle), - samples[s].range * sin(ray_angle) ); - - } - glEnd(); - PopColor(); - } - - - glDepthMask( GL_TRUE ); - glEndList(); - } // end if ( data_dirty ) - - glCallList( data_dl ); - - glPopMatrix(); -} - - -// void ModelLaser::LaserScanVis::DataVisualize( Camera* cam ) -// { -// puts( "LSV DataVisualize" ); -// laser->DataVisualize( cam ); -// } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-03-27 02:13:08 UTC (rev 7547) +++ code/stage/trunk/libstage/stage.hh 2009-03-27 07:18:14 UTC (rev 7548) @@ -1461,12 +1461,23 @@ }; /** Abstract class for adding visualizations to models. DataVisualize must be overloaded, and is then called in the models local coord system */ - class CustomVisualizer { + class Visualizer { + private: + const std::string menu_name; + const std::string worldfile_name; + 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) + Visualizer( const std::string& menu_name, + const std::string& worldfile_name ) + : menu_name( menu_name ), + worldfile_name( worldfile_name ) + { } + + virtual ~Visualizer( void ) { } + virtual void Visualize( Model* mod, Camera* cam ) = 0; + + const std::string& GetMenuName() { return menu_name; } + const std::string& GetWorldfileName() { return worldfile_name; } }; @@ -1759,6 +1770,13 @@ void DataVisualizeTree( Camera* cam ); + void DrawFlagList(); + + void DrawPose( Pose pose ); + + void LoadDataBaseEntries( Worldfile* wf, int entity ); + + public: virtual void PushColor( stg_color_t col ) { world->PushColor( col ); } @@ -1767,13 +1785,7 @@ virtual void PopColor(){ world->PopColor(); } - void DrawFlagList(); - void DrawPose( Pose pose ); - - void LoadDataBaseEntries( Worldfile* wf, int entity ); - - public: PowerPack* FindPowerPack(); void RecordRenderPoint( GSList** head, GSList* link, @@ -1797,10 +1809,10 @@ void Say( const char* str ); /** Attach a user supplied visualization to a model. */ - void AddCustomVisualizer( CustomVisualizer* custom_visual ); + void AddVisualizer( Visualizer* custom_visual ); /** remove user supplied visualization to a model - supply the same ptr passed to AddCustomVisualizer */ - void RemoveCustomVisualizer( CustomVisualizer* custom_visual ); + void RemoveVisualizer( Visualizer* custom_visual ); void BecomeParentOf( Model* child ); @@ -2182,10 +2194,28 @@ stg_usec_t interval; } stg_laser_cfg_t; + /// %ModelLaser class class ModelLaser : public Model { private: + + class Vis : public Visualizer + { + private: + static Option showArea; + static Option showStrikes; + static Option showFov; + static Option showBeams; + + public: + Vis( World* world ); + virtual ~Vis( void ){} + virtual void Visualize( Model* mod, Camera* cam ); + }; + + Vis vis; + /** OpenGL displaylist for laser data */ int data_dl; bool data_dirty; @@ -2195,32 +2225,7 @@ stg_meters_t range_max; stg_radians_t fov; uint32_t resolution; - - static Option showLaserData; - static Option showLaserStrikes; - static Option showLaserFov; - static Option showLaserBeams; - -// class LaserScanVis : public CustomVisualizer -// { -// public: -// LaserScanVis( ModelLaser* laser ) : -// CustomVisualizer(), -// laser( laser ) -// { /* nothing to do */ }; - -// virtual void DataVisualize( Camera* cam ); - -// // rtv - surely a static string member would be easier here? -// //must return a name for visualization (careful not to return stack-memory) - -// virtual const std::string& name() { return "LaserScanVisName"; } ; - -// private: -// ModelLaser* laser; -// }; - - + public: static const char* typestr; // constructor @@ -2235,7 +2240,6 @@ virtual void Update(); virtual void Load(); virtual void Print( char* prefix ); - virtual void DataVisualize( Camera* cam ); uint32_t GetSampleCount(){ return sample_count; } @@ -2257,14 +2261,6 @@ { public: - // class Viz : public CustomVisualizer -// { - - -// }; - -// static Viz viz; - enum paddle_state_t { PADDLE_OPEN = 0, // default state PADDLE_CLOSED, This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-27 08:51:00
|
Revision: 7549 http://playerstage.svn.sourceforge.net/playerstage/?rev=7549&view=rev Author: rtv Date: 2009-03-27 08:50:26 +0000 (Fri, 27 Mar 2009) Log Message: ----------- cleaned API for property database Modified Paths: -------------- code/stage/trunk/libstage/model_props.cc code/stage/trunk/libstage/stage.hh Modified: code/stage/trunk/libstage/model_props.cc =================================================================== --- code/stage/trunk/libstage/model_props.cc 2009-03-27 07:18:14 UTC (rev 7548) +++ code/stage/trunk/libstage/model_props.cc 2009-03-27 08:50:26 UTC (rev 7549) @@ -4,7 +4,7 @@ #define MATCH(A,B) (strcmp(A,B)== 0) -void* Model::GetProperty( char* key ) +void* Model::GetProperty( const char* key ) { // see if the key has the predefined-property prefix if( strncmp( key, MP_PREFIX, strlen(MP_PREFIX)) == 0 ) Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-03-27 07:18:14 UTC (rev 7548) +++ code/stage/trunk/libstage/stage.hh 2009-03-27 08:50:26 UTC (rev 7549) @@ -1623,6 +1623,7 @@ Visibility vis; + stg_usec_t GetSimInterval(){ return world->interval_sim; } void Lock() { @@ -2044,8 +2045,48 @@ /** named-property interface */ - void* GetProperty( char* key ); - + void* GetProperty( const char* key ); + + bool GetPropertyFloat( const char* key, float* f, float defaultval ) + { + float* fp = (float*)GetProperty( key ); + if( fp ) + { + *f = *fp; + return true; + } + + *f = defaultval; + return false; + } + + bool GetPropertyInt( const char* key, int* i, int defaultval ) + { + int* ip = (int*)GetProperty( key ); + if( ip ) + { + *i = *ip; + return true; + } + + *i = defaultval; + return false; + } + + bool GetPropertyStr( const char* key, char** c, char* defaultval ) + { + char* cp = (char*)GetProperty( key ); + + if( cp ) + { + *c = cp; + return true; + } + + *c = defaultval; + return false; + } + /** @brief Set a named property of a Stage model. Set a property of a Stage model. This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-27 23:37:47
|
Revision: 7552 http://playerstage.svn.sourceforge.net/playerstage/?rev=7552&view=rev Author: rtv Date: 2009-03-27 23:37:32 +0000 (Fri, 27 Mar 2009) Log Message: ----------- fixed crash bug when a saystring was present without a powerpack Modified Paths: -------------- code/stage/trunk/libstage/model_draw.cc code/stage/trunk/libstage/model_position.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/model_draw.cc =================================================================== --- code/stage/trunk/libstage/model_draw.cc 2009-03-27 16:15:13 UTC (rev 7551) +++ code/stage/trunk/libstage/model_draw.cc 2009-03-27 23:37:32 UTC (rev 7552) @@ -321,11 +321,7 @@ glRotatef( robotAngle - yaw, 0,0,1 ); glRotatef( -pitch, 1,0,0 ); - - //if( ! parent ) - // glRectf( 0,0,1,1 ); - - //if( power_pack->stored > 0.0 ) + if( power_pack ) power_pack->Visualize( cam ); if( say_string ) @@ -339,8 +335,7 @@ 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 Modified: code/stage/trunk/libstage/model_position.cc =================================================================== --- code/stage/trunk/libstage/model_position.cc 2009-03-27 16:15:13 UTC (rev 7551) +++ code/stage/trunk/libstage/model_position.cc 2009-03-27 23:37:32 UTC (rev 7552) @@ -655,6 +655,8 @@ Gl::pose_inverse_shift( pose ); Gl::pose_shift( est_origin ); + glTranslatef( 0,0, 0.02 ); + // draw waypoints for( unsigned int i=0; i < waypoint_count; i++ ) waypoints[i].Draw(); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-03-27 16:15:13 UTC (rev 7551) +++ code/stage/trunk/libstage/stage.hh 2009-03-27 23:37:32 UTC (rev 7552) @@ -875,6 +875,8 @@ void LoadBlockGroup( Worldfile* wf, int entity, GHashTable* entitytable ); void LoadPuck( Worldfile* wf, int entity, GHashTable* entitytable ); + virtual Model* RecentlySelectedModel(){ return NULL; } + SuperRegion* AddSuperRegion( const stg_point_int_t& coord ); SuperRegion* GetSuperRegion( const stg_point_int_t& coord ); SuperRegion* GetSuperRegionCached( const stg_point_int_t& coord); @@ -1369,14 +1371,15 @@ WorldGui(int W,int H,const char*L=0); ~WorldGui(); - virtual bool Update(); - + virtual bool Update(); virtual void Load( const char* filename ); virtual void UnLoad(); virtual bool Save( const char* filename ); inline virtual bool IsGUI() { return true; } + virtual Model* RecentlySelectedModel(); + void DrawBoundingBoxTree(); void Start(){ paused = false; }; @@ -1755,7 +1758,7 @@ static void DrawBlocks( gpointer dummykey, Model* mod, void* arg ); - + virtual void DrawPicker(); virtual void DataVisualize( Camera* cam ); Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2009-03-27 16:15:13 UTC (rev 7551) +++ code/stage/trunk/libstage/worldgui.cc 2009-03-27 23:37:32 UTC (rev 7552) @@ -878,3 +878,6 @@ void WorldGui::PopColor() { canvas->PopColor(); } + +Model* WorldGui::RecentlySelectedModel() +{ return canvas->last_selection; } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-28 00:10:49
|
Revision: 7554 http://playerstage.svn.sourceforge.net/playerstage/?rev=7554&view=rev Author: rtv Date: 2009-03-28 00:10:37 +0000 (Sat, 28 Mar 2009) Log Message: ----------- cleaned up property API Modified Paths: -------------- code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/model_props.cc code/stage/trunk/libstage/stage.hh Modified: code/stage/trunk/libstage/model_load.cc =================================================================== --- code/stage/trunk/libstage/model_load.cc 2009-03-27 23:50:18 UTC (rev 7553) +++ code/stage/trunk/libstage/model_load.cc 2009-03-28 00:10:37 UTC (rev 7554) @@ -354,31 +354,14 @@ assert( type ); assert( value ); - //printf( "\nkey: %s type: %s value: %s\n", - // key, type, value ); - if( strcmp( type, "int" ) == 0 ) - { - int* i = new int( atoi(value) ); - SetProperty( strdup(key), (void*)i ); - } + SetPropertyInt( strdup(key), atoi(value) ); else if( strcmp( type, "float" ) == 0 ) - { - float* f = new float( strtod(value, NULL) ); - SetProperty( strdup(key), (void*)f ); - } + SetPropertyFloat( strdup(key), strtod(value, NULL) ); else if( strcmp( type, "string" ) == 0 ) - SetProperty( strdup(key), (void*)strdup(value) ); + SetPropertyStr( strdup(key), value ); else - PRINT_ERR1( "unknown database entry type \"%s\"\n", type ); - -// int* i = (int*)GetProperty( key ); -// float* f = (float*)GetProperty( key ); -// char* c = (char*)GetProperty( key ); - -// printf( "property %s has int value %d\n", key, *i ); -// printf( "property %s has float value %.2f\n", key, *f ); -// printf( "property %s has char* value %s\n", key, c ); + PRINT_ERR1( "unknown database entry type \"%s\"\n", type ); } } Modified: code/stage/trunk/libstage/model_props.cc =================================================================== --- code/stage/trunk/libstage/model_props.cc 2009-03-27 23:50:18 UTC (rev 7553) +++ code/stage/trunk/libstage/model_props.cc 2009-03-28 00:10:37 UTC (rev 7554) @@ -26,8 +26,8 @@ return g_datalist_get_data( &this->props, key ); } -int Model::SetProperty( char* key, - void* data ) +int Model::SetProperty( const char* key, + const void* data ) { // see if the key has the predefined-property prefix if( strncmp( key, MP_PREFIX, strlen(MP_PREFIX)) == 0 ) @@ -80,15 +80,74 @@ } // otherwise it's an arbitary property and we store the pointer - g_datalist_set_data( &this->props, key, data ); + g_datalist_set_data( &this->props, key, (void*)data ); return 0; // ok } -void Model::UnsetProperty( char* key ) +void Model::UnsetProperty( const char* key ) { 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 ); } + + +bool Model::GetPropertyFloat( const char* key, float* f, float defaultval ) +{ + float* fp = (float*)GetProperty( key ); + if( fp ) + { + *f = *fp; + return true; + } + + *f = defaultval; + return false; +} + +bool Model::GetPropertyInt( const char* key, int* i, int defaultval ) +{ + int* ip = (int*)GetProperty( key ); + if( ip ) + { + *i = *ip; + return true; + } + + *i = defaultval; + return false; +} + +bool Model::GetPropertyStr( const char* key, char** c, char* defaultval ) +{ + char* cp = (char*)GetProperty( key ); + + if( cp ) + { + *c = cp; + return true; + } + + *c = defaultval; + return false; +} + +void Model::SetPropertyInt( const char* key, int i ) +{ + int* ip = new int(i); + SetProperty( key, (void*)ip ); +} + +void Model::SetPropertyFloat( const char* key, float f ) +{ + float* fp = new float(f); + SetProperty( key, (void*)fp ); +} + +void Model::SetPropertyStr( const char* key, const char* str ) +{ + SetProperty( key, (void*)strdup(str) ); +} + Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-03-27 23:50:18 UTC (rev 7553) +++ code/stage/trunk/libstage/stage.hh 2009-03-28 00:10:37 UTC (rev 7554) @@ -2035,61 +2035,18 @@ { RemoveCallback( &hooks.save, cb ); } void AddUpdateCallback( stg_model_callback_t cb, void* user ) - { - AddCallback( &hooks.update, cb, user ); - //Subscribe(); // if attaching a callback here, assume we want updates to happen - } + { AddCallback( &hooks.update, cb, user ); } void RemoveUpdateCallback( stg_model_callback_t cb ) - { - RemoveCallback( &hooks.update, cb ); - //Unsubscribe(); - } + { RemoveCallback( &hooks.update, cb ); } /** named-property interface */ - void* GetProperty( const char* key ); - - bool GetPropertyFloat( const char* key, float* f, float defaultval ) - { - float* fp = (float*)GetProperty( key ); - if( fp ) - { - *f = *fp; - return true; - } - - *f = defaultval; - return false; - } + void* GetProperty( const char* key ); + bool GetPropertyFloat( const char* key, float* f, float defaultval ); + bool GetPropertyInt( const char* key, int* i, int defaultval ); + bool GetPropertyStr( const char* key, char** c, char* defaultval ); - bool GetPropertyInt( const char* key, int* i, int defaultval ) - { - int* ip = (int*)GetProperty( key ); - if( ip ) - { - *i = *ip; - return true; - } - - *i = defaultval; - return false; - } - - bool GetPropertyStr( const char* key, char** c, char* defaultval ) - { - char* cp = (char*)GetProperty( key ); - - if( cp ) - { - *c = cp; - return true; - } - - *c = defaultval; - return false; - } - /** @brief Set a named property of a Stage model. Set a property of a Stage model. @@ -2120,8 +2077,12 @@ 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 ); + int SetProperty( const char* key, const void* data ); + void SetPropertyInt( const char* key, int i ); + void SetPropertyFloat( const char* key, float f ); + void SetPropertyStr( const char* key, const char* str ); + + void UnsetProperty( const char* key ); virtual void Print( char* prefix ); virtual const char* PrintWithPose(); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-29 01:04:18
|
Revision: 7557 http://playerstage.svn.sourceforge.net/playerstage/?rev=7557&view=rev Author: rtv Date: 2009-03-29 01:03:23 +0000 (Sun, 29 Mar 2009) Log Message: ----------- working on energy monitoring Modified Paths: -------------- code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_draw.cc code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/powerpack.cc 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-03-29 00:31:56 UTC (rev 7556) +++ code/stage/trunk/libstage/model.cc 2009-03-29 01:03:23 UTC (rev 7557) @@ -690,7 +690,7 @@ { // consume energy stored in the power pack stg_joules_t consumed = watts * (world->interval_sim * 1e-6); - pp->Subtract( consumed ); + pp->Dissipate( consumed, this->pose ); } } @@ -776,7 +776,7 @@ // detach charger from all the packs charged last time for( GList* it = pps_charging; it; it = it->next ) - ((PowerPack*)it->data)->charging = false; + ((PowerPack*)it->data)->ChargeStop(); g_list_free( pps_charging ); pps_charging = NULL; @@ -798,10 +798,12 @@ //printf ( "moving %.2f joules from %s to %s\n", // amount, token, toucher->token ); - + + // set his charging flag + hispp->ChargeStart(); + // 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/model_draw.cc =================================================================== --- code/stage/trunk/libstage/model_draw.cc 2009-03-29 00:31:56 UTC (rev 7556) +++ code/stage/trunk/libstage/model_draw.cc 2009-03-29 01:03:23 UTC (rev 7557) @@ -312,8 +312,7 @@ float robotAngle = -rtod(gpz.a); glPushMatrix(); - - + // move above the robot glTranslatef( 0, 0, 0.5 ); Modified: code/stage/trunk/libstage/model_load.cc =================================================================== --- code/stage/trunk/libstage/model_load.cc 2009-03-29 00:31:56 UTC (rev 7556) +++ code/stage/trunk/libstage/model_load.cc 2009-03-29 01:03:23 UTC (rev 7557) @@ -20,40 +20,30 @@ assert( wf_entity ); PRINT_DEBUG1( "Model \"%s\" loading...", token ); - if( wf->PropertyExists( wf_entity, "joules" ) ) { if( !power_pack ) power_pack = new PowerPack( this ); - power_pack->stored = - wf->ReadFloat( wf_entity, "joules", power_pack->stored ); - + stg_joules_t j = wf->ReadFloat( wf_entity, "joules", + power_pack->GetStored() ) ; + /* assume that the store is full, so the capacity is the same as the charge */ - power_pack->capacity = power_pack->stored; + power_pack->SetStored( j ); + power_pack->SetCapacity( j ); } - + if( wf->PropertyExists( wf_entity, "joules_capacity" ) ) { if( !power_pack ) power_pack = new PowerPack( this ); - power_pack->capacity = - wf->ReadFloat( wf_entity, "joules_capacity", power_pack->capacity ); + power_pack->SetCapacity( wf->ReadFloat( wf_entity, "joules_capacity", + power_pack->GetCapacity() ) ); } - - /** if the capacity has been specified, limit the store to the capacity */ - if( power_pack && (power_pack->stored > power_pack->capacity) ) - { - power_pack->stored = power_pack->capacity; - PRINT_WARN3( "model %s energy storage exceeds capacity (%.2f / %.2f joules). Limited stored energy to max capactity.", - token, - power_pack->stored, - power_pack->capacity ); - } - + // use my own pack or an ancestor's for the other energy properties PowerPack* pp = FindPowerPack(); Modified: code/stage/trunk/libstage/powerpack.cc =================================================================== --- code/stage/trunk/libstage/powerpack.cc 2009-03-29 00:31:56 UTC (rev 7556) +++ code/stage/trunk/libstage/powerpack.cc 2009-03-29 01:03:23 UTC (rev 7557) @@ -9,27 +9,43 @@ #include "texture_manager.hh" using namespace Stg; + +stg_joules_t PowerPack::global_stored = 0.0; +stg_joules_t PowerPack::global_input = 0.0; +stg_joules_t PowerPack::global_capacity = 0.0; +stg_joules_t PowerPack::global_dissipated = 0.0; +stg_watts_t PowerPack::global_power = 0.0; +stg_watts_t PowerPack::global_power_smoothed = 0.0; +double PowerPack::global_smoothing_constant = 0.05; + + PowerPack::PowerPack( Model* mod ) : - mod( mod), stored( 0.0 ), capacity( 0.0 ), charging( false ) + dissipation_vec(), + event_vis( dissipation_vec ), + mod( mod), + stored( 0.0 ), + capacity( 0.0 ), + charging( false ) { // tell the world about this new pp - mod->world->AddPowerPack( this ); + mod->world->AddPowerPack( this ); + //mod->AddVisualizer( &event_vis ); }; PowerPack::~PowerPack() { - // tell the world about this new pp mod->world->RemovePowerPack( this ); + mod->RemoveVisualizer( &event_vis ); } -void PowerPack::Print( char* prefix ) +void PowerPack::Print( char* prefix ) const { printf( "%s stored %.2f/%.2f joules\n", prefix, stored, capacity ); } /** OpenGL visualization of the powerpack state */ -void PowerPack::Visualize( Camera* cam ) +void PowerPack::Visualize( Camera* cam ) const { const double height = 0.5; const double width = 0.2; @@ -107,6 +123,7 @@ glLineWidth( 1.0 ); } + // draw the percentage //gl_draw_string( -0.2, 0, 0, buf ); @@ -115,20 +132,31 @@ } -stg_joules_t PowerPack::RemainingCapacity() +stg_joules_t PowerPack::RemainingCapacity() const { return( capacity - stored ); } void PowerPack::Add( stg_joules_t j ) { - stored += MIN( RemainingCapacity(), j ); + stg_joules_t amount = MIN( RemainingCapacity(), j ); + stored += amount; + global_stored += amount; + + if( amount > 0 ) charging = true; } void PowerPack::Subtract( stg_joules_t j ) { - if( stored >= 0.0 ) - stored -= MIN( stored, j ); + if( stored < 0 ) // infinte supply! + { + global_input += j; // record energy entering the system + return; + } + stg_joules_t amount = MIN( stored, j ); + + stored -= amount; + global_stored -= amount; } void PowerPack::TransferTo( PowerPack* dest, stg_joules_t amount ) @@ -152,3 +180,96 @@ mod->NeedRedraw(); } + + +void PowerPack::SetCapacity( stg_joules_t cap ) +{ + global_capacity -= capacity; + capacity = cap; + global_capacity += capacity; + + if( stored > cap ) + { + global_stored -= stored; + stored = cap; + global_stored += stored; + } +} + +stg_joules_t PowerPack::GetCapacity() const +{ + return capacity; +} + +stg_joules_t PowerPack::GetStored() const +{ + return stored; +} + +void PowerPack::SetStored( stg_joules_t j ) +{ + global_stored -= stored; + stored = j; + global_stored += stored; +} + +void PowerPack::Dissipate( stg_joules_t j ) +{ + stg_joules_t amount = MIN( stored, j ); + + Subtract( amount ); + global_dissipated += amount; +} + +void PowerPack::Dissipate( stg_joules_t j, const Pose& p ) +{ + Dissipate( j ); + DissEvent ev( j, p ); + //dissipation_vec.push_back( ev ); +} + + +//------------------------------------------------------------------------------ +// Dissipation Event class + +PowerPack::DissEvent::DissEvent( stg_joules_t j, const Pose& p ) + : pose(p), joules(j) +{ + // empty +} + +void PowerPack::DissEvent::Draw() const +{ + float scale = 0.1; + float delta = 0.05; + + glColor4f( 1,0,0, MIN( joules * scale, 1.0 ) ); + glRectf( pose.x-delta, pose.y-delta, pose.x+delta, pose.z+delta ); +} + +//------------------------------------------------------------------------------ +// Dissipation Visualizer class + +PowerPack::DissipationVis::DissipationVis( std::vector<DissEvent>& events ) + : Visualizer( "energy dissipation", "energy_dissipation" ), + events( events ) +{ /* nothing to do */ } + + +void PowerPack::DissipationVis::Visualize( Model* mod, Camera* cam ) +{ + // go into world coordinates + + glPushMatrix(); + + + // draw the dissipation events in world coordinates + for( std::vector<DissEvent>::const_iterator i = events.begin(); + i != events.end(); + i++ ) + { + i->Draw(); + } + + glPopMatrix(); +} Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-03-29 00:31:56 UTC (rev 7556) +++ code/stage/trunk/libstage/stage.hh 2009-03-29 01:03:23 UTC (rev 7557) @@ -625,6 +625,26 @@ Flag( stg_color_t color, double size ); Flag* Nibble( double portion ); }; + + /** Abstract class for adding visualizations to models. DataVisualize must be overloaded, and is then called in the models local coord system */ + class Visualizer { + private: + const std::string menu_name; + const std::string worldfile_name; + + public: + Visualizer( const std::string& menu_name, + const std::string& worldfile_name ) + : menu_name( menu_name ), + worldfile_name( worldfile_name ) + { } + + virtual ~Visualizer( void ) { } + virtual void Visualize( Model* mod, Camera* cam ) = 0; + + const std::string& GetMenuName() { return menu_name; } + const std::string& GetWorldfileName() { return worldfile_name; } + }; typedef int(*stg_model_callback_t)(Model* mod, void* user ); @@ -1403,35 +1423,79 @@ virtual void RemoveChild( Model* mod ); }; - /** energy data packet */ class PowerPack { + friend class World; + friend class WorldGui; + public: - PowerPack( Model* mod ); - ~PowerPack(); + class DissEvent + { + public: + Pose pose; + stg_joules_t joules; + + DissEvent( stg_joules_t j, const Pose& p ); + ~DissEvent() { /* empty */ }; + + void Draw() const; + }; + + + protected: + + std::vector<DissEvent> dissipation_vec; + + class DissipationVis : public Visualizer + { + private: + std::vector<DissEvent>& events; + + public: + DissipationVis( std::vector<DissEvent>& events ); + virtual ~DissipationVis(){ /* empty */ } + + virtual void Visualize( Model* mod, Camera* cam ); + }; + + DissipationVis event_vis; + /** 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 */ bool charging; + + static stg_joules_t global_stored; + static stg_joules_t global_capacity; + static stg_joules_t global_dissipated; + static stg_joules_t global_input; + static stg_watts_t global_power; + static stg_watts_t global_power_smoothed; + static double global_smoothing_constant; + + public: + PowerPack( Model* mod ); + ~PowerPack(); + /** OpenGL visualization of the powerpack state */ - void Visualize( Camera* cam ); + void Visualize( Camera* cam ) const; /** Print human-readable status on stdout, prefixed with the argument string */ - void Print( char* prefix ); + void Print( char* prefix ) const; /** Returns the energy capacity minus the current amount stored */ - stg_joules_t RemainingCapacity(); + stg_joules_t RemainingCapacity() const; /** Add to the energy store */ void Add( stg_joules_t j ); @@ -1442,11 +1506,28 @@ /** Transfer some stored energy to another power pack */ void TransferTo( PowerPack* dest, stg_joules_t amount ); - double ProportionRemaining() + double ProportionRemaining() const { return( stored / capacity ); } - void Print( const char* prefix ) + void Print( const char* prefix ) const { printf( "%s PowerPack %.2f/%.2f J\n", prefix, stored, capacity ); } + + stg_joules_t GetStored() const; + stg_joules_t GetCapacity() const; + void SetCapacity( stg_joules_t j ); + void SetStored( stg_joules_t j ); + + /** Returns true iff the device received energy at the last timestep */ + bool GetCharging() const { return charging; } + + void ChargeStart(){ charging = true; } + void ChargeStop(){ charging = false; } + + /** Lose energy as work or heat */ + void Dissipate( stg_joules_t j ); + + /** Lose energy as work or heat, and record the event */ + void Dissipate( stg_joules_t j, const Pose& p ); }; class Visibility @@ -1464,25 +1545,6 @@ void Load( Worldfile* wf, int wf_entity ); }; - /** Abstract class for adding visualizations to models. DataVisualize must be overloaded, and is then called in the models local coord system */ - class Visualizer { - private: - const std::string menu_name; - const std::string worldfile_name; - - public: - Visualizer( const std::string& menu_name, - const std::string& worldfile_name ) - : menu_name( menu_name ), - worldfile_name( worldfile_name ) - { } - - virtual ~Visualizer( void ) { } - virtual void Visualize( Model* mod, Camera* cam ) = 0; - - const std::string& GetMenuName() { return menu_name; } - const std::string& GetWorldfileName() { return worldfile_name; } - }; /* Hooks for attaching special callback functions (not used as Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2009-03-29 00:31:56 UTC (rev 7556) +++ code/stage/trunk/libstage/worldgui.cc 2009-03-29 01:03:23 UTC (rev 7557) @@ -390,9 +390,9 @@ std::string WorldGui::ClockString() { const uint32_t usec_per_hour = 3600000000U; - const uint32_t usec_per_minute = 60000000; - const uint32_t usec_per_second = 1000000; - const uint32_t usec_per_msec = 1000; + const uint32_t usec_per_minute = 60000000U; + const uint32_t usec_per_second = 1000000U; + const uint32_t usec_per_msec = 1000U; uint32_t hours = sim_time / usec_per_hour; uint32_t minutes = (sim_time % usec_per_hour) / usec_per_minute; @@ -415,11 +415,21 @@ status_stream << std::setw( 2 ) << minutes << "m" << std::setw( 2 ) << seconds << "." << std::setprecision( 3 ) << std::setw( 3 ) << msec << "s "; - char str[ 80 ]; - snprintf( str, 80, "[%.2f]", localratio ); + char str[ 256 ]; + snprintf( str, 255, "[%.2f]", localratio ); status_stream << str; + + + snprintf( str, 255, "<stored: %.0f/%.0fKJ input: %.0fKJ dissipated: %.0fKJ power: %.2f(%.2f)W>", + PowerPack::global_stored / 1e3, + PowerPack::global_capacity /1e3, + PowerPack::global_input / 1e3, + PowerPack::global_dissipated / 1e3, + PowerPack::global_power, + PowerPack::global_power_smoothed ); + + status_stream << str; - if( paused == true ) status_stream << " [ PAUSED ]"; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-30 05:55:36
|
Revision: 7558 http://playerstage.svn.sourceforge.net/playerstage/?rev=7558&view=rev Author: rtv Date: 2009-03-30 05:55:10 +0000 (Mon, 30 Mar 2009) Log Message: ----------- working on power models Modified Paths: -------------- code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_draw.cc code/stage/trunk/libstage/model_laser.cc code/stage/trunk/libstage/powerpack.cc 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-03-29 01:03:23 UTC (rev 7557) +++ code/stage/trunk/libstage/model.cc 2009-03-30 05:55:10 UTC (rev 7558) @@ -690,7 +690,7 @@ { // consume energy stored in the power pack stg_joules_t consumed = watts * (world->interval_sim * 1e-6); - pp->Dissipate( consumed, this->pose ); + pp->Dissipate( consumed, GetGlobalPose() ); } } Modified: code/stage/trunk/libstage/model_draw.cc =================================================================== --- code/stage/trunk/libstage/model_draw.cc 2009-03-29 01:03:23 UTC (rev 7557) +++ code/stage/trunk/libstage/model_draw.cc 2009-03-30 05:55:10 UTC (rev 7558) @@ -245,7 +245,7 @@ glPopMatrix(); } -void Model::AddVisualizer( Visualizer* custom_visual ) +void Model::AddVisualizer( Visualizer* custom_visual, bool on_by_default ) { if( !custom_visual ) return; @@ -267,7 +267,7 @@ Option* op = new Option( custom_visual->GetMenuName(), custom_visual->GetWorldfileName(), "", - true, + on_by_default, world_gui ); canvas->_custom_options[ custom_visual->GetMenuName() ] = op; RegisterOption( op ); Modified: code/stage/trunk/libstage/model_laser.cc =================================================================== --- code/stage/trunk/libstage/model_laser.cc 2009-03-29 01:03:23 UTC (rev 7557) +++ code/stage/trunk/libstage/model_laser.cc 2009-03-30 05:55:10 UTC (rev 7558) @@ -218,7 +218,7 @@ // set default color SetColor( stg_lookup_color(DEFAULT_COLOR)); - AddVisualizer( &vis ); + AddVisualizer( &vis, true ); } Modified: code/stage/trunk/libstage/powerpack.cc =================================================================== --- code/stage/trunk/libstage/powerpack.cc 2009-03-29 01:03:23 UTC (rev 7557) +++ code/stage/trunk/libstage/powerpack.cc 2009-03-30 05:55:10 UTC (rev 7558) @@ -20,8 +20,7 @@ PowerPack::PowerPack( Model* mod ) : - dissipation_vec(), - event_vis( dissipation_vec ), + event_vis( 32,32,1.0 ), mod( mod), stored( 0.0 ), capacity( 0.0 ), @@ -29,7 +28,7 @@ { // tell the world about this new pp mod->world->AddPowerPack( this ); - //mod->AddVisualizer( &event_vis ); + mod->AddVisualizer( &event_vis, false ); }; PowerPack::~PowerPack() @@ -219,57 +218,81 @@ Subtract( amount ); global_dissipated += amount; + + //stg_watts_t w = j / (interval / 1e6); } void PowerPack::Dissipate( stg_joules_t j, const Pose& p ) { Dissipate( j ); - DissEvent ev( j, p ); - //dissipation_vec.push_back( ev ); + event_vis.Accumulate( p.x, p.y, j ); } - //------------------------------------------------------------------------------ -// Dissipation Event class +// Dissipation Visualizer class -PowerPack::DissEvent::DissEvent( stg_joules_t j, const Pose& p ) - : pose(p), joules(j) -{ - // empty -} -void PowerPack::DissEvent::Draw() const +PowerPack::DissipationVis::DissipationVis( stg_meters_t width, + stg_meters_t height, + stg_meters_t cellsize ) + : Visualizer( "energy dissipation", "energy_dissipation" ), + columns(width/cellsize), + rows(height/cellsize), + width(width), + height(height), + cells( new stg_joules_t[columns*rows] ), + peak_value(0), + cellsize(cellsize) +{ /* nothing to do */ } + +PowerPack::DissipationVis::~DissipationVis() { - float scale = 0.1; - float delta = 0.05; - - glColor4f( 1,0,0, MIN( joules * scale, 1.0 ) ); - glRectf( pose.x-delta, pose.y-delta, pose.x+delta, pose.z+delta ); + if( cells ) + delete[] cells; } -//------------------------------------------------------------------------------ -// Dissipation Visualizer class - -PowerPack::DissipationVis::DissipationVis( std::vector<DissEvent>& events ) - : Visualizer( "energy dissipation", "energy_dissipation" ), - events( events ) -{ /* nothing to do */ } - - void PowerPack::DissipationVis::Visualize( Model* mod, Camera* cam ) { // go into world coordinates glPushMatrix(); - - - // draw the dissipation events in world coordinates - for( std::vector<DissEvent>::const_iterator i = events.begin(); - i != events.end(); - i++ ) - { - i->Draw(); - } + Gl::pose_inverse_shift( mod->GetPose() ); + + glTranslatef( -width/2.0, -height/2.0, 0 ); + glScalef( cellsize, cellsize, 1 ); + + for( unsigned int y=0; y<rows; y++ ) + for( unsigned int x=0; x<columns; x++ ) + { + stg_joules_t j = cells[ y*columns + x ]; + if( j > 0 ) + { + glColor4f( 1.0, 0, 0, j/peak_value ); + glRectf( x,y,x+1,y+1 ); + } + } + glPopMatrix(); } + + + +void PowerPack::DissipationVis::Accumulate( stg_meters_t x, + stg_meters_t y, + stg_joules_t amount ) +{ + int ix = (x+width/2.0)/cellsize; + int iy = (y+height/2.0)/cellsize; + + assert( ix >= 0 ); + assert( ix < (int)columns ); + assert( iy >= 0 ); + assert( iy < (int)rows ); + + stg_joules_t* j = cells + (iy*columns + ix ); + + (*j) += amount; + if( (*j) > peak_value ) + peak_value = (*j); +} Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-03-29 01:03:23 UTC (rev 7557) +++ code/stage/trunk/libstage/stage.hh 2009-03-30 05:55:10 UTC (rev 7558) @@ -856,13 +856,13 @@ stg_usec_t quit_time; stg_usec_t real_time_now; ///< The current real time in microseconds stg_usec_t real_time_start; ///< the real time at which this world was created - GMutex* thread_mutex; - GThreadPool *threadpool; + GMutex* thread_mutex; ///< protect the worker thread management stuff + GThreadPool *threadpool; ///<worker threads for updating some sensor models in parallel int total_subs; ///< the total number of subscriptions to all models unsigned int update_jobs_pending; GList* velocity_list; ///< Models with non-zero velocity and should have their poses updated - unsigned int worker_threads; - GCond* worker_threads_done; + unsigned int worker_threads; ///< the number of worker threads to use + GCond* worker_threads_done; ///< signalled when there are no more updates for the worker threads to do protected: @@ -872,7 +872,7 @@ GHashTable* option_table; ///< GUI options (toggles) registered by models GList* powerpack_list; ///< List of all the powerpacks attached to models in the world GList* ray_list;///< List of rays traced for debug visualization - stg_usec_t sim_time; ///< the current sim time in this world in ms + stg_usec_t sim_time; ///< the current sim time in this world in microseconds GHashTable* superregions; SuperRegion* sr_cached; ///< The last superregion looked up by this world // GList* update_list; ///< Models that have a subscriber or controller, and need to be updated @@ -1426,38 +1426,29 @@ /** energy data packet */ class PowerPack { - friend class World; + //friend class World; friend class WorldGui; - public: - class DissEvent - { - public: - Pose pose; - stg_joules_t joules; - - DissEvent( stg_joules_t j, const Pose& p ); - ~DissEvent() { /* empty */ }; - - void Draw() const; - }; - - - protected: - std::vector<DissEvent> dissipation_vec; - class DissipationVis : public Visualizer { private: - std::vector<DissEvent>& events; - + unsigned int columns, rows; + stg_meters_t width, height; + stg_joules_t* cells; + double peak_value; + double cellsize; + public: - DissipationVis( std::vector<DissEvent>& events ); - virtual ~DissipationVis(){ /* empty */ } - + DissipationVis( stg_meters_t width, + stg_meters_t height, + stg_meters_t cellsize ); + + virtual ~DissipationVis(); virtual void Visualize( Model* mod, Camera* cam ); + + void Accumulate( stg_meters_t x, stg_meters_t y, stg_joules_t amount ); }; DissipationVis event_vis; @@ -1876,7 +1867,7 @@ void Say( const char* str ); /** Attach a user supplied visualization to a model. */ - void AddVisualizer( Visualizer* custom_visual ); + void AddVisualizer( Visualizer* custom_visual, bool on_by_default ); /** remove user supplied visualization to a model - supply the same ptr passed to AddCustomVisualizer */ void RemoveVisualizer( Visualizer* custom_visual ); Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2009-03-29 01:03:23 UTC (rev 7557) +++ code/stage/trunk/libstage/worldgui.cc 2009-03-30 05:55:10 UTC (rev 7558) @@ -420,13 +420,12 @@ status_stream << str; - snprintf( str, 255, "<stored: %.0f/%.0fKJ input: %.0fKJ dissipated: %.0fKJ power: %.2f(%.2f)W>", + snprintf( str, 255, "<stored: %.0f/%.0fKJ input: %.0fKJ dissipated: %.0fKJ power: %.2fKW>", PowerPack::global_stored / 1e3, PowerPack::global_capacity /1e3, PowerPack::global_input / 1e3, PowerPack::global_dissipated / 1e3, - PowerPack::global_power, - PowerPack::global_power_smoothed ); + (PowerPack::global_dissipated / (sim_time / 1e6)) / 1e3 ); status_stream << str; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-30 18:28:19
|
Revision: 7560 http://playerstage.svn.sourceforge.net/playerstage/?rev=7560&view=rev Author: rtv Date: 2009-03-30 18:27:45 +0000 (Mon, 30 Mar 2009) Log Message: ----------- added more powerpack API Modified Paths: -------------- code/stage/trunk/libstage/powerpack.cc code/stage/trunk/libstage/stage.hh Modified: code/stage/trunk/libstage/powerpack.cc =================================================================== --- code/stage/trunk/libstage/powerpack.cc 2009-03-30 06:57:21 UTC (rev 7559) +++ code/stage/trunk/libstage/powerpack.cc 2009-03-30 18:27:45 UTC (rev 7560) @@ -205,6 +205,11 @@ return stored; } +stg_joules_t PowerPack::GetDissipated() const +{ + return dissipated; +} + void PowerPack::SetStored( stg_joules_t j ) { global_stored -= stored; @@ -217,6 +222,7 @@ stg_joules_t amount = MIN( stored, j ); Subtract( amount ); + dissipated += amount; global_dissipated += amount; //stg_watts_t w = j / (interval / 1e6); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-03-30 06:57:21 UTC (rev 7559) +++ code/stage/trunk/libstage/stage.hh 2009-03-30 18:27:45 UTC (rev 7560) @@ -1465,6 +1465,8 @@ /** TRUE iff the device is receiving energy */ bool charging; + /** Energy dissipated */ + stg_joules_t dissipated; static stg_joules_t global_stored; static stg_joules_t global_capacity; @@ -1505,6 +1507,7 @@ stg_joules_t GetStored() const; stg_joules_t GetCapacity() const; + stg_joules_t GetDissipated() const; void SetCapacity( stg_joules_t j ); void SetStored( stg_joules_t j ); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-30 21:43:56
|
Revision: 7561 http://playerstage.svn.sourceforge.net/playerstage/?rev=7561&view=rev Author: rtv Date: 2009-03-30 21:43:22 +0000 (Mon, 30 Mar 2009) Log Message: ----------- vis stuff Modified Paths: -------------- code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/gl.cc code/stage/trunk/libstage/model_draw.cc code/stage/trunk/libstage/powerpack.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-03-30 18:27:45 UTC (rev 7560) +++ code/stage/trunk/libstage/canvas.cc 2009-03-30 21:43:22 UTC (rev 7561) @@ -147,7 +147,6 @@ GLuint mains_id = TextureManager::getInstance().loadTexture( fullpath.c_str() ); TextureManager::getInstance()._mains_texture_id = mains_id; - //TODO merge this code into the textureManager? int i, j; @@ -177,6 +176,8 @@ glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE); + fl_font( FL_HELVETICA, 12 ); + init_done = true; } @@ -940,7 +941,9 @@ } if( showClock ) - { + { + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + //use orthogonal projeciton without any zoom glMatrixMode (GL_PROJECTION); glPushMatrix(); //save old projection @@ -958,22 +961,31 @@ if( showFollow == true && last_selection ) clockstr.append( " [ FOLLOW MODE ]" ); - fl_font( FL_HELVETICA, 12 ); - float txtWidth = gl_width( clockstr.c_str() ); + float txtWidth = gl_width( clockstr.c_str()); + if( txtWidth < 200 ) txtWidth = 200; int txtHeight = gl_height(); - int width, height; - width = int( txtWidth / 10 ) * 10; - height = ( txtHeight / 5 + 1 ) * 5; - float margin = ( height - txtHeight ) * 0.75; + const int margin = 5; + int width, height; + width = txtWidth + 2 * margin; + height = txtHeight + 2 * margin; + // TIME BOX 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, 0, clockstr.c_str() ); + + // ENERGY BOX + colorstack.Push( 0.8,1.0,0.8,0.85 ); // pale green + glRectf( 0, height, width, 90 ); colorstack.Push( 0,0,0 ); // black - Gl::draw_string( margin, margin, 0, clockstr.c_str() ); + Gl::draw_string_multiline( margin, height + margin, txtWidth, 50, world->EnergyString().c_str(), (Fl_Align)( FL_ALIGN_LEFT | FL_ALIGN_BOTTOM) ); + colorstack.Pop(); + colorstack.Pop(); + colorstack.Pop(); + colorstack.Pop(); glEnable( GL_DEPTH_TEST ); glPopMatrix(); Modified: code/stage/trunk/libstage/gl.cc =================================================================== --- code/stage/trunk/libstage/gl.cc 2009-03-30 18:27:45 UTC (rev 7560) +++ code/stage/trunk/libstage/gl.cc 2009-03-30 21:43:22 UTC (rev 7561) @@ -24,9 +24,16 @@ void Stg::Gl::draw_string( float x, float y, float z, const char *str ) { - glRasterPos3f( x, y, z ); + 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::draw_string_multiline( float x, float y, float w, float h, const char *str, Fl_Align align ) +{ + //glRasterPos3f( x, y, z ); //printf( "[%.2f %.2f %.2f] string %u %s\n", x,y,z,(unsigned int)strlen(str), str ); - gl_draw(str); + gl_draw(str, x, y, w, h, align ); // fltk function } void Stg::Gl::draw_speech_bubble( float x, float y, float z, const char* str ) @@ -50,6 +57,23 @@ glEnd(); } +// draw an octagon with center rectangle dimensions w/h +// and outside margin m +void Stg::Gl::draw_octagon( float x, float y, float w, float h, float m ) +{ + glBegin(GL_POLYGON); + glVertex2f( x + m+w, y ); + glVertex2f( x+w+2*m, y+m ); + glVertex2f( x+w+2*m, y+h+m ); + glVertex2f( x+m+w, y+h+2*m ); + glVertex2f( x+m, y+h+2*m ); + glVertex2f( x, y+h+m ); + glVertex2f( x, y+m ); + glVertex2f( x+m, y ); + glEnd(); +} + + void Stg::Gl::draw_centered_rect( float x, float y, float dx, float dy ) { glRectf( x-0.5*dx, y-0.5*dy, x+0.5*dx, y+0.5*dy ); Modified: code/stage/trunk/libstage/model_draw.cc =================================================================== --- code/stage/trunk/libstage/model_draw.cc 2009-03-30 18:27:45 UTC (rev 7560) +++ code/stage/trunk/libstage/model_draw.cc 2009-03-30 21:43:22 UTC (rev 7561) @@ -325,6 +325,8 @@ if( say_string ) { + glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + //get raster positition, add gl_width, then project back to world coords glRasterPos3f( 0, 0, 0 ); GLfloat pos[ 4 ]; @@ -335,7 +337,7 @@ if( valid ) { - fl_font( FL_HELVETICA, 12 ); + //fl_font( FL_HELVETICA, 12 ); float w = gl_width( this->say_string ); // scaled text width float h = gl_height(); // scaled text height @@ -379,7 +381,7 @@ PushColor( BUBBLE_TEXT ); // draw text inside the bubble - Gl::draw_string( 2.5*m, 2.5*m, 0, this->say_string ); + Gl::draw_string( m, 2.5*m, 0, this->say_string ); PopColor(); } } Modified: code/stage/trunk/libstage/powerpack.cc =================================================================== --- code/stage/trunk/libstage/powerpack.cc 2009-03-30 18:27:45 UTC (rev 7560) +++ code/stage/trunk/libstage/powerpack.cc 2009-03-30 21:43:22 UTC (rev 7561) @@ -124,10 +124,7 @@ // draw the percentage - //gl_draw_string( -0.2, 0, 0, buf ); - - // ? - // glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); + //gl_draw_string( -0.2, 0, 0, buf ); } @@ -237,6 +234,7 @@ //------------------------------------------------------------------------------ // Dissipation Visualizer class +stg_joules_t PowerPack::DissipationVis::global_peak_value = 0.0; PowerPack::DissipationVis::DissipationVis( stg_meters_t width, stg_meters_t height, @@ -274,7 +272,7 @@ stg_joules_t j = cells[ y*columns + x ]; if( j > 0 ) { - glColor4f( 1.0, 0, 0, j/peak_value ); + glColor4f( 1.0, 0, 0, j/global_peak_value ); glRectf( x,y,x+1,y+1 ); } } @@ -300,5 +298,10 @@ (*j) += amount; if( (*j) > peak_value ) - peak_value = (*j); + { + peak_value = (*j); + + if( peak_value > global_peak_value ) + global_peak_value = peak_value; + } } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-03-30 18:27:45 UTC (rev 7560) +++ code/stage/trunk/libstage/stage.hh 2009-03-30 21:43:22 UTC (rev 7561) @@ -468,8 +468,11 @@ 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_string_multiline( float x, float y, float w, float h, + const char *string, Fl_Align align ); void draw_speech_bubble( float x, float y, float z, const char* str ); void draw_octagon( float w, float h, float m ); + void draw_octagon( float x, float y, float w, float h, float m ); void draw_vector( double x, double y, double z ); void draw_origin( double len ); /** Draws a rectangle with center at x,y, with sides of length dx,dy */ @@ -1414,6 +1417,9 @@ /** Get human readable string that describes the current simulation time. */ std::string ClockString( void ); + + /** Get human readable string that describes the current global energy state. */ + std::string EnergyString( void ); /** Set the minimum real time interval between world updates, in microeconds. */ @@ -1437,8 +1443,10 @@ unsigned int columns, rows; stg_meters_t width, height; stg_joules_t* cells; - double peak_value; + stg_joules_t peak_value; double cellsize; + + static stg_joules_t global_peak_value; public: DissipationVis( stg_meters_t width, Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2009-03-30 18:27:45 UTC (rev 7560) +++ code/stage/trunk/libstage/worldgui.cc 2009-03-30 21:43:22 UTC (rev 7561) @@ -406,36 +406,41 @@ average_real_interval /= INTERVAL_LOG_LEN; double localratio = (double)interval_sim / (double)average_real_interval; + + std::string str; + char buf[256]; - std::ostringstream status_stream; - status_stream.fill( '0' ); - if( hours > 0 ) - status_stream << hours << "h"; - - status_stream << std::setw( 2 ) << minutes << "m" - << std::setw( 2 ) << seconds << "." << std::setprecision( 3 ) << std::setw( 3 ) << msec << "s "; - - char str[ 256 ]; - snprintf( str, 255, "[%.2f]", localratio ); - status_stream << str; - - - snprintf( str, 255, "<stored: %.0f/%.0fKJ input: %.0fKJ dissipated: %.0fKJ power: %.2fKW>", - PowerPack::global_stored / 1e3, - PowerPack::global_capacity /1e3, - PowerPack::global_input / 1e3, - PowerPack::global_dissipated / 1e3, - (PowerPack::global_dissipated / (sim_time / 1e6)) / 1e3 ); - - status_stream << str; + if( hours > 0 ) + { + snprintf( buf, 255, "%uh", hours ); + str += buf; + } - if( paused == true ) - status_stream << " [ PAUSED ]"; - - - return status_stream.str(); + snprintf( buf, 255, "%um%02us%03umsec [%.2f]", minutes, seconds, msec, localratio ); + str += buf; + + if( paused == true ) + str += " [ PAUSED ]"; + + return str; } +std::string WorldGui::EnergyString() +{ + char str[512]; + + snprintf( str, 255, "Energy\n stored: %.0f / %.0f KJ\n input: %.0f KJ\n output: %.0f KJ at %.2f KW\n", + PowerPack::global_stored / 1e3, + PowerPack::global_capacity /1e3, + PowerPack::global_input / 1e3, + PowerPack::global_dissipated / 1e3, + (PowerPack::global_dissipated / (sim_time / 1e6)) / 1e3 ); + + std::string s( str ); + return s; +} + + // callback wrapper for SuperRegion::Draw() static void Draw_cb( gpointer dummykey, SuperRegion* sr, This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-03-31 17:07:42
|
Revision: 7567 http://playerstage.svn.sourceforge.net/playerstage/?rev=7567&view=rev Author: rtv Date: 2009-03-31 17:07:29 +0000 (Tue, 31 Mar 2009) Log Message: ----------- adding strip plot vis Modified Paths: -------------- code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/canvas.hh code/stage/trunk/libstage/gl.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_fiducial.cc code/stage/trunk/libstage/powerpack.cc code/stage/trunk/libstage/stage.hh Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2009-03-31 07:31:00 UTC (rev 7566) +++ code/stage/trunk/libstage/CMakeLists.txt 2009-03-31 17:07:29 UTC (rev 7567) @@ -3,7 +3,9 @@ # for config.h include_directories(${PROJECT_BINARY_DIR}) -set( stageSrcs ancestor.cc +set( stageSrcs + vis_strip.cc + ancestor.cc block.cc blockgroup.cc camera.cc Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-03-31 07:31:00 UTC (rev 7566) +++ code/stage/trunk/libstage/canvas.cc 2009-03-31 17:07:29 UTC (rev 7567) @@ -125,8 +125,7 @@ glPolygonMode( GL_FRONT_AND_BACK, GL_FILL ); // install a font - //gl_font( FL_HELVETICA, 12 ); - gl_font( FL_COURIER, 12 ); + gl_font( FL_HELVETICA, 12 ); blur = false; @@ -177,7 +176,7 @@ glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE); - fl_font( FL_HELVETICA, 12 ); + // fl_font( FL_HELVETICA, 16 ); init_done = true; } @@ -983,7 +982,7 @@ colorstack.Push( 0.8,1.0,0.8,0.85 ); // pale green glRectf( 0, height, width, 90 ); colorstack.Push( 0,0,0 ); // black - Gl::draw_string_multiline( margin, height + margin, txtWidth, 50, + Gl::draw_string_multiline( margin, height + margin, width, 50, world->EnergyString().c_str(), (Fl_Align)( FL_ALIGN_LEFT | FL_ALIGN_BOTTOM) ); colorstack.Pop(); @@ -1006,6 +1005,30 @@ } +void Canvas::EnterScreenCS() +{ + //use orthogonal projeciton without any zoom + glMatrixMode (GL_PROJECTION); + glPushMatrix(); //save old projection + glLoadIdentity (); + glOrtho( 0, w(), 0, h(), -100, 100 ); + glMatrixMode (GL_MODELVIEW); + + glPushMatrix(); + glLoadIdentity(); + glDisable( GL_DEPTH_TEST ); +} + +void Canvas::LeaveScreenCS() +{ + glEnable( GL_DEPTH_TEST ); + glPopMatrix(); + glMatrixMode (GL_PROJECTION); + glPopMatrix(); + glMatrixMode (GL_MODELVIEW); +} + + void Canvas::Screenshot() { Modified: code/stage/trunk/libstage/canvas.hh =================================================================== --- code/stage/trunk/libstage/canvas.hh 2009-03-31 07:31:00 UTC (rev 7566) +++ code/stage/trunk/libstage/canvas.hh 2009-03-31 17:07:29 UTC (rev 7567) @@ -130,10 +130,15 @@ void PopColor(){ colorstack.Pop(); } void InvertView( uint32_t invertflags ); + + bool VisualizeAll(){ return ! visualizeAll; } static void TimerCallback( Canvas* canvas ); static void perspectiveCb( Fl_Widget* w, void* p ); + void EnterScreenCS(); + void LeaveScreenCS(); + void Load( Worldfile* wf, int section ); void Save( Worldfile* wf, int section ); }; Modified: code/stage/trunk/libstage/gl.cc =================================================================== --- code/stage/trunk/libstage/gl.cc 2009-03-31 07:31:00 UTC (rev 7566) +++ code/stage/trunk/libstage/gl.cc 2009-03-31 17:07:29 UTC (rev 7567) @@ -21,7 +21,48 @@ coord_shift( -pose.x, -pose.y, -pose.z, 0 ); } +void Stg::Gl::draw_array( float x, float y, float w, float h, + float* data, size_t len, size_t offset, + float min, float max ) +{ + float sample_spacing = w / (float)len; + float yscale = h / (max-min); + + //printf( "min %.2f max %.2f\n", min, max ); + glBegin( GL_LINE_STRIP ); + + for( unsigned int i=0; i<len; i++ ) + glVertex3f( x + (float)i*sample_spacing, y+(data[(i+offset)%len]-min)*yscale, 0.01 ); + + glEnd(); + + + glColor3f( 0,0,0 ); + char buf[64]; + snprintf( buf, 63, "%.2f", min ); + Gl::draw_string( x,y,0,buf ); + snprintf( buf, 63, "%.2f", max ); + Gl::draw_string( x,y+h-fl_height(),0,buf ); + +} + +void Stg::Gl::draw_array( float x, float y, float w, float h, + float* data, size_t len, size_t offset ) +{ + // wild initial bounds + float smallest = 1e16; + float largest = -1e16; + + for( size_t i=0; i<len; i++ ) + { + smallest = MIN( smallest, data[i] ); + largest = MAX( largest, data[i] ); + } + + draw_array( x,y,w,h,data,len,offset,smallest,largest ); +} + void Stg::Gl::draw_string( float x, float y, float z, const char *str ) { glRasterPos3f( x, y, z ); Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-03-31 07:31:00 UTC (rev 7566) +++ code/stage/trunk/libstage/model.cc 2009-03-31 17:07:29 UTC (rev 7567) @@ -356,8 +356,6 @@ if( flag_list == NULL ) return NULL; - printf( "pop flag" ); - Flag* flag = (Flag*)flag_list->data; flag_list = g_list_remove( flag_list, flag ); @@ -366,7 +364,6 @@ return flag; } - void Model::ClearBlocks( void ) { UnMap(); Modified: code/stage/trunk/libstage/model_fiducial.cc =================================================================== --- code/stage/trunk/libstage/model_fiducial.cc 2009-03-31 07:31:00 UTC (rev 7566) +++ code/stage/trunk/libstage/model_fiducial.cc 2009-03-31 17:07:29 UTC (rev 7567) @@ -28,7 +28,7 @@ const stg_watts_t DEFAULT_FIDUCIAL_WATTS = 10.0; //TODO make instance attempt to register an option (as customvisualizations do) -Option ModelFiducial::showFiducialData( "Show Fiducial", "show_fiducial", "", true, NULL ); +Option ModelFiducial::showFiducialData( "Show Fiducial", "show_fiducial", "", false, NULL ); /** @ingroup model Modified: code/stage/trunk/libstage/powerpack.cc =================================================================== --- code/stage/trunk/libstage/powerpack.cc 2009-03-31 07:31:00 UTC (rev 7566) +++ code/stage/trunk/libstage/powerpack.cc 2009-03-31 17:07:29 UTC (rev 7567) @@ -21,6 +21,8 @@ PowerPack::PowerPack( Model* mod ) : event_vis( 32,32,1.0 ), + output_vis( 0,100,200,40, 1200, stg_color_pack(1,0,0,0), stg_color_pack(0,0,0,0.5), "energy output", "energy_input" ), + stored_vis( 0,142,200,40, 1200, stg_color_pack(0,1,0,0), stg_color_pack(0,0,0,0.5), "energy stored", "energy_stored" ), mod( mod), stored( 0.0 ), capacity( 0.0 ), @@ -28,13 +30,18 @@ { // tell the world about this new pp mod->world->AddPowerPack( this ); + mod->AddVisualizer( &event_vis, false ); -}; + mod->AddVisualizer( &output_vis, true ); + mod->AddVisualizer( &stored_vis, true ); +} PowerPack::~PowerPack() { mod->world->RemovePowerPack( this ); mod->RemoveVisualizer( &event_vis ); + mod->RemoveVisualizer( &output_vis ); + mod->RemoveVisualizer( &stored_vis ); } @@ -222,6 +229,8 @@ dissipated += amount; global_dissipated += amount; + output_vis.AppendValue( amount ); + stored_vis.AppendValue( stored ); //stg_watts_t w = j / (interval / 1e6); } Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-03-31 07:31:00 UTC (rev 7566) +++ code/stage/trunk/libstage/stage.hh 2009-03-31 17:07:29 UTC (rev 7567) @@ -475,6 +475,11 @@ void draw_octagon( float x, float y, float w, float h, float m ); void draw_vector( double x, double y, double z ); void draw_origin( double len ); + void draw_array( float x, float y, float w, float h, + float* data, size_t len, size_t offset, + float min, float max ); + void draw_array( float x, float y, float w, float h, + float* data, size_t len, size_t offset ); /** Draws a rectangle with center at x,y, with sides of length dx,dy */ void draw_centered_rect( float x, float y, float dx, float dy ); } @@ -1380,6 +1385,7 @@ virtual void AddModel( Model* mod ); + protected: virtual void PushColor( stg_color_t col ); virtual void PushColor( double r, double g, double b, double a ); @@ -1388,8 +1394,6 @@ void DrawTree( bool leaves ); void DrawFloor(); - Canvas* GetCanvas( void ) { return canvas; } - public: WorldGui(int W,int H,const char*L=0); @@ -1411,6 +1415,8 @@ void TogglePause(){ paused = !paused; }; bool Paused(){ return( paused ); }; + Canvas* GetCanvas( void ) { return canvas; } + /** show the window - need to call this if you don't Load(). */ void Show(); @@ -1429,6 +1435,29 @@ virtual void RemoveChild( Model* mod ); }; + + class StripPlotVis : public Visualizer + { + private: + + Model* mod; + float* data; + size_t len; + size_t count; + unsigned int index; + float x,y,w,h,min,max; + stg_color_t fgcolor, bgcolor; + + public: + StripPlotVis( float x, float y, float w, float h, + size_t len, + stg_color_t fgcolor, stg_color_t bgcolor, + const char* name, const char* wfname ); + virtual ~StripPlotVis(); + virtual void Visualize( Model* mod, Camera* cam ); + void AppendValue( float value ); + }; + /** energy data packet */ class PowerPack { @@ -1460,6 +1489,8 @@ }; DissipationVis event_vis; + StripPlotVis output_vis; + StripPlotVis stored_vis; /** The model that owns this object */ Model* mod; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-04-02 01:04:35
|
Revision: 7570 http://playerstage.svn.sourceforge.net/playerstage/?rev=7570&view=rev Author: rtv Date: 2009-04-02 01:04:09 +0000 (Thu, 02 Apr 2009) Log Message: ----------- added missing initializer in powerpack Modified Paths: -------------- code/stage/trunk/libstage/powerpack.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/powerpack.cc =================================================================== --- code/stage/trunk/libstage/powerpack.cc 2009-04-02 00:06:20 UTC (rev 7569) +++ code/stage/trunk/libstage/powerpack.cc 2009-04-02 01:04:09 UTC (rev 7570) @@ -14,11 +14,7 @@ stg_joules_t PowerPack::global_input = 0.0; stg_joules_t PowerPack::global_capacity = 0.0; stg_joules_t PowerPack::global_dissipated = 0.0; -stg_watts_t PowerPack::global_power = 0.0; -stg_watts_t PowerPack::global_power_smoothed = 0.0; -double PowerPack::global_smoothing_constant = 0.05; - PowerPack::PowerPack( Model* mod ) : event_vis( 32,32,1.0 ), output_vis( 0,100,200,40, 1200, stg_color_pack(1,0,0,0), stg_color_pack(0,0,0,0.5), "energy output", "energy_input" ), @@ -26,7 +22,8 @@ mod( mod), stored( 0.0 ), capacity( 0.0 ), - charging( false ) + charging( false ), + dissipated( 0.0 ) { // tell the world about this new pp mod->world->AddPowerPack( this ); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-04-02 00:06:20 UTC (rev 7569) +++ code/stage/trunk/libstage/stage.hh 2009-04-02 01:04:09 UTC (rev 7570) @@ -1511,9 +1511,6 @@ static stg_joules_t global_capacity; static stg_joules_t global_dissipated; static stg_joules_t global_input; - static stg_watts_t global_power; - static stg_watts_t global_power_smoothed; - static double global_smoothing_constant; public: PowerPack( Model* mod ); Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-04-02 00:06:20 UTC (rev 7569) +++ code/stage/trunk/libstage/world.cc 2009-04-02 01:04:09 UTC (rev 7570) @@ -542,7 +542,7 @@ for( uint32_t s=0; s < sample_count; s++ ) { - raypose.a = s * fov / (double)sample_count) - starta; + raypose.a = (s * fov / (double)sample_count) - starta; samples[s] = Raytrace( raypose, range, func, model, arg, ztest ); } } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-04-03 19:03:34
|
Revision: 7575 http://playerstage.svn.sourceforge.net/playerstage/?rev=7575&view=rev Author: rtv Date: 2009-04-03 19:03:02 +0000 (Fri, 03 Apr 2009) Log Message: ----------- eliminated crash or assert fail in powerpack when moving outside the previous world bounds Modified Paths: -------------- code/stage/trunk/libstage/powerpack.cc code/stage/trunk/libstage/stage.hh Modified: code/stage/trunk/libstage/powerpack.cc =================================================================== --- code/stage/trunk/libstage/powerpack.cc 2009-04-03 08:01:42 UTC (rev 7574) +++ code/stage/trunk/libstage/powerpack.cc 2009-04-03 19:03:02 UTC (rev 7575) @@ -294,10 +294,11 @@ int ix = (x+width/2.0)/cellsize; int iy = (y+height/2.0)/cellsize; - assert( ix >= 0 ); - assert( ix < (int)columns ); - assert( iy >= 0 ); - assert( iy < (int)rows ); + // don't accumulate if we're outside the grid + if( ! ix >= 0 ) return; + if( ! ix < columns ) return; + if( ! iy >= 0 ) return; + if( ! iy < rows ) return; stg_joules_t* j = cells + (iy*columns + ix ); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-04-03 08:01:42 UTC (rev 7574) +++ code/stage/trunk/libstage/stage.hh 2009-04-03 19:03:02 UTC (rev 7575) @@ -2253,7 +2253,7 @@ stg_radians_t fov; stg_radians_t pan; - static const char* typestr; + static const char* typestr; // constructor ModelBlobfinder( World* world, @@ -2401,27 +2401,19 @@ struct config_t { Size paddle_size; ///< paddle dimensions - paddle_state_t paddles; - lift_state_t lift; - + 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 - Model* gripped; - - bool paddles_stalled; // true iff some solid object stopped - // the paddles closing or opening - + bool paddles_stalled; // true iff some solid object stopped the paddles closing or opening double close_limit; ///< How far the gripper can close. If < 1.0, the gripper has its mouth full. bool autosnatch; ///< if true, cycle the gripper through open-close-up-down automatically - double break_beam_inset[2]; ///< distance from the end of the paddle - Model* beam[2]; ///< points to a model detected by the beams Model* contact[2]; ///< pointers to a model detected by the contacts }; - + private: virtual void Update(); virtual void DataVisualize( Camera* cam ); @@ -2460,7 +2452,6 @@ /** Set the current activity of the gripper. */ void SetCommand( cmd_t cmd ) { this->cmd = cmd; } - /** Command the gripper paddles to close. Wrapper for SetCommand( CMD_CLOSE ). */ void CommandClose() { SetCommand( CMD_CLOSE ); } /** Command the gripper paddles to open. Wrapper for SetCommand( CMD_OPEN ). */ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-04-07 19:02:08
|
Revision: 7582 http://playerstage.svn.sourceforge.net/playerstage/?rev=7582&view=rev Author: rtv Date: 2009-04-07 19:01:36 +0000 (Tue, 07 Apr 2009) Log Message: ----------- removed a malloc Modified Paths: -------------- code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/blockgroup.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/model_load.cc code/stage/trunk/libstage/stage.cc code/stage/trunk/libstage/stage.hh Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2009-04-07 01:47:30 UTC (rev 7581) +++ code/stage/trunk/libstage/block.cc 2009-04-07 19:01:36 UTC (rev 7582) @@ -360,8 +360,9 @@ for( W=0; W<pt_count; W++ ) { - double px = pts[W ].x; - double py = pts[(W+1)%pt_count].x; + double px = pts[W].x; + double py = pts[W].y; + unsigned int keep_W = W; int xa = floor( (pts[W ].x + offsetx) * scalex ); @@ -369,6 +370,8 @@ int xb = floor( (pts[(W+1)%pt_count].x + offsetx) * scalex ); int yb = floor( (pts[(W+1)%pt_count].y + offsety) * scaley ); + mod->rastervis.AddPoint( px, py ); + int keep_xa = xa; int keep_xb = xb; Modified: code/stage/trunk/libstage/blockgroup.cc =================================================================== --- code/stage/trunk/libstage/blockgroup.cc 2009-04-07 01:47:30 UTC (rev 7581) +++ code/stage/trunk/libstage/blockgroup.cc 2009-04-07 19:01:36 UTC (rev 7582) @@ -270,9 +270,12 @@ //printf( "found %d rects in \"%s\" at %p\n", // rect_count, full, rects ); - + if( rects && (rect_count > 0) ) { + // TODO fix this + stg_color_t col = stg_color_pack( 1.0, 0,0,1.0 ); + for( unsigned int r=0; r<rect_count; r++ ) { stg_point_t pts[4]; @@ -291,9 +294,6 @@ pts[3].x = x; pts[3].y = y + h; - // TODO fix this - stg_color_t col = stg_color_pack( 1.0, 0,0,1.0 ); - AppendBlock( new Block( mod, pts,4, 0,1, @@ -302,16 +302,11 @@ } free( rects ); } - + CalcSize(); } -#include <math.h> /* for round() */ -extern void output (int x, int y); /* forward declaration for user-defined output */ - - - void BlockGroup::Rasterize( uint8_t* data, unsigned int width, unsigned int height ) { for( GList* it = blocks; it; it=it->next ) Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-04-07 01:47:30 UTC (rev 7581) +++ code/stage/trunk/libstage/model.cc 2009-04-07 19:01:36 UTC (rev 7582) @@ -1025,6 +1025,7 @@ void Model::Rasterize( uint8_t* data, unsigned int width, unsigned int height ) { + rastervis.ClearPts(); blockgroup.Rasterize( data, width, height ); rastervis.SetData( data, width, height ); } @@ -1036,7 +1037,8 @@ : Visualizer( "Rasterization", "raster_vis" ), data(NULL), width(0), - height(0) + height(0), + pts(NULL) { } @@ -1047,26 +1049,44 @@ // go into world coordinates glPushMatrix(); - mod->PushColor( 0,0,0,0.5 ); + mod->PushColor( 1,0,0,0.5 ); Gl::pose_inverse_shift( mod->GetGlobalPose() ); + glPushMatrix(); + + Size sz = mod->blockgroup.GetSize(); glTranslatef( -mod->geom.size.x / 2.0, -mod->geom.size.y/2.0, 0 ); + glScalef( mod->geom.size.x / sz.x, mod->geom.size.y / sz.y, 1 ); + + // now we're in world meters coordinates + glPointSize( 4 ); + glBegin( GL_POINTS ); + for( GList* it=pts; it; it=it->next ) + { + stg_point_t* pt = (stg_point_t*)it->data; + glVertex2f( pt->x, pt->y ); + + char buf[128]; + snprintf( buf, 127, "[%.2f x %.2f]", pt->x, pt->y ); + Gl::draw_string( pt->x, pt->y, 0, buf ); + } + glEnd(); + + mod->PopColor(); + + glPopMatrix(); + // go into bitmap pixel coords + glTranslatef( -mod->geom.size.x / 2.0, -mod->geom.size.y/2.0, 0 ); glScalef( mod->geom.size.x / width, mod->geom.size.y / height, 1 ); + mod->PushColor( 0,0,0,0.5 ); glPolygonMode( GL_FRONT, GL_FILL ); for( unsigned int y=0; y<height; y++ ) for( unsigned int x=0; x<width; x++ ) { // printf( "[%u %u] ", x, y ); - - if( (x == (92/5)) && (y == (750/10) )) - { - mod->PushColor( 1,0,0,0.5 ); - glRectf( x, y, x+1, y+1 ); - mod->PopColor(); - } - else if( data[ x + y*width ] ) + if( data[ x + y*width ] ) glRectf( x, y, x+1, y+1 ); } @@ -1077,9 +1097,9 @@ for( unsigned int y=0; y<height; y++ ) for( unsigned int x=0; x<width; x++ ) { - //if( data[ x + y*width ] ) + if( data[ x + y*width ] ) glRectf( x, y, x+1, y+1 ); - + // char buf[128]; // snprintf( buf, 127, "[%u x %u]", x, y ); // Gl::draw_string( x, y, 0, buf ); @@ -1096,21 +1116,40 @@ snprintf( buf, 127, "[%u x %u]", width, height ); glTranslatef( 0,0,0.01 ); Gl::draw_string( 1, height-1, 0, buf ); + mod->PopColor(); glPopMatrix(); } -void Model::RasterVis::SetData( uint8_t* data, unsigned int width, unsigned int height ) +void Model::RasterVis::SetData( uint8_t* data, + unsigned int width, + unsigned int height ) { // copy the raster for test visualization if( this->data ) - free( this->data ); + delete[] this->data; size_t len = sizeof(uint8_t) * width * height; - this->data = (uint8_t*)malloc( len ); + printf( "allocating %lu bytes\n", len ); + this->data = new uint8_t[len]; memcpy( this->data, data, len ); this->width = width; this->height = height; } +void Model::RasterVis::AddPoint( stg_meters_t x, stg_meters_t y ) +{ + stg_point_t* pt = new stg_point_t; + pt->x = x; + pt->y = y; + pts = g_list_prepend( pts, pt ); +} + +void Model::RasterVis::ClearPts() +{ + if( pts ) + for( GList* it=pts; it; it=it->next ) + if( it->data ) + delete (stg_point_t*)it->data; +} Modified: code/stage/trunk/libstage/model_load.cc =================================================================== --- code/stage/trunk/libstage/model_load.cc 2009-04-07 01:47:30 UTC (rev 7581) +++ code/stage/trunk/libstage/model_load.cc 2009-04-07 19:01:36 UTC (rev 7582) @@ -147,16 +147,13 @@ { const char* bitmapfile = wf->ReadString( wf_entity, "bitmap", NULL ); assert( bitmapfile ); - + if( has_default_block ) - { - blockgroup.Clear(); - has_default_block = false; - } - - //puts( "clearing blockgroup" ); - //blockgroup.Clear(); - //puts( "loading bitmap" ); + { + blockgroup.Clear(); + has_default_block = false; + } + blockgroup.LoadBitmap( this, bitmapfile, wf ); } Modified: code/stage/trunk/libstage/stage.cc =================================================================== --- code/stage/trunk/libstage/stage.cc 2009-04-07 01:47:30 UTC (rev 7581) +++ code/stage/trunk/libstage/stage.cc 2009-04-07 19:01:36 UTC (rev 7582) @@ -284,8 +284,8 @@ size_t rects_allocated = allocation_unit; *rects = (stg_rotrect_t*)calloc( sizeof(stg_rotrect_t), rects_allocated ); - int img_width = img->w();//gdk_pixbuf_get_width(pb); - int img_height = img->h();//gdk_pixbuf_get_height(pb); + int img_width = img->w(); + int img_height = img->h(); // if the caller wanted to know the dimensions if( widthp ) *widthp = img_width; @@ -356,6 +356,17 @@ latest->size.x = x - startx; latest->size.y = height; + assert( latest->pose.x >= 0 ); + assert( latest->pose.y >= 0 ); + assert( latest->pose.x <= img_width ); + assert( latest->pose.y <= img_height); + //assert( latest->size.x > 0 ); + //assert( latest->size.y > 0 ); + + if( latest->size.x < 1 || latest->size.y < 1 ) + printf( "p [%.2f %.2f] s [%.2f %.2f]\n", + latest->pose.x, latest->pose.y, latest->size.x, latest->size.y ); + //printf( "rect %d (%.2f %.2f %.2f %.2f %.2f\n", // *rect_count, // latest->x, latest->y, latest->a, latest->w, latest->h ); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-04-07 01:47:30 UTC (rev 7581) +++ code/stage/trunk/libstage/stage.hh 2009-04-07 19:01:36 UTC (rev 7582) @@ -1637,7 +1637,8 @@ private: uint8_t* data; unsigned int width, height; - + GList* pts; + public: RasterVis(); virtual ~RasterVis( void ){} @@ -1646,6 +1647,10 @@ void SetData( uint8_t* data, unsigned int width, unsigned int height ); + + void AddPoint( stg_meters_t x, stg_meters_t y ); + void ClearPts(); + }; protected: This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-04-07 20:41:51
|
Revision: 7584 http://playerstage.svn.sourceforge.net/playerstage/?rev=7584&view=rev Author: rtv Date: 2009-04-07 20:41:40 +0000 (Tue, 07 Apr 2009) Log Message: ----------- fixing bugs Modified Paths: -------------- code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-04-07 19:06:20 UTC (rev 7583) +++ code/stage/trunk/libstage/model.cc 2009-04-07 20:41:40 UTC (rev 7584) @@ -1053,29 +1053,34 @@ Gl::pose_inverse_shift( mod->GetGlobalPose() ); - glPushMatrix(); - - Size sz = mod->blockgroup.GetSize(); - glTranslatef( -mod->geom.size.x / 2.0, -mod->geom.size.y/2.0, 0 ); - glScalef( mod->geom.size.x / sz.x, mod->geom.size.y / sz.y, 1 ); - - // now we're in world meters coordinates - glPointSize( 4 ); - glBegin( GL_POINTS ); - for( GList* it=pts; it; it=it->next ) + + if( pts ) { - stg_point_t* pt = (stg_point_t*)it->data; - glVertex2f( pt->x, pt->y ); - - char buf[128]; - snprintf( buf, 127, "[%.2f x %.2f]", pt->x, pt->y ); - Gl::draw_string( pt->x, pt->y, 0, buf ); + glPushMatrix(); + Size sz = mod->blockgroup.GetSize(); + glTranslatef( -mod->geom.size.x / 2.0, -mod->geom.size.y/2.0, 0 ); + glScalef( mod->geom.size.x / sz.x, mod->geom.size.y / sz.y, 1 ); + + // now we're in world meters coordinates + glPointSize( 4 ); + glBegin( GL_POINTS ); + for( GList* it=pts; it; it=it->next ) + { + stg_point_t* pt = (stg_point_t*)it->data; + assert( pt ); + glVertex2f( pt->x, pt->y ); + + char buf[128]; + snprintf( buf, 127, "[%.2f x %.2f]", pt->x, pt->y ); + Gl::draw_string( pt->x, pt->y, 0, buf ); + } + glEnd(); + + mod->PopColor(); + + glPopMatrix(); } - glEnd(); - mod->PopColor(); - - glPopMatrix(); // go into bitmap pixel coords glTranslatef( -mod->geom.size.x / 2.0, -mod->geom.size.y/2.0, 0 ); glScalef( mod->geom.size.x / width, mod->geom.size.y / height, 1 ); Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-04-07 19:06:20 UTC (rev 7583) +++ code/stage/trunk/libstage/stage.hh 2009-04-07 20:41:40 UTC (rev 7584) @@ -996,7 +996,7 @@ virtual ~World(); - stg_usec_t SimTimeNow(void){ return sim_time; } + stg_usec_t SimTimeNow(void); stg_usec_t RealTimeNow(void); stg_usec_t RealTimeSinceStart(void); Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-04-07 19:06:20 UTC (rev 7583) +++ code/stage/trunk/libstage/world.cc 2009-04-07 20:41:40 UTC (rev 7584) @@ -992,3 +992,6 @@ // TODO XX figure out how to handle velcoties a bit better //velocity_list = g_list_remove( velocity_list, mod ); } + +stg_usec_t World::SimTimeNow(void) +{ return sim_time; } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-04-07 23:00:28
|
Revision: 7585 http://playerstage.svn.sourceforge.net/playerstage/?rev=7585&view=rev Author: rtv Date: 2009-04-07 23:00:21 +0000 (Tue, 07 Apr 2009) Log Message: ----------- big hunting Modified Paths: -------------- code/stage/trunk/libstage/glcolorstack.cc code/stage/trunk/libstage/worldfile.cc Modified: code/stage/trunk/libstage/glcolorstack.cc =================================================================== --- code/stage/trunk/libstage/glcolorstack.cc 2009-04-07 20:41:40 UTC (rev 7584) +++ code/stage/trunk/libstage/glcolorstack.cc 2009-04-07 23:00:21 UTC (rev 7585) @@ -9,14 +9,12 @@ GlColorStack::~GlColorStack() { - } void GlColorStack::Push( GLdouble col[4] ) { - size_t sz = 4 * sizeof(col[0]); - GLdouble *keep = (GLdouble*)malloc( sz ); - memcpy( keep, col, sz ); + GLdouble *keep = new GLdouble[4]; + memcpy( keep, col, 4*sizeof(GLdouble) ); g_queue_push_head( colorstack, keep ); @@ -71,6 +69,6 @@ { GLdouble *col = (GLdouble*)g_queue_pop_head( colorstack ); glColor4dv( col ); - free( col ); + delete[] col; } } Modified: code/stage/trunk/libstage/worldfile.cc =================================================================== --- code/stage/trunk/libstage/worldfile.cc 2009-04-07 20:41:40 UTC (rev 7584) +++ code/stage/trunk/libstage/worldfile.cc 2009-04-07 23:00:21 UTC (rev 7585) @@ -127,7 +127,7 @@ // accordingly if found: char *stagepath = getenv("STAGEPATH"); char *token = strtok(stagepath, ":"); - char *fullpath = (char*) malloc(PATH_MAX); + char* fullpath = new char[PATH_MAX]; char *tmp = strdup(filename); char *base = basename(tmp); while (token != NULL) { @@ -509,7 +509,7 @@ // we need to make a copy of the filename. // There's no bounds-checking, but what the heck. char *tmp = strdup(this->filename); - fullpath = (char*) malloc(PATH_MAX); + fullpath = new char[PATH_MAX]; memset(fullpath, 0, PATH_MAX); strcat( fullpath, dirname(tmp)); strcat( fullpath, "/" ); @@ -523,7 +523,7 @@ // we need to make a copy of the filename. // There's no bounds-checking, but what the heck. char *tmp = strdup(this->filename); - fullpath = (char*) malloc(PATH_MAX); + fullpath = new char[PATH_MAX]; getcwd(fullpath, PATH_MAX); strcat( fullpath, "/" ); strcat( fullpath, dirname(tmp)); @@ -542,7 +542,8 @@ { PRINT_ERR2("unable to open include file %s : %s", fullpath, strerror(errno)); - free(fullpath); + //free(fullpath); + delete[] fullpath; return false; } @@ -556,7 +557,8 @@ { fclose( infile ); //DumpTokens(); - free(fullpath); + //free(fullpath); + delete[] fullpath; return false; } @@ -568,7 +570,7 @@ while ( ch != '\n' ) ch = fgetc(file); - free(fullpath); + delete[] fullpath; return true; } @@ -1628,7 +1630,7 @@ // we need to make a copy of the filename. // There's no bounds-checking, but what the heck. char *tmp = strdup(this->filename); - char *fullpath = (char*) malloc(PATH_MAX); + char* fullpath = new char[PATH_MAX]; memset(fullpath, 0, PATH_MAX); strcat( fullpath, dirname(tmp)); strcat( fullpath, "/" ); @@ -1644,7 +1646,7 @@ // we need to make a copy of the filename. // There's no bounds-checking, but what the heck. char *tmp = strdup(this->filename); - char *fullpath = (char*) malloc(PATH_MAX); + char* fullpath = new char[PATH_MAX]; getcwd(fullpath, PATH_MAX); strcat( fullpath, "/" ); strcat( fullpath, dirname(tmp)); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-04-09 17:13:14
|
Revision: 7588 http://playerstage.svn.sourceforge.net/playerstage/?rev=7588&view=rev Author: rtv Date: 2009-04-09 17:12:41 +0000 (Thu, 09 Apr 2009) Log Message: ----------- fixed threadpool bug Modified Paths: -------------- code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/model.cc Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2009-04-08 08:35:18 UTC (rev 7587) +++ code/stage/trunk/libstage/block.cc 2009-04-09 17:12:41 UTC (rev 7588) @@ -477,12 +477,12 @@ // draw filled color polygons stg_color_t col = inherit_color ? mod->color : color; - mod->PushColor( col ); - glEnable(GL_POLYGON_OFFSET_FILL); - glPolygonOffset(1.0, 1.0); - DrawSides(); - DrawTop(); - glDisable(GL_POLYGON_OFFSET_FILL); + mod->PushColor( col ); + glEnable(GL_POLYGON_OFFSET_FILL); + glPolygonOffset(1.0, 1.0); + DrawSides(); + DrawTop(); + glDisable(GL_POLYGON_OFFSET_FILL); // // draw the block outline in a darker version of the same color double r,g,b,a; Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-04-08 08:35:18 UTC (rev 7587) +++ code/stage/trunk/libstage/model.cc 2009-04-09 17:12:41 UTC (rev 7588) @@ -684,7 +684,7 @@ bool Model::UpdateDue( void ) { - return( world->sim_time >= (last_update + interval) ); + return( (last_update == 0) || (world->sim_time >= (last_update + interval)) ); } void Model::Update( void ) @@ -705,12 +705,14 @@ stg_joules_t consumed = watts * (world->interval_sim * 1e-6); pp->Dissipate( consumed, GetGlobalPose() ); } + + last_update = world->sim_time; } void Model::CallUpdateCallbacks( void ) { - CallCallbacks( &hooks.update ); - last_update = world->sim_time; + if( last_update == world->sim_time ) + CallCallbacks( &hooks.update ); } stg_meters_t Model::ModelHeight() const @@ -1135,7 +1137,7 @@ if( this->data ) delete[] this->data; size_t len = sizeof(uint8_t) * width * height; - printf( "allocating %lu bytes\n", len ); + //printf( "allocating %lu bytes\n", len ); this->data = new uint8_t[len]; memcpy( this->data, data, len ); this->width = width; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-04-09 17:53:44
|
Revision: 7589 http://playerstage.svn.sourceforge.net/playerstage/?rev=7589&view=rev Author: rtv Date: 2009-04-09 17:53:34 +0000 (Thu, 09 Apr 2009) Log Message: ----------- fixed -g crash bug Modified Paths: -------------- code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/blockgroup.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc code/stage/trunk/libstage/worldgui.cc Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2009-04-09 17:12:41 UTC (rev 7588) +++ code/stage/trunk/libstage/block.cc 2009-04-09 17:53:34 UTC (rev 7589) @@ -68,8 +68,7 @@ pts[p].x += x; pts[p].y += y; } - - // force redraw + mod->blockgroup.BuildDisplayList( mod ); } @@ -363,7 +362,7 @@ double px = pts[W].x; double py = pts[W].y; - unsigned int keep_W = W; + //unsigned int keep_W = W; int xa = floor( (pts[W ].x + offsetx) * scalex ); int ya = floor( (pts[W ].y + offsety) * scaley ); @@ -372,8 +371,8 @@ mod->rastervis.AddPoint( px, py ); - int keep_xa = xa; - int keep_xb = xb; + //int keep_xa = xa; + //int keep_xb = xb; //printf( " line (%d,%d) to (%d,%d)\n", xa,ya,xb,yb ); Modified: code/stage/trunk/libstage/blockgroup.cc =================================================================== --- code/stage/trunk/libstage/blockgroup.cc 2009-04-09 17:12:41 UTC (rev 7588) +++ code/stage/trunk/libstage/blockgroup.cc 2009-04-09 17:53:34 UTC (rev 7589) @@ -149,6 +149,9 @@ void BlockGroup::BuildDisplayList( Model* mod ) { + if( ! mod->world->IsGUI() ) + return; + //printf( "display list for model %s\n", mod->token ); if( displaylist == 0 ) Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-04-09 17:12:41 UTC (rev 7588) +++ code/stage/trunk/libstage/stage.hh 2009-04-09 17:53:34 UTC (rev 7589) @@ -892,12 +892,17 @@ Worldfile* wf; ///< If set, points to the worldfile used to create this world 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 /** hint that the world needs to be redrawn if a GUI is attached */ void NeedRedraw(){ dirty = true; }; + /** Get human readable string that describes the current simulation + time. */ + virtual std::string ClockString( void ); + Model* CreateModel( Model* parent, const char* typestr ); void LoadModel( Worldfile* wf, int entity, GHashTable* entitytable ); void LoadBlock( Worldfile* wf, int entity, GHashTable* entitytable ); @@ -1393,9 +1398,9 @@ bool closeWindowQuery(); virtual void AddModel( Model* mod ); + + protected: - - protected: virtual void PushColor( stg_color_t col ); virtual void PushColor( double r, double g, double b, double a ); virtual void PopColor(); @@ -1408,6 +1413,7 @@ WorldGui(int W,int H,const char*L=0); ~WorldGui(); + virtual std::string ClockString(); virtual bool Update(); virtual void Load( const char* filename ); virtual void UnLoad(); @@ -1429,10 +1435,6 @@ /** show the window - need to call this if you don't Load(). */ void Show(); - /** Get human readable string that describes the current simulation - time. */ - std::string ClockString( void ); - /** Get human readable string that describes the current global energy state. */ std::string EnergyString( void ); Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-04-09 17:12:41 UTC (rev 7588) +++ code/stage/trunk/libstage/world.cc 2009-04-09 17:53:34 UTC (rev 7589) @@ -425,7 +425,33 @@ return( (quit_time > 0) && (sim_time >= quit_time) ); } +std::string World::ClockString() +{ + const uint32_t usec_per_hour = 3600000000U; + const uint32_t usec_per_minute = 60000000U; + const uint32_t usec_per_second = 1000000U; + const uint32_t usec_per_msec = 1000U; + + uint32_t hours = sim_time / usec_per_hour; + uint32_t minutes = (sim_time % usec_per_hour) / usec_per_minute; + uint32_t seconds = (sim_time % usec_per_minute) / usec_per_second; + uint32_t msec = (sim_time % usec_per_second) / usec_per_msec; + + std::string str; + char buf[256]; + if( hours > 0 ) + { + snprintf( buf, 255, "%uh", hours ); + str += buf; + } + + snprintf( buf, 255, " %um %02us %03umsec", minutes, seconds, msec); + str += buf; + + return str; +} + bool World::Update() { PRINT_DEBUG( "World::Update()" ); @@ -480,9 +506,16 @@ LISTMETHOD( reentrant_update_list, Model*, CallUpdateCallbacks ); } + if( this->updates % 100 == 0 ) + { + printf( "\r[Stage: %s]", ClockString().c_str() ); + fflush( stdout ); + } + this->sim_time += this->interval_sim; this->updates++; + return false; } Modified: code/stage/trunk/libstage/worldgui.cc =================================================================== --- code/stage/trunk/libstage/worldgui.cc 2009-04-09 17:12:41 UTC (rev 7588) +++ code/stage/trunk/libstage/worldgui.cc 2009-04-09 17:53:34 UTC (rev 7589) @@ -355,7 +355,10 @@ usleep( (stg_usec_t)MIN(sleeptime,20000) ); // check the GUI at 10Hz min } } while( interval < interval_real ); - + +// if( !IsGUI() ) +// printf( "[Stage: %s\n", ClockString().cstr() + //printf( "\r \t\t timenow %lu", timenow ); //printf( "interval_real %.20f\n", interval_real ); @@ -371,6 +374,29 @@ return val; } +std::string WorldGui::ClockString() +{ + std::string str = World::ClockString(); + + // find the average length of the last few realtime intervals; + stg_usec_t average_real_interval = 0; + for( uint32_t i=0; i<INTERVAL_LOG_LEN; i++ ) + average_real_interval += interval_log[i]; + average_real_interval /= INTERVAL_LOG_LEN; + + double localratio = (double)interval_sim / (double)average_real_interval; + + char buf[32]; + snprintf( buf, 32, " [%.2f]", localratio ); + str + buf; + + if( paused == true ) + str += " [ PAUSED ]"; + + return str; +} + + void WorldGui::AddModel( Model* mod ) { if( mod->parent == NULL ) @@ -387,44 +413,7 @@ World::RemoveChild( mod ); } -std::string WorldGui::ClockString() -{ - const uint32_t usec_per_hour = 3600000000U; - const uint32_t usec_per_minute = 60000000U; - const uint32_t usec_per_second = 1000000U; - const uint32_t usec_per_msec = 1000U; - - uint32_t hours = sim_time / usec_per_hour; - uint32_t minutes = (sim_time % usec_per_hour) / usec_per_minute; - uint32_t seconds = (sim_time % usec_per_minute) / usec_per_second; - uint32_t msec = (sim_time % usec_per_second) / usec_per_msec; - - // find the average length of the last few realtime intervals; - stg_usec_t average_real_interval = 0; - for( uint32_t i=0; i<INTERVAL_LOG_LEN; i++ ) - average_real_interval += interval_log[i]; - average_real_interval /= INTERVAL_LOG_LEN; - - double localratio = (double)interval_sim / (double)average_real_interval; - - std::string str; - char buf[256]; - if( hours > 0 ) - { - snprintf( buf, 255, "%uh", hours ); - str += buf; - } - - snprintf( buf, 255, " %um %02us %03umsec [%.2f]", minutes, seconds, msec, localratio ); - str += buf; - - if( paused == true ) - str += " [ PAUSED ]"; - - return str; -} - std::string WorldGui::EnergyString() { char str[512]; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-04-09 18:16:34
|
Revision: 7590 http://playerstage.svn.sourceforge.net/playerstage/?rev=7590&view=rev Author: rtv Date: 2009-04-09 18:16:27 +0000 (Thu, 09 Apr 2009) Log Message: ----------- added clock output option Modified Paths: -------------- code/stage/trunk/libstage/main.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/main.cc =================================================================== --- code/stage/trunk/libstage/main.cc 2009-04-09 17:53:34 UTC (rev 7589) +++ code/stage/trunk/libstage/main.cc 2009-04-09 18:16:27 UTC (rev 7590) @@ -13,8 +13,10 @@ const char* USAGE = "USAGE: stage [options] [<worldfile>]\n" "Available [options] are:\n" - " --gui : run without a GUI (same as -g)\n" - " -g : run without a GUI (same as --gui)\n" + " --clock : print simulation time peridically on standard output\n" + " -c : print simulation time peridically on standard output\n" + " --gui : run without a GUI\n" + " -g : run without a GUI\n" " --help : print this message.\n" " -h : print this message.\n" " -? : print this message.\n" @@ -23,6 +25,7 @@ /* options descriptor */ static struct option longopts[] = { { "gui", optional_argument, NULL, 'g' }, + { "clock", optional_argument, NULL, 'c' }, { "help", optional_argument, NULL, 'h' }, { NULL, 0, NULL, 0 } }; @@ -36,14 +39,19 @@ int ch=0, optindex=0; bool usegui = true; + bool showclock = false; - while ((ch = getopt_long(argc, argv, "gh?", longopts, &optindex)) != -1) + while ((ch = getopt_long(argc, argv, "cgh?", longopts, &optindex)) != -1) { switch( ch ) { case 0: // long option given printf( "option %s given\n", longopts[optindex].name ); break; + case 'c': + showclock = true; + printf( "[Clock enabled]" ); + break; case 'g': usegui = false; printf( "[GUI disabled]" ); @@ -76,6 +84,7 @@ new WorldGui( 400, 300, worldfilename ) : new World( worldfilename ) ); world->Load( worldfilename ); + world->ShowClock( showclock ); loaded_world_file = true; } optindex++; Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-04-09 17:53:34 UTC (rev 7589) +++ code/stage/trunk/libstage/stage.hh 2009-04-09 18:16:27 UTC (rev 7590) @@ -864,6 +864,8 @@ stg_usec_t quit_time; stg_usec_t real_time_now; ///< The current real time in microseconds stg_usec_t real_time_start; ///< the real time at which this world was created + bool show_clock; ///< iff true, print the sim time on stdout + unsigned int show_clock_interval; ///< updates between clock xoutputs GMutex* thread_mutex; ///< protect the worker thread management stuff GThreadPool *threadpool; ///<worker threads for updating some sensor models in parallel int total_subs; ///< the total number of subscriptions to all models @@ -1041,6 +1043,9 @@ /// Register an Option for pickup by the GUI void RegisterOption( Option* opt ); + + /// Control printing time to stdout + void ShowClock( bool enable ){ show_clock = enable; }; }; class Block Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-04-09 17:53:34 UTC (rev 7589) +++ code/stage/trunk/libstage/world.cc 2009-04-09 18:16:27 UTC (rev 7590) @@ -83,6 +83,8 @@ quit_time( 0 ), real_time_now( RealTimeNow() ), real_time_start( real_time_now ), + show_clock( false ), + show_clock_interval( 100 ), // 10 simulated seconds using defaults thread_mutex( g_mutex_new() ), threadpool( NULL ), total_subs( 0 ), @@ -506,7 +508,7 @@ LISTMETHOD( reentrant_update_list, Model*, CallUpdateCallbacks ); } - if( this->updates % 100 == 0 ) + if( show_clock && ((this->updates % show_clock_interval) == 0) ) { printf( "\r[Stage: %s]", ClockString().c_str() ); fflush( stdout ); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-04-11 05:32:40
|
Revision: 7595 http://playerstage.svn.sourceforge.net/playerstage/?rev=7595&view=rev Author: rtv Date: 2009-04-11 05:32:07 +0000 (Sat, 11 Apr 2009) Log Message: ----------- cleaning up code Modified Paths: -------------- code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/blockgroup.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2009-04-11 00:39:13 UTC (rev 7594) +++ code/stage/trunk/libstage/block.cc 2009-04-11 05:32:07 UTC (rev 7595) @@ -8,14 +8,15 @@ blocks. The point data is copied, so pts can safely be freed after calling this.*/ Block::Block( Model* mod, - stg_point_t* pts, - size_t pt_count, - stg_meters_t zmin, - stg_meters_t zmax, - stg_color_t color, - bool inherit_color - ) : + 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 ), + mpts(NULL), pt_count( pt_count ), pts( (stg_point_t*)g_memdup( pts, pt_count * sizeof(stg_point_t)) ), color( color ), @@ -36,6 +37,7 @@ Worldfile* wf, int entity) : mod( mod ), + mpts(NULL), pt_count(0), pts(NULL), color(0), @@ -252,101 +254,69 @@ mapped = true; } -stg_point_t Block::BlockPointToModelMeters( const stg_point_t& bpt ) +inline stg_point_t Block::BlockPointToModelMeters( const stg_point_t& bpt ) { - Pose gpose = mod->GetGlobalPose(); - gpose = pose_sum( gpose, mod->geom.pose ); // add local offset - Size bgsize = mod->blockgroup.GetSize(); stg_point3_t bgoffset = mod->blockgroup.GetOffset(); - stg_point3_t scale; - scale.x = mod->geom.size.x / bgsize.x; - scale.y = mod->geom.size.y / bgsize.y; - scale.z = mod->geom.size.z / bgsize.z; + return stg_point_t( (bpt.x - bgoffset.x) * (mod->geom.size.x/bgsize.x), + (bpt.y - bgoffset.y) * (mod->geom.size.y/bgsize.y)); +} - stg_point_t mpt; - mpt.x = (bpt.x - bgoffset.x) * scale.x; - mpt.y = (bpt.y - bgoffset.y) * scale.y; - return mpt; +stg_point_t* Block::GetPointsInModelCoords() +{ + if( ! mpts ) + { + // no valid cache of model coord points, so generate them + mpts = new stg_point_t[pt_count]; + + for( unsigned int i=0; i<pt_count; i++ ) + mpts[i] = BlockPointToModelMeters( pts[i] ); + } + + return mpts; } -void Block::GenerateCandidateCells() +void Block::InvalidateModelPointCache() +{ + // this doesn't happen often, so this simple strategy isn't too wasteful + if( mpts ) + { + delete[] mpts; + mpts = NULL; + } +} +void Block::GenerateCandidateCells() { - Pose gpose = mod->GetGlobalPose(); + stg_point_t* mpts = GetPointsInModelCoords(); + + // convert the mpts in model coords into global coords + stg_point_t* gpts = new stg_point_t[pt_count]; + for( unsigned int i=0; i<pt_count; i++ ) + gpts[i] = mod->LocalToGlobal( mpts[i] ); - // add local offset - gpose = pose_sum( gpose, mod->geom.pose ); + g_ptr_array_set_size( candidate_cells, 0 ); - Size bgsize = mod->blockgroup.GetSize(); - stg_point3_t bgoffset = mod->blockgroup.GetOffset(); + mod->world-> + ForEachCellInPolygon( gpts, pt_count, + (stg_cell_callback_t)AppendCellToPtrArray, + candidate_cells ); + delete[] gpts; - stg_point3_t scale; - scale.x = mod->geom.size.x / bgsize.x; - scale.y = mod->geom.size.y / bgsize.y; - scale.z = mod->geom.size.z / bgsize.z; - - - g_ptr_array_set_size( candidate_cells, 0 ); + // set global Z + Pose gpose = mod->GetGlobalPose(); + gpose.z += mod->geom.pose.z; + double scalez = mod->geom.size.z / mod->blockgroup.GetSize().z; + stg_meters_t z = gpose.z - mod->blockgroup.GetOffset().z; - // compute the global location of the first point - Pose local( (pts[0].x - bgoffset.x) * scale.x , - (pts[0].y - bgoffset.y) * scale.y, - -bgoffset.z, - 0 ); - - Pose 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; + global_z.min = (scalez * local_z.min) + z; + global_z.max = (scalez * local_z.max) + z; - // now loop from the the second to the last - for( unsigned int p=1; p<pt_count; p++ ) - { - Pose local( (pts[p].x - bgoffset.x) * scale.x , - (pts[p].y - bgoffset.y) * scale.y, - -bgoffset.z, - 0 ); - - Pose 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; } -// void Block::Rasterize( uint8_t* data, -// unsigned int width, -// unsigned int height, -// stg_meters_t cellwidth, -// stg_meters_t cellheight ) -// { -// // add local offset -// // pose = pose_sum( pose, mod->geom.pose ); - -// Size bgsize = mod->blockgroup.GetSize(); - -// double scalex = (width*cellwidth) / bgsize.x; -// double scaley = (height*cellheight) / bgsize.y; - -// Rasterize( data, width, height, scalex, scaley, 0,0 ); -// } - void swap( int& a, int& b ) { int tmp = a; @@ -354,71 +324,6 @@ b = tmp; } -// void Block::Rasterize( uint8_t* data, -// unsigned int width, unsigned int height, -// double scalex, double scaley, -// double offsetx, double offsety ) -// { -// //printf( "rasterize block %p : w: %u h: %u scale %.2f %.2f offset %.2f %.2f\n", -// // this, width, height, scalex, scaley, offsetx, offsety ); - -// for( unsigned int i=0; i<pt_count; i++ ) -// { -// double px = pts[i].x; -// double py = pts[i].y; - -// //unsigned int keep_i = i; - -// int xa = floor( (pts[i ].x + offsetx) * scalex ); -// int ya = floor( (pts[i ].y + offsety) * scaley ); -// int xb = floor( (pts[(i+1)%pt_count].x + offsetx) * scalex ); -// int yb = floor( (pts[(i+1)%pt_count].y + offsety) * scaley ); - -// mod->rastervis.AddPoint( px, py ); - -// //printf( " line (%d,%d) to (%d,%d)\n", xa,ya,xb,yb ); - -// bool steep = abs( yb-ya ) > abs( xb-xa ); -// if( steep ) -// { -// swap( xa, ya ); -// swap( xb, yb ); -// } - -// if( xa > xb ) -// { -// swap( xa, xb ); -// swap( ya, yb ); -// } - -// double dydx = (double) (yb - ya) / (double) (xb - xa); -// double y = ya; -// for(int x=xa; x<=xb; x++) -// { -// if( steep ) -// { -// if( ! (floor(y) >= 0) ) continue; -// if( ! (floor(y) < (int)width) ) continue; -// if( ! (x >= 0) ) continue; -// if( ! (x < (int)height) ) continue; -// } -// else -// { -// if( ! (x >= 0) ) continue; -// if( ! (x < (int)width) ) continue; -// if( ! (floor(y) >= 0) ) continue; -// if( ! (floor(y) < (int)height) ) continue; -// } - -// if( steep ) -// data[ (int)floor(y) + (x * width)] = 1; -// else -// data[ x + ((int)floor(y) * width)] = 1; -// y += dydx; -// } -// } -// } - void Block::Rasterize( uint8_t* data, unsigned int width, unsigned int height, Modified: code/stage/trunk/libstage/blockgroup.cc =================================================================== --- code/stage/trunk/libstage/blockgroup.cc 2009-04-11 00:39:13 UTC (rev 7594) +++ code/stage/trunk/libstage/blockgroup.cc 2009-04-11 05:32:07 UTC (rev 7595) @@ -102,6 +102,8 @@ offset.x = minx + size.x/2.0; offset.y = miny + size.y/2.0; offset.z = 0; // todo? + + InvalidateModelPointCache(); } Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-04-11 00:39:13 UTC (rev 7594) +++ code/stage/trunk/libstage/model.cc 2009-04-11 05:32:07 UTC (rev 7595) @@ -561,6 +561,12 @@ return pose_sum( pose_sum( GetGlobalPose(), geom.pose ), pose ); } +stg_point_t Model::LocalToGlobal( const stg_point_t& pt) const +{ + Pose gpose = LocalToGlobal( Pose( pt.x, pt.y, 0, 0 ) ); + return stg_point_t( gpose.x, gpose.y ); +} + void Model::MapWithChildren() { UnMap(); @@ -1159,10 +1165,7 @@ void Model::RasterVis::AddPoint( stg_meters_t x, stg_meters_t y ) { - stg_point_t* pt = new stg_point_t; - pt->x = x; - pt->y = y; - pts = g_list_prepend( pts, pt ); + pts = g_list_prepend( pts, new stg_point_t( x, y ) ); } void Model::RasterVis::ClearPts() Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-04-11 00:39:13 UTC (rev 7594) +++ code/stage/trunk/libstage/stage.hh 2009-04-11 05:32:07 UTC (rev 7595) @@ -387,10 +387,21 @@ } stg_fov_t; /** Define a point on a 2d plane */ - typedef struct + class stg_point_t { + public: stg_meters_t x, y; - } stg_point_t; + + // init + stg_point_t( stg_meters_t x, stg_meters_t y ) + : x(x), y(y){} + + // init + stg_point_t() : x(0), y(0){} + + // copy + stg_point_t( const stg_point_t& pt) : x(pt.x), y(pt.y){} + }; /** Define a point in 3d space */ typedef struct @@ -919,19 +930,14 @@ inline Cell* GetCell( const int32_t x, const int32_t y ); void ForEachCellInPolygon( const stg_point_t pts[], - const uint32_t pt_count, + const unsigned int pt_count, stg_cell_callback_t cb, void* cb_arg ); - void ForEachCellInLine( const stg_point_t pt1, - const stg_point_t pt2, + void ForEachCellInLine( const stg_point_t& pt1, + const stg_point_t& pt2, stg_cell_callback_t cb, void* cb_arg ); - - void ForEachCellInLine( stg_meters_t x1, stg_meters_t y1, - stg_meters_t x2, stg_meters_t y2, - stg_cell_callback_t cb, - void* cb_arg ); /** convert a distance in meters to a distance in world occupancy grid pixels */ @@ -1141,6 +1147,7 @@ private: Model* mod; ///< model to which this block belongs + stg_point_t* mpts; ///< cache of this->pts in model coordindates size_t pt_count; ///< the number of points stg_point_t* pts; ///< points defining a polygon @@ -1175,6 +1182,12 @@ // find the position of a block's internal point in meters // relative to the model stg_point_t BlockPointToModelMeters( const stg_point_t& bpt ); + + /** Update the cache of block points converted to model coordinates */ + stg_point_t* GetPointsInModelCoords(); + + /** invalidate the cache of points in model coordinates */ + void InvalidateModelPointCache(); }; @@ -1232,6 +1245,9 @@ void Rasterize( uint8_t* data, unsigned int width, unsigned int height, stg_meters_t cellwidth, stg_meters_t cellheight ); + + void InvalidateModelPointCache() + { LISTMETHOD( blocks, Block*, InvalidateModelPointCache ); } }; @@ -2258,10 +2274,14 @@ pose specified in the model's local coordinate system */ Pose LocalToGlobal( const Pose& pose ) const; - /** Return the 3d point in world coordinates of a 3d point +// /** Return the 3d point in world coordinates of a 3d point +// specified in the model's local coordinate system */ +// stg_point3_t LocalToGlobal( const stg_point3_t local ) const; + + /** Return the 2d point in world coordinates of a 2d point specified in the model's local coordinate system */ - stg_point3_t LocalToGlobal( const stg_point3_t local ) const; - + stg_point_t LocalToGlobal( const stg_point_t& pt) const; + /** returns the first descendent of this model that is unsubscribed and has the type indicated by the string */ Model* GetUnsubscribedModelOfType( const stg_model_type_t type ) const; Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-04-11 00:39:13 UTC (rev 7594) +++ code/stage/trunk/libstage/world.cc 2009-04-11 05:32:07 UTC (rev 7595) @@ -914,16 +914,26 @@ ->GetCell( CELL(x), CELL(y) )) ; } -void World::ForEachCellInLine( stg_meters_t x1, stg_meters_t y1, - stg_meters_t x2, stg_meters_t y2, - stg_cell_callback_t cb, - void* cb_arg ) +void World::ForEachCellInPolygon( const stg_point_t pts[], + const unsigned int pt_count, + stg_cell_callback_t cb, + void* cb_arg ) +{ + for( unsigned int i=0; i<pt_count; i++ ) + ForEachCellInLine( pts[i], pts[(i+1)%pt_count], cb, cb_arg ); + +} + +void World::ForEachCellInLine( const stg_point_t& pt1, + const stg_point_t& pt2, + stg_cell_callback_t cb, + void* cb_arg ) { - int32_t x = MetersToPixels( x1 ); // global pixel coords - int32_t y = MetersToPixels( y1 ); + int32_t x = MetersToPixels( pt1.x ); // global pixel coords + int32_t y = MetersToPixels( pt1.y ); - int32_t dx = MetersToPixels( x2 - x1 ); - int32_t dy = MetersToPixels( y2 - y1 ); + int32_t dx = MetersToPixels( pt2.x - pt1.x ); + int32_t dy = MetersToPixels( pt2.y - pt1.y ); // line rasterization adapted from Cohen's 3D version in // Graphics Gems II. Should be very fast. This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-04-11 06:23:16
|
Revision: 7596 http://playerstage.svn.sourceforge.net/playerstage/?rev=7596&view=rev Author: rtv Date: 2009-04-11 06:22:41 +0000 (Sat, 11 Apr 2009) Log Message: ----------- spring cleaning Modified Paths: -------------- code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/model_callbacks.cc code/stage/trunk/libstage/stage.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2009-04-11 05:32:07 UTC (rev 7595) +++ code/stage/trunk/libstage/block.cc 2009-04-11 06:22:41 UTC (rev 7596) @@ -56,7 +56,7 @@ { if( mapped ) UnMap(); - stg_points_destroy( pts ); + if( pts ) delete[] pts; g_ptr_array_free( rendered_cells, TRUE ); g_ptr_array_free( candidate_cells, TRUE ); @@ -349,29 +349,27 @@ mpt2.y += mod->geom.size.y/2.0; // convert from meters to cells - int xa = floor( mpt1.x / cellwidth ); - int ya = floor( mpt1.y / cellheight ); - int xb = floor( mpt2.x / cellwidth ); - int yb = floor( mpt2.y / cellheight ); - - //printf( " line (%d,%d) to (%d,%d)\n", xa,ya,xb,yb ); + stg_point_int_t a( floor( mpt1.x / cellwidth ), + floor( mpt1.y / cellheight )); + stg_point_int_t b( floor( mpt2.x / cellwidth ), + floor( mpt2.y / cellheight ) ); - bool steep = abs( yb-ya ) > abs( xb-xa ); + bool steep = abs( b.y-a.y ) > abs( b.x-a.x ); if( steep ) { - swap( xa, ya ); - swap( xb, yb ); + swap( a.x, a.y ); + swap( b.x, b.y ); } - if( xa > xb ) + if( a.x > b.x ) { - swap( xa, xb ); - swap( ya, yb ); + swap( a.x, b.x ); + swap( a.y, b.y ); } - double dydx = (double) (yb - ya) / (double) (xb - xa); - double y = ya; - for(int x=xa; x<=xb; x++) + double dydx = (double) (b.y - a.y) / (double) (b.x - a.x); + double y = a.y; + for(int x=a.x; x<=b.x; x++) { if( steep ) { @@ -473,10 +471,10 @@ //printf( "Block::Load entity %d\n", entity ); if( pts ) - stg_points_destroy( pts ); + delete[] pts; pt_count = wf->ReadInt( entity, "points", 0); - pts = stg_points_create( pt_count ); + pts = new stg_point_t[ pt_count ]; //printf( "reading %d points\n", // pt_count ); Modified: code/stage/trunk/libstage/model_callbacks.cc =================================================================== --- code/stage/trunk/libstage/model_callbacks.cc 2009-04-11 05:32:07 UTC (rev 7595) +++ code/stage/trunk/libstage/model_callbacks.cc 2009-04-11 06:22:41 UTC (rev 7596) @@ -19,7 +19,7 @@ // mod->token, *key ); // add the callback & argument to the list - cb_list = g_list_prepend( cb_list, cb_create( cb, user ) ); + cb_list = g_list_prepend( cb_list, new stg_cb_t( cb, user ) ); // and replace the list in the hash table g_hash_table_insert( callbacks, address, cb_list ); @@ -56,12 +56,7 @@ // we're done with that //free( el->data ); - // TODO - fix leak of cb_t - - // if we just removed a model's last update callback, - // remove this model from the world's update list - //if( (member == (void*)&update) && (cb_list == NULL) ) - //stg_world_stop_updating_model( world, this ); + // TODO - fix leak of stg_cb_t } else { Modified: code/stage/trunk/libstage/stage.cc =================================================================== --- code/stage/trunk/libstage/stage.cc 2009-04-11 05:32:07 UTC (rev 7595) +++ code/stage/trunk/libstage/stage.cc 2009-04-11 06:22:41 UTC (rev 7596) @@ -152,53 +152,6 @@ return (stg_color_t)0; } -////////////////////////////////////////////////////////////////////////// -// scale an array of rectangles so they fit in a unit square -void Stg::stg_rotrects_normalize( stg_rotrect_t* rects, int num ) -{ - // assuming the rectangles fit in a square +/- one billion units - double minx, miny, maxx, maxy; - minx = miny = billion; - maxx = maxy = -billion; - - int r; - for( r=0; r<num; r++ ) - { - // test the origin of the rect - if( rects[r].pose.x < minx ) minx = rects[r].pose.x; - if( rects[r].pose.y < miny ) miny = rects[r].pose.y; - if( rects[r].pose.x > maxx ) maxx = rects[r].pose.x; - if( rects[r].pose.y > maxy ) maxy = rects[r].pose.y; - - // test the extremes of the rect - if( (rects[r].pose.x+rects[r].size.x) < minx ) - minx = (rects[r].pose.x+rects[r].size.x); - - if( (rects[r].pose.y+rects[r].size.y) < miny ) - miny = (rects[r].pose.y+rects[r].size.y); - - if( (rects[r].pose.x+rects[r].size.x) > maxx ) - maxx = (rects[r].pose.x+rects[r].size.x); - - if( (rects[r].pose.y+rects[r].size.y) > maxy ) - maxy = (rects[r].pose.y+rects[r].size.y); - } - - // now normalize all lengths so that the rects all fit inside - // rectangle from 0,0 to 1,1 - double scalex = maxx - minx; - double scaley = maxy - miny; - - for( r=0; r<num; r++ ) - { - rects[r].pose.x = (rects[r].pose.x - minx) / scalex; - rects[r].pose.y = (rects[r].pose.y - miny) / scaley; - rects[r].size.x = rects[r].size.x / scalex; - rects[r].size.y = rects[r].size.y / scaley; - } -} - - // returns the resultant of vector [p1] and [p2] Pose Stg::pose_scale( const Pose& p1, const double sx, const double sy, const double sz ) { @@ -219,25 +172,6 @@ return( pixels + index ); } -/* -static void pb_set_pixel( Fl_Shared_Image* pb, int x, int y, uint8_t val ) -{ - // bounds checking - int width = pb->w(); - int height = pb->h(); - if( x >=0 && x < width && y >= 0 && y < height ) - { - // zeroing - guchar* pix = pb_get_pixel( pb, x, y ); - unsigned int bytes_per_sample = 1; - unsigned int num_samples = pb->d(); - memset( pix, val, num_samples * bytes_per_sample ); - } - else - PRINT_WARN4( "pb_set_pixel coordinate %d,%d out of range (image dimensions %d by %d)", x, y, width, height ); -} -*/ - // set all the pixels in a rectangle static void pb_set_rect( Fl_Shared_Image* pb, int x, int y, int width, int height, uint8_t val ) { @@ -363,9 +297,9 @@ //assert( latest->size.x > 0 ); //assert( latest->size.y > 0 ); - if( latest->size.x < 1 || latest->size.y < 1 ) - printf( "p [%.2f %.2f] s [%.2f %.2f]\n", - latest->pose.x, latest->pose.y, latest->size.x, latest->size.y ); +// if( latest->size.x < 1 || latest->size.y < 1 ) +// printf( "p [%.2f %.2f] s [%.2f %.2f]\n", +// latest->pose.x, latest->pose.y, latest->size.x, latest->size.y ); //printf( "rect %d (%.2f %.2f %.2f %.2f %.2f\n", // *rect_count, @@ -382,20 +316,9 @@ // POINTS ----------------------------------------------------------- -stg_point_t* Stg::stg_points_create( size_t count ) -{ - return( (stg_point_t*)g_new( stg_point_t, count )); -} - -void Stg::stg_points_destroy( stg_point_t* pts ) -{ - g_free( pts ); -} - - stg_point_t* Stg::stg_unit_square_points_create( void ) { - stg_point_t * pts = stg_points_create( 4 ); + stg_point_t * pts = new stg_point_t[4]; pts[0].x = 0; pts[0].y = 0; @@ -409,22 +332,6 @@ return pts; } - -// CALLBACKS ------------------------------------------------------- - -stg_cb_t* Stg::cb_create( stg_model_callback_t callback, void* arg ) -{ - stg_cb_t* cb = (stg_cb_t*)g_new( stg_cb_t, 1 ); - cb->callback = callback; - cb->arg = arg; - return cb; -} - -void Stg::cb_destroy( stg_cb_t* cb ) -{ - free( cb ); -} - // return a value based on val, but limited minval <= val >= maxval double Stg::constrain( double val, double minval, double maxval ) { Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-04-11 05:32:07 UTC (rev 7595) +++ code/stage/trunk/libstage/stage.hh 2009-04-11 06:22:41 UTC (rev 7596) @@ -209,7 +209,7 @@ typedef double stg_watts_t; /** boolean */ - typedef uint32_t stg_bool_t; + typedef bool stg_bool_t; /** 32-bit ARGB color packed 0xAARRGGBB */ typedef uint32_t stg_color_t; @@ -358,27 +358,25 @@ /// smallest value in range, initially zero double min; - Bounds() : max(0), min(0) - { /* empty*/ }; + Bounds() : max(0), min(0) { /* empty*/ } }; - - /** Bound a volume along the x,y,z axes. All bounds initialized to zero. */ - typedef struct + + /** Define a three-dimensional bounding box, initialized to zero */ + class stg_bounds3d_t { + public: /// 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; + + stg_bounds3d_t() : x(), y(), z() {} + stg_bounds3d_t( const Bounds& x, const Bounds& y, const Bounds& z) + : x(x), y(y), z(z) {} + }; - /** Define a three-dimensional bounding box, initialized to zero */ - typedef struct - { - Bounds x, y, z; - } stg_bbox3d_t; - /** Define a field-of-view: an angle and range bounds */ typedef struct { @@ -391,60 +389,38 @@ { public: stg_meters_t x, y; - - // init - stg_point_t( stg_meters_t x, stg_meters_t y ) - : x(x), y(y){} - - // init - stg_point_t() : x(0), y(0){} - - // copy - stg_point_t( const stg_point_t& pt) : x(pt.x), y(pt.y){} + stg_point_t( stg_meters_t x, stg_meters_t y ) : x(x), y(y){} + stg_point_t() : x(0.0), y(0.0){} }; - + /** Define a point in 3d space */ - typedef struct + class stg_point3_t { - float x, y, z; - } stg_vertex_t; - - /** Define vertex and its color */ - typedef struct - { - float x, y, z, r, g, b, a; - } stg_colorvertex_t; - - /** Define a point in 3d space */ - typedef struct - { - stg_meters_t x, y, z; - } stg_point3_t; + public: + stg_meters_t x,y,z; + stg_point3_t( int x, int y ) : x(x), y(y){} + stg_point3_t() : x(0.0), y(0.0), z(0.0) {} + }; /** Define an integer point on the 2d plane */ - typedef struct + class stg_point_int_t { - int32_t x,y; - } stg_point_int_t; + public: + int x,y; + stg_point_int_t( int x, int y ) : x(x), y(y){} + stg_point_int_t() : x(0), y(0){} + }; + - /** Create an array of [count] points. Caller must free the returned - pointer, preferably with stg_points_destroy(). */ - stg_point_t* stg_points_create( size_t count ); - - /** frees a point array */ - void stg_points_destroy( stg_point_t* pts ); - /** create an array of 4 points containing the corners of a unit square. */ stg_point_t* stg_unit_square_points_create(); + + typedef uint32_t stg_movemask_t; + const stg_movemask_t STG_MOVE_TRANS = (1 << 0); ///< bitmask + const stg_movemask_t STG_MOVE_ROT = (1 << 1); ///< bitmask + const stg_movemask_t STG_MOVE_SCALE = (1 << 2); ///< bitmask - - 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 char MP_PREFIX[] = "_mp_"; const char MP_POSE[] = "_mp_pose"; const char MP_VELOCITY[] = "_mp_velocity"; @@ -458,7 +434,6 @@ const char MP_GRIPPER_RETURN[] = "_mp_gripper_return"; const char MP_MASS[] = "_mp_mass"; - /// laser return value typedef enum { @@ -466,7 +441,6 @@ LaserVisible, ///< detected by laser with a reflected intensity of 0 LaserBright ///< detected by laser with a reflected intensity of 1 } stg_laser_return_t; - /** Convenient OpenGL drawing routines, used by visualization code. */ @@ -688,15 +662,18 @@ /** container for a callback function and a single argument, so they can be stored together in a list with a single pointer. */ - typedef struct + class stg_cb_t { + public: stg_model_callback_t callback; void* arg; - } stg_cb_t; + + stg_cb_t( stg_model_callback_t cb, void* arg ) + : callback(cb), arg(arg) {} + + stg_cb_t() : callback(NULL), arg(NULL) {} + }; - 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] */ typedef struct { @@ -704,11 +681,6 @@ Size size; } stg_rotrect_t; // rotated rectangle - /** normalizes the set [rects] of [num] rectangles, so that they fit - exactly in a unit square. - */ - void stg_rotrects_normalize( stg_rotrect_t* rects, int num ); - /** load the image file [filename] and convert it to an array of rectangles, filling in the number of rects, width and height. Memory is allocated for the rectangle array [rects], so @@ -824,7 +796,7 @@ { return token; } void SetToken( const char* str ) - { token = strdup( str ); } // little memory leak + { token = strdup( str ); } // minor memory leak }; /** raytrace sample @@ -1177,10 +1149,9 @@ written, and the pointers to the rendered and potential cells are switched for next time (avoiding a memory copy).*/ GPtrArray* candidate_cells; - - // find the position of a block's internal point in meters - // relative to the model + /** find the position of a block's point in model coordinates + (m) */ stg_point_t BlockPointToModelMeters( const stg_point_t& bpt ); /** Update the cache of block points converted to model coordinates */ Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-04-11 05:32:07 UTC (rev 7595) +++ code/stage/trunk/libstage/world.cc 2009-04-11 06:22:41 UTC (rev 7596) @@ -683,8 +683,8 @@ // x,y, dx,dy, n ); // superregion coords - stg_point_int_t lastsup = {INT_MAX, INT_MAX }; - stg_point_int_t lastreg = {INT_MAX, INT_MAX }; + stg_point_int_t lastsup( INT_MAX, INT_MAX ); + stg_point_int_t lastreg( INT_MAX, INT_MAX ); SuperRegion* sr = NULL; Region* r = NULL; This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-04-22 02:49:33
|
Revision: 7604 http://playerstage.svn.sourceforge.net/playerstage/?rev=7604&view=rev Author: rtv Date: 2009-04-22 02:48:52 +0000 (Wed, 22 Apr 2009) Log Message: ----------- adding pose history logging Modified Paths: -------------- code/stage/trunk/libstage/CMakeLists.txt code/stage/trunk/libstage/canvas.cc code/stage/trunk/libstage/model.cc code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc Added Paths: ----------- code/stage/trunk/libstage/logentry.cc Modified: code/stage/trunk/libstage/CMakeLists.txt =================================================================== --- code/stage/trunk/libstage/CMakeLists.txt 2009-04-21 00:15:40 UTC (rev 7603) +++ code/stage/trunk/libstage/CMakeLists.txt 2009-04-22 02:48:52 UTC (rev 7604) @@ -12,6 +12,7 @@ file_manager.hh gl.cc glcolorstack.cc + logentry.cc model.cc model_blinkenlight.cc model_blobfinder.cc Modified: code/stage/trunk/libstage/canvas.cc =================================================================== --- code/stage/trunk/libstage/canvas.cc 2009-04-21 00:15:40 UTC (rev 7603) +++ code/stage/trunk/libstage/canvas.cc 2009-04-22 02:48:52 UTC (rev 7604) @@ -357,90 +357,100 @@ invalidate(); redraw(); return 1; - + case FL_MOVE: // moused moved while no button was pressed - if ( startx >=0 ) { - // mouse pointing to valid value - - if( Fl::event_state( FL_CTRL ) ) - { - int dx = Fl::event_x() - startx; - int dy = Fl::event_y() - starty; - - if( pCamOn == true ) { - perspective_camera.addYaw( -dx ); - perspective_camera.addPitch( -dy ); - } - else { - camera.addPitch( - 0.5 * static_cast<double>( dy ) ); - camera.addYaw( - 0.5 * static_cast<double>( dx ) ); + if( Fl::event_state( FL_META ) ) + { + puts( "TODO: HANDLE HISTORY" ); + //world->paused = ! world->paused; + return 1; + } + + if ( startx >=0 ) + { + // mouse pointing to valid value + + if( Fl::event_state( FL_CTRL ) ) + { + int dx = Fl::event_x() - startx; + int dy = Fl::event_y() - starty; + + if( pCamOn == true ) { + perspective_camera.addYaw( -dx ); + perspective_camera.addPitch( -dy ); + } + else { + camera.addPitch( - 0.5 * static_cast<double>( dy ) ); + camera.addYaw( - 0.5 * static_cast<double>( dx ) ); + } + invalidate(); + redraw(); } - invalidate(); - redraw(); - } - else if( Fl::event_state( FL_ALT ) ) - { - int dx = Fl::event_x() - startx; - int dy = Fl::event_y() - starty; - - if( pCamOn == true ) { - perspective_camera.move( -dx, dy, 0.0 ); - } - else { - camera.move( -dx, dy ); + else if( Fl::event_state( FL_ALT ) ) + { + int dx = Fl::event_x() - startx; + int dy = Fl::event_y() - starty; + + if( pCamOn == true ) { + perspective_camera.move( -dx, dy, 0.0 ); + } + else { + camera.move( -dx, dy ); + } + invalidate(); } - invalidate(); - } - } + } startx = Fl::event_x(); starty = Fl::event_y(); return 1; - case FL_PUSH: // button pressed { - Model* mod = getModel( startx, starty ); - startx = Fl::event_x(); - starty = Fl::event_y(); - selectedModel = false; - switch( Fl::event_button() ) + //else { - 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 + Model* mod = getModel( startx, starty ); + startx = Fl::event_x(); + starty = Fl::event_y(); + selectedModel = false; + switch( Fl::event_button() ) + { + case 1: + clicked_empty_space = ( mod == NULL ); + empty_space_startx = startx; + empty_space_starty = starty; + if( mod ) { + // clicked a model + if ( Fl::event_state( FL_SHIFT ) ) { + // holding shift, toggle selection + if ( selected( mod ) ) + unSelect( mod ); + else { + select( mod ); + selectedModel = true; // selected a model + } + } + else { + if ( !selected( mod ) ) { + // clicked on an unselected model while + // not holding shift, this is the new + // selection + unSelectAll(); + select( mod ); + } + selectedModel = true; // selected a model + } } - } - else { - if ( !selected( mod ) ) { - // clicked on an unselected model while - // not holding shift, this is the new - // selection - unSelectAll(); - select( mod ); + + return 1; + case 3: + { + // leave selections alone + // rotating handled within FL_DRAG + return 1; } - selectedModel = true; // selected a model - } - } - - 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 Added: code/stage/trunk/libstage/logentry.cc =================================================================== --- code/stage/trunk/libstage/logentry.cc (rev 0) +++ code/stage/trunk/libstage/logentry.cc 2009-04-22 02:48:52 UTC (rev 7604) @@ -0,0 +1,29 @@ +#include "stage.hh" +using namespace Stg; + +// static data members +std::vector<LogEntry> LogEntry::log; + +LogEntry::LogEntry( stg_usec_t timestamp, Model* mod ) : + timestamp( timestamp ), + mod( mod ), + pose( mod->GetPose() ) +{ + // all log entries are added to the static vector history + log.push_back( *this ); +} + + + +void LogEntry::Print() +{ + for( size_t i=0; i<log.size(); i++ ) + { + LogEntry* e = &log[i]; + + printf( "%.3f\t%u\t%s\n", + (double)e->timestamp / 1e6, + e->mod->GetId(), + e->mod->PoseString().c_str() ); + } +} Modified: code/stage/trunk/libstage/model.cc =================================================================== --- code/stage/trunk/libstage/model.cc 2009-04-21 00:15:40 UTC (rev 7603) +++ code/stage/trunk/libstage/model.cc 2009-04-22 02:48:52 UTC (rev 7604) @@ -209,6 +209,7 @@ initfunc(NULL), interval((stg_usec_t)1e4), // 10msec last_update(0), + log_state(false), map_resolution(0.1), mass(0), on_update_list( false ), @@ -713,6 +714,9 @@ } last_update = world->sim_time; + + if( log_state ) + world->Log( this ); } void Model::CallUpdateCallbacks( void ) Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-04-21 00:15:40 UTC (rev 7603) +++ code/stage/trunk/libstage/stage.hh 2009-04-22 02:48:52 UTC (rev 7604) @@ -279,6 +279,14 @@ printf( "%s pose [x:%.3f y:%.3f z:%.3f a:%.3f]\n", prefix, x,y,z,a ); } + + std::string String() + { + char buf[256]; + snprintf( buf, 256, "[ %.3f %.3f %.3f %.3f ]", + x,y,z,a ); + return std::string(buf); + } /* returns true iff all components of the velocity are zero. */ bool IsZero() const { return( !(x || y || z || a )); }; @@ -818,6 +826,28 @@ class BlockGroup; class PowerPack; + class LogEntry + { + stg_usec_t timestamp; + Model* mod; + Pose pose; + + public: + LogEntry( stg_usec_t timestamp, Model* mod ); + + /** A log of all LogEntries ever created */ + static std::vector<LogEntry> log; + + /** Returns the number of logged entries */ + static size_t Count(){ return log.size(); } + + /** Clear the log */ + static void Clear(){ log.clear(); } + + /** Print the log on stdout */ + static void Print(); + }; + /// %World class class World : public Ancestor { @@ -879,6 +909,9 @@ 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 + /** Log the state of a Model */ + void Log( Model* mod ); + /** hint that the world needs to be redrawn if a GUI is attached */ void NeedRedraw(){ dirty = true; }; @@ -1699,6 +1732,7 @@ ctrlinit_t* initfunc; stg_usec_t interval; ///< time between updates in us stg_usec_t last_update; ///< time of last update in us + bool log_state; ///< iff true, model state is logged stg_meters_t map_resolution; stg_kg_t mass; bool on_update_list; @@ -1935,6 +1969,8 @@ void PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax, stg_meters_t ymin, stg_meters_t ymax ); + std::string PoseString(){ return pose.String(); } + /** 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 ); } Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-04-21 00:15:40 UTC (rev 7603) +++ code/stage/trunk/libstage/world.cc 2009-04-22 02:48:52 UTC (rev 7604) @@ -53,6 +53,7 @@ #include "option.hh" using namespace Stg; + // static data members unsigned int World::next_id = 0; bool World::quit_all = false; @@ -1040,3 +1041,13 @@ stg_usec_t World::SimTimeNow(void) { return sim_time; } + +void World::Log( Model* mod ) +{ + LogEntry( sim_time, mod); + + printf( "log entry count %lu\n", LogEntry::Count() ); + //LogEntry::Print(); +} + + This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-04-23 01:46:55
|
Revision: 7605 http://playerstage.svn.sourceforge.net/playerstage/?rev=7605&view=rev Author: rtv Date: 2009-04-23 01:44:18 +0000 (Thu, 23 Apr 2009) Log Message: ----------- changed from GSList to std::list in raytracing Modified Paths: -------------- code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/region.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 2009-04-22 02:48:52 UTC (rev 7604) +++ code/stage/trunk/libstage/block.cc 2009-04-23 01:44:18 UTC (rev 7605) @@ -139,26 +139,28 @@ return( inherit_color ? mod->color : color ); } -GList* Block::AppendTouchingModels( GList* list ) +GList* Block::AppendTouchingModels( GList* l ) { // for every cell we are rendered into for( unsigned int i=0; i<rendered_cells->len; i++ ) { - Cell* cell = (Cell*)g_ptr_array_index( rendered_cells, i); + Cell* c = (Cell*)g_ptr_array_index( rendered_cells, i); // for every block rendered into that cell - for( GSList* it = cell->list; it; it=it->next ) - { - Block* testblock = (Block*)it->data; - Model* testmod = testblock->mod; + for( std::list<Block*>::iterator it = c->blocks.begin(); + it != c->blocks.end(); + ++it ) + { + //Block* testblock = *it; + Model* testmod = (*it)->mod; - if( !mod->IsRelated( testmod )) - if( ! g_list_find( list, testmod ) ) - list = g_list_append( list, testmod ); - } + if( !mod->IsRelated( testmod )) + if( ! g_list_find( l, testmod ) ) + l = g_list_append( l, testmod ); + } } - - return list; + + return l; } Model* Block::TestCollision() @@ -172,25 +174,26 @@ // for every cell we may be rendered into for( unsigned 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 ) - { - Block* testblock = (Block*)it->data; - Model* 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->vis.obstacle_return && - !mod->IsRelated( testmod )) - { - //puts( "HIT"); - return testmod; // bail immediately with the bad news - } - } + Cell* c = (Cell*)g_ptr_array_index(candidate_cells, i); + + // for every rendered into that cell + for( std::list<Block*>::iterator it = c->blocks.begin(); + it != c->blocks.end(); + ++it ) + { + Model* testmod = (*it)->mod; + + //printf( " testing block %p of model %s\n", testblock, testmod->Token() ); + + // if the tested model is an obstacle and it's not attached to this model + if( (testmod != this->mod) && + testmod->vis.obstacle_return && + !mod->IsRelated( testmod )) + { + //puts( "HIT"); + return testmod; // bail immediately with the bad news + } + } } //printf( "model %s block %p collision done. no hits.\n", mod->Token(), this ); Modified: code/stage/trunk/libstage/region.cc =================================================================== --- code/stage/trunk/libstage/region.cc 2009-04-22 02:48:52 UTC (rev 7604) +++ code/stage/trunk/libstage/region.cc 2009-04-23 01:44:18 UTC (rev 7605) @@ -129,11 +129,10 @@ snprintf( buf, 15, "%lu", r->count ); Gl::draw_string( x<<RBITS, y<<RBITS, 0, buf ); - for( unsigned int p=0; p<Region::WIDTH; p++ ) for( unsigned int q=0; q<Region::WIDTH; q++ ) - if( r->cells[p+(q*Region::WIDTH)].list ) + if( r->cells[p+(q*Region::WIDTH)].blocks.size() ) { GLfloat xx = p+(x<<RBITS); GLfloat yy = q+(y<<RBITS); @@ -145,11 +144,12 @@ } else // draw a rectangular solid { - for( GSList* it = r->cells[p+(q*Region::WIDTH)].list; - it; - it=it->next ) + Cell* c = &r->cells[p+(q*Region::WIDTH)]; + for( std::list<Block*>::iterator it = c->blocks.begin(); + it != c->blocks.end(); + ++it ) { - Block* block = (Block*)it->data; + Block* block = *it;//(Block*)it->data; //printf( "zb %.2f %.2f\n", ent->zbounds.min, ent->zbounds.max ); Modified: code/stage/trunk/libstage/region.hh =================================================================== --- code/stage/trunk/libstage/region.hh 2009-04-22 02:48:52 UTC (rev 7604) +++ code/stage/trunk/libstage/region.hh 2009-04-23 01:44:18 UTC (rev 7605) @@ -5,6 +5,9 @@ Copyright Richard Vaughan 2008 */ +#include <list> // STL containers +// #include <vector> + #include "stage.hh" namespace Stg @@ -21,6 +24,7 @@ #define SUPERREGIONWIDTH (1<<SBITS) #define SUPERREGIONSIZE SUPERREGIONWIDTH*SUPERREGIONWIDTH + class Cell { friend class Region; @@ -29,13 +33,14 @@ friend class Block; private: - Region* region; - GSList* list; + Region* region; + std::list<Block*> blocks; + public: Cell() : region( NULL), - list(NULL) - { /* do nothing */ } + blocks() + { /* empty */ } inline void RemoveBlock( Block* b ); inline void AddBlock( Block* b ); @@ -51,17 +56,16 @@ static const uint32_t WIDTH; static const uint32_t SIZE; - //Cell cells[REGIONSIZE]; Cell* cells; SuperRegion* superregion; - + public: unsigned long count; // number of blocks rendered into these cells Region(); ~Region(); - Cell* GetCell( int32_t x, int32_t y ) + Cell* GetCellCreate( int32_t x, int32_t y ) { if( ! cells ) { @@ -73,6 +77,11 @@ }; + Cell* GetCellNoCreate( int32_t x, int32_t y ) + { + return( &cells[x + (y*Region::WIDTH)] ); + }; + void DecrementOccupancy(); void IncrementOccupancy(); }; @@ -127,23 +136,18 @@ inline void Cell::RemoveBlock( Block* b ) { // linear time removal, but these lists should be very short. - list = g_slist_remove( list, b ); + blocks.remove( b ); + region->DecrementOccupancy(); } inline void Cell::AddBlock( Block* b ) { // constant time prepend - list = g_slist_prepend( list, b ); + blocks.push_front( b ); region->IncrementOccupancy(); b->RecordRendering( this ); } -inline void Cell::AddBlockNoRecord( Block* b ) -{ - list = g_slist_prepend( list, b ); - // don't add this cell to the block - we assume it's already there -} - }; // namespace Stg Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-04-22 02:48:52 UTC (rev 7604) +++ code/stage/trunk/libstage/world.cc 2009-04-23 01:44:18 UTC (rev 7605) @@ -71,8 +71,8 @@ World::World( const char* token, - stg_msec_t interval_sim, - double ppm ) + stg_msec_t interval_sim, + double ppm ) : // private charge_list( NULL ), @@ -103,7 +103,7 @@ ray_list( NULL ), sim_time( 0 ), superregions( g_hash_table_new( (GHashFunc)PointIntHash, - (GEqualFunc)PointIntEqual ) ), + (GEqualFunc)PointIntEqual ) ), sr_cached(NULL), reentrant_update_list( NULL ), nonreentrant_update_list( NULL ), @@ -154,7 +154,7 @@ for( GList* it = World::world_list; it; it=it->next ) { if( ((World*)it->data)->Update() == false ) - quit = false; + quit = false; } return quit; } @@ -189,7 +189,7 @@ { // lookup the group in which this was defined Model* mod = (Model*)g_hash_table_lookup( entitytable, - (gpointer)wf->GetEntityParent( entity ) ); + (gpointer)wf->GetEntityParent( entity ) ); if( ! mod ) PRINT_ERR( "block has no model for a parent" ); @@ -247,7 +247,7 @@ int parent_entity = wf->GetEntityParent( entity ); PRINT_DEBUG2( "wf entity %d parent entity %d\n", - entity, parent_entity ); + entity, parent_entity ); Model* parent = (Model*)g_hash_table_lookup( entitytable, (gpointer)parent_entity ); @@ -299,11 +299,11 @@ this->interval_sim = (stg_usec_t)thousand * wf->ReadInt( entity, "interval_sim", - (int)(this->interval_sim/thousand) ); + (int)(this->interval_sim/thousand) ); if( wf->PropertyExists( entity, "quit_time" ) ) { this->quit_time = (stg_usec_t) ( million * - wf->ReadFloat( entity, "quit_time", 0 ) ); + wf->ReadFloat( entity, "quit_time", 0 ) ); } if( wf->PropertyExists( entity, "resolution" ) ) @@ -317,22 +317,22 @@ int count = wf->ReadInt( entity, "threadpool", worker_threads ); if( count && (count != (int)worker_threads) ) - { - worker_threads = count; + { + worker_threads = count; - if( threadpool == NULL ) - threadpool = g_thread_pool_new( (GFunc)update_thread_entry, - this, - worker_threads, - true, - NULL ); - else - g_thread_pool_set_max_threads( threadpool, - worker_threads, - NULL ); + if( threadpool == NULL ) + threadpool = g_thread_pool_new( (GFunc)update_thread_entry, + this, + worker_threads, + true, + NULL ); + else + g_thread_pool_set_max_threads( threadpool, + worker_threads, + NULL ); - printf( "[threadpool %u]", worker_threads ); - } + printf( "[threadpool %u]", worker_threads ); + } } // Iterate through entitys and create objects of the appropriate type @@ -347,8 +347,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 ); } @@ -366,7 +366,7 @@ if( debug ) printf( "[Load time %.3fsec]\n", - (load_end_time - load_start_time) / 1000000.0 ); + (load_end_time - load_start_time) / 1000000.0 ); else putchar( '\n' ); } @@ -629,11 +629,11 @@ } stg_raytrace_result_t World::Raytrace( const Pose &gpose, - const stg_meters_t range, - const stg_ray_test_func_t func, - const Model* mod, - const void* arg, - const bool ztest ) + const stg_meters_t range, + const stg_ray_test_func_t func, + const Model* mod, + const void* arg, + const bool ztest ) { stg_raytrace_result_t sample; @@ -711,71 +711,73 @@ // find the starting superregion sr = GetSuperRegionCached( sup ); // possibly NULL, but unlikely - + while ( n-- ) { //printf( "pixel [%d %d]\tS[ %d %d ]\t", // x, y, sup.x, sup.y ); 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 ); - } + { + // printf( "R[ %d %d ]\t", reg.x, reg.y ); + + // if the region coordinate has changed since the last loop + if( reg.x != lastreg.x || reg.y != lastreg.y ) + { + // lookup the new region + 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 ); - - Cell* c = r->GetCell( cell.x, cell.y ); - assert(c); - - for( GSList* list = c->list; - list; - list = list->next ) - { - Block* block = (Block*)list->data; - assert( block ); - - //printf( "ENT %p mod %p z [%.2f %.2f] color %X\n", - // ent, - // ent->mod, - // ent->zbounds.min, - // ent->zbounds.max, - // ent->color ); - - // skip if not in the right z range - if( ztest && - ( gpose.z < block->global_z.min || - gpose.z > block->global_z.max ) ) - { - //max( "failed ztest: ray at %.2f block at [%.2f %.2f]\n", - // pose.z, ent->zbounds.min, ent->zbounds.max ); - continue; - } + if( nonempty_region ) + { + //printf( "C[ %d %d ]\t", cell.x, cell.y ); + + Cell* c = r->GetCellNoCreate( cell.x, cell.y ); + assert(c); + + for( std::list<Block*>::iterator it = c->blocks.begin(); + it != c->blocks.end(); + ++it ) + { + Block* block = *it; + assert( block ); - //printf( "RT: mod %p testing mod %p at %d %d\n", - // mod, ent->mod, x, y ); - // printf( "RT: mod %p %s testing mod %p %s at %d %d\n", - // mod, mod->Token(), ent->mod, ent->mod->Token(), x, y ); + //printf( "ENT %p mod %p z [%.2f %.2f] color %X\n", + // ent, + // ent->mod, + // ent->zbounds.min, + // ent->zbounds.max, + // ent->color ); - // test the predicate we were passed - if( (*func)( block->mod, (Model*)mod, arg )) // TODO - { - // a hit! - sample.color = block->GetColor(); - sample.mod = block->mod; - sample.range = hypot( (glob.x-start.x)/ppm, (glob.y-start.y)/ppm ); - return sample; - } - } - } - } + // skip if not in the right z range + if( ztest && + ( gpose.z < block->global_z.min || + gpose.z > block->global_z.max ) ) + { + //max( "failed ztest: ray at %.2f block at [%.2f %.2f]\n", + // pose.z, ent->zbounds.min, ent->zbounds.max ); + continue; + } + + //printf( "RT: mod %p testing mod %p at %d %d\n", + // mod, ent->mod, x, y ); + // printf( "RT: mod %p %s testing mod %p %s at %d %d\n", + // mod, mod->Token(), ent->mod, ent->mod->Token(), x, y ); + + // test the predicate we were passed + if( (*func)( block->mod, (Model*)mod, arg )) + { + // a hit! + sample.color = block->GetColor(); + sample.mod = block->mod; + sample.range = hypot( (glob.x-start.x)/ppm, (glob.y-start.y)/ppm ); + return sample; + } + } + } + } // printf( "\t step %d n %d pixel [ %d, %d ] block [ %d %d ] index [ %d %d ] \n", // //coarse [ %d %d ]\n", @@ -783,41 +785,41 @@ // increment our pixel in the correct direction if( exy < 0 ) // we're iterating along X - { - glob.x += sx; - exy += by; + { + glob.x += sx; + exy += by; - sup.x = SUPERREGION( glob.x ); - if( sup.x != lastsup.x ) - { - sr = (SuperRegion*)g_hash_table_lookup( superregions, (void*)&sup ); - lastsup = sup; // remember these coords - } + sup.x = SUPERREGION( glob.x ); + if( sup.x != lastsup.x ) + { + sr = (SuperRegion*)g_hash_table_lookup( superregions, (void*)&sup ); + lastsup = sup; // remember these coords + } - // recompute current region and cell (we can skip these if - // sr==NULL, but profiling suggests it's faster to do them - // than add a conditional) - reg.x = REGION( glob.x); - cell.x = CELL( glob.x ); - } + // recompute current region and cell (we can skip these if + // sr==NULL, but profiling suggests it's faster to do them + // than add a conditional) + reg.x = REGION( glob.x); + cell.x = CELL( glob.x ); + } else // we're iterating along Y - { - glob.y += sy; - exy -= bx; + { + glob.y += sy; + exy -= bx; - sup.y = SUPERREGION( glob.y ); - if( sup.y != lastsup.y ) - { - sr = (SuperRegion*)g_hash_table_lookup( superregions, (void*)&sup ); - lastsup = sup; // remember these coords - } + sup.y = SUPERREGION( glob.y ); + if( sup.y != lastsup.y ) + { + sr = (SuperRegion*)g_hash_table_lookup( superregions, (void*)&sup ); + lastsup = sup; // remember these coords + } - // recompute current region and cell (we can skip these if - // sr==NULL, but profiling suggests it's faster to do them - // than add a conditional) - reg.y = REGION( glob.y ); - cell.y = CELL( glob.y ); - } + // recompute current region and cell (we can skip these if + // sr==NULL, but profiling suggests it's faster to do them + // than add a conditional) + reg.y = REGION( glob.y ); + cell.y = CELL( glob.y ); + } } // hit nothing @@ -911,8 +913,8 @@ //printf( "GC[ %d %d ] ", glob.x, glob.y ); return( GetSuperRegionCached( SUPERREGION(glob) ) - ->GetRegion( REGION(x), REGION(y) ) - ->GetCell( CELL(x), CELL(y) )) ; + ->GetRegion( REGION(x), REGION(y) ) + ->GetCellCreate( CELL(x), CELL(y) )) ; } void World::ForEachCellInPolygon( const stg_point_t pts[], @@ -961,15 +963,15 @@ // cleverly skip to the next cell if( exy < 0 ) - { - x += sx; - exy += by; - } + { + x += sx; + exy += by; + } else - { - y += sy; - exy -= bx; - } + { + y += sy; + exy -= bx; + } } } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <rt...@us...> - 2009-04-30 00:46:53
|
Revision: 7606 http://playerstage.svn.sourceforge.net/playerstage/?rev=7606&view=rev Author: rtv Date: 2009-04-30 00:46:52 +0000 (Thu, 30 Apr 2009) Log Message: ----------- added callbacks to world that runat the end of an update cycle, after all model callbacks have been called but before the clock is ticked Modified Paths: -------------- code/stage/trunk/libstage/model_callbacks.cc code/stage/trunk/libstage/region.cc code/stage/trunk/libstage/region.hh code/stage/trunk/libstage/stage.hh code/stage/trunk/libstage/world.cc Modified: code/stage/trunk/libstage/model_callbacks.cc =================================================================== --- code/stage/trunk/libstage/model_callbacks.cc 2009-04-23 01:44:18 UTC (rev 7605) +++ code/stage/trunk/libstage/model_callbacks.cc 2009-04-30 00:46:52 UTC (rev 7606) @@ -54,9 +54,8 @@ // store the new, shorter, list of callbacks g_hash_table_insert( callbacks, member, cb_list ); - // we're done with that - //free( el->data ); - // TODO - fix leak of stg_cb_t + // we're done with the stored data + delete (stg_cb_t*)(el->data); } else { Modified: code/stage/trunk/libstage/region.cc =================================================================== --- code/stage/trunk/libstage/region.cc 2009-04-23 01:44:18 UTC (rev 7605) +++ code/stage/trunk/libstage/region.cc 2009-04-30 00:46:52 UTC (rev 7606) @@ -233,9 +233,7 @@ } } } - - - glPopMatrix(); + glPopMatrix(); } @@ -247,3 +245,4 @@ glRecti( 0,0, 1<<SRBITS, 1<<SRBITS ); glPopMatrix(); } + Modified: code/stage/trunk/libstage/region.hh =================================================================== --- code/stage/trunk/libstage/region.hh 2009-04-23 01:44:18 UTC (rev 7605) +++ code/stage/trunk/libstage/region.hh 2009-04-30 00:46:52 UTC (rev 7606) @@ -74,14 +74,37 @@ cells[i].region = this; } return( &cells[x + (y*Region::WIDTH)] ); + } - }; + Cell* GetCellCreate( const stg_point_int_t& c ) + { + if( ! cells ) + { + cells = new Cell[REGIONSIZE]; + for( unsigned int i=0; i<Region::SIZE; i++ ) + cells[i].region = this; + } + return( &cells[c.x + (c.y*Region::WIDTH)] ); + } - Cell* GetCellNoCreate( int32_t x, int32_t y ) + Cell* GetCellNoCreate( int32_t x, int32_t y ) const { return( &cells[x + (y*Region::WIDTH)] ); - }; + } + + Cell* GetCellNoCreate( const stg_point_int_t& c ) const + { + return( &cells[c.x + (c.y*Region::WIDTH)] ); + } +// Cell* GetCellNoCreateBoundsCheck( const stg_point_int_t& c ) const +// { +// if( c.x < 0 || c.x > WIDTH || c.y < 0 || c.y > WIDTH ) +// return NULL; +// else +// return( &cells[c.x + (c.y*Region::WIDTH)] ); +// } + void DecrementOccupancy(); void IncrementOccupancy(); }; Modified: code/stage/trunk/libstage/stage.hh =================================================================== --- code/stage/trunk/libstage/stage.hh 2009-04-23 01:44:18 UTC (rev 7605) +++ code/stage/trunk/libstage/stage.hh 2009-04-30 00:46:52 UTC (rev 7606) @@ -41,6 +41,8 @@ #include <sys/time.h> #include <iostream> #include <vector> +#include <list> +//#include <pair> // we use GLib's data structures extensively. Perhaps we'll move to // C++ STL types to lose this dependency one day. @@ -647,6 +649,7 @@ typedef int(*stg_model_callback_t)(Model* mod, void* user ); + typedef int(*stg_world_callback_t)(World* world, void* user ); typedef int(*stg_cell_callback_t)(Cell* cell, void* user ); // return val, or minval if val < minval, or maxval if val > maxval @@ -673,14 +676,18 @@ class stg_cb_t { public: - stg_model_callback_t callback; + stg_model_callback_t callback; void* arg; stg_cb_t( stg_model_callback_t cb, void* arg ) : callback(cb), arg(arg) {} + + stg_cb_t( stg_world_callback_t cb, void* arg ) + : callback(NULL), arg(arg) {} stg_cb_t() : callback(NULL), arg(NULL) {} }; + /** Defines a rectangle of [size] located at [pose] */ typedef struct @@ -886,7 +893,8 @@ GCond* worker_threads_done; ///< signalled when there are no more updates for the worker threads to do protected: - + + std::list<std::pair<stg_world_callback_t,void*> > cb_list; ///< List of callback functions and arguments stg_bounds3d_t extent; ///< Describes the 3D volume of the world bool graphics;///< true iff we have a GUI stg_usec_t interval_sim; ///< temporal resolution: microseconds that elapse between simulated time steps @@ -904,11 +912,21 @@ long unsigned int updates; ///< the number of simulated time steps executed so far Worldfile* wf; ///< If set, points to the worldfile used to create this world + void CallUpdateCallbacks(); ///< Call all calbacks in cb_list, removing any that return true; + 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 + /** Attach a callback function, to be called with the argument at + the end of a complete update step */ + void AddUpdateCallback( stg_world_callback_t cb, void* user ); + + /** Remove a callback function. Any argument data passed to + AddUpdateCallback is not automatically freed. */ + int RemoveUpdateCallback( stg_world_callback_t cb, void* user ); + /** Log the state of a Model */ void Log( Model* mod ); @@ -1630,6 +1648,7 @@ int shutdown; int startup; int update; + int update_done; }; /** Records model state and functionality in the GUI, if used */ Modified: code/stage/trunk/libstage/world.cc =================================================================== --- code/stage/trunk/libstage/world.cc 2009-04-23 01:44:18 UTC (rev 7605) +++ code/stage/trunk/libstage/world.cc 2009-04-30 00:46:52 UTC (rev 7606) @@ -95,6 +95,7 @@ worker_threads_done( g_cond_new() ), // protected + cb_list(NULL), extent(), graphics( false ), interval_sim( (stg_usec_t)thousand * interval_sim ), @@ -455,6 +456,54 @@ return str; } +void World::AddUpdateCallback( stg_world_callback_t cb, + void* user ) +{ + // add the callback & argument to the list + std::pair<stg_world_callback_t,void*> p(cb, user); + cb_list.push_back( p ); +} + +int World::RemoveUpdateCallback( stg_world_callback_t cb, + void* user ) +{ + std::pair<stg_world_callback_t,void*> p( cb, user ); + + std::list<std::pair<stg_world_callback_t,void*> >::iterator it; + for( it = cb_list.begin(); + it != cb_list.end(); + it++ ) + { + if( (*it) == p ) + { + cb_list.erase( it ); + break; + } + } + + // return the number of callbacks now in the list. Useful for + // detecting when the list is empty. + return cb_list.size(); +} + +void World::CallUpdateCallbacks() +{ + + // for each callback in the list + for( std::list<std::pair<stg_world_callback_t,void*> >::iterator it = cb_list.begin(); + it != cb_list.end(); + it++ ) + { + //printf( "cbs %p data %p cvs->next %p\n", cbs, cbs->data, cbs->next ); + + if( ((*it).first )( this, (*it).second ) ) + { + //printf( "callback returned TRUE - schedule removal from list\n" ); + it = cb_list.erase( it ); + } + } +} + bool World::Update() { PRINT_DEBUG( "World::Update()" ); @@ -515,8 +564,11 @@ fflush( stdout ); } + this->updates++; + + CallUpdateCallbacks(); + this->sim_time += this->interval_sim; - this->updates++; return false; @@ -1052,4 +1104,3 @@ //LogEntry::Print(); } - This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <th...@us...> - 2009-05-09 15:17:05
|
Revision: 7631 http://playerstage.svn.sourceforge.net/playerstage/?rev=7631&view=rev Author: thjc Date: 2009-05-09 15:16:57 +0000 (Sat, 09 May 2009) Log Message: ----------- fixing some memory leaks Modified Paths: -------------- code/stage/trunk/libstage/block.cc code/stage/trunk/libstage/worldfile.cc Modified: code/stage/trunk/libstage/block.cc =================================================================== --- code/stage/trunk/libstage/block.cc 2009-05-08 21:41:48 UTC (rev 7630) +++ code/stage/trunk/libstage/block.cc 2009-05-09 15:16:57 UTC (rev 7631) @@ -7,8 +7,8 @@ /** 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.*/ -Block::Block( Model* mod, - stg_point_t* pts, +Block::Block( Model* mod, + stg_point_t* pts, size_t pt_count, stg_meters_t zmin, stg_meters_t zmax, @@ -33,31 +33,32 @@ } /** A from-file constructor */ -Block::Block( Model* mod, - Worldfile* wf, - int entity) +Block::Block( Model* mod, + Worldfile* wf, + int entity) : mod( mod ), mpts(NULL), - pt_count(0), - pts(NULL), + pt_count(0), + pts(NULL), color(0), inherit_color(true), - rendered_cells( g_ptr_array_sized_new(32) ), + rendered_cells( g_ptr_array_sized_new(32) ), candidate_cells( g_ptr_array_sized_new(32) ) { assert(mod); assert(wf); assert(entity); - + Load( wf, entity ); } Block::~Block() -{ +{ if( mapped ) UnMap(); - + if( pts ) delete[] pts; - + InvalidateModelPointCache(); + g_ptr_array_free( rendered_cells, TRUE ); g_ptr_array_free( candidate_cells, TRUE ); } @@ -69,7 +70,7 @@ pts[p].x += x; pts[p].y += y; } - + mod->blockgroup.BuildDisplayList( mod ); } @@ -77,13 +78,13 @@ { double min = billion; double max = -billion; - + for( unsigned int p=0; p<pt_count; p++) { if( pts[p].y > max ) max = pts[p].y; if( pts[p].y < min ) min = pts[p].y; } - + // return the value half way between max and min return( min + (max - min)/2.0 ); } @@ -92,13 +93,13 @@ { double min = billion; double max = -billion; - + for( unsigned int p=0; p<pt_count; p++) { if( pts[p].x > max ) max = pts[p].x; if( pts[p].x < min ) min = pts[p].x; } - + // return the value half way between maxx and min return( min + (max - min)/2.0 ); } @@ -145,21 +146,21 @@ for( unsigned int i=0; i<rendered_cells->len; i++ ) { Cell* c = (Cell*)g_ptr_array_index( rendered_cells, i); - + // for every block rendered into that cell for( std::list<Block*>::iterator it = c->blocks.begin(); it != c->blocks.end(); - ++it ) + ++it ) { //Block* testblock = *it; Model* testmod = (*it)->mod; - + if( !mod->IsRelated( testmod )) if( ! g_list_find( l, testmod ) ) l = g_list_append( l, testmod ); } } - + return l; } @@ -169,48 +170,48 @@ // find the set of cells we would render into given the current global pose GenerateCandidateCells(); - + if( mod->vis.obstacle_return ) // for every cell we may be rendered into for( unsigned int i=0; i<candidate_cells->len; i++ ) { Cell* c = (Cell*)g_ptr_array_index(candidate_cells, i); - + // for every rendered into that cell for( std::list<Block*>::iterator it = c->blocks.begin(); it != c->blocks.end(); - ++it ) + ++it ) { Model* testmod = (*it)->mod; - + //printf( " testing block %p of model %s\n", testblock, testmod->Token() ); - + // if the tested model is an obstacle and it's not attached to this model - if( (testmod != this->mod) && - testmod->vis.obstacle_return && + if( (testmod != this->mod) && + testmod->vis.obstacle_return && !mod->IsRelated( testmod )) { //puts( "HIT"); return testmod; // bail immediately with the bad news - } + } } } - + //printf( "model %s block %p collision done. no hits.\n", mod->Token(), this ); return NULL; // no hit } void Block::RemoveFromCellArray( GPtrArray* ptrarray ) -{ - for( unsigned int i=0; i<ptrarray->len; i++ ) - ((Cell*)g_ptr_array_index(ptrarray, i))->RemoveBlock( this ); +{ + for( unsigned int i=0; i<ptrarray->len; i++ ) + ((Cell*)g_ptr_array_index(ptrarray, i))->RemoveBlock( this ); } void Block::AddToCellArray( GPtrArray* ptrarray ) -{ - for( unsigned int i=0; i<ptrarray->len; i++ ) - ((Cell*)g_ptr_array_index(ptrarray, i))->AddBlock( this ); +{ + for( unsigned int i=0; i<ptrarray->len; i++ ) + ((Cell*)g_ptr_array_index(ptrarray, i))->AddBlock( this ); } @@ -239,7 +240,7 @@ void Block::UnMap() { RemoveFromCellArray( rendered_cells ); - + g_ptr_array_set_size( rendered_cells, 0 ); mapped = false; } @@ -248,7 +249,7 @@ { RemoveFromCellArray( rendered_cells ); AddToCellArray( candidate_cells ); - + // switch current and candidate cell pointers GPtrArray* tmp = rendered_cells; rendered_cells = candidate_cells; @@ -261,7 +262,7 @@ { Size bgsize = mod->blockgroup.GetSize(); stg_point3_t bgoffset = mod->blockgroup.GetOffset(); - + return stg_point_t( (bpt.x - bgoffset.x) * (mod->geom.size.x/bgsize.x), (bpt.y - bgoffset.y) * (mod->geom.size.y/bgsize.y)); } @@ -272,11 +273,11 @@ { // no valid cache of model coord points, so generate them mpts = new stg_point_t[pt_count]; - + for( unsigned int i=0; i<pt_count; i++ ) mpts[i] = BlockPointToModelMeters( pts[i] ); } - + return mpts; } @@ -284,7 +285,7 @@ { // this doesn't happen often, so this simple strategy isn't too wasteful if( mpts ) - { + { delete[] mpts; mpts = NULL; } @@ -293,7 +294,7 @@ void Block::GenerateCandidateCells() { stg_point_t* mpts = GetPointsInModelCoords(); - + // convert the mpts in model coords into global coords stg_point_t* gpts = new stg_point_t[pt_count]; for( unsigned int i=0; i<pt_count; i++ ) @@ -312,11 +313,11 @@ gpose.z += mod->geom.pose.z; double scalez = mod->geom.size.z / mod->blockgroup.GetSize().z; stg_meters_t z = gpose.z - mod->blockgroup.GetOffset().z; - + // store the block's absolute z bounds at this rendering global_z.min = (scalez * local_z.min) + z; - global_z.max = (scalez * local_z.max) + z; - + global_z.max = (scalez * local_z.max) + z; + mapped = true; } @@ -327,9 +328,9 @@ b = tmp; } -void Block::Rasterize( uint8_t* data, - unsigned int width, - unsigned int height, +void Block::Rasterize( uint8_t* data, + unsigned int width, + unsigned int height, stg_meters_t cellwidth, stg_meters_t cellheight ) { @@ -341,38 +342,38 @@ // convert points from local to model coords stg_point_t mpt1 = BlockPointToModelMeters( pts[i] ); stg_point_t mpt2 = BlockPointToModelMeters( pts[(i+1)%pt_count] ); - + // record for debug visualization mod->rastervis.AddPoint( mpt1.x, mpt1.y ); - + // shift to the bottom left of the model mpt1.x += mod->geom.size.x/2.0; mpt1.y += mod->geom.size.y/2.0; mpt2.x += mod->geom.size.x/2.0; mpt2.y += mod->geom.size.y/2.0; - + // convert from meters to cells stg_point_int_t a( floor( mpt1.x / cellwidth ), floor( mpt1.y / cellheight )); stg_point_int_t b( floor( mpt2.x / cellwidth ), floor( mpt2.y / cellheight ) ); - + bool steep = abs( b.y-a.y ) > abs( b.x-a.x ); if( steep ) { swap( a.x, a.y ); swap( b.x, b.y ); } - + if( a.x > b.x ) { swap( a.x, b.x ); swap( a.y, b.y ); } - + double dydx = (double) (b.y - a.y) / (double) (b.x - a.x); double y = a.y; - for(int x=a.x; x<=b.x; x++) + for(int x=a.x; x<=b.x; x++) { if( steep ) { @@ -388,9 +389,9 @@ if( ! (floor(y) >= 0) ) continue; if( ! (floor(y) < (int)height) ) continue; } - + if( steep ) - data[ (int)floor(y) + (x * width)] = 1; + data[ (int)floor(y) + (x * width)] = 1; else data[ x + ((int)floor(y) * width)] = 1; y += dydx; @@ -406,11 +407,11 @@ for( unsigned int i=0; i<pt_count; i++ ) glVertex3f( pts[i].x, pts[i].y, local_z.max ); glEnd(); -} +} void Block::DrawSides() { - // construct a strip that wraps around the polygon + // construct a strip that wraps around the polygon glBegin(GL_QUAD_STRIP); for( unsigned int p=0; p<pt_count; p++) { @@ -433,28 +434,28 @@ void Block::Draw( Model* mod ) { - // draw filled color polygons - stg_color_t col = inherit_color ? mod->color : color; - + // 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(); } @@ -472,27 +473,27 @@ void Block::Load( Worldfile* wf, int entity ) { //printf( "Block::Load entity %d\n", entity ); - + if( pts ) delete[] pts; - + pt_count = wf->ReadInt( entity, "points", 0); pts = new stg_point_t[ pt_count ]; - + //printf( "reading %d points\n", // pt_count ); - - char key[128]; + + char key[128]; for( unsigned int p=0; p<pt_count; p++ ) { snprintf(key, sizeof(key), "point[%d]", p ); - + pts[p].x = wf->ReadTupleLength(entity, key, 0, 0); pts[p].y = wf->ReadTupleLength(entity, key, 1, 0); } - + local_z.min = wf->ReadTupleLength( entity, "z", 0, 0.0 ); local_z.max = wf->ReadTupleLength( entity, "z", 1, 1.0 ); - + const char* colorstr = wf->ReadString( entity, "color", NULL ); if( colorstr ) { @@ -503,5 +504,5 @@ inherit_color = true; } - - + + Modified: code/stage/trunk/libstage/worldfile.cc =================================================================== --- code/stage/trunk/libstage/worldfile.cc 2009-05-08 21:41:48 UTC (rev 7630) +++ code/stage/trunk/libstage/worldfile.cc 2009-05-09 15:16:57 UTC (rev 7631) @@ -68,11 +68,18 @@ // return g_str_hash( prop->key ); // } +void destroy_property(gpointer value) +{ + CProperty * prop = reinterpret_cast<CProperty *> (value); + free(prop->key); + free(prop->values); + g_free(value); +} /////////////////////////////////////////////////////////////////////////// // Default constructor -Worldfile::Worldfile() +Worldfile::Worldfile() { this->filename = NULL; @@ -96,7 +103,7 @@ this->unit_length = 1.0; this->unit_angle = M_PI / 180; - this->nametable = g_hash_table_new( g_str_hash, g_str_equal ); + this->nametable = g_hash_table_new_full( g_str_hash, g_str_equal, NULL, destroy_property ); } @@ -157,13 +164,13 @@ { // Shouldnt call load more than once, // so this should be null. - + if(this->filename == NULL) { this->filename = strdup(filename); } - - + + // Open the file //FILE *file = fopen(this->filename, "r"); FILE *file = FileOpen(this->filename, "r"); @@ -173,23 +180,23 @@ this->filename, strerror(errno)); return false; } - + ClearTokens(); - + // Read tokens from the file if (!LoadTokens(file, 0)) { //DumpTokens(); return false; } - + // Parse the tokens to identify entities if (!ParseTokens()) { //DumpTokens(); return false; } - + // Dump contents and exit if this file is meant for debugging only. if (ReadInt(0, "test", 0) != 0) { @@ -200,7 +207,7 @@ DumpProperties(); return false; } - + // Work out what the length units are const char *unit = ReadString(0, "unit_length", "m"); if (strcmp(unit, "m") == 0) @@ -209,14 +216,14 @@ this->unit_length = 0.01; else if (strcmp(unit, "mm") == 0) this->unit_length = 0.001; - + // Work out what the angle units are unit = ReadString(0, "unit_angle", "degrees"); if (strcmp(unit, "degrees") == 0) this->unit_angle = M_PI / 180; else if (strcmp(unit, "radians") == 0) this->unit_angle = 1; - + return true; } @@ -349,7 +356,7 @@ else if ( 0x0d == ch ) { ch = fgetc(file); - if ( 0x0a != ch ) + if ( 0x0a != ch ) ungetc(ch, file); line++; AddToken(TokenEOL, "\n", include); @@ -357,7 +364,7 @@ else if ( 0x0a == ch ) { ch = fgetc(file); - if ( 0x0d != ch ) + if ( 0x0d != ch ) ungetc(ch, file); line++; AddToken(TokenEOL, "\n", include); @@ -1335,7 +1342,7 @@ if( this->nametable ) g_hash_table_destroy( this->nametable ); - this->nametable = g_hash_table_new( g_str_hash, g_str_equal ); + this->nametable = g_hash_table_new_full( g_str_hash, g_str_equal, NULL, destroy_property ); } @@ -1388,7 +1395,7 @@ /////////////////////////////////////////////////////////////////////////// -// Get an property +// Get an property CProperty* Worldfile::GetProperty(int entity, const char *name) { char key[128]; @@ -1431,7 +1438,7 @@ /////////////////////////////////////////////////////////////////////////// -// Get the value of an property +// Get the value of an property const char *Worldfile::GetPropertyValue(CProperty* property, int index) { assert(property); @@ -1511,7 +1518,7 @@ WriteString(entity, name, "0" ); // compact zeros make the file // more readable else - { + { char default_str[64]; snprintf(default_str, sizeof(default_str), "%.3f", value); WriteString(entity, name, default_str); @@ -1545,12 +1552,12 @@ void Worldfile::WriteLength(int entity, const char *name, double value) { value /= this->unit_length; - + if( fabs(value) < 0.001 ) // nearly 0 WriteString(entity, name, "0" ); // compact zeros make the file // more readable else - { + { char default_str[64]; snprintf(default_str, sizeof(default_str), "%.3f", value ); WriteString(entity, name, default_str); @@ -1709,9 +1716,9 @@ int index, double value) { if( fabs(value) < 0.001 ) // nearly 0 - WriteTupleString(entity, name, index, "0" ); + WriteTupleString(entity, name, index, "0" ); else - { + { char default_str[64]; snprintf(default_str, sizeof(default_str), "%.3f", value); WriteTupleString(entity, name, index, default_str); @@ -1737,11 +1744,11 @@ int index, double value) { value /= this->unit_length; - + if( fabs(value) < 0.001 ) // nearly 0 - WriteTupleString(entity, name, index, "0" ); + WriteTupleString(entity, name, index, "0" ); else - { + { char default_str[64]; snprintf(default_str, sizeof(default_str), "%.3f", value ); WriteTupleString(entity, name, index, default_str); @@ -1766,11 +1773,11 @@ int index, double value) { value /= this->unit_angle; - + if( fabs(value) < 0.001 ) // nearly 0 - WriteTupleString(entity, name, index, "0" ); + WriteTupleString(entity, name, index, "0" ); else - { + { char default_str[64]; snprintf(default_str, sizeof(default_str), "%.3f", value ); WriteTupleString(entity, name, index, default_str); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |