Permalink
Browse files

removed glib2 dependency (after many years of good service. Thanks GL…

…ib.)
  • Loading branch information...
1 parent 5a18cfe commit 910541e2e0312aaee3597ffb597bbd2a7373e985 rtv committed Jul 22, 2009
View
14 CMakeLists.txt
@@ -84,14 +84,6 @@ include(FindPkgConfig)
MESSAGE( STATUS "Checking for required libraries..." )
SET( INDENT " * " )
-pkg_search_module( GLIB REQUIRED gthread-2.0 )
-IF( GLIB_FOUND )
-# MESSAGE( STATUS ${INDENT} "Glib version ${GLIB_VERSION} detected at ${GLIB_PREFIX}" )
-# MESSAGE( STATUS " GLIB_CFLAGS = ${GLIB_CFLAGS}" )
-# MESSAGE( STATUS " GLIB_LDFLAGS = ${GLIB_LDFLAGS}" )
-ELSE( GLIB_FOUND )
- MESSAGE( ${INDENT} "Glib not detected" )
-ENDIF( GLIB_FOUND )
pkg_search_module( LIBPNG REQUIRED libpng )
IF( LIBPNG_FOUND )
@@ -203,16 +195,14 @@ MESSAGE( STATUS "Installation path CMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}"
include_directories( .
libstage
replace
- ${GLIB_INCLUDE_DIRS}
${LIBPNG_INCLUDE_DIRS}
${CMAKE_INCLUDE_PATH}
- ${WEBSIM_INCLUDE_DIRS}
+ ${WEBSIM_INCLUDE_DIRS}
)
# all targets need these library directories
-link_directories(${GLIB_LIBRARY_DIRS}
- ${GLIB_LIBRARY_DIRS}
+link_directories(
${LIBPNG_LIBRARY_DIRS}
${WEBSIM_LIBRARY_DIRS}
)
View
2 examples/ctrl/CMakeLists.txt
@@ -14,6 +14,8 @@ SET( PLUGINS
# SET( PLUGINS ${PLUGINS} fasr_plan )
#endif( PLAYER_FOUND )
+set_source_files_properties( ${PLUGINS} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" )
+
# create a library module for each plugin and link libstage to each
foreach( PLUGIN ${PLUGINS} )
View
4 examples/ctrl/fasr.cc
@@ -236,9 +236,9 @@ class Robot
}
if( i > sample_count/2 )
- minleft = MIN( minleft, scan[i].range );
+ minleft = std::min( minleft, scan[i].range );
else
- minright = MIN( minright, scan[i].range );
+ minright = std::min( minright, scan[i].range );
}
if( verbose )
View
4 examples/ctrl/wander.cc
@@ -76,9 +76,9 @@ int LaserUpdate( Model* mod, robot_t* robot )
}
if( i > sample_count/2 )
- minleft = MIN( minleft, scan[i].range );
+ minleft = std::min( minleft, scan[i].range );
else
- minright = MIN( minright, scan[i].range );
+ minright = std::min( minright, scan[i].range );
}
if( verbose )
View
21 libstage/CMakeLists.txt
@@ -4,6 +4,7 @@ MESSAGE( STATUS "Configuring libstage" )
include_directories(${PROJECT_BINARY_DIR})
set( stageSrcs
+ model_props.cc
ancestor.cc
block.cc
blockgroup.cc
@@ -26,7 +27,6 @@ set( stageSrcs
model_laser.cc
model_load.cc
model_position.cc
- model_props.cc
model_ranger.cc
model_loadcell.cc
model_lightindicator.cc
@@ -48,25 +48,14 @@ set( stageSrcs
options_dlg.hh
)
-# TODO
-# puck.cc
-
-# todo - build without FLTK - tricky since we use its image loading, etc.
-#if( BUILD_GUI )
-# list( APPEND stageSrcs
-# # gui-only sources go here
-# )
-#endif( BUILD_GUI )
-
-add_library(stage SHARED ${stageSrcs})
-
set_source_files_properties( ${stageSrcs} PROPERTIES COMPILE_FLAGS "${FLTK_CFLAGS}" )
+
+add_library(stage SHARED ${stageSrcs})
target_link_libraries( stage
- ${GLIB_LIBRARIES}
${OPENGL_LIBRARIES}
- ${LTDL_LIB}
- ${FLTK_LIBS2}
+ ${LTDL_LIB}
+ ${FLTK_LIBS2}
)
# causes the shared library to have a version number
View
5 libstage/ancestor.cc
@@ -5,10 +5,13 @@ using namespace Stg;
Ancestor::Ancestor() :
children(),
debug( false ),
- token( NULL )
+ token( NULL ),
+ access_mutex()
{
for( int i=0; i<MODEL_TYPE_COUNT; i++ )
child_type_counts[i] = 0;
+
+ pthread_mutex_init( &access_mutex, NULL );
}
Ancestor::~Ancestor()
View
2 libstage/blockgroup.cc
@@ -86,7 +86,7 @@ void BlockGroup::CalcSize()
if( pt->y > maxy ) maxy = pt->y;
}
- size.z = MAX( block->local_z.max, size.z );
+ size.z = std::max( block->local_z.max, size.z );
}
// store these bounds for normalization purposes
View
53 libstage/canvas.cc
@@ -66,7 +66,7 @@ Canvas::Canvas( WorldGui* world,
last_selection( NULL ),
interval( 50 ), //msec between redraws
// initialize Option objects
- showBlinken( "Blinkenlights", "show_blinkenlights", "", true, world ),
+ // showBlinken( "Blinkenlights", "show_blinkenlights", "", true, world ),
showBlocks( "Blocks", "show_blocks", "b", true, world ),
showClock( "Clock", "show_clock", "c", true, world ),
showData( "Data", "show_data", "d", false, world ),
@@ -91,8 +91,7 @@ Canvas::Canvas( WorldGui* world,
frames_rendered_count( 0 ),
screenshot_frame_skip( 1 )
{
- end();
-
+ end();
//show(); // must do this so that the GL context is created before configuring GL
// but that line causes a segfault in Linux/X11! TODO: test in OS X
@@ -888,15 +887,27 @@ void Canvas::renderFrame()
DrawFloor();
if( showFootprints )
- {
- glDisable( GL_DEPTH_TEST );
-
- FOR_EACH( it, models_sorted )
- (*it)->DrawTrailFootprint();
-
- glEnable( GL_DEPTH_TEST );
- }
+ {
+ glDisable( GL_DEPTH_TEST ); // using alpha blending
+
+ FOR_EACH( it, models_sorted )
+ (*it)->DrawTrailFootprint();
+
+ glEnable( GL_DEPTH_TEST );
+ }
+
+ if( showFlags )
+ FOR_EACH( it, models_sorted )
+ (*it)->DrawFlagList();
+ if( showTrailArrows )
+ FOR_EACH( it, models_sorted )
+ (*it)->DrawTrailArrows();
+
+ if( showTrailRise )
+ FOR_EACH( it, models_sorted )
+ (*it)->DrawTrailBlocks();
+
if( showBlocks )
DrawBlocks();
@@ -905,8 +916,6 @@ void Canvas::renderFrame()
// TODO - finish this properly
//LISTMETHOD( models_sorted, Model*, DrawWaypoints );
-
-
// MOTION BLUR
if( 0 )//showBlur )
@@ -1030,18 +1039,8 @@ void Canvas::renderFrame()
FOR_EACH( it, models_sorted )
(*it)->DrawGrid();
- if( showFlags )
- FOR_EACH( it, models_sorted )
- (*it)->DrawFlagList();
-
- if( showBlinken )
- FOR_EACH( it, models_sorted )
- (*it)->DrawBlinkenlights();
-
if( showStatus )
{
- glDisable( GL_DEPTH_TEST );
-
glPushMatrix();
//ensure two icons can't be in the exact same plane
if( camera.pitch() == 0 && !pCamOn )
@@ -1050,7 +1049,6 @@ void Canvas::renderFrame()
FOR_EACH( it, models_sorted )
(*it)->DrawStatusTree( &camera );
- glEnable( GL_DEPTH_TEST );
glPopMatrix();
}
@@ -1089,7 +1087,7 @@ void Canvas::renderFrame()
std::string clockstr = world->ClockString();
if( showFollow == true && last_selection )
- clockstr.append( " [ FOLLOW MODE ]" );
+ clockstr.append( " [FOLLOW MODE]" );
float txtWidth = gl_width( clockstr.c_str());
if( txtWidth < 200 ) txtWidth = 200;
@@ -1253,11 +1251,10 @@ void Canvas::createMenuItems( Fl_Menu_Bar* menu, std::string path )
pCamOn.createMenuItem( menu, path );
pCamOn.menuCallback( perspectiveCb, this );
showOccupancy.createMenuItem( menu, path );
- //showTrailArrows.createMenuItem( menu, path ); // broken
+ showTrailArrows.createMenuItem( menu, path );
showTrails.createMenuItem( menu, path );
- //showTrailRise.createMenuItem( menu, path ); // broken
+ showTrailRise.createMenuItem( menu, path ); // broken
showBBoxes.createMenuItem( menu, path );
- //showBlur.createMenuItem( menu, path );
showTree.createMenuItem( menu, path );
showScreenshots.createMenuItem( menu, path );
}
View
4 libstage/canvas.hh
@@ -77,7 +77,7 @@ namespace Stg
void AddModel( Model* mod );
void RemoveModel( Model* mod );
- Option showBlinken,
+ Option //showBlinken,
showBlocks,
showClock,
showData,
@@ -134,7 +134,7 @@ namespace Stg
inline void setDirtyBuffer( void ) { dirty_buffer = true; }
inline bool dirtyBuffer( void ) const { return dirty_buffer; }
- inline void PushColor( Color col )
+ void PushColor( Color col )
{ colorstack.Push( col ); }
void PushColor( double r, double g, double b, double a )
View
2 libstage/color.cc
@@ -49,7 +49,7 @@ Color::Color( const char *name) :
PRINT_DEBUG( "Success!" );
// load the file into the hash table
- while (TRUE)
+ while(1)
{
char line[1024];
if (!fgets(line, sizeof(line), file))
View
5 libstage/gl.cc
@@ -36,7 +36,6 @@ void Stg::Gl::draw_array( float x, float y, float w, float h,
glVertex3f( x + (float)i*sample_spacing, y+(data[(i+offset)%len]-min)*yscale, 0.01 );
glEnd();
-
glColor3f( 0,0,0 );
char buf[64];
@@ -56,8 +55,8 @@ void Stg::Gl::draw_array( float x, float y, float w, float h,
for( size_t i=0; i<len; i++ )
{
- smallest = MIN( smallest, data[i] );
- largest = MAX( largest, data[i] );
+ smallest = std::min( smallest, data[i] );
+ largest = std::max( largest, data[i] );
}
draw_array( x,y,w,h,data,len,offset,smallest,largest );
View
66 libstage/model.cc
@@ -188,19 +188,17 @@ void GuiState::Load( Worldfile* wf, int wf_entity )
// constructor
Model::Model( World* world,
- Model* parent,
- const stg_model_type_t type )
+ Model* parent,
+ const stg_model_type_t type )
: Ancestor(),
- access_mutex(NULL),
- blinkenlights( g_ptr_array_new() ),
blockgroup(),
blocks_dl(0),
boundary(false),
- callbacks(),// g_hash_table_new( g_direct_hash, g_direct_equal ) ),
+ callbacks(),
color( 1,0,0 ), // red
data_fresh(false),
disabled(false),
- cv_list(),
+ cv_list(),
flag_list(),
geom(),
has_default_block( true ),
@@ -216,27 +214,27 @@ Model::Model( World* world,
pose(),
power_pack( NULL ),
pps_charging(),
- props(NULL),
+ props(),
rastervis(),
rebuild_displaylist(true),
say_string(NULL),
stall(false),
subs(0),
thread_safe( false ),
trail(),
- trail_length(25),
- trail_interval(5),
+ trail_length(50),
+ trail_interval(5),
type(type),
- update_list_num( -1 ),
+ update_list_num( -1 ),
used(false),
velocity(),
watts(0.0),
- watts_give(0.0),
- watts_take(0.0),
+ watts_give(0.0),
+ watts_take(0.0),
wf(NULL),
wf_entity(0),
world(world),
- world_gui( dynamic_cast<WorldGui*>( world ) )
+ world_gui( dynamic_cast<WorldGui*>( world ) )
{
//assert( modelsbyid );
assert( world );
@@ -262,8 +260,7 @@ Model::Model( World* world,
world->AddModel( this );
this->friction = DEFAULT_FRICTION;
- g_datalist_init( &this->props );
-
+
// now we can add the basic square shape
AddBlockRect( -0.5, -0.5, 1.0, 1.0, 1.0 );
@@ -283,10 +280,6 @@ Model::~Model( void )
ModelPtrVec& vec = parent ? parent->children : world->children;
vec.erase( std::remove( vec.begin(), vec.end(), this ));
- //if( callbacks ) g_hash_table_destroy( callbacks );
-
- g_datalist_clear( &props );
-
modelsbyid.erase(id);
world->RemoveModel( this );
@@ -701,7 +694,15 @@ void Model::Update( void )
// if we're drawing current and a power pack has been installed
+ if( trail_length > 0 && world->updates % trail_interval == 0 )
+ {
+ trail.push_back( TrailItem( world->sim_time, GetGlobalPose(), color ) );
+
+ if( trail.size() > trail_length )
+ trail.pop_front();
+ }
+ // TODO - this is not thread-safe! fix!
PowerPack* pp = FindPowerPack();
if( pp && ( watts > 0 ))
{
@@ -813,7 +814,7 @@ void Model::UpdateCharge()
//printf( " toucher %s can take up to %.2f wats\n",
// toucher->Token(), toucher->watts_take );
- stg_watts_t rate = MIN( watts_give, toucher->watts_take );
+ stg_watts_t rate = std::min( watts_give, toucher->watts_take );
stg_joules_t amount = rate * (world->interval_sim * 1e-6);
//printf ( "moving %.2f joules from %s to %s\n",
@@ -865,15 +866,7 @@ void Model::UpdatePose( void )
{
if( disabled )
return;
-
- if( world->updates % trail_interval == 0 )
- {
- trail.push_back( TrailItem( world->sim_time, GetGlobalPose(), color ) );
-
- if( trail.size() > trail_length )
- trail.pop_front();
- }
-
+
// convert usec to sec
double interval( (double)world->interval_sim / 1e6 );
@@ -890,21 +883,6 @@ void Model::UpdatePose( void )
SetStall( ConditionalMove( pose + p ) );
}
-
-int Model::TreeToPtrArray( GPtrArray* array ) const
-{
- g_ptr_array_add( array, (void*)this );
-
- //printf( " added %s to array at %p\n", root->token, array );
-
- int added = 1;
-
- FOR_EACH( it, children )
- added += (*it)->TreeToPtrArray( array );
-
- return added;
-}
-
Model* Model::GetUnsubscribedModelOfType( stg_model_type_t type ) const
{
if( (this->type == type) && (this->subs == 0) )
View
2 libstage/model_actuator.cc
@@ -76,7 +76,7 @@ ModelActuator::ModelActuator( World* world,
// sensible position defaults
this->SetVelocity( Velocity(0,0,0,0) );
- this->SetBlobReturn( TRUE );
+ this->SetBlobReturn(true);
}
View
4 libstage/model_blobfinder.cc
@@ -243,8 +243,8 @@ void ModelBlobfinder::Update( void )
int blobtop = scan_height/2 - (int)(startyangle/yRadsPerPixel);
int blobbottom = scan_height/2 -(int)(endyangle/yRadsPerPixel);
- blobtop = MAX( blobtop, 0 );
- blobbottom = MIN( blobbottom, (int)scan_height );
+ blobtop = std::max( blobtop, 0 );
+ blobbottom = std::min( blobbottom, (int)scan_height );
// fill in an array entry for this blob
Blob blob;
View
188 libstage/model_draw.cc
@@ -52,104 +52,86 @@ void Model::DrawTrailFootprint()
{
double darkness = 0;
double fade = 0.5 / (double)(trail_length+1);
-
+
+ PushColor( 0,0,0,1 ); // dummy pushL just saving the color
+
FOR_EACH( it, trail )
- {
- TrailItem& checkpoint = *it;
-
- glPushMatrix();
- Pose pz = checkpoint.pose;
- pz.z += 0.02;
- Gl::pose_shift( pz );
- Gl::pose_shift( geom.pose );
-
- darkness += fade;
-
- Color c = checkpoint.color;
- c.a = darkness;
- PushColor( c );
-
- blockgroup.DrawFootPrint( geom );
-
- glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
- PushColor( c.r/2, c.g/2, c.b/2, darkness );
-
- blockgroup.DrawFootPrint( geom );
-
- PopColor();
- PopColor();
- glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
- glPopMatrix();
+ {
+ TrailItem& checkpoint = *it;
+
+ glPushMatrix();
+ Pose pz = checkpoint.pose;
+
+ Gl::pose_shift( pz );
+ Gl::pose_shift( geom.pose );
+
+ darkness += fade;
+ Color c = checkpoint.color;
+ c.a = darkness;
+ glColor4f( c.r, c.g, c.b, c.a );
+
+ blockgroup.DrawFootPrint( geom );
+
+ glPopMatrix();
}
+
+ PopColor();
}
void Model::DrawTrailBlocks()
{
double timescale = 0.0000001;
FOR_EACH( it, trail )
- {
- TrailItem& checkpoint = *it;
-
- Pose pose = checkpoint.pose;
- pose.z = (world->sim_time - checkpoint.time) * timescale;
+ {
+ TrailItem& checkpoint = *it;
+
+ glPushMatrix();
+ Pose pz = checkpoint.pose;
+ pz.z = (world->sim_time - checkpoint.time) * timescale;
+
+ Gl::pose_shift( pz );
+ Gl::pose_shift( geom.pose );
+
+ DrawBlocks();
- PushLocalCoords();
- DrawBlocksTree();
- PopCoords();
+ glPopMatrix();
}
}
void Model::DrawTrailArrows()
{
double dx = 0.2;
double dy = 0.07;
- double timescale = 0.0000001;
-
- FOR_EACH( it, trail )
- {
- TrailItem& checkpoint = *it;
-
- Pose pose = checkpoint.pose;
- pose.z = (world->sim_time - checkpoint.time) * timescale;
+ double timescale = 1e-7;
+
+ PushColor( 0,0,0,1 ); // dummy push
- PushLocalCoords();
+ FOR_EACH( it, trail )
+ {
+ TrailItem& checkpoint = *it;
- // set the height proportional to age
-
- PushColor( checkpoint.color );
-
- glEnable(GL_POLYGON_OFFSET_FILL);
- glPolygonOffset(1.0, 1.0);
-
- glBegin( GL_TRIANGLES );
- glVertex3f( 0, -dy, 0);
- glVertex3f( dx, 0, 0 );
- glVertex3f( 0, +dy, 0 );
- glEnd();
- glDisable(GL_POLYGON_OFFSET_FILL);
-
- glPolygonMode( GL_FRONT_AND_BACK, GL_LINE );
-
- Color c = checkpoint.color;
- c.r /= 2.0;
- c.g /= 2.0;
- c.b /= 2.0;
- PushColor( c ); // darker color
-
- glDepthMask(GL_FALSE);
- glBegin( GL_TRIANGLES );
- glVertex3f( 0, -dy, 0);
- glVertex3f( dx, 0, 0 );
- glVertex3f( 0, +dy, 0 );
- glEnd();
- glDepthMask(GL_TRUE);
-
- glPolygonMode( GL_FRONT_AND_BACK, GL_FILL );
- PopColor();
- PopColor();
- PopCoords();
- }
+ glPushMatrix();
+ Pose pz = checkpoint.pose;
+ // set the height proportional to age
+ pz.z = (world->sim_time - checkpoint.time) * timescale;
+
+ Gl::pose_shift( pz );
+ Gl::pose_shift( geom.pose );
+
+ Color& c = checkpoint.color;
+ glColor4f( c.r, c.g, c.b, c.a );
+
+ glBegin( GL_TRIANGLES );
+ glVertex3f( 0, -dy, 0);
+ glVertex3f( dx, 0, 0 );
+ glVertex3f( 0, +dy, 0 );
+ glEnd();
+
+ glPopMatrix();
+ }
+
+ PopColor();
}
void Model::DrawOriginTree()
@@ -474,39 +456,39 @@ void Model::DrawFlagList( void )
}
-void Model::DrawBlinkenlights()
-{
- PushLocalCoords();
+// void Model::DrawBlinkenlights()
+// {
+// PushLocalCoords();
- GLUquadric* quadric = gluNewQuadric();
- //glTranslatef(0,0,1); // jump up
- //Pose gpose = GetGlobalPose();
- //glRotatef( 180 + rtod(-gpose.a),0,0,1 );
+// GLUquadric* quadric = gluNewQuadric();
+// //glTranslatef(0,0,1); // jump up
+// //Pose gpose = GetGlobalPose();
+// //glRotatef( 180 + rtod(-gpose.a),0,0,1 );
- for( unsigned int i=0; i<blinkenlights->len; i++ )
- {
- stg_blinkenlight_t* b =
- (stg_blinkenlight_t*)g_ptr_array_index( blinkenlights, i );
- assert(b);
+// for( unsigned int i=0; i<blinkenlights->len; i++ )
+// {
+// stg_blinkenlight_t* b =
+// (stg_blinkenlight_t*)g_ptr_array_index( blinkenlights, i );
+// assert(b);
- glTranslatef( b->pose.x, b->pose.y, b->pose.z );
+// glTranslatef( b->pose.x, b->pose.y, b->pose.z );
- PushColor( b->color );
+// PushColor( b->color );
- if( b->enabled )
- gluQuadricDrawStyle( quadric, GLU_FILL );
- else
- gluQuadricDrawStyle( quadric, GLU_LINE );
+// if( b->enabled )
+// gluQuadricDrawStyle( quadric, GLU_FILL );
+// else
+// gluQuadricDrawStyle( quadric, GLU_LINE );
- gluSphere( quadric, b->size/2.0, 8,8 );
+// gluSphere( quadric, b->size/2.0, 8,8 );
- PopColor();
- }
+// PopColor();
+// }
- gluDeleteQuadric( quadric );
+// gluDeleteQuadric( quadric );
- PopCoords();
-}
+// PopCoords();
+// }
void Model::DrawPicker( void )
{
View
2 libstage/model_gripper.cc
@@ -468,7 +468,7 @@ void ModelGripper::UpdateContacts()
// // calculate how far closed we can get the paddles now
double puckw = hitgeom.size.y;
double gripperw = geom.size.y;
- cfg.close_limit = MAX( 0.0, 1.0 - puckw/(gripperw - cfg.paddle_size.y/2.0 ));
+ cfg.close_limit = std::max( 0.0, 1.0 - puckw/(gripperw - cfg.paddle_size.y/2.0 ));
}
}
}
View
2 libstage/model_laser.cc
@@ -175,7 +175,7 @@ void ModelLaser::Update( void )
double bearing( -fov/2.0 );
// make the first and last rays exactly at the extremes of the FOV
- double sample_incr( fov / MAX(sample_count-1,1) );
+ double sample_incr( fov / std::max(sample_count-1, (unsigned int)1) );
// find the global origin of our first emmitted ray
Pose rayorg( geom.pose );
View
1 libstage/model_load.cc
@@ -295,7 +295,6 @@ void Model::Save( void )
}
-// todo - use GLib's portable module code here
void Model::LoadControllerModule( char* lib )
{
//printf( "[Ctrl \"%s\"", lib );
View
18 libstage/model_position.cc
@@ -105,7 +105,7 @@ ModelPosition::ModelPosition( World* world,
this->SetVelocity( Velocity(0,0,0,0) );
- this->SetBlobReturn( TRUE );
+ this->SetBlobReturn( true );
AddVisualizer( &wpvis, true );
AddVisualizer( &posevis, false );
@@ -286,9 +286,9 @@ void ModelPosition::Update( void )
// this is easy - we just reduce the errors in each axis
// independently with a proportional controller, speed
// limited
- vel.x = MIN( x_error, max_speed_x );
- vel.y = MIN( y_error, max_speed_y );
- vel.a = MIN( a_error, max_speed_a );
+ vel.x = std::min( x_error, max_speed_x );
+ vel.y = std::min( y_error, max_speed_y );
+ vel.a = std::min( a_error, max_speed_a );
}
break;
@@ -310,8 +310,8 @@ void ModelPosition::Update( void )
{
PRINT_DEBUG( "TURNING ON THE SPOT" );
// turn on the spot to minimize the error
- calc.a = MIN( a_error, max_speed_a );
- calc.a = MAX( a_error, -max_speed_a );
+ calc.a = std::min( a_error, max_speed_a );
+ calc.a = std::max( a_error, -max_speed_a );
}
else
{
@@ -321,8 +321,8 @@ void ModelPosition::Update( void )
double goal_distance = hypot( y_error, x_error );
a_error = normalize( goal_angle - est_pose.a );
- calc.a = MIN( a_error, max_speed_a );
- calc.a = MAX( a_error, -max_speed_a );
+ calc.a = std::min( a_error, max_speed_a );
+ calc.a = std::max( a_error, -max_speed_a );
PRINT_DEBUG2( "steer errors: %.2f %.2f \n", a_error, goal_distance );
@@ -331,7 +331,7 @@ void ModelPosition::Update( void )
if( fabs(a_error) < M_PI/16 )
{
PRINT_DEBUG( "DRIVING TOWARDS THE GOAL" );
- calc.x = MIN( goal_distance, max_speed_x );
+ calc.x = std::min( goal_distance, max_speed_x );
}
}
View
13 libstage/model_props.cc
@@ -4,7 +4,7 @@ using namespace Stg;
#define MATCH(A,B) (strcmp(A,B)== 0)
-void* Model::GetProperty( const char* key ) const
+const void* Model::GetProperty( const char* key ) const
{
// see if the key has the predefined-property prefix
if( strncmp( key, MP_PREFIX, strlen(MP_PREFIX)) == 0 )
@@ -23,7 +23,9 @@ void* Model::GetProperty( const char* key ) const
}
// otherwise it may be an arbitrary named property
- return g_datalist_get_data( (GData**)&this->props, key ); // cast to discard const
+ //return g_datalist_get_data( (GData**)&this->props, key ); // cast to discard const
+
+ return props.find(key)->second;
}
int Model::SetProperty( const char* key,
@@ -81,7 +83,9 @@ int Model::SetProperty( const char* key,
}
// otherwise it's an arbitary property and we store the pointer
- g_datalist_set_data( &this->props, key, (void*)data );
+ //g_datalist_set_data( &this->props, key, (void*)data );
+ props[key] = data;
+
return 0; // ok
}
@@ -91,7 +95,8 @@ 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 );
+ //g_datalist_remove_data( &this->props, key );
+ props.erase( key );
}
View
4 libstage/model_ranger.cc
@@ -118,7 +118,7 @@ ModelRanger::ModelRanger( World* world,
this->SetGeom( Geom( Pose(), RANGER_SIZE ));
// spread the transducers around the ranger's body
- double offset = MIN(geom.size.x, geom.size.y) / 2.0;
+ double offset = std::min(geom.size.x, geom.size.y) / 2.0;
sensors.resize( RANGER_SENSORCOUNT );
@@ -276,7 +276,7 @@ void ModelRanger::Update( void )
ranger_match,
NULL );
- s.range = MAX( ray.range, s.bounds_range.min );
+ s.range = std::max( ray.range, s.bounds_range.min );
}
}
View
10 libstage/powerpack.cc
@@ -139,7 +139,7 @@ stg_joules_t PowerPack::RemainingCapacity() const
void PowerPack::Add( stg_joules_t j )
{
- stg_joules_t amount = MIN( RemainingCapacity(), j );
+ stg_joules_t amount = std::min( RemainingCapacity(), j );
stored += amount;
global_stored += amount;
@@ -154,7 +154,7 @@ void PowerPack::Subtract( stg_joules_t j )
return;
}
- stg_joules_t amount = MIN( stored, j );
+ stg_joules_t amount = std::min( stored, j );
stored -= amount;
global_stored -= amount;
@@ -168,10 +168,10 @@ void PowerPack::TransferTo( PowerPack* dest, stg_joules_t amount )
// if stored is non-negative we can't transfer more than the stored
// amount. If it is negative, we have infinite energy stored
if( stored >= 0.0 )
- amount = MIN( stored, amount );
+ amount = std::min( stored, amount );
// we can't transfer more than he can take
- amount = MIN( amount, dest->RemainingCapacity() );
+ amount = std::min( amount, dest->RemainingCapacity() );
//printf( "%s gives %.3f J to %s\n",
// mod->Token(), amount, dest->mod->Token() );
@@ -221,7 +221,7 @@ void PowerPack::SetStored( stg_joules_t j )
void PowerPack::Dissipate( stg_joules_t j )
{
- stg_joules_t amount = (stored < 0) ? j : MIN( stored, j );
+ stg_joules_t amount = (stored < 0) ? j : std::min( stored, j );
Subtract( amount );
dissipated += amount;
View
2 libstage/resource.cc
@@ -13,7 +13,7 @@ Flag* Flag::Nibble( double chunk )
if( size > 0 )
{
- chunk = MIN( chunk, this->size );
+ chunk = std::min( chunk, this->size );
piece = new Flag( this->color, chunk );
this->size -= chunk;
}
View
12 libstage/stage.cc
@@ -33,7 +33,7 @@ void Stg::Init( int* argc, char** argv[] )
if(!setlocale(LC_ALL,"POSIX"))
PRINT_WARN("Failed to setlocale(); config file may not be parse correctly\n" );
- if (!g_thread_supported ()) g_thread_init (NULL);
+ //if (!g_thread_supported ()) g_thread_init (NULL);
RegisterModels();
@@ -48,9 +48,9 @@ bool Stg::InitDone()
return init_called;
}
-static inline guchar* pb_get_pixel( Fl_Shared_Image* img, int x, int y )
+static inline uint8_t* pb_get_pixel( Fl_Shared_Image* img, int x, int y )
{
- guchar* pixels = (guchar*)(img->data()[0]);
+ uint8_t* pixels = (uint8_t*)(img->data()[0]);
unsigned int index = (y * img->w() * img->d()) + (x * img->d());
return( pixels + index );
}
@@ -66,15 +66,15 @@ static inline void pb_set_rect( Fl_Shared_Image* pb, int x, int y, int width, in
for( b = x; b < x+width; b++ )
{
// zeroing
- guchar* pix = pb_get_pixel( pb, b, a );
+ uint8_t* pix = pb_get_pixel( pb, b, a );
memset( pix, val, num_samples * bytes_per_sample );
}
}
// returns true if the value in the first channel is above threshold
-static inline gboolean pb_pixel_is_set( Fl_Shared_Image* img, int x, int y, int threshold )
+static inline bool pb_pixel_is_set( Fl_Shared_Image* img, int x, int y, int threshold )
{
- guchar* pixel = pb_get_pixel( img,x,y );
+ uint8_t* pixel = pb_get_pixel( img,x,y );
return( pixel[0] > threshold );
}
View
120 libstage/stage.hh
@@ -39,6 +39,7 @@
#include <string.h>
#include <sys/types.h>
#include <sys/time.h>
+#include <pthread.h>
// C++ libs
#include <cmath>
@@ -47,15 +48,10 @@
#include <list>
#include <map>
#include <ext/hash_map>
-//#include <unordered_map>
#include <set>
#include <queue>
#include <algorithm>
-// we use GLib's data structures extensively. Gradually we're moving
-// to C++ STL types. May be able to lose this dependency one day.
-#include <glib.h>
-
// FLTK Gui includes
#include <FL/Fl.H>
#include <FL/Fl_Box.H>
@@ -67,10 +63,10 @@
// except GLU & GLUT
#ifdef __APPLE__
#include <OpenGL/glu.h>
-#include <GLUT/glut.h>
+//#include <GLUT/glut.h>
#else
#include <GL/glu.h>
-#include <GL/glut.h>
+//#include <GL/glut.h>
#endif
/** @brief The Stage library uses its own namespace */
@@ -703,10 +699,11 @@ namespace Stg
friend class Canvas; // allow Canvas access to our private members
protected:
- ModelPtrVec children;
+ ModelPtrVec children;
bool debug;
char* token;
-
+ pthread_mutex_t access_mutex; ///< Used by Lock() and Unlock() to prevent parallel access to this model
+
void Load( Worldfile* wf, int section );
void Save( Worldfile* wf, int section );
@@ -733,6 +730,9 @@ namespace Stg
void SetToken( const char* str )
{ token = strdup( str ); } // minor memory leak
+
+ void Lock(){ pthread_mutex_lock( &access_mutex ); }
+ void Unlock(){ pthread_mutex_unlock( &access_mutex ); }
};
/** raytrace sample
@@ -820,8 +820,6 @@ namespace Stg
static bool quit_all; ///< quit all worlds ASAP
static void UpdateCb( World* world);
static unsigned int next_id; ///<initially zero, used to allocate unique sequential world ids
-
- GMutex* access_mutex; ///< Used by Lock() and Unlock() to prevent parallel access to this model
ModelPtrSet charge_list; ///< Models which receive charge are listed here
bool destroy;
@@ -851,10 +849,13 @@ namespace Stg
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
+ //GMutex* thread_mutex; ///< protect the worker thread management stuff
+ pthread_mutex_t thread_mutex; ///< protect the worker thread management stuff
unsigned int threads_working; ///< the number of worker threads not yet finished
- GCond* threads_start_cond; ///< signalled to unblock worker threads
- GCond* threads_done_cond; ///< signalled by last worker thread to unblock main thread
+ //GCond* threads_start_cond; ///< signalled to unblock worker threads
+ //GCond* threads_done_cond; ///< signalled by last worker thread to unblock main thread
+ pthread_cond_t threads_start_cond; ///< signalled to unblock worker threads
+ pthread_cond_t threads_done_cond; ///< signalled by last worker thread to unblock main thread
int total_subs; ///< the total number of subscriptions to all models
ModelPtrSet velocity_list; ///< Models with non-zero velocity and should have their poses updated
unsigned int worker_threads; ///< the number of worker threads to use
@@ -1003,7 +1004,7 @@ namespace Stg
void ChargeListAdd( Model* mod );
void ChargeListRemove( Model* mod );
- static gpointer update_thread_entry( std::pair<World*,int>* info );
+ static void* update_thread_entry( std::pair<World*,int>* info );
public:
/** returns true when time to quit, false otherwise */
@@ -1062,20 +1063,6 @@ namespace Stg
/** Return the floor model */
Model* GetGround() {return ground;};
- void Lock()
- {
- if( access_mutex == NULL )
- access_mutex = g_mutex_new();
-
- assert( access_mutex );
- g_mutex_lock( access_mutex );
- }
-
- void Unlock()
- {
- assert( access_mutex );
- g_mutex_unlock( access_mutex );
- }
};
class Block
@@ -1687,8 +1674,7 @@ namespace Stg
const std::vector<Option*>& getOptions() const { return drawOptions; }
protected:
- GMutex* access_mutex;
- GPtrArray* blinkenlights;
+ pthread_mutex_t access_mutex;
BlockGroup blockgroup;
/** OpenGL display list identifier for the blockgroup */
int blocks_dl;
@@ -1781,8 +1767,9 @@ namespace Stg
/** GData datalist can contain arbitrary named data items. Can be used
by derived model types to store properties, and for user code
to associate arbitrary items with a model. */
- GData* props;
-
+ //GData* props;
+ std::map<const char*,const void*> props;
+
/** Visualize the most recent rasterization operation performed by this model */
class RasterVis : public Visualizer
{
@@ -1865,20 +1852,20 @@ namespace Stg
unsigned int width, unsigned int height,
stg_meters_t cellwidth, stg_meters_t cellheight );
- void Lock()
- {
- if( access_mutex == NULL )
- access_mutex = g_mutex_new();
+// void Lock()
+// {
+// if( access_mutex == NULL )
+// access_mutex = g_mutex_new();
- assert( access_mutex );
- g_mutex_lock( access_mutex );
- }
+// assert( access_mutex );
+// g_mutex_lock( access_mutex );
+// }
- void Unlock()
- {
- assert( access_mutex );
- g_mutex_unlock( access_mutex );
- }
+// void Unlock()
+// {
+// assert( access_mutex );
+// g_mutex_unlock( access_mutex );
+// }
private:
/** Private copy constructor declared but not defined, to make it
@@ -1917,8 +1904,6 @@ namespace Stg
void MapFromRoot();
void UnMapFromRoot();
- int TreeToPtrArray( GPtrArray* array ) const;
-
/** raytraces a single ray from the point and heading identified by
pose, in local coords */
stg_raytrace_result_t Raytrace( const Pose &pose,
@@ -2000,26 +1985,21 @@ namespace Stg
void DrawTrailBlocks();
void DrawTrailArrows();
void DrawGrid();
- void DrawBlinkenlights();
+ // void DrawBlinkenlights();
void DataVisualizeTree( Camera* cam );
void DrawFlagList();
void DrawPose( Pose pose );
void LoadDataBaseEntries( Worldfile* wf, int entity );
public:
- virtual void PushColor( Color col )
- { world->PushColor( col ); }
-
- virtual void PushColor( double r, double g, double b, double a )
- { world->PushColor( r,g,b,a ); }
-
- virtual void PopColor()
- { world->PopColor(); }
+ virtual void PushColor( Color col ){ world->PushColor( col ); }
+ virtual void PushColor( double r, double g, double b, double a ){ world->PushColor( r,g,b,a ); }
+ virtual void PopColor() { world->PopColor(); }
PowerPack* FindPowerPack() const;
- void RecordRenderPoint( GSList** head, GSList* link,
- unsigned int* c1, unsigned int* c2 );
+ //void RecordRenderPoint( GSList** head, GSList* link,
+ // unsigned int* c1, unsigned int* c2 );
void PlaceInFreeSpace( stg_meters_t xmin, stg_meters_t xmax,
stg_meters_t ymin, stg_meters_t ymax );
@@ -2080,14 +2060,14 @@ namespace Stg
int GetFlagCount() const { return flag_list.size(); }
- /** Add a pointer to a blinkenlight to the model. */
- void AddBlinkenlight( stg_blinkenlight_t* b )
- { g_ptr_array_add( this->blinkenlights, b ); }
+// /** Add a pointer to a blinkenlight to the model. */
+// void AddBlinkenlight( stg_blinkenlight_t* b )
+// { g_ptr_array_add( this->blinkenlights, b ); }
- /** Clear all blinkenlights from the model. Does not destroy the
- blinkenlight objects. */
- void ClearBlinkenlights()
- { g_ptr_array_set_size( this->blinkenlights, 0 ); }
+// /** Clear all blinkenlights from the model. Does not destroy the
+// blinkenlight objects. */
+// void ClearBlinkenlights()
+// { g_ptr_array_set_size( this->blinkenlights, 0 ); }
/** Disable the model. Its pose will not change due to velocity
until re-enabled using Enable(). This is used for example when
@@ -2291,7 +2271,7 @@ namespace Stg
/** named-property interface
*/
- void* GetProperty( const char* key ) const;
+ const void* GetProperty( const char* key ) const;
bool GetPropertyFloat( const char* key, float* f, float defaultval ) const;
bool GetPropertyInt( const char* key, int* i, int defaultval ) const;
bool GetPropertyStr( const char* key, char** c, char* defaultval ) const;
@@ -2787,10 +2767,10 @@ private:
static Model* Create( World* world, Model* parent )
{ return new ModelRanger( world, parent ); }
- virtual void Load();
- virtual void Print( char* prefix );
-
- std::vector<Sensor> sensors;
+ virtual void Load();
+ virtual void Print( char* prefix );
+
+ std::vector<Sensor> sensors;
private:
static Option showRangerData;
View
4 libstage/texture_manager.cc
@@ -38,11 +38,11 @@ GLuint TextureManager::loadTexture( const char *filename )
//TODO check for correct width/height - or convert it.
- guchar* pixels = (guchar*)(img->data()[0]);
+ uint8_t* pixels = (uint8_t*)(img->data()[0]);
//vertically flip the image
int img_size = img->w() * img->h() * img->d();
- guchar* img_flip = new guchar[ img_size ];
+ uint8_t* img_flip = new uint8_t[ img_size ];
const int row_width = img->w() * img->d();
for( int i = 0; i < img->h(); i++ )
View
1 libstage/texture_manager.hh
@@ -11,7 +11,6 @@
#include "stage.hh"
#include <FL/Fl_Shared_Image.H>
-#include <glib.h>
#include <iostream>
namespace Stg
View
4 libstage/vis_strip.cc
@@ -58,6 +58,6 @@ void StripPlotVis::AppendValue( float value )
data[count%len] = value;
count++;
- min = MIN( value, min );
- max = MAX( value, max );
+ min = std::min( value, min );
+ max = std::max( value, max );
}
View
100 libstage/world.cc
@@ -56,7 +56,6 @@
#include <assert.h>
#include <string.h> // for strdup(3)
#include <locale.h>
-#include <glib-object.h> // for g_type_init() used by GDKPixbuf objects
#include <limits.h>
#include <libgen.h> // for dirname(3)
@@ -77,7 +76,6 @@ World::World( const char* token,
double ppm )
:
// private
- access_mutex(NULL),
charge_list(),
destroy( false ),
dirty( true ),
@@ -90,10 +88,10 @@ World::World( const char* token,
real_time_start( real_time_now ),
show_clock( false ),
show_clock_interval( 100 ), // 10 simulated seconds using defaults
- thread_mutex( g_mutex_new() ),
+ thread_mutex(),
threads_working( 0 ),
- threads_start_cond( g_cond_new() ),
- threads_done_cond( g_cond_new() ),
+ threads_start_cond(),
+ threads_done_cond(),
total_subs( 0 ),
velocity_list(),
worker_threads( 0 ),
@@ -121,20 +119,18 @@ World::World( const char* token,
PRINT_WARN( "Stg::Init() must be called before a World is created." );
exit(-1);
}
-
+
+ pthread_mutex_init( &thread_mutex, NULL );
+ pthread_cond_init( &threads_start_cond, NULL );
+ pthread_cond_init( &threads_done_cond, NULL );
+
World::world_set.insert( this );
}
-
World::~World( void )
{
PRINT_DEBUG2( "destroying world %d %s", id, token );
-
if( wf ) delete wf;
-
- //g_hash_table_destroy( models_by_name );
- g_free( token );
-
World::world_set.erase( this );
}
@@ -168,19 +164,26 @@ bool World::UpdateAll()
return quit;
}
-gpointer World::update_thread_entry( std::pair<World*,int> *thread_info )
+void* World::update_thread_entry( std::pair<World*,int> *thread_info )
{
World* world = thread_info->first;
int thread_instance = thread_info->second;
- g_mutex_lock( world->thread_mutex );
+ //g_mutex_lock( world->thread_mutex );
+ pthread_mutex_lock( &world->thread_mutex );
while( 1 )
{
// wait until the main thread signals us
//puts( "worker waiting for start signal" );
- g_cond_wait( world->threads_start_cond, world->thread_mutex );
- g_mutex_unlock( world->thread_mutex );
+
+ //g_cond_wait( world->threads_start_cond, world->thread_mutex );
+ //g_mutex_unlock( world->thread_mutex );
+ pthread_cond_wait( &world->threads_start_cond, &world->thread_mutex );
+ pthread_mutex_unlock( &world->thread_mutex );
+
+
+
//puts( "worker thread awakes" );
// loop over the list of rentrant models for this thread
@@ -197,11 +200,15 @@ gpointer World::update_thread_entry( std::pair<World*,int> *thread_info )
// done working, so increment the counter. If this was the last
// thread to finish working, signal the main thread, which is
// blocked waiting for this to happen
- g_mutex_lock( world->thread_mutex );
+
+ //g_mutex_lock( world->thread_mutex );
+ pthread_mutex_lock( &world->thread_mutex );
+
if( --world->threads_working == 0 )
{
//puts( "last worker signalling main thread" );
- g_cond_signal( world->threads_done_cond );
+ //g_cond_signal( world->threads_done_cond );
+ pthread_cond_signal( &world->threads_done_cond );
}
// keep lock going round the loop
}
@@ -216,11 +223,11 @@ void World::RemoveModel( Model* mod )
models_by_name.erase( mod->token );
}
-// wrapper to startup all models from the hash table
-void init_models( gpointer dummy1, Model* mod, gpointer dummy2 )
-{
- mod->Init();
-}
+// // wrapper to startup all models from the hash table
+// void init_models( gpointer dummy1, Model* mod, gpointer dummy2 )
+// {
+// mod->Init();
+// }
void World::LoadBlock( Worldfile* wf, int entity )
{
@@ -345,10 +352,19 @@ void World::Load( const char* worldfile_path )
for( unsigned int t=0; t<worker_threads; t++ )
{
std::pair<World*,int> *p = new std::pair<World*,int>( this, t+1 );
- g_thread_create( (GThreadFunc)World::update_thread_entry,
- p,
- false,
- NULL );
+ // g_thread_create( (GThreadFunc)World::update_thread_entry,
+ // p,
+ // false,
+ // NULL );
+
+ //normal posix pthread C function pointer
+ typedef void* (*func_ptr) (void*);
+
+ pthread_t pt;
+ pthread_create( &pt,
+ NULL,
+ (func_ptr)World::update_thread_entry,
+ p );
}
printf( "[threads %u]", worker_threads );
@@ -408,16 +424,14 @@ void World::UnLoad()
token = NULL;
}
-stg_usec_t World::RealTimeNow()
+inline stg_usec_t World::RealTimeNow()
{
- // TODO - move to GLib's portable timing code, so we can port to Windows more easily?
struct timeval tv;
gettimeofday( &tv, NULL ); // slow system call: use sparingly
-
return( tv.tv_sec*1000000 + tv.tv_usec );
}
-stg_usec_t World::RealTimeSinceStart()
+inline stg_usec_t World::RealTimeSinceStart()
{
stg_usec_t timenow = RealTimeNow();
@@ -549,20 +563,24 @@ bool World::Update()
if( worker_threads > 0 )
{
- g_mutex_lock( thread_mutex );
+ //g_mutex_lock( thread_mutex );
+ pthread_mutex_lock( &thread_mutex );
threads_working = worker_threads;
// unblock the workers - they are waiting on this condition var
//puts( "main thread signalling workers" );
- g_cond_broadcast( threads_start_cond );
+ //g_cond_broadcast( threads_start_cond );
+ pthread_cond_broadcast( &threads_start_cond );
// wait for all the last update job to complete - it will
// signal the worker_threads_done condition var
while( threads_working > 0 )
{
//puts( "main thread waiting for workers to finish" );
- g_cond_wait( threads_done_cond, thread_mutex );
+ //g_cond_wait( threads_done_cond, thread_mutex );
+ pthread_cond_wait( &threads_done_cond, &thread_mutex );
}
- g_mutex_unlock( thread_mutex );
+ //g_mutex_unlock( thread_mutex );
+ pthread_mutex_unlock( &thread_mutex );
//puts( "main thread awakes" );
// TODO: allow threadsafe callbacks to be called in worker
@@ -1043,12 +1061,12 @@ void World::ForEachCellInLine( const stg_point_int_t& start,
void World::Extend( stg_point3_t pt )
{
- extent.x.min = MIN( extent.x.min, pt.x);
- extent.x.max = MAX( extent.x.max, pt.x );
- extent.y.min = MIN( extent.y.min, pt.y );
- extent.y.max = MAX( extent.y.max, pt.y );
- extent.z.min = MIN( extent.z.min, pt.z );
- extent.z.max = MAX( extent.z.max, pt.z );
+ extent.x.min = std::min( extent.x.min, pt.x);
+ extent.x.max = std::max( extent.x.max, pt.x );
+ extent.y.min = std::min( extent.y.min, pt.y );
+ extent.y.max = std::max( extent.y.max, pt.y );
+ extent.z.min = std::min( extent.z.min, pt.z );
+ extent.z.max = std::max( extent.z.max, pt.z );
}
View
92 libstage/worldfile.cc
@@ -68,44 +68,33 @@ using namespace Stg;
// 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);
+// 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() :
+ token_size( 0),
+ token_count( 0),
+ tokens( NULL),
+ macro_size( 0),
+ macro_count( 0),
+ macros( NULL),
+ entity_size( 0),
+ entity_count( 0),
+ entities( NULL),
+ property_count( 0),
+ filename( NULL),
+ unit_length( 1.0),
+ unit_angle( M_PI / 180),
+ nametable()
{
- this->filename = NULL;
-
- this->token_size = 0;
- this->token_count = 0;
- this->tokens = NULL;
-
- this->macro_count = 0;
- this->macro_size = 0;
- this->macros = NULL;
-
- this->entity_count = 0;
- this->entity_size = 0;
- this->entities = NULL;
-
- this->property_count = 0;
- //this->property_size = 0;
- //this->properties = NULL;
-
- // Set defaults units
- this->unit_length = 1.0;
- this->unit_angle = M_PI / 180;
-
- // this attempt to fix memory leak breaks parsing of model properties - investigate
- //this->nametable = g_hash_table_new_full( g_str_hash, g_str_equal, NULL, destroy_property );
- this->nametable = g_hash_table_new( g_str_hash, g_str_equal );
}
@@ -118,8 +107,6 @@ Worldfile::~Worldfile()
ClearEntities();
ClearTokens();
- g_hash_table_destroy( this->nametable );
-
if (this->filename)
free(this->filename);
}
@@ -1326,7 +1313,7 @@ int Worldfile::LookupEntity(const char *type)
}
-void PrintProp( char* key, CProperty* prop, void* user )
+void PrintProp( const char* key, CProperty* prop )
{
if( prop )
printf( "Print key %s prop ent %d name %s\n", key, prop->entity, prop->name );
@@ -1338,7 +1325,8 @@ void Worldfile::DumpEntities()
{
printf("\n## begin entities\n");
- g_hash_table_foreach( this->nametable, (GHFunc)PrintProp, NULL );
+ FOR_EACH( it, nametable )
+ PrintProp( it->first.c_str(), it->second );
printf("## end entities\n");
}
@@ -1350,12 +1338,7 @@ void Worldfile::ClearProperties()
{
this->property_count = 0;
- if( this->nametable )
- g_hash_table_destroy( this->nametable );
-
- // this attempt to fix memory leak breaks parsing of model properties - investigate
- //this->nametable = g_hash_table_new_full( g_str_hash, g_str_equal, NULL, destroy_property );
- this->nametable = g_hash_table_new( g_str_hash, g_str_equal );
+ nametable.clear();
}
@@ -1364,7 +1347,7 @@ void Worldfile::ClearProperties()
CProperty* Worldfile::AddProperty(int entity, const char *name, int line)
{
//int i;
- CProperty *property = g_new0( CProperty, 1 );
+ CProperty *property = new CProperty();
property->entity = entity;
property->name = name;
@@ -1377,10 +1360,9 @@ CProperty* Worldfile::AddProperty(int entity, const char *name, int line)
snprintf( key, 127, "%d%s", entity, name );
property->key = strdup( key );
- // add this property to a hash table keyed by name for fast lookup
- g_hash_table_insert( nametable, property->key, property );
+ nametable[ property->key ] = property;
- //printf( "added key %s for prop %p entity %d name %s\n", key, property, property->entity, property->name );
+ //printf( "added key %s for prop %p entity %d name %s\n", property->key, property, property->entity, property->name );
this->property_count++;
@@ -1416,15 +1398,13 @@ CProperty* Worldfile::GetProperty(int entity, const char *name)
// printf( "looking up key %s for entity %d name %s\n", key, entity, name );
- // g_hash_table_foreach( this->nametable, (GHFunc)PrintProp, NULL );
-
- CProperty* prop = (CProperty*)g_hash_table_lookup( this->nametable, key );
-
- // if( prop )
- // printf( "found entity %d name %s\n", prop->entity, prop->name );
- // else
- // printf( "key %s not found\n", key );
-
+ CProperty* prop = nametable[ key ];
+
+// if( prop )
+// printf( "found entity %d name %s\n", prop->entity, prop->name );
+// else
+// printf( "key %s not found\n", key );
+
return prop;
}
View
8 libstage/worldfile.hh
@@ -30,7 +30,6 @@
#include <stdint.h> // for portable int types eg. uint32_t
#include <stdio.h> // for FILE ops
-#include <glib.h>
namespace Stg {
@@ -341,20 +340,19 @@ protected: FILE* FileOpen(const char *filename, const char* method);
private: int entity_count;
private: CEntity *entities;
-
// Property list
- //private: int property_size;
private: int property_count;
- //private: CProperty *properties;
- private: GHashTable* nametable;
+ private: CProperty *properties;
// Name of the file we loaded
public: char *filename;
// Conversion units
private: double unit_length;
private: double unit_angle;
+
+private: std::map<std::string,CProperty*> nametable;
};
};
View
2 libstage/worldgui.cc
@@ -367,7 +367,7 @@ bool WorldGui::Update()
// sleeptime );
if( (sleeptime > 0) || paused )
- usleep( (stg_usec_t)MIN(sleeptime,20000) ); // check the GUI at 10Hz min
+ usleep( (stg_usec_t)std::min(sleeptime,20000.0) ); // check the GUI at 10Hz min
}
} while( interval < interval_real );
View
31 libstageplugin/p_driver.cc
@@ -267,10 +267,10 @@ InterfaceModel::InterfaceModel( player_devaddr_t addr,
// configure the underlying driver to queue incoming commands and use a very long queue.
StgDriver::StgDriver(ConfigFile* cf, int section)
- : Driver(cf, section, false, 4096 )
+ : Driver(cf, section, false, 4096 ),
+ devices()
{
// init the array of device ids
- this->devices = g_ptr_array_new();
int device_count = cf->GetTupleCount( section, "provides" );
@@ -407,7 +407,8 @@ StgDriver::StgDriver(ConfigFile* cf, int section)
}
// store the Interaface in our device list
- g_ptr_array_add( this->devices, ifsrc );
+ //g_ptr_array_add( this->devices, ifsrc );
+ devices.push_back( ifsrc );
}
else
{
@@ -464,17 +465,18 @@ int StgDriver::Setup()
// todo - faster lookup with a better data structure
Interface* StgDriver::LookupDevice( player_devaddr_t addr )
{
- for( int i=0; i<(int)this->devices->len; i++ )
+ //for( int i=0; i<(int)this->devices->len; i++ )
+ FOR_EACH( it, this->devices )
{
- Interface* candidate =
- (Interface*)g_ptr_array_index( this->devices, i );
-
+ Interface* candidate = *it;
+ //(Interface*)g_ptr_array_index( this->devices, i );
+
if( candidate->addr.robot == addr.robot &&
- candidate->addr.interf == addr.interf &&
- candidate->addr.index == addr.index )
- return candidate; // found
+ candidate->addr.interf == addr.interf &&
+ candidate->addr.index == addr.index )
+ return candidate; // found
}
-
+
return NULL; // not found
}
@@ -580,9 +582,10 @@ void StgDriver::Update(void)
// puts( "STG driver update" );
- for( int i=0; i<(int)this->devices->len; i++ )
- {
- Interface* interface = (Interface*)g_ptr_array_index( this->devices, i );
+ //for( int i=0; i<(int)this->devices->len; i++ )
+ FOR_EACH( it, this->devices )
+ {
+ Interface* interface = *it; //(Interface*)g_ptr_array_index( this->devices, i );
assert( interface );
View
3 libstageplugin/p_driver.h
@@ -50,7 +50,8 @@ class StgDriver : public Driver
protected:
/// an array of pointers to Interface objects, defined below
- GPtrArray* devices;
+ //GPtrArray* devices;
+ std::vector<Interface*> devices;
};
View
11 libstageplugin/p_simulation.cc
@@ -102,11 +102,12 @@ InterfaceSimulation::InterfaceSimulation( player_devaddr_t addr,
}
// a little sanity testing
- if( !g_file_test( fullname, G_FILE_TEST_EXISTS ) )
- {
- PRINT_ERR1( "worldfile \"%s\" does not exist", worldfile_name );
- return;
- }
+ // XX TODO
+ // if( !g_file_test( fullname, G_FILE_TEST_EXISTS ) )
+ //{
+ // PRINT_ERR1( "worldfile \"%s\" does not exist", worldfile_name );
+ // return;
+ //}
// create a passel of Stage models in the local cache based on the
// worldfile

0 comments on commit 910541e

Please sign in to comment.